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?
User | Revision | Line number | New 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 | } |