Michael Shimniok / Mbed 2 deprecated DataBus

Code for autonomous rover for Sparkfun AVC. DataBus won 3rd in 2012 and the same code was used on Troubled Child, a 1986 Jeep Grand Wagoneer to win 1st in 2014.

Dependencies:   mbed Watchdog SDFileSystem DigoleSerialDisp

Estimation/kalman.cpp@25:bb5356402687, 2018-11-30 (annotated)

Committer:
shimniok
Date:
Fri Nov 30 16:11:53 2018 +0000
Revision:
25:bb5356402687
Parent:
3:42f3821c4e54 
Initial publish of revised version.

Who changed what in which revision?

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