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.
64 lines
2.6 KiB
64 lines
2.6 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 = 1.0f/t4; |
|
float t22 = magE*t5; |
|
float t23 = magN*t5; |
|
float t6 = P[16][16]*t22; |
|
float t13 = P[17][16]*t23; |
|
float t7 = t6-t13; |
|
float t8 = t22*t7; |
|
float t9 = P[16][17]*t22; |
|
float t14 = P[17][17]*t23; |
|
float t10 = t9-t14; |
|
float t15 = t23*t10; |
|
float t11 = R_DECL+t8-t15; // innovation variance |
|
float t12 = 1.0f/t11; |
|
|
|
// 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] = -magE*t5; |
|
H_DECL[17] = magN*t5; |
|
|
|
// Calculate the Kalman gains |
|
Kfusion[0] = -t12*(P[0][16]*t22-P[0][17]*t23); |
|
Kfusion[1] = -t12*(P[1][16]*t22-P[1][17]*t23); |
|
Kfusion[2] = -t12*(P[2][16]*t22-P[2][17]*t23); |
|
Kfusion[3] = -t12*(P[3][16]*t22-P[3][17]*t23); |
|
Kfusion[4] = -t12*(P[4][16]*t22-P[4][17]*t23); |
|
Kfusion[5] = -t12*(P[5][16]*t22-P[5][17]*t23); |
|
Kfusion[6] = -t12*(P[6][16]*t22-P[6][17]*t23); |
|
Kfusion[7] = -t12*(P[7][16]*t22-P[7][17]*t23); |
|
Kfusion[8] = -t12*(P[8][16]*t22-P[8][17]*t23); |
|
Kfusion[9] = -t12*(P[9][16]*t22-P[9][17]*t23); |
|
Kfusion[10] = -t12*(P[10][16]*t22-P[10][17]*t23); |
|
Kfusion[11] = -t12*(P[11][16]*t22-P[11][17]*t23); |
|
Kfusion[12] = -t12*(P[12][16]*t22-P[12][17]*t23); |
|
Kfusion[13] = -t12*(P[13][16]*t22-P[13][17]*t23); |
|
Kfusion[14] = -t12*(P[14][16]*t22-P[14][17]*t23); |
|
Kfusion[15] = -t12*(P[15][16]*t22-P[15][17]*t23); |
|
Kfusion[16] = -t12*(t6-P[16][17]*t23); |
|
Kfusion[17] = t12*(t14-P[17][16]*t22); |
|
Kfusion[18] = -t12*(P[18][16]*t22-P[18][17]*t23); |
|
Kfusion[19] = -t12*(P[19][16]*t22-P[19][17]*t23); |
|
Kfusion[20] = -t12*(P[20][16]*t22-P[20][17]*t23); |
|
Kfusion[21] = -t12*(P[21][16]*t22-P[21][17]*t23); |
|
Kfusion[22] = -t12*(P[22][16]*t22-P[22][17]*t23); |
|
Kfusion[23] = -t12*(P[23][16]*t22-P[23][17]*t23); |
|
|
|
|