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
Diff: Estimation/kalman.cpp
- Revision:
- 0:a6a169de725f
- Child:
- 2:fbc6e3cf3ed8
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Estimation/kalman.cpp Mon May 27 13:26:03 2013 +0000 @@ -0,0 +1,228 @@ +#include "mbed.h" +#include "Matrix.h" + +#define DEBUG 1 + +#define clamp360(x) ((((x) < 0) ? 360: 0) + fmod((x), 360)) + +/* + * Kalman Filter Setup + */ +static float x[2]={ 0, 0 }; // System State: hdg, hdg rate +float z[2]={ 0, 0 }; // measurements, hdg, hdg rate +static float A[4]={ 1, 0, 0, 1}; // State transition matrix; A[1] should be dt +static float H[4]={ 1, 0, 0, 1 }; // Observer matrix maps measurements to state transition +float K[4]={ 0, 0, 0, 0 }; // Kalman gain +static float P[4]={ 1000, 0, 0, 1000 }; // Covariance matrix +static float R[4]={ 3, 0, 0, 0.03 }; // Measurement noise, hdg, hdg rate +static float Q[4]={ 0.01, 0, 0, 0.01 }; // Process noise matrix +static float I[4]={ 1, 0, 0, 1 }; // Identity matrix + +float kfGetX(int i) +{ + return (i >= 0 && i < 2) ? x[i] : 0xFFFFFFFF; +} + +/** headingKalmanInit + * + * initialize x, z, K, and P + */ +void headingKalmanInit(float x0) +{ + x[0] = x0; + x[1] = 0; + + z[0] = 0; + z[1] = 0; + + K[0] = 0; K[1] = 0; + K[2] = 0; K[3] = 0; + + P[0] = 1000; P[1] = 0; + P[2] = 0; P[3] = 1000; +} + + +/* headingKalman + * + * Implements a 1-dimensional, 1st order Kalman Filter + * + * That is, it deals with heading and heading rate (h and h') but no other + * state variables. The state equations are: + * + * X = A X^ + * h = h + h'dt --> | h | = | 1 dt | | h | + * h' = h' | h' | | 0 1 | | h' | + * + * Kalman Filtering is not that hard. If it's hard you haven't found the right + * teacher. Try taking CS373 from Udacity.com + * + * This notation is Octave (Matlab) syntax and is based on the Bishop-Welch + * paper and references the equation numbers in that paper. + * http://www.cs.unc.edu/~welch/kalman/kalmanIntro.html + * + * returns : current heading estimate + */ +float headingKalman(float dt, float Hgps, bool gps, float dHgyro, bool gyro) +{ + A[1] = dt; + + /* Initialize, first time thru + x = H*z0 + */ + + //fprintf(stdout, "gyro? %c gps? %c\n", (gyro)?'Y':'N', (gps)?'Y':'N'); + + // Depending on what sensor measurements we've gotten, + // switch between observer (H) matrices and measurement noise (R) matrices + // TODO: incorporate HDOP or sat count in R + if (gps) { + H[0] = 1.0; + z[0] = Hgps; + } else { + H[0] = 0; + z[0] = 0; + } + + if (gyro) { + H[3] = 1.0; + z[1] = dHgyro; + } else { + H[3] = 0; + z[1] = 0; + } + + //Matrix_print(2,2, A, "1. A"); + //Matrix_print(2,2, P, " P"); + //Matrix_print(2,1, x, " x"); + //Matrix_print(2,1, K, " K"); + //Matrix_print(2,2, H, "2. H"); + //Matrix_print(2,1, z, " z"); + + /********************************************************************** + * Predict + % + * In this step we "move" our state estimate according to the equation + * + * x = A*x; // Eq 1.9 + ***********************************************************************/ + float xp[2]; + Matrix_Multiply(2,2,1, xp, A, x); + + //Matrix_print(2,1, xp, "3. xp"); + + /********************************************************************** + * We also have to "move" our uncertainty and add noise. Whenever we move, + * we lose certainty because of system noise. + * + * P = A*P*A' + Q; // Eq 1.10 + ***********************************************************************/ + float At[4]; + Matrix_Transpose(2,2, At, A); + float AP[4]; + Matrix_Multiply(2,2,2, AP, A, P); + float APAt[4]; + Matrix_Multiply(2,2,2, APAt, AP, At); + Matrix_Add(2,2, P, APAt, Q); + + //Matrix_print(2,2, P, "4. P"); + + /********************************************************************** + * Measurement aka Correct + * First, we have to figure out the Kalman Gain which is basically how + * much we trust the sensor measurement versus our prediction. + * + * K = P*H'*inv(H*P*H' + R); // Eq 1.11 + ***********************************************************************/ + float Ht[4]; + //Matrix_print(2,2, H, "5. H"); + Matrix_Transpose(2,2, Ht, H); + //Matrix_print(2,2, Ht, "5. Ht"); + + float HP[2]; + //Matrix_print(2,2, P, "5. P"); + Matrix_Multiply(2,2,2, HP, H, P); + //Matrix_print(2,2, HP, "5. HP"); + + float HPHt[4]; + Matrix_Multiply(2,2,2, HPHt, HP, Ht); + //Matrix_print(2,2, HPHt, "5. HPHt"); + + float HPHtR[4]; + //Matrix_print(2,2, R, "5. R"); + Matrix_Add(2,2, HPHtR, HPHt, R); + //Matrix_print(2,2, HPHtR, "5. HPHtR"); + + Matrix_Inverse(2, HPHtR); + //Matrix_print(2,2, HPHtR, "5. HPHtR"); + + float PHt[2]; + //Matrix_print(2,2, P, "5. P"); + //Matrix_print(2,2, Ht, "5. Ht"); + Matrix_Multiply(2,2,2, PHt, P, Ht); + //Matrix_print(2,2, PHt, "5. PHt"); + + Matrix_Multiply(2,2,2, K, PHt, HPHtR); + + //Matrix_print(2,2, K, "5. K"); + + /********************************************************************** + * Then we determine the discrepancy between prediction and measurement + * with the "Innovation" or Residual: z-H*x, multiply that by the + * Kalman gain to correct the estimate towards the prediction a little + * at a time. + * + * x = x + K*(z-H*x); // Eq 1.12 + ***********************************************************************/ + float Hx[2]; + Matrix_Multiply(2,2,1, Hx, H, xp); + + //Matrix_print(2,2, H, "6. H"); + //Matrix_print(2,1, x, "6. x"); + //Matrix_print(2,1, Hx, "6. Hx"); + + float zHx[2]; + Matrix_Subtract(2,1, zHx, z, Hx); + + // At this point we need to be sure to correct heading to -180 to 180 range + if (zHx[0] > 180.0) zHx[0] -= 360.0; + if (zHx[0] <= -180.0) zHx[0] += 360.0; + + //Matrix_print(2,1, z, "6. z"); + //Matrix_print(2,1, zHx, "6. zHx"); + + float KzHx[2]; + Matrix_Multiply(2,2,1, KzHx, K, zHx); + + //Matrix_print(2,2, K, "6. K"); + //Matrix_print(2,1, KzHx, "6. KzHx"); + + Matrix_Add(2,1, x, xp, KzHx); + + // Clamp to 0-360 range + while (x[0] < 0) x[0] += 360.0; + while (x[0] >= 360.0) x[0] -= 360.0; + + //Matrix_print(2,1, x, "6. x"); + + /********************************************************************** + * We also have to adjust the certainty. With a new measurement, the + * estimate certainty always increases. + * + * P = (I-K*H)*P; // Eq 1.13 + ***********************************************************************/ + float KH[4]; + //Matrix_print(2,2, K, "7. K"); + Matrix_Multiply(2,2,2, KH, K, H); + //Matrix_print(2,2, KH, "7. KH"); + float IKH[4]; + Matrix_Subtract(2,2, IKH, I, KH); + //Matrix_print(2,2, IKH, "7. IKH"); + float P2[4]; + Matrix_Multiply(2,2,2, P2, IKH, P); + Matrix_Copy(2, 2, P, P2); + + //Matrix_print(2,2, P, "7. P"); + + return x[0]; +}