Michael Shimniok / Mbed 2 deprecated AVC_2012

Code for autonomous ground vehicle, Data Bus, 3rd place winner in 2012 Sparkfun AVC.

Dependencies:   Watchdog mbed Schedule SimpleFilter LSM303DLM PinDetect DebounceIn Servo

Estimation/kalman.c@0:826c6171fc1b, 2012-06-20 (annotated)

Committer:
shimniok
Date:
Wed Jun 20 14:57:48 2012 +0000
Revision:
0:826c6171fc1b
Updated documentation

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimniok 0:826c6171fc1b 1 #include "mbed.h"
shimniok 0:826c6171fc1b 2 #include "Matrix.h"
shimniok 0:826c6171fc1b 3
shimniok 0:826c6171fc1b 4 #define DEBUG 1
shimniok 0:826c6171fc1b 5
shimniok 0:826c6171fc1b 6 #define clamp360(x) ((((x) < 0) ? 360: 0) + fmod((x), 360))
shimniok 0:826c6171fc1b 7
shimniok 0:826c6171fc1b 8 /*
shimniok 0:826c6171fc1b 9 * Kalman Filter Setup
shimniok 0:826c6171fc1b 10 */
shimniok 0:826c6171fc1b 11 static float x[2]={ 0, 0 }; // System State: hdg, hdg rate
shimniok 0:826c6171fc1b 12 float z[2]={ 0, 0 }; // measurements, hdg, hdg rate
shimniok 0:826c6171fc1b 13 static float A[4]={ 1, 0, 0, 1}; // State transition matrix; A[1] should be dt
shimniok 0:826c6171fc1b 14 static float H[4]={ 1, 0, 0, 1 }; // Observer matrix maps measurements to state transition
shimniok 0:826c6171fc1b 15 float K[4]={ 0, 0, 0, 0 }; // Kalman gain
shimniok 0:826c6171fc1b 16 static float P[4]={ 1000, 0, 0, 1000 }; // Covariance matrix
shimniok 0:826c6171fc1b 17 static float R[4]={ 3, 0, 0, 0.03 }; // Measurement noise, hdg, hdg rate
shimniok 0:826c6171fc1b 18 static float Q[4]={ 0.01, 0, 0, 0.01 }; // Process noise matrix
shimniok 0:826c6171fc1b 19 static float I[4]={ 1, 0, 0, 1 }; // Identity matrix
shimniok 0:826c6171fc1b 20
shimniok 0:826c6171fc1b 21 float kfGetX(int i)
shimniok 0:826c6171fc1b 22 {
shimniok 0:826c6171fc1b 23 return (i >= 0 && i < 2) ? x[i] : 0xFFFFFFFF;
shimniok 0:826c6171fc1b 24 }
shimniok 0:826c6171fc1b 25
shimniok 0:826c6171fc1b 26 /** headingKalmanInit
shimniok 0:826c6171fc1b 27 *
shimniok 0:826c6171fc1b 28 * initialize x, z, K, and P
shimniok 0:826c6171fc1b 29 */
shimniok 0:826c6171fc1b 30 void headingKalmanInit(float x0)
shimniok 0:826c6171fc1b 31 {
shimniok 0:826c6171fc1b 32 x[0] = x0;
shimniok 0:826c6171fc1b 33 x[1] = 0;
shimniok 0:826c6171fc1b 34
shimniok 0:826c6171fc1b 35 z[0] = 0;
shimniok 0:826c6171fc1b 36 z[1] = 0;
shimniok 0:826c6171fc1b 37
shimniok 0:826c6171fc1b 38 K[0] = 0; K[1] = 0;
shimniok 0:826c6171fc1b 39 K[2] = 0; K[3] = 0;
shimniok 0:826c6171fc1b 40
shimniok 0:826c6171fc1b 41 P[0] = 1000; P[1] = 0;
shimniok 0:826c6171fc1b 42 P[2] = 0; P[3] = 1000;
shimniok 0:826c6171fc1b 43 }
shimniok 0:826c6171fc1b 44
shimniok 0:826c6171fc1b 45
shimniok 0:826c6171fc1b 46 /* headingKalman
shimniok 0:826c6171fc1b 47 *
shimniok 0:826c6171fc1b 48 * Implements a 1-dimensional, 1st order Kalman Filter
shimniok 0:826c6171fc1b 49 *
shimniok 0:826c6171fc1b 50 * That is, it deals with heading and heading rate (h and h') but no other
shimniok 0:826c6171fc1b 51 * state variables. The state equations are:
shimniok 0:826c6171fc1b 52 *
shimniok 0:826c6171fc1b 53 * X = A X^
shimniok 0:826c6171fc1b 54 * h = h + h'dt --> | h | = | 1 dt | | h |
shimniok 0:826c6171fc1b 55 * h' = h' | h' | | 0 1 | | h' |
shimniok 0:826c6171fc1b 56 *
shimniok 0:826c6171fc1b 57 * Kalman Filtering is not that hard. If it's hard you haven't found the right
shimniok 0:826c6171fc1b 58 * teacher. Try taking CS373 from Udacity.com
shimniok 0:826c6171fc1b 59 *
shimniok 0:826c6171fc1b 60 * This notation is Octave (Matlab) syntax and is based on the Bishop-Welch
shimniok 0:826c6171fc1b 61 * paper and references the equation numbers in that paper.
shimniok 0:826c6171fc1b 62 * http://www.cs.unc.edu/~welch/kalman/kalmanIntro.html
shimniok 0:826c6171fc1b 63 *
shimniok 0:826c6171fc1b 64 * returns : current heading estimate
shimniok 0:826c6171fc1b 65 */
shimniok 0:826c6171fc1b 66 float headingKalman(float dt, float Hgps, bool gps, float dHgyro, bool gyro)
shimniok 0:826c6171fc1b 67 {
shimniok 0:826c6171fc1b 68 A[1] = dt;
shimniok 0:826c6171fc1b 69
shimniok 0:826c6171fc1b 70 /* Initialize, first time thru
shimniok 0:826c6171fc1b 71 x = H*z0
shimniok 0:826c6171fc1b 72 */
shimniok 0:826c6171fc1b 73
shimniok 0:826c6171fc1b 74 //fprintf(stdout, "gyro? %c gps? %c\n", (gyro)?'Y':'N', (gps)?'Y':'N');
shimniok 0:826c6171fc1b 75
shimniok 0:826c6171fc1b 76 // Depending on what sensor measurements we've gotten,
shimniok 0:826c6171fc1b 77 // switch between observer (H) matrices and measurement noise (R) matrices
shimniok 0:826c6171fc1b 78 // TODO: incorporate HDOP or sat count in R
shimniok 0:826c6171fc1b 79 if (gps) {
shimniok 0:826c6171fc1b 80 H[0] = 1.0;
shimniok 0:826c6171fc1b 81 z[0] = Hgps;
shimniok 0:826c6171fc1b 82 } else {
shimniok 0:826c6171fc1b 83 H[0] = 0;
shimniok 0:826c6171fc1b 84 z[0] = 0;
shimniok 0:826c6171fc1b 85 }
shimniok 0:826c6171fc1b 86
shimniok 0:826c6171fc1b 87 if (gyro) {
shimniok 0:826c6171fc1b 88 H[3] = 1.0;
shimniok 0:826c6171fc1b 89 z[1] = dHgyro;
shimniok 0:826c6171fc1b 90 } else {
shimniok 0:826c6171fc1b 91 H[3] = 0;
shimniok 0:826c6171fc1b 92 z[1] = 0;
shimniok 0:826c6171fc1b 93 }
shimniok 0:826c6171fc1b 94
shimniok 0:826c6171fc1b 95 //Matrix_print(2,2, A, "1. A");
shimniok 0:826c6171fc1b 96 //Matrix_print(2,2, P, " P");
shimniok 0:826c6171fc1b 97 //Matrix_print(2,1, x, " x");
shimniok 0:826c6171fc1b 98 //Matrix_print(2,1, K, " K");
shimniok 0:826c6171fc1b 99 //Matrix_print(2,2, H, "2. H");
shimniok 0:826c6171fc1b 100 //Matrix_print(2,1, z, " z");
shimniok 0:826c6171fc1b 101
shimniok 0:826c6171fc1b 102 /**********************************************************************
shimniok 0:826c6171fc1b 103 * Predict
shimniok 0:826c6171fc1b 104 %
shimniok 0:826c6171fc1b 105 * In this step we "move" our state estimate according to the equation
shimniok 0:826c6171fc1b 106 *
shimniok 0:826c6171fc1b 107 * x = A*x; // Eq 1.9
shimniok 0:826c6171fc1b 108 ***********************************************************************/
shimniok 0:826c6171fc1b 109 float xp[2];
shimniok 0:826c6171fc1b 110 Matrix_Multiply(2,2,1, xp, A, x);
shimniok 0:826c6171fc1b 111
shimniok 0:826c6171fc1b 112 //Matrix_print(2,1, xp, "3. xp");
shimniok 0:826c6171fc1b 113
shimniok 0:826c6171fc1b 114 /**********************************************************************
shimniok 0:826c6171fc1b 115 * We also have to "move" our uncertainty and add noise. Whenever we move,
shimniok 0:826c6171fc1b 116 * we lose certainty because of system noise.
shimniok 0:826c6171fc1b 117 *
shimniok 0:826c6171fc1b 118 * P = A*P*A' + Q; // Eq 1.10
shimniok 0:826c6171fc1b 119 ***********************************************************************/
shimniok 0:826c6171fc1b 120 float At[4];
shimniok 0:826c6171fc1b 121 Matrix_Transpose(2,2, At, A);
shimniok 0:826c6171fc1b 122 float AP[4];
shimniok 0:826c6171fc1b 123 Matrix_Multiply(2,2,2, AP, A, P);
shimniok 0:826c6171fc1b 124 float APAt[4];
shimniok 0:826c6171fc1b 125 Matrix_Multiply(2,2,2, APAt, AP, At);
shimniok 0:826c6171fc1b 126 Matrix_Add(2,2, P, APAt, Q);
shimniok 0:826c6171fc1b 127
shimniok 0:826c6171fc1b 128 //Matrix_print(2,2, P, "4. P");
shimniok 0:826c6171fc1b 129
shimniok 0:826c6171fc1b 130 /**********************************************************************
shimniok 0:826c6171fc1b 131 * Measurement aka Correct
shimniok 0:826c6171fc1b 132 * First, we have to figure out the Kalman Gain which is basically how
shimniok 0:826c6171fc1b 133 * much we trust the sensor measurement versus our prediction.
shimniok 0:826c6171fc1b 134 *
shimniok 0:826c6171fc1b 135 * K = P*H'*inv(H*P*H' + R); // Eq 1.11
shimniok 0:826c6171fc1b 136 ***********************************************************************/
shimniok 0:826c6171fc1b 137 float Ht[4];
shimniok 0:826c6171fc1b 138 //Matrix_print(2,2, H, "5. H");
shimniok 0:826c6171fc1b 139 Matrix_Transpose(2,2, Ht, H);
shimniok 0:826c6171fc1b 140 //Matrix_print(2,2, Ht, "5. Ht");
shimniok 0:826c6171fc1b 141
shimniok 0:826c6171fc1b 142 float HP[2];
shimniok 0:826c6171fc1b 143 //Matrix_print(2,2, P, "5. P");
shimniok 0:826c6171fc1b 144 Matrix_Multiply(2,2,2, HP, H, P);
shimniok 0:826c6171fc1b 145 //Matrix_print(2,2, HP, "5. HP");
shimniok 0:826c6171fc1b 146
shimniok 0:826c6171fc1b 147 float HPHt[4];
shimniok 0:826c6171fc1b 148 Matrix_Multiply(2,2,2, HPHt, HP, Ht);
shimniok 0:826c6171fc1b 149 //Matrix_print(2,2, HPHt, "5. HPHt");
shimniok 0:826c6171fc1b 150
shimniok 0:826c6171fc1b 151 float HPHtR[4];
shimniok 0:826c6171fc1b 152 //Matrix_print(2,2, R, "5. R");
shimniok 0:826c6171fc1b 153 Matrix_Add(2,2, HPHtR, HPHt, R);
shimniok 0:826c6171fc1b 154 //Matrix_print(2,2, HPHtR, "5. HPHtR");
shimniok 0:826c6171fc1b 155
shimniok 0:826c6171fc1b 156 Matrix_Inverse(2, HPHtR);
shimniok 0:826c6171fc1b 157 //Matrix_print(2,2, HPHtR, "5. HPHtR");
shimniok 0:826c6171fc1b 158
shimniok 0:826c6171fc1b 159 float PHt[2];
shimniok 0:826c6171fc1b 160 //Matrix_print(2,2, P, "5. P");
shimniok 0:826c6171fc1b 161 //Matrix_print(2,2, Ht, "5. Ht");
shimniok 0:826c6171fc1b 162 Matrix_Multiply(2,2,2, PHt, P, Ht);
shimniok 0:826c6171fc1b 163 //Matrix_print(2,2, PHt, "5. PHt");
shimniok 0:826c6171fc1b 164
shimniok 0:826c6171fc1b 165 Matrix_Multiply(2,2,2, K, PHt, HPHtR);
shimniok 0:826c6171fc1b 166
shimniok 0:826c6171fc1b 167 //Matrix_print(2,2, K, "5. K");
shimniok 0:826c6171fc1b 168
shimniok 0:826c6171fc1b 169 /**********************************************************************
shimniok 0:826c6171fc1b 170 * Then we determine the discrepancy between prediction and measurement
shimniok 0:826c6171fc1b 171 * with the "Innovation" or Residual: z-H*x, multiply that by the
shimniok 0:826c6171fc1b 172 * Kalman gain to correct the estimate towards the prediction a little
shimniok 0:826c6171fc1b 173 * at a time.
shimniok 0:826c6171fc1b 174 *
shimniok 0:826c6171fc1b 175 * x = x + K*(z-H*x); // Eq 1.12
shimniok 0:826c6171fc1b 176 ***********************************************************************/
shimniok 0:826c6171fc1b 177 float Hx[2];
shimniok 0:826c6171fc1b 178 Matrix_Multiply(2,2,1, Hx, H, xp);
shimniok 0:826c6171fc1b 179
shimniok 0:826c6171fc1b 180 //Matrix_print(2,2, H, "6. H");
shimniok 0:826c6171fc1b 181 //Matrix_print(2,1, x, "6. x");
shimniok 0:826c6171fc1b 182 //Matrix_print(2,1, Hx, "6. Hx");
shimniok 0:826c6171fc1b 183
shimniok 0:826c6171fc1b 184 float zHx[2];
shimniok 0:826c6171fc1b 185 Matrix_Subtract(2,1, zHx, z, Hx);
shimniok 0:826c6171fc1b 186
shimniok 0:826c6171fc1b 187 // At this point we need to be sure to correct heading to -180 to 180 range
shimniok 0:826c6171fc1b 188 if (zHx[0] > 180.0) zHx[0] -= 360.0;
shimniok 0:826c6171fc1b 189 if (zHx[0] <= -180.0) zHx[0] += 360.0;
shimniok 0:826c6171fc1b 190
shimniok 0:826c6171fc1b 191 //Matrix_print(2,1, z, "6. z");
shimniok 0:826c6171fc1b 192 //Matrix_print(2,1, zHx, "6. zHx");
shimniok 0:826c6171fc1b 193
shimniok 0:826c6171fc1b 194 float KzHx[2];
shimniok 0:826c6171fc1b 195 Matrix_Multiply(2,2,1, KzHx, K, zHx);
shimniok 0:826c6171fc1b 196
shimniok 0:826c6171fc1b 197 //Matrix_print(2,2, K, "6. K");
shimniok 0:826c6171fc1b 198 //Matrix_print(2,1, KzHx, "6. KzHx");
shimniok 0:826c6171fc1b 199
shimniok 0:826c6171fc1b 200 Matrix_Add(2,1, x, xp, KzHx);
shimniok 0:826c6171fc1b 201
shimniok 0:826c6171fc1b 202 // Clamp to 0-360 range
shimniok 0:826c6171fc1b 203 while (x[0] < 0) x[0] += 360.0;
shimniok 0:826c6171fc1b 204 while (x[0] >= 360.0) x[0] -= 360.0;
shimniok 0:826c6171fc1b 205
shimniok 0:826c6171fc1b 206 //Matrix_print(2,1, x, "6. x");
shimniok 0:826c6171fc1b 207
shimniok 0:826c6171fc1b 208 /**********************************************************************
shimniok 0:826c6171fc1b 209 * We also have to adjust the certainty. With a new measurement, the
shimniok 0:826c6171fc1b 210 * estimate certainty always increases.
shimniok 0:826c6171fc1b 211 *
shimniok 0:826c6171fc1b 212 * P = (I-K*H)*P; // Eq 1.13
shimniok 0:826c6171fc1b 213 ***********************************************************************/
shimniok 0:826c6171fc1b 214 float KH[4];
shimniok 0:826c6171fc1b 215 //Matrix_print(2,2, K, "7. K");
shimniok 0:826c6171fc1b 216 Matrix_Multiply(2,2,2, KH, K, H);
shimniok 0:826c6171fc1b 217 //Matrix_print(2,2, KH, "7. KH");
shimniok 0:826c6171fc1b 218 float IKH[4];
shimniok 0:826c6171fc1b 219 Matrix_Subtract(2,2, IKH, I, KH);
shimniok 0:826c6171fc1b 220 //Matrix_print(2,2, IKH, "7. IKH");
shimniok 0:826c6171fc1b 221 float P2[4];
shimniok 0:826c6171fc1b 222 Matrix_Multiply(2,2,2, P2, IKH, P);
shimniok 0:826c6171fc1b 223 Matrix_Copy(2, 2, P, P2);
shimniok 0:826c6171fc1b 224
shimniok 0:826c6171fc1b 225 //Matrix_print(2,2, P, "7. P");
shimniok 0:826c6171fc1b 226
shimniok 0:826c6171fc1b 227 return x[0];
shimniok 0:826c6171fc1b 228 }