Autopilot: Kalman Filtering Page 1 of 3

Autopilot
Overview:
FAQ
heli-sim
Roadmap
Scenarios
Download
Credits
Kalman Filtering
Links l Collection of Kalman filter links

Mailing list: l Our Kalman filter source code

Archives
Subscribe The core of the Kalman filtering algorithm is the state propagation matrix and the
Admin weights of the estimate and measurement matrices. The state estimation
Design: propagation for the discrete time filter looks like this:
Prototypes
PCB v2 .
X = AX
PCB v1 .
AHRS X = normq( X + X dt )
Systems
Sensors T
Field support P = APA + Q

Weight Where X is the current state estimate vector for the attitude (normalized to be of unit length), P is
the covariance matrix and the A matrix is the quaterion omega matrix for the current rotational
SourceForge:
Summary rate sensors. More technically, A represents all the partial derivatives of the state vector:
Bugs
( d(q0) d(q0) d(q0) d(q0) )
Tasks ( ----- ----- ----- ----- )
Stats ( d(q0) d(q1) d(q2) d(q3) )
Non-SSL
( d(q1) d(q1) d(q1) d(q1) )
( ----- ----- ----- ----- )
( d(q0) d(q1) d(q2) d(q3) ) 1 ( 0 -p -q -r )
A = = - ( p 0 r -q )
( d(q2) d(q2) d(q2) d(q2) ) 2 ( q -r 0 p )
( ----- ----- ----- ----- ) ( r p -q 0 )
( d(q0) d(q1) d(q2) d(q3) )

( d(q2) d(q2) d(q2) d(q2) )

( ----- ----- ----- ----- )
( d(q0) d(q1) d(q2) d(q3) )

Where (p,q,r) represent the rolling, pitching and yawing angular velocities in the strapdown
inertial reference frame, as measured by the rotational rate sensors on the IMU.

The filter requires a translation matrix to translate the Euler angles to the Quaterion frame. The
source code builds this matrix C from the current estimated state X, the Direction Cosine Matrix
DCM of the estimated state and a temporary vector. Please see the ahrs.c source code file for
details. Here is the high-level form of C:

( d(phi) d(phi) d(phi) d(phi) )

( -------- -------- -------- -------- )

http://autopilot.sourceforge.net/kalman.html 5/16/02
Autopilot: Kalman Filtering Page 2 of 3

( d(q0) d(q1) d(q2) d(q3) )

( d(theta) d(theta) d(theta) d(theta) )

C = ( -------- -------- -------- -------- )
( d(q0) d(q1) d(q2) d(q3) )

( d(psi) d(psi) d(psi) d(psi) )

( -------- -------- -------- -------- )
( d(q0) d(q1) d(q2) d(q3) )

For future reference, the Direction Cosine Matrix may be computed from the quaterion form of
the estimate state vector X:

X = [ q0 q1 q2 q3 ]

( 1 - 2(q2q2 + q3q3 ) 2(q1q2 + q0q3) 2(q1q3 - q0q2) )

DCM = ( 2(q1q2 - q0q3 ) 1 - 2(q1q1 + q3q3) 2(q2q3 + q0q1) )
( 2(q1q3 + q0q2 ) 2(q2q3 - q0q1) 1 - 2(q1q1 + q2q2) )

The Kalman Filter update algorithm uses the measured state M, the matrix C and the gain matrix
R to update the covariance matrix P and the estimated state X:

T
E = C P C + R

T -1
K = P C E

Xe = quat2euler( X )

X = X + K ( M - Xe )

P = P - K C P

The new estimated Euler angles for the attitude may be derived from X at this time, again with
quat2euler. This function may be found in the vector.c file.

Putting it all together with the (Phi_m,Theta_m,Psi_m) Euler angles derived from the
accelerometer readings, we get this psudeo code:

while( get_samples( x, y, z, p, q, r, compass ) )

{
/* State Propagation */
A = compute_A( p, q, r );

.
X = A X
.
X = normq( X + Xdt )

T
P = A P A + Q

/* Variable setup */

http://autopilot.sourceforge.net/kalman.html 5/16/02
Autopilot: Kalman Filtering Page 3 of 3

/* Euler angle measurements */

Xm = [
Phi = atan2( y, -z )
Theta = asin( x / -g )
Psi = compass
]
Xe = quat2euler( X )
C = compute_C( X )

/* Kalman Filter update */

T
E = C P C + R

T -1
K = P C E

X = X + K * ( Xm - Xe )

P = P - K C P

/* Estimated attitude extraction */

Xe = quat2euler( X )
}

Implementation details
For this simple AHRS with the seven inputs, only a few matrix operations are required. These
are:

l Vector subtraction 3, 4
l Matrix multiply 3x4 * 4x4, 4x4 * 4x3, 3x4 * 4x3 and 4x3 * 3x4
l Vector/Matrix mutiply 3 * 4x3
l Matrix addition 3x3, 4x4
l Matrix subtraction 3x3, 4x4
l Matrix transpose 4x4, 3x4
l Matrix invert 3x3

For speed and to make use of the limited stack space on board, these functions are all inlined and
written as specifically as possible. The only speed vs memory trade off is that all matrices are
fixed allocation of four entry rows. This avoids the expensive array index multiply.

The temporary variables are statically allocated rather than on the stack so that we can be certain
of finding enough space for them. It might make sense to store Q and R in program memory
rather than on the heap so that they may be non-volatile.

$Id: kalman.html,v 1.16 2002/05/07 23:59:50 tramm Exp $

http://autopilot.sourceforge.net/kalman.html 5/16/02