Revision 1:270647f1e086, committed 2021-03-28

Comitter:

fitzpatrick

Date:

Sun Mar 28 20:19:54 2021 +0000

Parent:

0:8a460b0d6f09

Commit message:

To Debug Ethernet Includes

Changed in this revision

diff -r 8a460b0d6f09 -r 270647f1e086 kalman.c
--- a/kalman.c	Wed Aug 15 22:17:07 2012 +0000
+++ b/kalman.c	Sun Mar 28 20:19:54 2021 +0000
@@ -9,39 +9,30 @@
 /******************************************************************** 
 *    Function Name:  kalman_init                                    * 
 *    Return Value:   none                                           * 
-*    Parameters:     struct filter, dt, R_angle, Q_gyro, Q_angle    * 
+*    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) // double dt,
+void kalman_init(kalman *filter, double R_angle, double Q_gyro, double Q_angle) // Initialization
 { 
-    // Initialize the two states, the angle and the gyro bias. As a 
-    // byproduct of computing the angle, we also have an unbiased 
-    // angular rate available. 
+    // 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 the delta in seconds between gyro samples. 
-    //filter->dt = dt; 
- 
-    // Initialize the measurement noise covariance matrix values. 
-    // In this case, R is a 1x1 matrix tha represents expected 
-    // jitter from the accelerometer. 
+    // Initialize measurement noise covariance, a 1x1 matrix representing expected accelerometer jitter
+    // R_angel is set to 0.59
     filter->R_angle = R_angle; 
  
-    // Initialize the process noise covariance matrix values. 
-    // In this case, Q indicates how much we trust the acceleromter 
-    // relative to the gyros. 
-    // Q_gyro set to 0.003 and Q_angle set to 0.001. 
+    // 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. 
+    // tracking the actual state. Initially an Identity matrix
     
     filter->P_00 = 1.0; 
     filter->P_01 = 0.0; 
@@ -50,29 +41,27 @@
 } 
 
 /******************************************************************** 
-*    Function Name:  kalman_predict                            * 
+*    Function Name:  kalman_predict                                 * 
 *    Return Value:   none                                           * 
-*    Parameters:     struct filter, measured gyroscope value        * 
-*    Description:    Called every dt(Timer 1 overflow with a biased * 
-*                    gyro. Also updates the current rate and angle  * 
-*                    estimate).                                     * 
+*    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) 
 { 
-    // Static so these are kept off the stack. 
+    // 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;
     
-    // Unbias our gyro. 
-    gyro_rate_unbiased= dot_angle - filter->x_bias; 
+    // 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; 
      
-    // Update the angle estimate. 
+    //   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 
@@ -92,13 +81,12 @@
 *    Function Name:  kalman_update                                   * 
 *    Return Value:   none                                            * 
 *    Parameters:     struct filter, measured angle value             *
-*    Description:    Called when a new accelerometer angle           * 
-*                    measurement is available. Updates the estimated * 
-*                    angle that will be used.                        * 
+*    Description:    Called with new accelerometer angle             * 
+*                    Updates  estimated  angle                       * 
 *********************************************************************/
 void kalman_update(kalman *filter, double angle_measured)
 {
-    // Static so these are kept off the stack. 
+    // Static to keep off the stack. 
     static double y;
     static double S;
     static double K_0;
@@ -110,16 +98,16 @@
     y= angle_measured - filter->x_angle; 
  
     // Compute the error estimate. 
-    // S = C P C' + R 
+    // S = H P H' + R 
     S = filter->R_angle + filter->P_00; 
  
     // Compute the kalman filter gains. 
-    // K = P C' inv(S) 
+    // 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 C P 
+    // 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; 
@@ -127,7 +115,7 @@
    
     // 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 = 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;  
      

diff -r 8a460b0d6f09 -r 270647f1e086 kalman.h
--- a/kalman.h	Wed Aug 15 22:17:07 2012 +0000
+++ b/kalman.h	Sun Mar 28 20:19:54 2021 +0000
@@ -7,10 +7,10 @@
  
 typedef struct 
 { 
-    // Two states, angle and gyro bias. Unbiased angular rate is a byproduct. 
-    double x_bias;
-    double x_rate; 
-    double x_angle; 
+    // Three states, gyro bias, angular rate, angle 
+    double x_bias;      // Estimated error in angle
+    double x_rate;      // Unbiased Gyro Estimate
+    double x_angle;     // Kalman filtered angle Pitch or Roll
  
     // Covariance of estimation error matrix. 
     double P_00;