AIRPLANE ORIENTATION

The orientation of an airplane is defined by the rotation of a frame attached to its body with respect to another fixed
frame attached to Earth. These frames are orthogonal with their z axis directed downward.

The rotation of the body frame relatively to the Earth frame may be expressed by a set of Euler angles, by a 9x9
rotation matrix or by a quaternion. Each of these representations has its pros and cons. The quaternion is interesting
for the Kalman filter because it is compact (dimension of the state vector), fast and numerically stable.

You can find a lot of documentation on these three representations, on Wikipedia for example.

We also need to express the derivative of the representation with respect to time in order to propagate our best
estimation of the airplane orientation.

Euler angles

xb ye





yb

xe


ze
zb 

Figure 1: Earth frame, body frame and Euler angles , , 

The Euler angles are the yaw angle , the pitch angle  and the roll angle .

The orientation of the body frame with respect to the fixed frame can be determined in the following manner:

1. Rotate the plane about its body z axis through the yaw angle  (Earth z axis and body z axis coincide during
this first rotation).
2. Rotate the plane about its body y axis through the pitch angle .
3. Rotate the plane about its body x axis through the roll angle .

Propagation:

ϕ̇=ω x + tan θ . ( ω y sin ϕ+ω z cos ϕ )

{ θ̇=ω y cos ϕ−ω z sin ϕ

ψ̇ = y
ω sin ϕ+ω z cos ϕ
cos θ
Direction Cosine Matrix
Let U be a vector whose coordinates are expressed in the body frame. The product of the direction cosine matrix R
with the vector U yields another vector who is identical to U but whose coordinates are expressed in the Earth
frame:

ub x ue x

[] []
U earth =R . U body U body= ub y U earth = ue x
u bz ue x

cosψ cosθ −sinψ cosϕ +cosψ sinθ sinϕ sinψ sinϕ+cosψ sinθ cosϕ

[
R= sinψ cosθ cosψ cosϕ+ sinψ sinθ sin ϕ −cosψ sinϕ+ sinψ sinθ cosϕ
−sinθ cosθ sinϕ cosθ cosϕ ]
Quaternion
TBD

Compensation for centrifugal effects

g and the acceleration of the airplane. We must therefore
The accelerometers measure both the gravity field ⃗
estimate in some way the airplane acceleration and subtract it from the measurement.

The derivative of an arbitrary vector A referred to a rotating body frame having an angular velocity  can be
represented by the following vector identity:

( ddAt ) =( dd At ) +Ω × A
I B

Where the subscript I and B refer to the inertial and body fixed frames of reference. Applying this identity to the
velocity vector yields:

( ddVt ) =( ddVt ) + Ω× V
I B

The scalar equations are:

ẍ=u̇+ ω y w−ω z v

{ ÿ= v̇+ ω z u−ω x w

z̈= ẇ+ω x v−ω y u

The speed V = (u v w ) and the angular velocity Ω=( ω x ω y ω z ) are expressed in the body frame of reference.

Assumption: in case the wind is insignificant, the second and third components of the velocity vector, v & w, and
their derivatives, are small relatively to its first component u.

In case of a constant wind, we may subtract the wind constant velocity vector to the airplane velocity vector. The
relations still stand because the derivative of the wind velocity is null.

ẍ=u̇a

{ ÿ =ω z ua
z̈=−ω y u a

We have thus established a rough expression of the airplane acceleration from the airplane longitudinal airspeed ua
and the rotation vector .
STATE VECTOR
Quaternion vector:Q=( q0 q 1 q 2 q 3 )

Gyrometer vector expressed in the body frame: G=( g x g y g z )

Gyrometer offsets vector: B=( bx b y b z )

Rotation vector expressed in the body frame: Ω=( ω x ω y ω z )

ω x =g x −b x

{
We have: ω y =g y −b y
ω z=g z−b z

The state vector of the filter is composed of the attitude quaternion Q and of the gyrometer offsets vector B:

q0

[]
q1
q2
X = Q = q3
[]
B
bx
by
bz
CONTINUOUS STATE-SPACE MODEL
The derivative of the system state X is a non linear function of the state itself X and of the gyrometer vector G:

dX
=f ( X , G )
dt

Time derivative of the unit quaternion:

d q0 1 1 1
=− ω . q1 − ω y . q2− ω . q3

{
dt 2 x 2 2 z
d q1 1 1 1
= ω . q0 − ω . q3 + ω z . q2
dt 2 x 2 y 2
d q2 1 1 1
= ω . q3 + ω . q 0− ω . q1
dt 2 x 2 y 2 z
d q3 1 1 1
=− ω . q 2+ ω . q1 + ω .q 0
dt 2 x 2 y 2 z

The gyrometer offsets are supposed to vary very slowly:

d bx

{
=0
dt
d by
=0
dt
d bz
=0
dt

Thus:

d q0 −1 1 1
f 0 ( X , G )= = ( g x −b x ) . q1− ( g y −b y ) . q2− ( g z −b z ) . q3

{
dt 2 2 2
d q1 1 1 1
f 1 ( X , G )= = ( g x −b x ) .q 0− ( g y −b y ) . q3 + ( g z−bz ) . q 2
dt 2 2 2
d q2 1 1 1
f 2 ( X , G )= = ( g x −b x ) .q 3 + ( g y −b y ) .q 0− ( g z−bz ) . q 1
dt 2 2 2
d q −1 1 1
f 3 ( X ,G ) = 3 = ( g x −bx ) . q2 + ( g y −b y ) . q1 + ( g z−b z ) . q 0
dt 2 2 2
d bx
f 4 ( X , G )= =0
dt
db
f 5 ( X , G )= y =0
dt
db
f 6 ( X , G )= z =0
dt
DISCRETE STATE-SPACE MODEL
Discretization of the state equation, small time step approximation (Euler integration step):

X n+1=X n+ f ( X n , G n ) . Δt ⟺ X n +1=f ' ( X n ,Gn )

We need to express the [7x7] Jacobian matrix of function f ' in order to propagate the covariance matrix:

−1 −1 1 1 1 1
1 ωx . Δ t ω y . Δ t − ω . Δt q . Δt q . Δt q .Δt

[ ]
2 2 2 z 2 1 2 2 2 3
1 1 1 −1 1 −1
ω . Δt 1 ω . Δt − ω . Δt q . Δt q . Δt q .Δt
∂ f '0 ∂ f '0 2 x 2 z 2 y 2 0 2 3 2 2

[ ]
⋯
∂q 0 ∂ bz 1 −1 1 −1 −1 1
ω . Δt ωz . Δt 1 ω .Δ t q . Δt q . Δt q . Δt
F= ⋮ ⋱ ⋮ = 2 y 2 2 x 2 3 2 0 2 1
∂ f '6 ∂ f '6 1 1 −1 1 −1 1
⋯ ω .Δ t ω . Δt ω . Δt 1 q . Δt q . Δ t − q . Δt
∂q 0 ∂ bz 2 z 2 y 2 x 2 2 2 1 2 0
0 0 0 0 1 0 0
0 0 0 0 0 1 0
0 0 0 0 0 0 1
DISCRETE OBSERVATION MODELS
Quaternion to DCM transformation
This transformation will help us link the quaternion to the measurements:

1−2 ( q 22 +q23 ) 2 ( q 1 . q2 −q0 . q3 ) 2 ( q1 . q3 +q 0 .q 2 )

[
DCM= 2 ( q1 . q2 +q 0 . q 3 ) 1−2 ( q12+ q23 )
2 ( q1 .q 3−q 0 .q 2 ) 2 ( q 2 . q 3+ q0 . q1 )
2 ( q2 . q3−q0 . q1 )
2
1−2 ( q1 +q 2)
2 ]
Z Earth vector observation matrix for pitch-roll correction
The accelerometers measure the direction of the Z Earth vector expressed in the body frame because they are
attached to the body. This vector is compared to the third row of the DCM matrix which is the unit Z Earth vector
expressed in the body frame:

Z ex =2 ( q1 . q3−q0 . q2 )

{
Z e=hPR ( Q ) Z ey =2 ( q2 .q 3 +q0 . q1 )
Z ez =1−2 ( q21 +q 22)

We need to express the [3x7] Jacobian matrix of function h PR:

∂ hPR 0 ∂ h PR0

[ ][
⋯
∂ q0 ∂ bz −2q 2 2q 3 −2 q 0 2 q 1 0 0 0
H PR= ⋮
∂ h PR2
∂ q0
⋱
⋯
⋮ = 2q 1
∂ h PR2
∂ bz
0
2q 0 2 q3 2 q 2 0 0 0
−4 q1 −4 q2 0 0 0 0 ]
Earth’s magnetic field vector observation matrix for yaw correction
The magnetometers measure Earth’s magnetic field vector Bbm in the body frame, because the magnetometers are
attached to the body. This vector is then compared to Bb, the Earth magnetic field vector Be rotated into the body
frame:

B bx = 1−2 ( q 22+ q23 ) Bex +2 ( q 1 . q2 +q 0 .q 3 ) B ey +2 ( q 1 . q3 −q0 . q2 ) Bez

[ ]
{
Bb=DCM . B e =hB ( Q ) Bb y =2 ( q1 .q 2−q 0 . q 3 ) Be x + 1−2 ( q 21+ q23 ) B +2 ( q2 . q3 +q 0 . q 1 ) Bez
[ ey ]
Bb z=2 ( q1 . q3 +q 0 .q 2 ) Be x +2 ( q 2 . q 3−q 0 . q1 ) Be y + 1−2 ( q21 +q 22) B ez
[ ]
Earth’s magnetic field vector expressed in the Earth frame Beis constant and may be measured and normalized
during the initial calibration process. Be is a unit vector and so is Bb.

We need to express the [3x7] Jacobian matrix of function h B:

 ∂ hB 0 ∂ h B0

[ ][
⋯
∂ q0 ∂ bz 2 q3 B ey −2q 2 Bez 2 q2 Be y +2 q3 B ez −4 q2 Be x +2 q 1 Be y −2 q0 B ez −4 q3 B ex +2 q 0 Be y +2 q1 Be z 0 0 0
HB= ⋮
∂ hB 2
∂ q0
⋱
⋯
⋮ = −2q 3 Bex +2 q1 B ez 2q 2 Bex −4 q 1 Be y +2 q0 B ez
∂ h B2
∂ bz
2 q1 B ex + 2q 3 Bez
2 q2 Be x −2q 1 Be y 2 q 3 Bex −2 q 0 Be y −4 q 1 Bez 2q 0 Be x +2 q 3 Be y −4 q2 B ez
−2 q 0 Bex −4 q 3 Be y +2 q2 B ez 0 0 0
2 q 1 Bex +2 q2 Be y 0 0 0 ]
KALMAN PROCESS
State propagation (every 25 ms)
Read and calibrate gyrometers
G=( g x g y g z )

Add to the gyrometers the last offsets estimation

ω x =g x −b x

{
ω y =g y −b y
ω z=g z−b z

Predict state

1 1 1
(
q0 =q0 + − ω . q 1− ω y . q2 − ω . q 3 . Δt
)

{
2 x 2 2 z

q 1=q 1+
( 12 ω . q − 12 ω . q + 12 ω . q ). Δ t
x
0
y
3 z 2

1 1 1
2
(2 2 2 )
q =q + ω . q + ω . q − ω . q . Δ t
2
x
3
y
0
z
1

3
( 12 12 12 )
q =q + − ω . q + ω . q + ω . q . Δ t
3
x
2
y
1
z
0

Assemble Jacobian matrix

−1 −1 1 1 1 1
1 ω x . Δt ω y . Δt − ω . Δt q . Δt q . Δt q .Δ t

[ ]
2 2 2 z 2 1 2 2 2 3
1 1 1 −1 1 −1
ω . Δt 1 ω . Δt − ω . Δt q . Δt q . Δt q .Δ t
2 x 2 z 2 y 2 0 2 3 2 2
1 −1 1 −1 −1 1
ω . Δt ωz . Δ t 1 ω .Δ t q . Δt q . Δt q .Δ t
F= 2 y 2 2 x 2 3 2 0 2 1
1 1 −1 1 −1 1
ω .Δ t ω . Δt ω . Δt 1 q . Δt q . Δ t − q . Δt
2 z 2 y 2 x 2 2 2 1 2 0
0 0 0 0 1 0 0
0 0 0 0 0 1 0
0 0 0 0 0 0 1

Predict covariance matrix

P=F . P . t F ' +Q
Pitch-Roll correction (every second)
Read and calibrate the accelerometers
Acc=( a x a y a z )

Compute the speed over ground from GPS

v og=√ v x+¿v ¿ y

Correct the acceleration for centrifugal effects

a y =a y + ω z . v og
{
a z=a z −ω y . v og

Normalize the acceleration to yield the measured Z Earth vector expressed in the body frame
ax

Z m=

{
√ a x + a y +a z
ay
√ a x + a y +a z
az
√ a x + a y +a z
Compute the predicted Z Earth vector expressed in the body frame
2 ( q 1 . q3 −q0 . q2 )

{
Z e= 2 ( q 2 . q 3+ q0 . q1 )
1−2 ( q21 + q22 )

Compute the roll-pitch estimation error

E PR=Z m−Z e

Assemble the roll-pitch observation matrix

−2 q 2 2 q 3 −2 q0 2 q1 0 0 0

0 [
H PR= 2 q 1 2 q0 2 q3 2 q2 0 0 0
−4 q1 −4 q 2 0 0 0 0 ]
Compute the roll-pitch estimation error covariance matrix
P PR=H PR . P. t H PR' + R PR

Compute the roll-pitch Kalman gain

K PR=P . t H PR' . P−1
PR

Update the system state

X =X + K PR . E PR

Update the system state covariance matrix

P=P−K PR . H PR . P
Yaw correction (every second)
Read and calibrate the magnetometers
Mag=( m x m y m z )

Normalize Earth’s magnetic field vector measured in the body frame

mx

Bbm=

{
√m x +m y +m z
my
√m x +m y +m z
m
√m x +m y +m z
Compute the predicted Earth magnetic field direction expressed in the body frame
2 2
[ 1−2 ( q +q )] B +2 ( q1 . q2 +q 0 . q 3 ) Be y +2 ( q1 . q3−q0 . q2 ) B ez

{
2 3 ex

Bb= 2 ( q1 . q2−q0 . q3 ) B ex + 1−2 ( q21 +q 23) B +2 ( q 2 . q3 + q0 . q1 ) B ez

[ ]
ey

2 ( q1 . q3 +q 0 .q 2 ) Be x +2 ( q 2 . q 3−q 0 . q1 ) Be y + 1−2 ( q21 +q 22) B ez

[ ]
Compute Earth’s magnetic field direction estimation error
E B=Bbm−Bb

Assemble Earth’s magnetic field observation matrix

HB (expression in page 6)

Compute Earth’s magnetic field estimation error covariance matrix

PB =H B . P. t H B' + R B

Compute Earth’s magnetic field Kalman gain

K B=P . t H B' . P−1
B

Update the system state

X =X + K B . E B

Update the system state covariance matrix

P=P−K B . H B . P