Modern Control System Lab Exercise

Nonlinear Dynamics of a Quad-rotor

Figure 1: Quad rotor

Assumptions
• Rigid body (no flexing).

• Symmetric structure (inertia matrix is diagonal).

• Thrust and drag are proportional to rotor speeds squared.

• Aerodynamic drag acts opposite to velocity in translational motion.

• Rotational drag (damping) opposes angular velocity.

Reference Frames
• Inertial frame (world frame, W): {x, y, z}.

• Body frame (B): Fixed to the quadrotor’s center of mass.

States
• Position (inertial frame): p = [x, y, z]T .

• Velocity (inertial frame): v = [ẋ, ẏ, ż]T .

• Orientation (Euler angles): η = [ϕ, θ, ψ]T (roll, pitch, yaw).

• Angular velocity (body frame): ω = [p, q, r]T .

1
Inputs (Rotor Thrusts)
Four rotors produce thrusts F1 , F2 , F3 , F4 :

• Total thrust: FT = F1 + F2 + F3 + F4 .

• Torques (roll, pitch, yaw):

τϕ = l(F4 − F2 ), τθ = l(F3 − F1 ), τψ = κ(F1 − F2 + F3 − F4 )

where l is the arm length and κ is the rotor torque coefficient.

Equations of Motion
Translational Dynamics (Newton-Euler)
 
0
mp̈ = mg + R⊤  0 
FT
where:

• m = mass,

• g = [0, 0, −g]T (gravity),

• R = rotation matrix (from body to inertial frame).

The rotation matrix R composed of rotations about the x, y, and z axes is given by:

R = Rx (ϕ) · Ry (θ) · Rz (ψ),

where the individual rotation matrices are:

     
1 0 0 cos θ 0 sin θ cos ψ − sin ψ 0
Rx (ϕ) = 0 cos ϕ − sin ϕ , Ry (θ) = 0 1 0 , Rz (ψ) =  sin ψ cos ψ 0 .
0 sin ϕ cos ϕ − sin θ 0 cos θ 0 0 1

The combined rotation matrix is:

 
cos θ cos ψ − cos θ sin ψ sin θ
R = cos ϕ sin ψ + sin ϕ sin θ cos ψ cos ϕ cos ψ − sin ϕ sin θ sin ψ − sin ϕ cos θ .
sin ϕ sin ψ − cos ϕ sin θ cos ψ sin ϕ cos ψ + cos ϕ sin θ sin ψ cos ϕ cos θ

Rotational Dynamics (Euler’s Equations)

Iω̇ + ω × Iω = τ
where:

• I = diag(Ixx , Iyy , Izz ) = inertia matrix,

• τ = [τϕ , τθ , τψ ]T .

2
Attitude Kinematics (Euler Angles)
Mapping velocities measured in body frame to inertial reference frame we use the following transfor-
mation matrix,
η̇ = Tω
where:  
1 sin ϕ tan θ cos ϕ tan θ
T = 0 cos ϕ − sin ϕ 
0 sin ϕ sec θ cos ϕ sec θ
For small angular rotation in x, y and z, the transformation becomes identity matrix, without losing
generality for small angle approximation we can assume the velocities in body frame are approximately
equal to velocities in inertial reference frame η̇ ≈ ω.

Drag Forces
Aerodynamic effects are modeled as:

Translational Drag
1
Fdrag = − ρCd A∥v∥v = k∥v∥v
2
where:

• ρ = air density,

• Cd = drag coefficient,

• A = reference area,

• v = velocity vector in inertial frame.

Linear drag approximation for small velocities:

Fdrag ≈ −Kt v, Kt = diag(kx , ky , kz )

Rotational Drag
τ drag = −Kr ω
where Kr = diag(kϕ , kθ , kψ ) is the damping matrix.

Modified Equations of Motion

Translational Dynamics (With Drag)
 
0
mp̈ = mg + R⊤  0  + Fdrag
FT
Expressed in components:

mẍ = FT (cos ϕ sin θ cos ψ + sin ϕ sin ψ) − kx ẋ

