File content as of revision 1:270647f1e086:

#include "kalman.h" 

/*********************************************************************  
 The implememted kalman filter estimates the angle that will be used 
 on the PID controller alogrithm. The filter consists of two states 
 [angle, gyro_bais].
 *********************************************************************/ 
 
/******************************************************************** 
*    Function Name:  kalman_init                                    * 
*    Return Value:   none                                           * 
*    Parameters:     struct filter, R_angle, Q_gyro, Q_angle    * 
*    Description:    Initialize the kalman Filter parameters.       * 
********************************************************************/  

void kalman_init(kalman *filter, double R_angle, double Q_gyro, double Q_angle) // Initialization
{ 
    // Initialize the three states, the angle, gyro bias and angular rate
    filter->x_bias = 0.0; 
    filter->x_rate = 0.0; 
    filter->x_angle = 0.0; 
 
    // Initialize measurement noise covariance, a 1x1 matrix representing expected accelerometer jitter
    // R_angel is set to 0.59
    filter->R_angle = R_angle; 
 
    // Initialize process noise covariance matrix, trust of acceleromter vs trust of gyro
    // Q_gyro set to 0.002 and Q_angle set to 0.001. 
    
    filter->Q_gyro = Q_gyro; 
    filter->Q_angle = Q_angle; 
 
    // Initialize covariance of estimate state.  This is updated 
    // at every time step to determine how well the sensors are 
    // tracking the actual state. Initially an Identity matrix
    
    filter->P_00 = 1.0; 
    filter->P_01 = 0.0; 
    filter->P_10 = 0.0; 
    filter->P_11 = 1.0; 
} 

/******************************************************************** 
*    Function Name:  kalman_predict                                 * 
*    Return Value:   none                                           * 
*    Parameters:     struct filter, measured gyro value             * 
*    Description:    Called at dt, updates current rate and angle   * 
********************************************************************/  
void kalman_predict(kalman *filter, double dot_angle,  double dt) 
{ 
    // so not on the stack. 
    static double gyro_rate_unbiased; 
    static double Pdot_00; 
    static double Pdot_01; 
    static double Pdot_10; 
    static double Pdot_11;
    
    // dot_angle is the gyro value. 
    gyro_rate_unbiased = dot_angle - filter->x_bias; 
 
    // Store our unbiased gyro estimate. 
    filter->x_rate= gyro_rate_unbiased; 
     
    //   x_angle = x_angle + (measured gyro - x_bias)*dt from X = AX(k-1) + Bu(k-1)
    filter->x_angle= filter->x_angle + (dot_angle - filter->x_bias)*dt; 
 
    // Compute the derivative of the covariance matrix 
    // Pdot = A*P + P*A' + Q 
    Pdot_00 = filter->Q_angle - filter->P_01 - filter->P_10; 
    Pdot_01 = -filter->P_11; 
    Pdot_10 = -filter->P_11; 
    Pdot_11 = filter->Q_gyro; 
 
    // Update the covariance matrix. 
    filter->P_00 += Pdot_00 * dt; 
    filter->P_01 += Pdot_01 * dt; 
    filter->P_10 += Pdot_10 * dt; 
    filter->P_11 += Pdot_11 * dt;
}
/********************************************************************* 
*    Function Name:  kalman_update                                   * 
*    Return Value:   none                                            * 
*    Parameters:     struct filter, measured angle value             *
*    Description:    Called with new accelerometer angle             * 
*                    Updates  estimated  angle                       * 
*********************************************************************/
void kalman_update(kalman *filter, double angle_measured)
{
    // Static to keep off the stack. 
    static double y;
    static double S;
    static double K_0;
    static double K_1;
    
    // Compute the error in the estimate. 
    // Innovation or Measurement Residual 
    // y = z - Hx 
    y= angle_measured - filter->x_angle; 
 
    // Compute the error estimate. 
    // S = H P H' + R 
    S = filter->R_angle + filter->P_00; 
 
    // Compute the kalman filter gains. 
    // K = P H' inv(S) = P H'/(H P H' + R)
     K_0 = filter->P_00 / S; 
     K_1 = filter->P_10 / S; 
 
    // Update covariance matrix. 
    // P = P - K H P = (I - KH)P
    filter->P_00 -= K_0 * filter->P_00; 
    filter->P_01 -= K_0 * filter->P_01; 
    filter->P_10 -= K_1 * filter->P_00; 
    filter->P_11 -= K_1 * filter->P_01; 
   
    // Update the state (new)estimates (Correct the prediction of the state). 
    // Also adjust the bias on the gyro at every iteration. 
    // x = x + K * y = x + K(z - Hx )
    filter->x_angle = filter->x_angle + K_0 * y; 
    filter->x_bias = filter->x_bias + K_1 * y;  
     
}