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
- Committer:
- shimniok
- Date:
- 2013-05-27
- Revision:
- 0:a6a169de725f
- Child:
- 2:fbc6e3cf3ed8
File content as of revision 0:a6a169de725f:
#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]; }