You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
67 lines
2.9 KiB
67 lines
2.9 KiB
/* |
|
Autocode for fusion of a magnetic declination estimate where the innovation is given by |
|
|
|
innovation = atanf(magMeasEarthFrameEast/magMeasEarthFrameNorth) - declinationAngle; |
|
|
|
magMeasEarthFrameEast and magMeasEarthFrameNorth are obtained by rotating the magnetometer measurements from body frame to earth frame. |
|
declinationAngle is the estimated declination as that location |
|
|
|
This fusion method is used to constrain the rotation of the earth field vector when there are no earth relative measurements |
|
(e.g. using optical flow without GPS, or when the vehicle is stationary) to provide an absolute yaw reference. In this situation the presence of yaw gyro errors |
|
can cause the magnetic declination of the earth field estimates to slowly rotate. |
|
|
|
Divide by zero protection and protection against a badly conditioned covariance matrix must be included. |
|
*/ |
|
|
|
// Calculate intermediate variable |
|
float t2 = magE*magE; |
|
float t3 = magN*magN; |
|
float t4 = t2+t3; |
|
float t5 = P[16][16]*t2; |
|
float t6 = P[17][17]*t3; |
|
float t7 = t2*t2; |
|
float t8 = R_DECL*t7; |
|
float t9 = t3*t3; |
|
float t10 = R_DECL*t9; |
|
float t11 = R_DECL*t2*t3*2.0f; |
|
float t14 = P[16][17]*magE*magN; |
|
float t15 = P[17][16]*magE*magN; |
|
float t12 = t5+t6+t8+t10+t11-t14-t15; |
|
float t13 = 1.0f / t12; |
|
float t16 = magE; |
|
float t17 = magN; |
|
float t18 = t16*t16; |
|
float t19 = t17*t17; |
|
float t20 = t18+t19; |
|
float t21 = 1.0f/t20; |
|
|
|
// Calculate the observation Jacobian |
|
// Note only 2 terms are non-zero which can be used in matrix operations for calculation of Kalman gains and covariance update to significantly reduce cost |
|
H_DECL[16] = -t16*t21; |
|
H_DECL[17] = t17*t21; |
|
|
|
// Calculate the Kalman gains |
|
Kfusion[0] = -t4*t13*(P[0][16]*magE-P[0][17]*magN); |
|
Kfusion[1] = -t4*t13*(P[1][16]*magE-P[1][17]*magN); |
|
Kfusion[2] = -t4*t13*(P[2][16]*magE-P[2][17]*magN); |
|
Kfusion[3] = -t4*t13*(P[3][16]*magE-P[3][17]*magN); |
|
Kfusion[4] = -t4*t13*(P[4][16]*magE-P[4][17]*magN); |
|
Kfusion[5] = -t4*t13*(P[5][16]*magE-P[5][17]*magN); |
|
Kfusion[6] = -t4*t13*(P[6][16]*magE-P[6][17]*magN); |
|
Kfusion[7] = -t4*t13*(P[7][16]*magE-P[7][17]*magN); |
|
Kfusion[8] = -t4*t13*(P[8][16]*magE-P[8][17]*magN); |
|
Kfusion[9] = -t4*t13*(P[9][16]*magE-P[9][17]*magN); |
|
Kfusion[10] = -t4*t13*(P[10][16]*magE-P[10][17]*magN); |
|
Kfusion[11] = -t4*t13*(P[11][16]*magE-P[11][17]*magN); |
|
Kfusion[12] = -t4*t13*(P[12][16]*magE-P[12][17]*magN); |
|
Kfusion[13] = -t4*t13*(P[13][16]*magE-P[13][17]*magN); |
|
Kfusion[14] = -t4*t13*(P[14][16]*magE-P[14][17]*magN); |
|
Kfusion[15] = -t4*t13*(P[15][16]*magE-P[15][17]*magN); |
|
Kfusion[16] = -t4*t13*(P[16][16]*magE-P[16][17]*magN); |
|
Kfusion[17] = -t4*t13*(P[17][16]*magE-P[17][17]*magN); |
|
Kfusion[18] = -t4*t13*(P[18][16]*magE-P[18][17]*magN); |
|
Kfusion[19] = -t4*t13*(P[19][16]*magE-P[19][17]*magN); |
|
Kfusion[20] = -t4*t13*(P[20][16]*magE-P[20][17]*magN); |
|
Kfusion[21] = -t4*t13*(P[21][16]*magE-P[21][17]*magN); |
|
Kfusion[22] = -t4*t13*(P[22][16]*magE-P[22][17]*magN); |
|
Kfusion[23] = -t4*t13*(P[23][16]*magE-P[23][17]*magN);
|
|
|