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

xe
ye
ze
xb
yb
zb
J
U
J
U
]
]

Figure 1: Earth frame, body frame and Euler angles ], J, U
The Euler angles are the yaw angle ], the pitch angle U and the roll angle J.
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 U.
3. Rotate the plane about its body x axis through the roll angle J.
Propagation:

tan

sin

cos

cos

sin

sin

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:

Quaternion
TBD
Compensation for centrifugal effects
The accelerometers measure both the gravity field and the acceleration of the airplane. We must therefore
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:

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

The scalar equations are:

The speed and the angular velocity

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.

We have thus established a rough expressionof the airplane acceleration from the airplane longitudinal airspeed

and the rotation vector [.
STATE VECTOR
Quaternion vector:

Gyrometer vector expressed in the body frame:

Gyrometer offsets vector:

Rotation vector expressed in the body frame:

We have:

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

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:

Time derivative of the unit quaternion:

The gyrometer offsets are supposed to vary very slowly:

Thus:

DISCRETE STATE-SPACE MODEL
Discretization of the state equation, small time step approximation (Euler integration step):

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

DISCRETE OBSERVATION MODELS
Quaternion to DCM transformation
This transformation will help us link the quaternion to the measurements:

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 theyare
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:

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

Earths magnetic field vector observation matrix for yaw correction
The magnetometers measure Earths magnetic field vector

in the body frame, because the magnetometers are

attached to the body. This vector is then compared to

, the Earth magnetic field vector B

e
rotated into the body
frame:

Earths magnetic field vector expressed in the Earth frame

is constant and may be measured and normalized

during the initial calibration process.

is a unit vector and so is

.
We need to express the [3x7] Jacobian matrix of function

KALMAN PROCESS
State propagation (every 25 ms)
Read and calibrate gyrometers

Add to the gyrometers the last offsets estimation

Predict state

Assemble Jacobian matrix

Predict covariance matrix




Pitch-Roll correction (every second)
Read and calibrate the accelerometers

Compute the speed over ground from GPS

Correct the acceleration for centrifugal effects

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

Compute the predicted Z Earth vector expressed in the body frame

Compute the roll-pitch estimation error

Assemble the roll-pitch observation matrix

Compute the roll-pitch estimation error covariance matrix

Compute the roll-pitch Kalman gain

Update the system state

Update the system state covariance matrix

Yaw correction (every second)
Read and calibrate the magnetometers

Normalize Earths magnetic field vector measured in the body frame

Compute the predicted Earth magnetic field direction expressed in the body frame

Compute Earths magnetic field direction estimation error

Assemble Earths magnetic field observation matrix

(expression in page 6)
Compute Earths magnetic field estimation error covariance matrix

Compute Earths magnetic field Kalman gain

Update the system state

Update the system state covariance matrix