mÿ = FT (cos ϕ sin θ sin ψ − sin ϕ cos ψ) − ky ẏ

mz̈ = FT cos ϕ cos θ − mg − kz ż


3
Rotational Dynamics (With Damping)
Iω̇ + ω × Iω = τ + τ drag
Component-wise: 
Ixx ϕ̈ = (Iyy − Izz )θ̇ψ̇ + τϕ − kϕ ϕ̇

Iyy θ̈ = (Izz − Ixx )ϕ̇ψ̇ + τθ − kθ θ̇

Izz ψ̈ = (Ixx − Iyy )θ̇ϕ̇ + τψ − kψ ψ̇


Linearized Model with Small-Angle Approximations

For hovering near equilibrium (ϕ, θ, ψ ≈ 0, FT = mg + FL ), where FL is the force to move the drone to
another hovering vertical position.

Linearized Translational Dynamics (With Drag)

kx


 mẍ = (mg + FL )(θ) − kx ẋ =⇒ ẍ = gθ − ẋ

 m
ky

mÿ = (mg + FL )(− sin ϕ) − ky ẏ =⇒ ÿ = −gϕ − ẏ
 m
FL kz


mz̈ = FL − kz ż =⇒ z̈ = − ż


m m

Linear Rotational Dynamics (With Damping)

1 kϕ


ϕ̈ = τϕ − ϕ̇


 Ixx Ixx
 1 kθ
θ̈ = τθ − θ̇

 Iyy Iyy

 1 kψ
ψ̈ =
 τψ − ψ̇
Izz Izz

State-Space Representation
Selecting a state vector
 ⊤  ⊤
x = x1 x2 x3 x4 x5 x6 x7 x8 x9 x10 x11 x12 = x ẋ y ẏ z ż ϕ ϕ̇ θ θ̇ ψ ψ̇
The input vector is
 ⊤
u = FL τϕ τθ τψ
The output of the system includes the position and orientation of the drone,
 ⊤
y= x y z ϕ θ ψ
Modified A matrix including drag terms:
 
0 1 0 0 0 0 0 0 0 0 0 0
0 −kx /m 0 0 0 0 0 0 g 0 0 0 
 
0 0 0 1 0 0 0 0 0 0 0 0 
 
0
 0 0 −k y /m 0 0 −g 0 0 0 0 0 

0 0 0 0 0 1 0 0 0 0 0 0 
 
0 0 0 0 0 −kz /m 0 0 0 0 0 0 
A= 0

 0 0 0 0 0 0 1 0 0 0 0 

0
 0 0 0 0 0 0 −kϕ /Ixx 0 0 0 0 

0 0 0 0 0 0 0 0 0 1 0 0 
 
0
 0 0 0 0 0 0 0 0 −kθ /Iyy 0 0 

0 0 0 0 0 0 0 0 0 0 0 1 
0 0 0 0 0 0 0 0 0 0 0 −kψ /Izz

4
Control Input Matrix B
 
0 0 0 0
 0 0 0 0 
 
 0 0 0 0 
 
 0 0 0 0 
 
 0 0 0 0 
 
1/m 0 0 0 
B=
 0

 0 0 0 

 0 1/Ixx 0 0 
 
 0 0 0 0 
 
 0 0 1/Iyy 0 
 
 0 0 0 0 
0 0 0 1/Izz

Output Matrix C
 
1 0 0 0 0 0 0 0 0 0 0 0
0 0 1 0 0 0 0 0 0 0 0 0
 
0 0 0 0 1 0 0 0 0 0 0 0
C=
0

 0 0 0 0 0 1 0 0 0 0 0
0 0 0 0 0 0 0 0 1 0 0 0
0 0 0 0 0 0 0 0 0 0 1 0

Direct Input Transmission Matrix D

 
0 0 0 0
0 0 0 0
 
0 0 0 0
D=
0

 0 0 0

0 0 0 0
0 0 0 0

Lab Activity
1. Test the Controllability and Observability of the system.

2. Design a linear state feedback Controller for the system.

3. Design a linear full state observer.

4. Design a Linear Quadratic Regulator.

5. Simulate the result using Matlab/Simulink.