/*
Returns the angular difference in the horizontal plane between the
"from" vector and north, in degrees.
Description of heading algorithm:
Shift and scale the magnetic reading based on calibration data to find
the North vector. Use the acceleration readings to determine the Up
vector (gravity is measured as an upward acceleration). The cross
product of North and Up vectors is East. The vectors East and North
form a basis for the horizontal plane. The From vector is projected
into the horizontal plane and the angle between the projected vector
and horizontal north is returned.
*/
double iCompass::iheading(int ix, int iy, int iz, double ax, double ay, double az, double mx, double my, double mz)
{
if (samples == maxSamples)
{
samples = 0;
myRA.clear();
}
vector<int> from = {ix, iy, iz};
vector<double> m = {mx, my, mz};
vector<double> a = {ax, ay, az};
// compute E and N
vector<double> E;
vector<double> N;
vector_cross(&m, &a, &E);
vector_normalize(&E);
vector_cross(&a, &E, &N);
vector_normalize(&N);
// compute heading
double heading = atan2(vector_dot(&E, &from), vector_dot(&N, &from)) * 180 / M_PI;
if (heading < 0) heading += 360;
if(heading < -9990) {
heading = 0;
}
heading = clamp360(iround(heading,1)+declinationAngle);
myRA.addValue(heading);
myRA.addValue(HeadingAvgCorr(heading, oldHeading));
oldHeading = heading;
samples++;
return myRA.getAverage();
}
float CartPosition::bearingTo(CartPosition to)
{
float result = clamp360(90 - 180/PI * atan2(to.y-y, to.x-x));
return result;
}
/* 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 3 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);
zHx[0] = clamp180(zHx[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);
x[0] = clamp360(x[0]); // Clamp to 0-360 range
//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];
}
