kalman.c

00001 #include "mbed.h"
00002 #include "Matrix.h"
00003 
00004 #define DEBUG 1
00005 
00006 #define clamp360(x) ((((x) < 0) ? 360: 0) + fmod((x), 360))
00007 
00008 /*
00009  * Kalman Filter Setup
00010  */
00011 static float x[2]={ 0, 0 };                 // System State: hdg, hdg rate
00012 float z[2]={ 0, 0 };                        // measurements, hdg, hdg rate
00013 static float A[4]={ 1, 0, 0, 1};            // State transition matrix; A[1] should be dt
00014 static float H[4]={ 1, 0, 0, 1 };           // Observer matrix maps measurements to state transition
00015 float K[4]={ 0, 0, 0, 0 };                  // Kalman gain
00016 static float P[4]={ 1000, 0, 0, 1000 };     // Covariance matrix
00017 static float R[4]={ 3, 0, 0, 0.03 };        // Measurement noise, hdg, hdg rate
00018 static float Q[4]={ 0.01, 0, 0, 0.01 };     // Process noise matrix
00019 static float I[4]={ 1, 0, 0, 1 };           // Identity matrix
00020 
00021 float kfGetX(int i)
00022 {
00023     return (i >= 0 && i < 2) ? x[i] : 0xFFFFFFFF;
00024 }
00025 
00026 /** headingKalmanInit
00027  *
00028  * initialize x, z, K, and P
00029  */
00030 void headingKalmanInit(float x0)
00031 {
00032     x[0] = x0;
00033     x[1] = 0;
00034 
00035     z[0] = 0;
00036     z[1] = 0;
00037 
00038     K[0] = 0; K[1] = 0;
00039     K[2] = 0; K[3] = 0;
00040     
00041     P[0] = 1000; P[1] = 0;
00042     P[2] = 0;    P[3] = 1000;
00043 }
00044 
00045 
00046 /* headingKalman 
00047  *
00048  * Implements a 1-dimensional, 1st order Kalman Filter
00049  *
00050  * That is, it deals with heading and heading rate (h and h') but no other
00051  * state variables.  The state equations are:
00052  *
00053  *                     X    =    A       X^
00054  * h = h + h'dt -->  | h  | = | 1 dt | | h  |
00055  * h' = h'           | h' |   | 0  1 | | h' |
00056  *
00057  * Kalman Filtering is not that hard. If it's hard you haven't found the right
00058  * teacher. Try taking CS373 from Udacity.com
00059  *
00060  * This notation is Octave (Matlab) syntax and is based on the Bishop-Welch
00061  * paper and references the equation numbers in that paper.
00062  * http://www.cs.unc.edu/~welch/kalman/kalmanIntro.html
00063  *
00064  * returns : current heading estimate
00065  */
00066 float headingKalman(float dt, float Hgps, bool gps, float dHgyro, bool gyro)
00067 {
00068     A[1] = dt;
00069 
00070     /* Initialize, first time thru
00071     x = H*z0
00072     */
00073 
00074     //fprintf(stdout, "gyro? %c  gps? %c\n", (gyro)?'Y':'N', (gps)?'Y':'N');
00075             
00076     // Depending on what sensor measurements we've gotten,
00077     // switch between observer (H) matrices and measurement noise (R) matrices
00078     // TODO: incorporate HDOP or sat count in R
00079     if (gps) {
00080         H[0] = 1.0;
00081         z[0] = Hgps;
00082     } else {
00083         H[0] = 0;
00084         z[0] = 0;
00085     }
00086 
00087     if (gyro) {
00088         H[3] = 1.0;
00089         z[1] = dHgyro;
00090     } else {
00091         H[3] = 0;
00092         z[1] = 0;
00093     }
00094 
00095     //Matrix_print(2,2, A, "1. A");
00096     //Matrix_print(2,2, P, "   P");
00097     //Matrix_print(2,1, x, "   x");
00098     //Matrix_print(2,1, K, "   K");
00099     //Matrix_print(2,2, H, "2. H");
00100     //Matrix_print(2,1, z, "   z");
00101    
00102    /**********************************************************************
00103      * Predict
00104      %
00105      * In this step we "move" our state estimate according to the equation
00106      *
00107      * x = A*x; // Eq 1.9
00108      ***********************************************************************/
00109     float xp[2];
00110     Matrix_Multiply(2,2,1, xp, A, x);
00111     
00112     //Matrix_print(2,1, xp, "3. xp");
00113 
00114     /**********************************************************************
00115      * We also have to "move" our uncertainty and add noise. Whenever we move,
00116      * we lose certainty because of system noise.
00117      *
00118      * P = A*P*A' + Q; // Eq 1.10
00119      ***********************************************************************/
00120     float At[4];
00121     Matrix_Transpose(2,2, At, A);
00122     float AP[4];
00123     Matrix_Multiply(2,2,2, AP, A, P);
00124     float APAt[4];
00125     Matrix_Multiply(2,2,2, APAt, AP, At);
00126     Matrix_Add(2,2, P, APAt, Q);
00127 
00128     //Matrix_print(2,2, P, "4. P");
00129 
00130     /**********************************************************************
00131      * Measurement aka Correct
00132      * First, we have to figure out the Kalman Gain which is basically how
00133      * much we trust the sensor measurement versus our prediction.
00134      *
00135      * K = P*H'*inv(H*P*H' + R);    // Eq 1.11
00136      ***********************************************************************/
00137     float Ht[4];
00138     //Matrix_print(2,2, H,    "5. H");
00139     Matrix_Transpose(2,2, Ht, H);
00140     //Matrix_print(2,2, Ht,    "5. Ht");
00141 
00142     float HP[2];
00143     //Matrix_print(2,2, P,    "5. P");
00144     Matrix_Multiply(2,2,2, HP, H, P);
00145     //Matrix_print(2,2, HP,    "5. HP");
00146 
00147     float HPHt[4];
00148     Matrix_Multiply(2,2,2, HPHt, HP, Ht);
00149     //Matrix_print(2,2, HPHt,    "5. HPHt");
00150     
00151     float HPHtR[4];
00152     //Matrix_print(2,2, R,    "5. R");
00153     Matrix_Add(2,2, HPHtR, HPHt, R);
00154     //Matrix_print(2,2, HPHtR,    "5. HPHtR");
00155 
00156     Matrix_Inverse(2, HPHtR);
00157     //Matrix_print(2,2, HPHtR,    "5. HPHtR");
00158 
00159     float PHt[2];
00160     //Matrix_print(2,2, P,    "5. P");
00161     //Matrix_print(2,2, Ht,    "5. Ht");
00162     Matrix_Multiply(2,2,2, PHt, P, Ht);
00163     //Matrix_print(2,2, PHt,    "5. PHt");
00164     
00165     Matrix_Multiply(2,2,2, K, PHt, HPHtR);
00166     
00167     //Matrix_print(2,2, K,    "5. K");
00168         
00169     /**********************************************************************
00170      * Then we determine the discrepancy between prediction and measurement 
00171      * with the "Innovation" or Residual: z-H*x, multiply that by the 
00172      * Kalman gain to correct the estimate towards the prediction a little 
00173      * at a time.
00174      *
00175      * x = x + K*(z-H*x);            // Eq 1.12
00176      ***********************************************************************/
00177     float Hx[2];
00178     Matrix_Multiply(2,2,1, Hx, H, xp);
00179 
00180     //Matrix_print(2,2, H, "6. H");
00181     //Matrix_print(2,1, x, "6. x");
00182     //Matrix_print(2,1, Hx, "6. Hx");
00183     
00184     float zHx[2];
00185     Matrix_Subtract(2,1, zHx, z, Hx);
00186 
00187     // At this point we need to be sure to correct heading to -180 to 180 range
00188     if (zHx[0] > 180.0)   zHx[0] -= 360.0;
00189     if (zHx[0] <= -180.0) zHx[0] += 360.0;
00190 
00191     //Matrix_print(2,1, z, "6. z");
00192     //Matrix_print(2,1, zHx, "6. zHx");
00193     
00194     float KzHx[2];
00195     Matrix_Multiply(2,2,1, KzHx, K, zHx);
00196 
00197     //Matrix_print(2,2, K, "6. K");
00198     //Matrix_print(2,1, KzHx, "6. KzHx");
00199     
00200     Matrix_Add(2,1, x, xp, KzHx);
00201 
00202     // Clamp to 0-360 range
00203     while (x[0] < 0) x[0] += 360.0;
00204     while (x[0] >= 360.0) x[0] -= 360.0;
00205 
00206     //Matrix_print(2,1, x, "6. x");
00207 
00208     /**********************************************************************
00209      * We also have to adjust the certainty. With a new measurement, the 
00210      * estimate certainty always increases.
00211      *
00212      * P = (I-K*H)*P;                // Eq 1.13
00213      ***********************************************************************/
00214     float KH[4];
00215     //Matrix_print(2,2, K, "7. K");
00216     Matrix_Multiply(2,2,2, KH, K, H);
00217     //Matrix_print(2,2, KH, "7. KH");
00218     float IKH[4];
00219     Matrix_Subtract(2,2, IKH, I, KH);
00220     //Matrix_print(2,2, IKH, "7. IKH");
00221     float P2[4];
00222     Matrix_Multiply(2,2,2, P2, IKH, P);
00223     Matrix_Copy(2, 2, P, P2);
00224 
00225     //Matrix_print(2,2, P, "7. P");
00226 
00227     return x[0];
00228 }