kalman.c

00001 #include "kalman.h" 
00002 
00003 /*********************************************************************  
00004  The implememted kalman filter estimates the angle that will be used 
00005  on the PID controller alogrithm. The filter consists of two states 
00006  [angle, gyro_bais].
00007  *********************************************************************/ 
00008  
00009 /******************************************************************** 
00010 *    Function Name:  kalman_init                                    * 
00011 *    Return Value:   none                                           * 
00012 *    Parameters:     struct filter, dt, R_angle, Q_gyro, Q_angle    * 
00013 *    Description:    Initialize the kalman Filter parameters.       * 
00014 ********************************************************************/  
00015 
00016 void kalman_init(kalman *filter, double R_angle, double Q_gyro, double Q_angle) // double dt,
00017 { 
00018     // Initialize the two states, the angle and the gyro bias. As a 
00019     // byproduct of computing the angle, we also have an unbiased 
00020     // angular rate available. 
00021     filter->x_bias = 0.0; 
00022     filter->x_rate = 0.0; 
00023     filter->x_angle = 0.0; 
00024  
00025  
00026     // Initialize the delta in seconds between gyro samples. 
00027     //filter->dt = dt; 
00028  
00029     // Initialize the measurement noise covariance matrix values. 
00030     // In this case, R is a 1x1 matrix tha represents expected 
00031     // jitter from the accelerometer. 
00032     filter->R_angle = R_angle; 
00033  
00034     // Initialize the process noise covariance matrix values. 
00035     // In this case, Q indicates how much we trust the acceleromter 
00036     // relative to the gyros. 
00037     // Q_gyro set to 0.003 and Q_angle set to 0.001. 
00038     
00039     filter->Q_gyro = Q_gyro; 
00040     filter->Q_angle = Q_angle; 
00041  
00042     // Initialize covariance of estimate state.  This is updated 
00043     // at every time step to determine how well the sensors are 
00044     // tracking the actual state. 
00045     
00046     filter->P_00 = 1.0; 
00047     filter->P_01 = 0.0; 
00048     filter->P_10 = 0.0; 
00049     filter->P_11 = 1.0; 
00050 } 
00051 
00052 /******************************************************************** 
00053 *    Function Name:  kalman_predict                            * 
00054 *    Return Value:   none                                           * 
00055 *    Parameters:     struct filter, measured gyroscope value        * 
00056 *    Description:    Called every dt(Timer 1 overflow with a biased * 
00057 *                    gyro. Also updates the current rate and angle  * 
00058 *                    estimate).                                     * 
00059 ********************************************************************/  
00060 void kalman_predict(kalman *filter, double dot_angle,  double dt) 
00061 { 
00062     // Static so these are kept off the stack. 
00063     static double gyro_rate_unbiased; 
00064     static double Pdot_00; 
00065     static double Pdot_01; 
00066     static double Pdot_10; 
00067     static double Pdot_11;
00068     
00069     // Unbias our gyro. 
00070     gyro_rate_unbiased= dot_angle - filter->x_bias; 
00071  
00072     // Store our unbiased gyro estimate. 
00073     filter->x_rate= gyro_rate_unbiased; 
00074      
00075     // Update the angle estimate. 
00076     filter->x_angle= filter->x_angle + (dot_angle - filter->x_bias)*dt; 
00077  
00078     // Compute the derivative of the covariance matrix 
00079     // Pdot = A*P + P*A' + Q 
00080     Pdot_00 = filter->Q_angle - filter->P_01 - filter->P_10; 
00081     Pdot_01 = -filter->P_11; 
00082     Pdot_10 = -filter->P_11; 
00083     Pdot_11 = filter->Q_gyro; 
00084  
00085     // Update the covariance matrix. 
00086     filter->P_00 += Pdot_00 * dt; 
00087     filter->P_01 += Pdot_01 * dt; 
00088     filter->P_10 += Pdot_10 * dt; 
00089     filter->P_11 += Pdot_11 * dt;
00090 }
00091 /********************************************************************* 
00092 *    Function Name:  kalman_update                                   * 
00093 *    Return Value:   none                                            * 
00094 *    Parameters:     struct filter, measured angle value             *
00095 *    Description:    Called when a new accelerometer angle           * 
00096 *                    measurement is available. Updates the estimated * 
00097 *                    angle that will be used.                        * 
00098 *********************************************************************/
00099 void kalman_update(kalman *filter, double angle_measured)
00100 {
00101     // Static so these are kept off the stack. 
00102     static double y;
00103     static double S;
00104     static double K_0;
00105     static double K_1;
00106     
00107     // Compute the error in the estimate. 
00108     // Innovation or Measurement Residual 
00109     // y = z - Hx 
00110     y= angle_measured - filter->x_angle; 
00111  
00112     // Compute the error estimate. 
00113     // S = C P C' + R 
00114     S = filter->R_angle + filter->P_00; 
00115  
00116     // Compute the kalman filter gains. 
00117     // K = P C' inv(S) 
00118      K_0 = filter->P_00 / S; 
00119      K_1 = filter->P_10 / S; 
00120  
00121     // Update covariance matrix. 
00122     // P = P - K C P 
00123     filter->P_00 -= K_0 * filter->P_00; 
00124     filter->P_01 -= K_0 * filter->P_01; 
00125     filter->P_10 -= K_1 * filter->P_00; 
00126     filter->P_11 -= K_1 * filter->P_01; 
00127    
00128     // Update the state (new)estimates (Correct the prediction of the state). 
00129     // Also adjust the bias on the gyro at every iteration. 
00130     // x = x + K * y 
00131     filter->x_angle = filter->x_angle + K_0 * y; 
00132     filter->x_bias = filter->x_bias + K_1 * y;  
00133      
00134 }