
Kalman FilteringThe core of the Kalman filtering algorithm is the state propagation matrix and the weights of the estimate and measurement matrices. The state estimation propagation for the discrete time filter looks like this:
. X = AX . X = normq( X + X dt ) T P = APA + Q 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 rate sensors. More technically, A represents all the partial derivatives of the state vector:
( d(q0) d(q0) d(q0) d(q0) ) (     ) ( d(q0) d(q1) d(q2) d(q3) ) ( 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 highlevel form of C:
( d(phi) d(phi) d(phi) d(phi) ) (     ) ( 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 */ /* 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 detailsFor this simple AHRS with the seven inputs, only a few matrix operations are required. These are:
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 nonvolatile.

$Id: kalman.html,v 1.17 2002/08/05 03:44:39 tramm Exp $ 