Kalman Filtering

• Collection of Kalman filter links
• Our Kalman filter source code

	.
	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 high-level 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 details

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

• Vector subtraction 3, 4
• Matrix multiply 3x4 * 4x4, 4x4 * 4x3, 3x4 * 4x3 and 4x3 * 3x4
• Vector/Matrix mutiply 3 * 4x3
• Matrix addition 3x3, 4x4
• Matrix subtraction 3x3, 4x4
• Matrix transpose 4x4, 3x4
• 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.