mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
AP_NavEKF2: Fix 3-axis mag fusion per axis error handling
Ensure that if fusion checks for an axis fails, fusion will not be perfomred for all axes.
This commit is contained in:
Paul Riseborough
Peter Barker
parent
cca1a5d9d2
commit
af19fea268
@ -295,16 +295,12 @@ void NavEKF2_core::SelectMagFusion()
|
|||||||
(frontend->_mag_ef_limit > 0 && have_table_earth_field)) {
|
(frontend->_mag_ef_limit > 0 && have_table_earth_field)) {
|
||||||
FuseDeclination(0.34f);
|
FuseDeclination(0.34f);
|
||||||
}
|
}
|
||||||
// fuse the three magnetometer componenents sequentially
|
|
||||||
for (mag_state.obsIndex = 0; mag_state.obsIndex <= 2; mag_state.obsIndex++) {
|
// fuse the three magnetometer componenents using sequential fusion of each axis
|
||||||
hal.util->perf_begin(_perf_test[0]);
|
hal.util->perf_begin(_perf_test[0]);
|
||||||
FuseMagnetometer();
|
FuseMagnetometer();
|
||||||
hal.util->perf_end(_perf_test[0]);
|
hal.util->perf_end(_perf_test[0]);
|
||||||
// don't continue fusion if unhealthy
|
|
||||||
if (!magHealth) {
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
// zero the test ratio output from the inactive simple magnetometer yaw fusion
|
// zero the test ratio output from the inactive simple magnetometer yaw fusion
|
||||||
yawTestRatio = 0.0f;
|
yawTestRatio = 0.0f;
|
||||||
}
|
}
|
||||||
@ -337,8 +333,6 @@ void NavEKF2_core::SelectMagFusion()
|
|||||||
*/
|
*/
|
||||||
void NavEKF2_core::FuseMagnetometer()
|
void NavEKF2_core::FuseMagnetometer()
|
||||||
{
|
{
|
||||||
hal.util->perf_begin(_perf_test[1]);
|
|
||||||
|
|
||||||
// declarations
|
// declarations
|
||||||
ftype &q0 = mag_state.q0;
|
ftype &q0 = mag_state.q0;
|
||||||
ftype &q1 = mag_state.q1;
|
ftype &q1 = mag_state.q1;
|
||||||
@ -350,7 +344,6 @@ void NavEKF2_core::FuseMagnetometer()
|
|||||||
ftype &magXbias = mag_state.magXbias;
|
ftype &magXbias = mag_state.magXbias;
|
||||||
ftype &magYbias = mag_state.magYbias;
|
ftype &magYbias = mag_state.magYbias;
|
||||||
ftype &magZbias = mag_state.magZbias;
|
ftype &magZbias = mag_state.magZbias;
|
||||||
uint8_t &obsIndex = mag_state.obsIndex;
|
|
||||||
Matrix3f &DCM = mag_state.DCM;
|
Matrix3f &DCM = mag_state.DCM;
|
||||||
Vector3f &MagPred = mag_state.MagPred;
|
Vector3f &MagPred = mag_state.MagPred;
|
||||||
ftype &R_MAG = mag_state.R_MAG;
|
ftype &R_MAG = mag_state.R_MAG;
|
||||||
@ -360,7 +353,101 @@ void NavEKF2_core::FuseMagnetometer()
|
|||||||
Vector6 SK_MY;
|
Vector6 SK_MY;
|
||||||
Vector6 SK_MZ;
|
Vector6 SK_MZ;
|
||||||
|
|
||||||
hal.util->perf_end(_perf_test[1]);
|
// copy required states to local variable names
|
||||||
|
q0 = stateStruct.quat[0];
|
||||||
|
q1 = stateStruct.quat[1];
|
||||||
|
q2 = stateStruct.quat[2];
|
||||||
|
q3 = stateStruct.quat[3];
|
||||||
|
magN = stateStruct.earth_magfield[0];
|
||||||
|
magE = stateStruct.earth_magfield[1];
|
||||||
|
magD = stateStruct.earth_magfield[2];
|
||||||
|
magXbias = stateStruct.body_magfield[0];
|
||||||
|
magYbias = stateStruct.body_magfield[1];
|
||||||
|
magZbias = stateStruct.body_magfield[2];
|
||||||
|
|
||||||
|
// rotate predicted earth components into body axes and calculate
|
||||||
|
// predicted measurements
|
||||||
|
DCM[0][0] = q0*q0 + q1*q1 - q2*q2 - q3*q3;
|
||||||
|
DCM[0][1] = 2.0f*(q1*q2 + q0*q3);
|
||||||
|
DCM[0][2] = 2.0f*(q1*q3-q0*q2);
|
||||||
|
DCM[1][0] = 2.0f*(q1*q2 - q0*q3);
|
||||||
|
DCM[1][1] = q0*q0 - q1*q1 + q2*q2 - q3*q3;
|
||||||
|
DCM[1][2] = 2.0f*(q2*q3 + q0*q1);
|
||||||
|
DCM[2][0] = 2.0f*(q1*q3 + q0*q2);
|
||||||
|
DCM[2][1] = 2.0f*(q2*q3 - q0*q1);
|
||||||
|
DCM[2][2] = q0*q0 - q1*q1 - q2*q2 + q3*q3;
|
||||||
|
MagPred[0] = DCM[0][0]*magN + DCM[0][1]*magE + DCM[0][2]*magD + magXbias;
|
||||||
|
MagPred[1] = DCM[1][0]*magN + DCM[1][1]*magE + DCM[1][2]*magD + magYbias;
|
||||||
|
MagPred[2] = DCM[2][0]*magN + DCM[2][1]*magE + DCM[2][2]*magD + magZbias;
|
||||||
|
|
||||||
|
// calculate the measurement innovation for each axis
|
||||||
|
for (uint8_t i = 0; i<=2; i++) {
|
||||||
|
innovMag[i] = MagPred[i] - magDataDelayed.mag[i];
|
||||||
|
}
|
||||||
|
|
||||||
|
// scale magnetometer observation error with total angular rate to allow for timing errors
|
||||||
|
R_MAG = sq(constrain_float(frontend->_magNoise, 0.01f, 0.5f)) + sq(frontend->magVarRateScale*delAngCorrected.length() / imuDataDelayed.delAngDT);
|
||||||
|
|
||||||
|
// calculate common expressions used to calculate observation jacobians an innovation variance for each component
|
||||||
|
SH_MAG[0] = sq(q0) - sq(q1) + sq(q2) - sq(q3);
|
||||||
|
SH_MAG[1] = sq(q0) + sq(q1) - sq(q2) - sq(q3);
|
||||||
|
SH_MAG[2] = sq(q0) - sq(q1) - sq(q2) + sq(q3);
|
||||||
|
SH_MAG[3] = 2.0f*q0*q1 + 2.0f*q2*q3;
|
||||||
|
SH_MAG[4] = 2.0f*q0*q3 + 2.0f*q1*q2;
|
||||||
|
SH_MAG[5] = 2.0f*q0*q2 + 2.0f*q1*q3;
|
||||||
|
SH_MAG[6] = magE*(2.0f*q0*q1 - 2.0f*q2*q3);
|
||||||
|
SH_MAG[7] = 2.0f*q1*q3 - 2.0f*q0*q2;
|
||||||
|
SH_MAG[8] = 2.0f*q0*q3;
|
||||||
|
|
||||||
|
// Calculate the innovation variance for each axis
|
||||||
|
// X axis
|
||||||
|
varInnovMag[0] = (P[19][19] + R_MAG - P[1][19]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][19]*SH_MAG[1] + P[17][19]*SH_MAG[4] + P[18][19]*SH_MAG[7] + P[2][19]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2)) - (magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5])*(P[19][1] - P[1][1]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][1]*SH_MAG[1] + P[17][1]*SH_MAG[4] + P[18][1]*SH_MAG[7] + P[2][1]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2))) + SH_MAG[1]*(P[19][16] - P[1][16]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][16]*SH_MAG[1] + P[17][16]*SH_MAG[4] + P[18][16]*SH_MAG[7] + P[2][16]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2))) + SH_MAG[4]*(P[19][17] - P[1][17]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][17]*SH_MAG[1] + P[17][17]*SH_MAG[4] + P[18][17]*SH_MAG[7] + P[2][17]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2))) + SH_MAG[7]*(P[19][18] - P[1][18]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][18]*SH_MAG[1] + P[17][18]*SH_MAG[4] + P[18][18]*SH_MAG[7] + P[2][18]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2))) + (magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2))*(P[19][2] - P[1][2]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][2]*SH_MAG[1] + P[17][2]*SH_MAG[4] + P[18][2]*SH_MAG[7] + P[2][2]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2))));
|
||||||
|
if (varInnovMag[0] >= R_MAG) {
|
||||||
|
faultStatus.bad_xmag = false;
|
||||||
|
} else {
|
||||||
|
// the calculation is badly conditioned, so we cannot perform fusion on this step
|
||||||
|
// we reset the covariance matrix and try again next measurement
|
||||||
|
CovarianceInit();
|
||||||
|
faultStatus.bad_xmag = true;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Y axis
|
||||||
|
varInnovMag[1] = (P[20][20] + R_MAG + P[0][20]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][20]*SH_MAG[0] + P[18][20]*SH_MAG[3] - (SH_MAG[8] - 2.0f*q1*q2)*(P[20][16] + P[0][16]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][16]*SH_MAG[0] + P[18][16]*SH_MAG[3] - P[2][16]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][16]*(SH_MAG[8] - 2.0f*q1*q2)) - P[2][20]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) + (magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5])*(P[20][0] + P[0][0]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][0]*SH_MAG[0] + P[18][0]*SH_MAG[3] - P[2][0]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][0]*(SH_MAG[8] - 2.0f*q1*q2)) + SH_MAG[0]*(P[20][17] + P[0][17]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][17]*SH_MAG[0] + P[18][17]*SH_MAG[3] - P[2][17]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][17]*(SH_MAG[8] - 2.0f*q1*q2)) + SH_MAG[3]*(P[20][18] + P[0][18]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][18]*SH_MAG[0] + P[18][18]*SH_MAG[3] - P[2][18]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][18]*(SH_MAG[8] - 2.0f*q1*q2)) - P[16][20]*(SH_MAG[8] - 2.0f*q1*q2) - (magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1])*(P[20][2] + P[0][2]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][2]*SH_MAG[0] + P[18][2]*SH_MAG[3] - P[2][2]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][2]*(SH_MAG[8] - 2.0f*q1*q2)));
|
||||||
|
if (varInnovMag[1] >= R_MAG) {
|
||||||
|
faultStatus.bad_ymag = false;
|
||||||
|
} else {
|
||||||
|
// the calculation is badly conditioned, so we cannot perform fusion on this step
|
||||||
|
// we reset the covariance matrix and try again next measurement
|
||||||
|
CovarianceInit();
|
||||||
|
faultStatus.bad_ymag = true;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Z axis
|
||||||
|
varInnovMag[2] = (P[21][21] + R_MAG + P[16][21]*SH_MAG[5] + P[18][21]*SH_MAG[2] - (2.0f*q0*q1 - 2.0f*q2*q3)*(P[21][17] + P[16][17]*SH_MAG[5] + P[18][17]*SH_MAG[2] - P[0][17]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2)) + P[1][17]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][17]*(2.0f*q0*q1 - 2.0f*q2*q3)) - P[0][21]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2)) + P[1][21]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) + SH_MAG[5]*(P[21][16] + P[16][16]*SH_MAG[5] + P[18][16]*SH_MAG[2] - P[0][16]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2)) + P[1][16]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][16]*(2.0f*q0*q1 - 2.0f*q2*q3)) + SH_MAG[2]*(P[21][18] + P[16][18]*SH_MAG[5] + P[18][18]*SH_MAG[2] - P[0][18]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2)) + P[1][18]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][18]*(2.0f*q0*q1 - 2.0f*q2*q3)) - (magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2))*(P[21][0] + P[16][0]*SH_MAG[5] + P[18][0]*SH_MAG[2] - P[0][0]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2)) + P[1][0]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][0]*(2.0f*q0*q1 - 2.0f*q2*q3)) - P[17][21]*(2.0f*q0*q1 - 2.0f*q2*q3) + (magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1])*(P[21][1] + P[16][1]*SH_MAG[5] + P[18][1]*SH_MAG[2] - P[0][1]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2)) + P[1][1]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][1]*(2.0f*q0*q1 - 2.0f*q2*q3)));
|
||||||
|
if (varInnovMag[2] >= R_MAG) {
|
||||||
|
faultStatus.bad_zmag = false;
|
||||||
|
} else {
|
||||||
|
// the calculation is badly conditioned, so we cannot perform fusion on this step
|
||||||
|
// we reset the covariance matrix and try again next measurement
|
||||||
|
CovarianceInit();
|
||||||
|
faultStatus.bad_zmag = true;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// calculate the innovation test ratios
|
||||||
|
for (uint8_t i = 0; i<=2; i++) {
|
||||||
|
magTestRatio[i] = sq(innovMag[i]) / (sq(MAX(0.01f * (float)frontend->_magInnovGate, 1.0f)) * varInnovMag[i]);
|
||||||
|
}
|
||||||
|
|
||||||
|
// check the last values from all components and set magnetometer health accordingly
|
||||||
|
magHealth = (magTestRatio[0] < 1.0f && magTestRatio[1] < 1.0f && magTestRatio[2] < 1.0f);
|
||||||
|
|
||||||
|
// if the magnetometer is unhealthy, do not proceed further
|
||||||
|
if (!magHealth) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
// perform sequential fusion of magnetometer measurements.
|
// perform sequential fusion of magnetometer measurements.
|
||||||
// this assumes that the errors in the different components are
|
// this assumes that the errors in the different components are
|
||||||
@ -369,402 +456,268 @@ void NavEKF2_core::FuseMagnetometer()
|
|||||||
// so we might as well take advantage of the computational efficiencies
|
// so we might as well take advantage of the computational efficiencies
|
||||||
// associated with sequential fusion
|
// associated with sequential fusion
|
||||||
// calculate observation jacobians and Kalman gains
|
// calculate observation jacobians and Kalman gains
|
||||||
if (obsIndex == 0)
|
for (uint8_t obsIndex = 0; obsIndex <= 2; obsIndex++) {
|
||||||
{
|
if (obsIndex == 0)
|
||||||
|
{
|
||||||
|
// calculate observation jacobians
|
||||||
|
memset(&H_MAG, 0, sizeof(H_MAG));
|
||||||
|
H_MAG[1] = SH_MAG[6] - magD*SH_MAG[2] - magN*SH_MAG[5];
|
||||||
|
H_MAG[2] = magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2);
|
||||||
|
H_MAG[16] = SH_MAG[1];
|
||||||
|
H_MAG[17] = SH_MAG[4];
|
||||||
|
H_MAG[18] = SH_MAG[7];
|
||||||
|
H_MAG[19] = 1.0f;
|
||||||
|
|
||||||
hal.util->perf_begin(_perf_test[2]);
|
// calculate Kalman gain
|
||||||
|
SK_MX[0] = 1.0f / varInnovMag[0];
|
||||||
|
SK_MX[1] = magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2);
|
||||||
|
SK_MX[2] = magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5];
|
||||||
|
SK_MX[3] = SH_MAG[7];
|
||||||
|
Kfusion[0] = SK_MX[0]*(P[0][19] + P[0][16]*SH_MAG[1] + P[0][17]*SH_MAG[4] - P[0][1]*SK_MX[2] + P[0][2]*SK_MX[1] + P[0][18]*SK_MX[3]);
|
||||||
|
Kfusion[1] = SK_MX[0]*(P[1][19] + P[1][16]*SH_MAG[1] + P[1][17]*SH_MAG[4] - P[1][1]*SK_MX[2] + P[1][2]*SK_MX[1] + P[1][18]*SK_MX[3]);
|
||||||
|
Kfusion[2] = SK_MX[0]*(P[2][19] + P[2][16]*SH_MAG[1] + P[2][17]*SH_MAG[4] - P[2][1]*SK_MX[2] + P[2][2]*SK_MX[1] + P[2][18]*SK_MX[3]);
|
||||||
|
Kfusion[3] = SK_MX[0]*(P[3][19] + P[3][16]*SH_MAG[1] + P[3][17]*SH_MAG[4] - P[3][1]*SK_MX[2] + P[3][2]*SK_MX[1] + P[3][18]*SK_MX[3]);
|
||||||
|
Kfusion[4] = SK_MX[0]*(P[4][19] + P[4][16]*SH_MAG[1] + P[4][17]*SH_MAG[4] - P[4][1]*SK_MX[2] + P[4][2]*SK_MX[1] + P[4][18]*SK_MX[3]);
|
||||||
|
Kfusion[5] = SK_MX[0]*(P[5][19] + P[5][16]*SH_MAG[1] + P[5][17]*SH_MAG[4] - P[5][1]*SK_MX[2] + P[5][2]*SK_MX[1] + P[5][18]*SK_MX[3]);
|
||||||
|
Kfusion[6] = SK_MX[0]*(P[6][19] + P[6][16]*SH_MAG[1] + P[6][17]*SH_MAG[4] - P[6][1]*SK_MX[2] + P[6][2]*SK_MX[1] + P[6][18]*SK_MX[3]);
|
||||||
|
Kfusion[7] = SK_MX[0]*(P[7][19] + P[7][16]*SH_MAG[1] + P[7][17]*SH_MAG[4] - P[7][1]*SK_MX[2] + P[7][2]*SK_MX[1] + P[7][18]*SK_MX[3]);
|
||||||
|
Kfusion[8] = SK_MX[0]*(P[8][19] + P[8][16]*SH_MAG[1] + P[8][17]*SH_MAG[4] - P[8][1]*SK_MX[2] + P[8][2]*SK_MX[1] + P[8][18]*SK_MX[3]);
|
||||||
|
Kfusion[9] = SK_MX[0]*(P[9][19] + P[9][16]*SH_MAG[1] + P[9][17]*SH_MAG[4] - P[9][1]*SK_MX[2] + P[9][2]*SK_MX[1] + P[9][18]*SK_MX[3]);
|
||||||
|
Kfusion[10] = SK_MX[0]*(P[10][19] + P[10][16]*SH_MAG[1] + P[10][17]*SH_MAG[4] - P[10][1]*SK_MX[2] + P[10][2]*SK_MX[1] + P[10][18]*SK_MX[3]);
|
||||||
|
Kfusion[11] = SK_MX[0]*(P[11][19] + P[11][16]*SH_MAG[1] + P[11][17]*SH_MAG[4] - P[11][1]*SK_MX[2] + P[11][2]*SK_MX[1] + P[11][18]*SK_MX[3]);
|
||||||
|
Kfusion[12] = SK_MX[0]*(P[12][19] + P[12][16]*SH_MAG[1] + P[12][17]*SH_MAG[4] - P[12][1]*SK_MX[2] + P[12][2]*SK_MX[1] + P[12][18]*SK_MX[3]);
|
||||||
|
Kfusion[13] = SK_MX[0]*(P[13][19] + P[13][16]*SH_MAG[1] + P[13][17]*SH_MAG[4] - P[13][1]*SK_MX[2] + P[13][2]*SK_MX[1] + P[13][18]*SK_MX[3]);
|
||||||
|
Kfusion[14] = SK_MX[0]*(P[14][19] + P[14][16]*SH_MAG[1] + P[14][17]*SH_MAG[4] - P[14][1]*SK_MX[2] + P[14][2]*SK_MX[1] + P[14][18]*SK_MX[3]);
|
||||||
|
Kfusion[15] = SK_MX[0]*(P[15][19] + P[15][16]*SH_MAG[1] + P[15][17]*SH_MAG[4] - P[15][1]*SK_MX[2] + P[15][2]*SK_MX[1] + P[15][18]*SK_MX[3]);
|
||||||
|
|
||||||
// copy required states to local variable names
|
// zero Kalman gains to inhibit wind state estimation
|
||||||
q0 = stateStruct.quat[0];
|
if (!inhibitWindStates) {
|
||||||
q1 = stateStruct.quat[1];
|
Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][16]*SH_MAG[1] + P[22][17]*SH_MAG[4] - P[22][1]*SK_MX[2] + P[22][2]*SK_MX[1] + P[22][18]*SK_MX[3]);
|
||||||
q2 = stateStruct.quat[2];
|
Kfusion[23] = SK_MX[0]*(P[23][19] + P[23][16]*SH_MAG[1] + P[23][17]*SH_MAG[4] - P[23][1]*SK_MX[2] + P[23][2]*SK_MX[1] + P[23][18]*SK_MX[3]);
|
||||||
q3 = stateStruct.quat[3];
|
} else {
|
||||||
magN = stateStruct.earth_magfield[0];
|
Kfusion[22] = 0.0f;
|
||||||
magE = stateStruct.earth_magfield[1];
|
Kfusion[23] = 0.0f;
|
||||||
magD = stateStruct.earth_magfield[2];
|
|
||||||
magXbias = stateStruct.body_magfield[0];
|
|
||||||
magYbias = stateStruct.body_magfield[1];
|
|
||||||
magZbias = stateStruct.body_magfield[2];
|
|
||||||
|
|
||||||
// rotate predicted earth components into body axes and calculate
|
|
||||||
// predicted measurements
|
|
||||||
DCM[0][0] = q0*q0 + q1*q1 - q2*q2 - q3*q3;
|
|
||||||
DCM[0][1] = 2.0f*(q1*q2 + q0*q3);
|
|
||||||
DCM[0][2] = 2.0f*(q1*q3-q0*q2);
|
|
||||||
DCM[1][0] = 2.0f*(q1*q2 - q0*q3);
|
|
||||||
DCM[1][1] = q0*q0 - q1*q1 + q2*q2 - q3*q3;
|
|
||||||
DCM[1][2] = 2.0f*(q2*q3 + q0*q1);
|
|
||||||
DCM[2][0] = 2.0f*(q1*q3 + q0*q2);
|
|
||||||
DCM[2][1] = 2.0f*(q2*q3 - q0*q1);
|
|
||||||
DCM[2][2] = q0*q0 - q1*q1 - q2*q2 + q3*q3;
|
|
||||||
MagPred[0] = DCM[0][0]*magN + DCM[0][1]*magE + DCM[0][2]*magD + magXbias;
|
|
||||||
MagPred[1] = DCM[1][0]*magN + DCM[1][1]*magE + DCM[1][2]*magD + magYbias;
|
|
||||||
MagPred[2] = DCM[2][0]*magN + DCM[2][1]*magE + DCM[2][2]*magD + magZbias;
|
|
||||||
|
|
||||||
// calculate the measurement innovation for each axis
|
|
||||||
for (uint8_t i = 0; i<=2; i++) {
|
|
||||||
innovMag[i] = MagPred[i] - magDataDelayed.mag[i];
|
|
||||||
}
|
|
||||||
|
|
||||||
// scale magnetometer observation error with total angular rate to allow for timing errors
|
|
||||||
R_MAG = sq(constrain_float(frontend->_magNoise, 0.01f, 0.5f)) + sq(frontend->magVarRateScale*delAngCorrected.length() / imuDataDelayed.delAngDT);
|
|
||||||
|
|
||||||
// calculate common expressions used to calculate observation jacobians an innovation variance for each component
|
|
||||||
SH_MAG[0] = sq(q0) - sq(q1) + sq(q2) - sq(q3);
|
|
||||||
SH_MAG[1] = sq(q0) + sq(q1) - sq(q2) - sq(q3);
|
|
||||||
SH_MAG[2] = sq(q0) - sq(q1) - sq(q2) + sq(q3);
|
|
||||||
SH_MAG[3] = 2.0f*q0*q1 + 2.0f*q2*q3;
|
|
||||||
SH_MAG[4] = 2.0f*q0*q3 + 2.0f*q1*q2;
|
|
||||||
SH_MAG[5] = 2.0f*q0*q2 + 2.0f*q1*q3;
|
|
||||||
SH_MAG[6] = magE*(2.0f*q0*q1 - 2.0f*q2*q3);
|
|
||||||
SH_MAG[7] = 2.0f*q1*q3 - 2.0f*q0*q2;
|
|
||||||
SH_MAG[8] = 2.0f*q0*q3;
|
|
||||||
|
|
||||||
// Calculate the innovation variance for each axis
|
|
||||||
// X axis
|
|
||||||
varInnovMag[0] = (P[19][19] + R_MAG - P[1][19]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][19]*SH_MAG[1] + P[17][19]*SH_MAG[4] + P[18][19]*SH_MAG[7] + P[2][19]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2)) - (magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5])*(P[19][1] - P[1][1]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][1]*SH_MAG[1] + P[17][1]*SH_MAG[4] + P[18][1]*SH_MAG[7] + P[2][1]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2))) + SH_MAG[1]*(P[19][16] - P[1][16]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][16]*SH_MAG[1] + P[17][16]*SH_MAG[4] + P[18][16]*SH_MAG[7] + P[2][16]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2))) + SH_MAG[4]*(P[19][17] - P[1][17]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][17]*SH_MAG[1] + P[17][17]*SH_MAG[4] + P[18][17]*SH_MAG[7] + P[2][17]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2))) + SH_MAG[7]*(P[19][18] - P[1][18]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][18]*SH_MAG[1] + P[17][18]*SH_MAG[4] + P[18][18]*SH_MAG[7] + P[2][18]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2))) + (magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2))*(P[19][2] - P[1][2]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][2]*SH_MAG[1] + P[17][2]*SH_MAG[4] + P[18][2]*SH_MAG[7] + P[2][2]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2))));
|
|
||||||
if (varInnovMag[0] >= R_MAG) {
|
|
||||||
faultStatus.bad_xmag = false;
|
|
||||||
} else {
|
|
||||||
// the calculation is badly conditioned, so we cannot perform fusion on this step
|
|
||||||
// we reset the covariance matrix and try again next measurement
|
|
||||||
CovarianceInit();
|
|
||||||
obsIndex = 1;
|
|
||||||
faultStatus.bad_xmag = true;
|
|
||||||
|
|
||||||
hal.util->perf_end(_perf_test[2]);
|
|
||||||
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Y axis
|
|
||||||
varInnovMag[1] = (P[20][20] + R_MAG + P[0][20]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][20]*SH_MAG[0] + P[18][20]*SH_MAG[3] - (SH_MAG[8] - 2.0f*q1*q2)*(P[20][16] + P[0][16]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][16]*SH_MAG[0] + P[18][16]*SH_MAG[3] - P[2][16]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][16]*(SH_MAG[8] - 2.0f*q1*q2)) - P[2][20]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) + (magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5])*(P[20][0] + P[0][0]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][0]*SH_MAG[0] + P[18][0]*SH_MAG[3] - P[2][0]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][0]*(SH_MAG[8] - 2.0f*q1*q2)) + SH_MAG[0]*(P[20][17] + P[0][17]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][17]*SH_MAG[0] + P[18][17]*SH_MAG[3] - P[2][17]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][17]*(SH_MAG[8] - 2.0f*q1*q2)) + SH_MAG[3]*(P[20][18] + P[0][18]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][18]*SH_MAG[0] + P[18][18]*SH_MAG[3] - P[2][18]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][18]*(SH_MAG[8] - 2.0f*q1*q2)) - P[16][20]*(SH_MAG[8] - 2.0f*q1*q2) - (magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1])*(P[20][2] + P[0][2]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][2]*SH_MAG[0] + P[18][2]*SH_MAG[3] - P[2][2]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][2]*(SH_MAG[8] - 2.0f*q1*q2)));
|
|
||||||
if (varInnovMag[1] >= R_MAG) {
|
|
||||||
faultStatus.bad_ymag = false;
|
|
||||||
} else {
|
|
||||||
// the calculation is badly conditioned, so we cannot perform fusion on this step
|
|
||||||
// we reset the covariance matrix and try again next measurement
|
|
||||||
CovarianceInit();
|
|
||||||
obsIndex = 2;
|
|
||||||
faultStatus.bad_ymag = true;
|
|
||||||
|
|
||||||
hal.util->perf_end(_perf_test[2]);
|
|
||||||
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Z axis
|
|
||||||
varInnovMag[2] = (P[21][21] + R_MAG + P[16][21]*SH_MAG[5] + P[18][21]*SH_MAG[2] - (2.0f*q0*q1 - 2.0f*q2*q3)*(P[21][17] + P[16][17]*SH_MAG[5] + P[18][17]*SH_MAG[2] - P[0][17]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2)) + P[1][17]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][17]*(2.0f*q0*q1 - 2.0f*q2*q3)) - P[0][21]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2)) + P[1][21]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) + SH_MAG[5]*(P[21][16] + P[16][16]*SH_MAG[5] + P[18][16]*SH_MAG[2] - P[0][16]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2)) + P[1][16]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][16]*(2.0f*q0*q1 - 2.0f*q2*q3)) + SH_MAG[2]*(P[21][18] + P[16][18]*SH_MAG[5] + P[18][18]*SH_MAG[2] - P[0][18]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2)) + P[1][18]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][18]*(2.0f*q0*q1 - 2.0f*q2*q3)) - (magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2))*(P[21][0] + P[16][0]*SH_MAG[5] + P[18][0]*SH_MAG[2] - P[0][0]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2)) + P[1][0]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][0]*(2.0f*q0*q1 - 2.0f*q2*q3)) - P[17][21]*(2.0f*q0*q1 - 2.0f*q2*q3) + (magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1])*(P[21][1] + P[16][1]*SH_MAG[5] + P[18][1]*SH_MAG[2] - P[0][1]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2)) + P[1][1]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][1]*(2.0f*q0*q1 - 2.0f*q2*q3)));
|
|
||||||
if (varInnovMag[2] >= R_MAG) {
|
|
||||||
faultStatus.bad_zmag = false;
|
|
||||||
} else {
|
|
||||||
// the calculation is badly conditioned, so we cannot perform fusion on this step
|
|
||||||
// we reset the covariance matrix and try again next measurement
|
|
||||||
CovarianceInit();
|
|
||||||
obsIndex = 3;
|
|
||||||
faultStatus.bad_zmag = true;
|
|
||||||
|
|
||||||
hal.util->perf_end(_perf_test[2]);
|
|
||||||
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// calculate the innovation test ratios
|
|
||||||
for (uint8_t i = 0; i<=2; i++) {
|
|
||||||
magTestRatio[i] = sq(innovMag[i]) / (sq(MAX(0.01f * (float)frontend->_magInnovGate, 1.0f)) * varInnovMag[i]);
|
|
||||||
}
|
|
||||||
|
|
||||||
// check the last values from all components and set magnetometer health accordingly
|
|
||||||
magHealth = (magTestRatio[0] < 1.0f && magTestRatio[1] < 1.0f && magTestRatio[2] < 1.0f);
|
|
||||||
|
|
||||||
// if the magnetometer is unhealthy, do not proceed further
|
|
||||||
if (!magHealth) {
|
|
||||||
hal.util->perf_end(_perf_test[2]);
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
for (uint8_t i = 0; i<=stateIndexLim; i++) H_MAG[i] = 0.0f;
|
|
||||||
H_MAG[1] = SH_MAG[6] - magD*SH_MAG[2] - magN*SH_MAG[5];
|
|
||||||
H_MAG[2] = magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2);
|
|
||||||
H_MAG[16] = SH_MAG[1];
|
|
||||||
H_MAG[17] = SH_MAG[4];
|
|
||||||
H_MAG[18] = SH_MAG[7];
|
|
||||||
H_MAG[19] = 1.0f;
|
|
||||||
|
|
||||||
// calculate Kalman gain
|
|
||||||
SK_MX[0] = 1.0f / varInnovMag[0];
|
|
||||||
SK_MX[1] = magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2);
|
|
||||||
SK_MX[2] = magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5];
|
|
||||||
SK_MX[3] = SH_MAG[7];
|
|
||||||
Kfusion[0] = SK_MX[0]*(P[0][19] + P[0][16]*SH_MAG[1] + P[0][17]*SH_MAG[4] - P[0][1]*SK_MX[2] + P[0][2]*SK_MX[1] + P[0][18]*SK_MX[3]);
|
|
||||||
Kfusion[1] = SK_MX[0]*(P[1][19] + P[1][16]*SH_MAG[1] + P[1][17]*SH_MAG[4] - P[1][1]*SK_MX[2] + P[1][2]*SK_MX[1] + P[1][18]*SK_MX[3]);
|
|
||||||
Kfusion[2] = SK_MX[0]*(P[2][19] + P[2][16]*SH_MAG[1] + P[2][17]*SH_MAG[4] - P[2][1]*SK_MX[2] + P[2][2]*SK_MX[1] + P[2][18]*SK_MX[3]);
|
|
||||||
Kfusion[3] = SK_MX[0]*(P[3][19] + P[3][16]*SH_MAG[1] + P[3][17]*SH_MAG[4] - P[3][1]*SK_MX[2] + P[3][2]*SK_MX[1] + P[3][18]*SK_MX[3]);
|
|
||||||
Kfusion[4] = SK_MX[0]*(P[4][19] + P[4][16]*SH_MAG[1] + P[4][17]*SH_MAG[4] - P[4][1]*SK_MX[2] + P[4][2]*SK_MX[1] + P[4][18]*SK_MX[3]);
|
|
||||||
Kfusion[5] = SK_MX[0]*(P[5][19] + P[5][16]*SH_MAG[1] + P[5][17]*SH_MAG[4] - P[5][1]*SK_MX[2] + P[5][2]*SK_MX[1] + P[5][18]*SK_MX[3]);
|
|
||||||
Kfusion[6] = SK_MX[0]*(P[6][19] + P[6][16]*SH_MAG[1] + P[6][17]*SH_MAG[4] - P[6][1]*SK_MX[2] + P[6][2]*SK_MX[1] + P[6][18]*SK_MX[3]);
|
|
||||||
Kfusion[7] = SK_MX[0]*(P[7][19] + P[7][16]*SH_MAG[1] + P[7][17]*SH_MAG[4] - P[7][1]*SK_MX[2] + P[7][2]*SK_MX[1] + P[7][18]*SK_MX[3]);
|
|
||||||
Kfusion[8] = SK_MX[0]*(P[8][19] + P[8][16]*SH_MAG[1] + P[8][17]*SH_MAG[4] - P[8][1]*SK_MX[2] + P[8][2]*SK_MX[1] + P[8][18]*SK_MX[3]);
|
|
||||||
Kfusion[9] = SK_MX[0]*(P[9][19] + P[9][16]*SH_MAG[1] + P[9][17]*SH_MAG[4] - P[9][1]*SK_MX[2] + P[9][2]*SK_MX[1] + P[9][18]*SK_MX[3]);
|
|
||||||
Kfusion[10] = SK_MX[0]*(P[10][19] + P[10][16]*SH_MAG[1] + P[10][17]*SH_MAG[4] - P[10][1]*SK_MX[2] + P[10][2]*SK_MX[1] + P[10][18]*SK_MX[3]);
|
|
||||||
Kfusion[11] = SK_MX[0]*(P[11][19] + P[11][16]*SH_MAG[1] + P[11][17]*SH_MAG[4] - P[11][1]*SK_MX[2] + P[11][2]*SK_MX[1] + P[11][18]*SK_MX[3]);
|
|
||||||
Kfusion[12] = SK_MX[0]*(P[12][19] + P[12][16]*SH_MAG[1] + P[12][17]*SH_MAG[4] - P[12][1]*SK_MX[2] + P[12][2]*SK_MX[1] + P[12][18]*SK_MX[3]);
|
|
||||||
Kfusion[13] = SK_MX[0]*(P[13][19] + P[13][16]*SH_MAG[1] + P[13][17]*SH_MAG[4] - P[13][1]*SK_MX[2] + P[13][2]*SK_MX[1] + P[13][18]*SK_MX[3]);
|
|
||||||
Kfusion[14] = SK_MX[0]*(P[14][19] + P[14][16]*SH_MAG[1] + P[14][17]*SH_MAG[4] - P[14][1]*SK_MX[2] + P[14][2]*SK_MX[1] + P[14][18]*SK_MX[3]);
|
|
||||||
Kfusion[15] = SK_MX[0]*(P[15][19] + P[15][16]*SH_MAG[1] + P[15][17]*SH_MAG[4] - P[15][1]*SK_MX[2] + P[15][2]*SK_MX[1] + P[15][18]*SK_MX[3]);
|
|
||||||
// end perf block
|
|
||||||
|
|
||||||
// zero Kalman gains to inhibit wind state estimation
|
|
||||||
if (!inhibitWindStates) {
|
|
||||||
Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][16]*SH_MAG[1] + P[22][17]*SH_MAG[4] - P[22][1]*SK_MX[2] + P[22][2]*SK_MX[1] + P[22][18]*SK_MX[3]);
|
|
||||||
Kfusion[23] = SK_MX[0]*(P[23][19] + P[23][16]*SH_MAG[1] + P[23][17]*SH_MAG[4] - P[23][1]*SK_MX[2] + P[23][2]*SK_MX[1] + P[23][18]*SK_MX[3]);
|
|
||||||
} else {
|
|
||||||
Kfusion[22] = 0.0f;
|
|
||||||
Kfusion[23] = 0.0f;
|
|
||||||
}
|
|
||||||
// zero Kalman gains to inhibit magnetic field state estimation
|
|
||||||
if (!inhibitMagStates) {
|
|
||||||
Kfusion[16] = SK_MX[0]*(P[16][19] + P[16][16]*SH_MAG[1] + P[16][17]*SH_MAG[4] - P[16][1]*SK_MX[2] + P[16][2]*SK_MX[1] + P[16][18]*SK_MX[3]);
|
|
||||||
Kfusion[17] = SK_MX[0]*(P[17][19] + P[17][16]*SH_MAG[1] + P[17][17]*SH_MAG[4] - P[17][1]*SK_MX[2] + P[17][2]*SK_MX[1] + P[17][18]*SK_MX[3]);
|
|
||||||
Kfusion[18] = SK_MX[0]*(P[18][19] + P[18][16]*SH_MAG[1] + P[18][17]*SH_MAG[4] - P[18][1]*SK_MX[2] + P[18][2]*SK_MX[1] + P[18][18]*SK_MX[3]);
|
|
||||||
Kfusion[19] = SK_MX[0]*(P[19][19] + P[19][16]*SH_MAG[1] + P[19][17]*SH_MAG[4] - P[19][1]*SK_MX[2] + P[19][2]*SK_MX[1] + P[19][18]*SK_MX[3]);
|
|
||||||
Kfusion[20] = SK_MX[0]*(P[20][19] + P[20][16]*SH_MAG[1] + P[20][17]*SH_MAG[4] - P[20][1]*SK_MX[2] + P[20][2]*SK_MX[1] + P[20][18]*SK_MX[3]);
|
|
||||||
Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][16]*SH_MAG[1] + P[21][17]*SH_MAG[4] - P[21][1]*SK_MX[2] + P[21][2]*SK_MX[1] + P[21][18]*SK_MX[3]);
|
|
||||||
} else {
|
|
||||||
for (uint8_t i=16; i<=21; i++) {
|
|
||||||
Kfusion[i] = 0.0f;
|
|
||||||
}
|
}
|
||||||
}
|
// zero Kalman gains to inhibit magnetic field state estimation
|
||||||
|
if (!inhibitMagStates) {
|
||||||
// reset the observation index to 0 (we start by fusing the X measurement)
|
Kfusion[16] = SK_MX[0]*(P[16][19] + P[16][16]*SH_MAG[1] + P[16][17]*SH_MAG[4] - P[16][1]*SK_MX[2] + P[16][2]*SK_MX[1] + P[16][18]*SK_MX[3]);
|
||||||
obsIndex = 0;
|
Kfusion[17] = SK_MX[0]*(P[17][19] + P[17][16]*SH_MAG[1] + P[17][17]*SH_MAG[4] - P[17][1]*SK_MX[2] + P[17][2]*SK_MX[1] + P[17][18]*SK_MX[3]);
|
||||||
|
Kfusion[18] = SK_MX[0]*(P[18][19] + P[18][16]*SH_MAG[1] + P[18][17]*SH_MAG[4] - P[18][1]*SK_MX[2] + P[18][2]*SK_MX[1] + P[18][18]*SK_MX[3]);
|
||||||
// set flags to indicate to other processes that fusion has been performed and is required on the next frame
|
Kfusion[19] = SK_MX[0]*(P[19][19] + P[19][16]*SH_MAG[1] + P[19][17]*SH_MAG[4] - P[19][1]*SK_MX[2] + P[19][2]*SK_MX[1] + P[19][18]*SK_MX[3]);
|
||||||
// this can be used by other fusion processes to avoid fusing on the same frame as this expensive step
|
Kfusion[20] = SK_MX[0]*(P[20][19] + P[20][16]*SH_MAG[1] + P[20][17]*SH_MAG[4] - P[20][1]*SK_MX[2] + P[20][2]*SK_MX[1] + P[20][18]*SK_MX[3]);
|
||||||
magFusePerformed = true;
|
Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][16]*SH_MAG[1] + P[21][17]*SH_MAG[4] - P[21][1]*SK_MX[2] + P[21][2]*SK_MX[1] + P[21][18]*SK_MX[3]);
|
||||||
magFuseRequired = true;
|
} else {
|
||||||
|
for (uint8_t i=16; i<=21; i++) {
|
||||||
hal.util->perf_end(_perf_test[2]);
|
Kfusion[i] = 0.0f;
|
||||||
|
}
|
||||||
}
|
|
||||||
else if (obsIndex == 1) // we are now fusing the Y measurement
|
|
||||||
{
|
|
||||||
|
|
||||||
hal.util->perf_begin(_perf_test[3]);
|
|
||||||
|
|
||||||
// calculate observation jacobians
|
|
||||||
for (uint8_t i = 0; i<=stateIndexLim; i++) H_MAG[i] = 0.0f;
|
|
||||||
H_MAG[0] = magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5];
|
|
||||||
H_MAG[2] = - magE*SH_MAG[4] - magD*SH_MAG[7] - magN*SH_MAG[1];
|
|
||||||
H_MAG[16] = 2.0f*q1*q2 - SH_MAG[8];
|
|
||||||
H_MAG[17] = SH_MAG[0];
|
|
||||||
H_MAG[18] = SH_MAG[3];
|
|
||||||
H_MAG[20] = 1.0f;
|
|
||||||
|
|
||||||
// calculate Kalman gain
|
|
||||||
SK_MY[0] = 1.0f / varInnovMag[1];
|
|
||||||
SK_MY[1] = magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1];
|
|
||||||
SK_MY[2] = magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5];
|
|
||||||
SK_MY[3] = SH_MAG[8] - 2.0f*q1*q2;
|
|
||||||
Kfusion[0] = SK_MY[0]*(P[0][20] + P[0][17]*SH_MAG[0] + P[0][18]*SH_MAG[3] + P[0][0]*SK_MY[2] - P[0][2]*SK_MY[1] - P[0][16]*SK_MY[3]);
|
|
||||||
Kfusion[1] = SK_MY[0]*(P[1][20] + P[1][17]*SH_MAG[0] + P[1][18]*SH_MAG[3] + P[1][0]*SK_MY[2] - P[1][2]*SK_MY[1] - P[1][16]*SK_MY[3]);
|
|
||||||
Kfusion[2] = SK_MY[0]*(P[2][20] + P[2][17]*SH_MAG[0] + P[2][18]*SH_MAG[3] + P[2][0]*SK_MY[2] - P[2][2]*SK_MY[1] - P[2][16]*SK_MY[3]);
|
|
||||||
Kfusion[3] = SK_MY[0]*(P[3][20] + P[3][17]*SH_MAG[0] + P[3][18]*SH_MAG[3] + P[3][0]*SK_MY[2] - P[3][2]*SK_MY[1] - P[3][16]*SK_MY[3]);
|
|
||||||
Kfusion[4] = SK_MY[0]*(P[4][20] + P[4][17]*SH_MAG[0] + P[4][18]*SH_MAG[3] + P[4][0]*SK_MY[2] - P[4][2]*SK_MY[1] - P[4][16]*SK_MY[3]);
|
|
||||||
Kfusion[5] = SK_MY[0]*(P[5][20] + P[5][17]*SH_MAG[0] + P[5][18]*SH_MAG[3] + P[5][0]*SK_MY[2] - P[5][2]*SK_MY[1] - P[5][16]*SK_MY[3]);
|
|
||||||
Kfusion[6] = SK_MY[0]*(P[6][20] + P[6][17]*SH_MAG[0] + P[6][18]*SH_MAG[3] + P[6][0]*SK_MY[2] - P[6][2]*SK_MY[1] - P[6][16]*SK_MY[3]);
|
|
||||||
Kfusion[7] = SK_MY[0]*(P[7][20] + P[7][17]*SH_MAG[0] + P[7][18]*SH_MAG[3] + P[7][0]*SK_MY[2] - P[7][2]*SK_MY[1] - P[7][16]*SK_MY[3]);
|
|
||||||
Kfusion[8] = SK_MY[0]*(P[8][20] + P[8][17]*SH_MAG[0] + P[8][18]*SH_MAG[3] + P[8][0]*SK_MY[2] - P[8][2]*SK_MY[1] - P[8][16]*SK_MY[3]);
|
|
||||||
Kfusion[9] = SK_MY[0]*(P[9][20] + P[9][17]*SH_MAG[0] + P[9][18]*SH_MAG[3] + P[9][0]*SK_MY[2] - P[9][2]*SK_MY[1] - P[9][16]*SK_MY[3]);
|
|
||||||
Kfusion[10] = SK_MY[0]*(P[10][20] + P[10][17]*SH_MAG[0] + P[10][18]*SH_MAG[3] + P[10][0]*SK_MY[2] - P[10][2]*SK_MY[1] - P[10][16]*SK_MY[3]);
|
|
||||||
Kfusion[11] = SK_MY[0]*(P[11][20] + P[11][17]*SH_MAG[0] + P[11][18]*SH_MAG[3] + P[11][0]*SK_MY[2] - P[11][2]*SK_MY[1] - P[11][16]*SK_MY[3]);
|
|
||||||
Kfusion[12] = SK_MY[0]*(P[12][20] + P[12][17]*SH_MAG[0] + P[12][18]*SH_MAG[3] + P[12][0]*SK_MY[2] - P[12][2]*SK_MY[1] - P[12][16]*SK_MY[3]);
|
|
||||||
Kfusion[13] = SK_MY[0]*(P[13][20] + P[13][17]*SH_MAG[0] + P[13][18]*SH_MAG[3] + P[13][0]*SK_MY[2] - P[13][2]*SK_MY[1] - P[13][16]*SK_MY[3]);
|
|
||||||
Kfusion[14] = SK_MY[0]*(P[14][20] + P[14][17]*SH_MAG[0] + P[14][18]*SH_MAG[3] + P[14][0]*SK_MY[2] - P[14][2]*SK_MY[1] - P[14][16]*SK_MY[3]);
|
|
||||||
Kfusion[15] = SK_MY[0]*(P[15][20] + P[15][17]*SH_MAG[0] + P[15][18]*SH_MAG[3] + P[15][0]*SK_MY[2] - P[15][2]*SK_MY[1] - P[15][16]*SK_MY[3]);
|
|
||||||
// zero Kalman gains to inhibit wind state estimation
|
|
||||||
if (!inhibitWindStates) {
|
|
||||||
Kfusion[22] = SK_MY[0]*(P[22][20] + P[22][17]*SH_MAG[0] + P[22][18]*SH_MAG[3] + P[22][0]*SK_MY[2] - P[22][2]*SK_MY[1] - P[22][16]*SK_MY[3]);
|
|
||||||
Kfusion[23] = SK_MY[0]*(P[23][20] + P[23][17]*SH_MAG[0] + P[23][18]*SH_MAG[3] + P[23][0]*SK_MY[2] - P[23][2]*SK_MY[1] - P[23][16]*SK_MY[3]);
|
|
||||||
} else {
|
|
||||||
Kfusion[22] = 0.0f;
|
|
||||||
Kfusion[23] = 0.0f;
|
|
||||||
}
|
|
||||||
// zero Kalman gains to inhibit magnetic field state estimation
|
|
||||||
if (!inhibitMagStates) {
|
|
||||||
Kfusion[16] = SK_MY[0]*(P[16][20] + P[16][17]*SH_MAG[0] + P[16][18]*SH_MAG[3] + P[16][0]*SK_MY[2] - P[16][2]*SK_MY[1] - P[16][16]*SK_MY[3]);
|
|
||||||
Kfusion[17] = SK_MY[0]*(P[17][20] + P[17][17]*SH_MAG[0] + P[17][18]*SH_MAG[3] + P[17][0]*SK_MY[2] - P[17][2]*SK_MY[1] - P[17][16]*SK_MY[3]);
|
|
||||||
Kfusion[18] = SK_MY[0]*(P[18][20] + P[18][17]*SH_MAG[0] + P[18][18]*SH_MAG[3] + P[18][0]*SK_MY[2] - P[18][2]*SK_MY[1] - P[18][16]*SK_MY[3]);
|
|
||||||
Kfusion[19] = SK_MY[0]*(P[19][20] + P[19][17]*SH_MAG[0] + P[19][18]*SH_MAG[3] + P[19][0]*SK_MY[2] - P[19][2]*SK_MY[1] - P[19][16]*SK_MY[3]);
|
|
||||||
Kfusion[20] = SK_MY[0]*(P[20][20] + P[20][17]*SH_MAG[0] + P[20][18]*SH_MAG[3] + P[20][0]*SK_MY[2] - P[20][2]*SK_MY[1] - P[20][16]*SK_MY[3]);
|
|
||||||
Kfusion[21] = SK_MY[0]*(P[21][20] + P[21][17]*SH_MAG[0] + P[21][18]*SH_MAG[3] + P[21][0]*SK_MY[2] - P[21][2]*SK_MY[1] - P[21][16]*SK_MY[3]);
|
|
||||||
} else {
|
|
||||||
for (uint8_t i=16; i<=21; i++) {
|
|
||||||
Kfusion[i] = 0.0f;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// set flags to indicate to other processes that fusion has been performed
|
||||||
|
// this can be used by other fusion processes to avoid fusing on the same frame as this expensive step
|
||||||
|
magFusePerformed = true;
|
||||||
|
|
||||||
}
|
}
|
||||||
|
else if (obsIndex == 1) // we are now fusing the Y measurement
|
||||||
|
{
|
||||||
|
// calculate observation jacobians
|
||||||
|
memset(&H_MAG, 0, sizeof(H_MAG));
|
||||||
|
H_MAG[0] = magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5];
|
||||||
|
H_MAG[2] = - magE*SH_MAG[4] - magD*SH_MAG[7] - magN*SH_MAG[1];
|
||||||
|
H_MAG[16] = 2.0f*q1*q2 - SH_MAG[8];
|
||||||
|
H_MAG[17] = SH_MAG[0];
|
||||||
|
H_MAG[18] = SH_MAG[3];
|
||||||
|
H_MAG[20] = 1.0f;
|
||||||
|
|
||||||
// set flags to indicate to other processes that fusion has been performed and is required on the next frame
|
// calculate Kalman gain
|
||||||
// this can be used by other fusion processes to avoid fusing on the same frame as this expensive step
|
SK_MY[0] = 1.0f / varInnovMag[1];
|
||||||
magFusePerformed = true;
|
SK_MY[1] = magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1];
|
||||||
magFuseRequired = true;
|
SK_MY[2] = magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5];
|
||||||
|
SK_MY[3] = SH_MAG[8] - 2.0f*q1*q2;
|
||||||
hal.util->perf_end(_perf_test[3]);
|
Kfusion[0] = SK_MY[0]*(P[0][20] + P[0][17]*SH_MAG[0] + P[0][18]*SH_MAG[3] + P[0][0]*SK_MY[2] - P[0][2]*SK_MY[1] - P[0][16]*SK_MY[3]);
|
||||||
|
Kfusion[1] = SK_MY[0]*(P[1][20] + P[1][17]*SH_MAG[0] + P[1][18]*SH_MAG[3] + P[1][0]*SK_MY[2] - P[1][2]*SK_MY[1] - P[1][16]*SK_MY[3]);
|
||||||
}
|
Kfusion[2] = SK_MY[0]*(P[2][20] + P[2][17]*SH_MAG[0] + P[2][18]*SH_MAG[3] + P[2][0]*SK_MY[2] - P[2][2]*SK_MY[1] - P[2][16]*SK_MY[3]);
|
||||||
else if (obsIndex == 2) // we are now fusing the Z measurement
|
Kfusion[3] = SK_MY[0]*(P[3][20] + P[3][17]*SH_MAG[0] + P[3][18]*SH_MAG[3] + P[3][0]*SK_MY[2] - P[3][2]*SK_MY[1] - P[3][16]*SK_MY[3]);
|
||||||
{
|
Kfusion[4] = SK_MY[0]*(P[4][20] + P[4][17]*SH_MAG[0] + P[4][18]*SH_MAG[3] + P[4][0]*SK_MY[2] - P[4][2]*SK_MY[1] - P[4][16]*SK_MY[3]);
|
||||||
|
Kfusion[5] = SK_MY[0]*(P[5][20] + P[5][17]*SH_MAG[0] + P[5][18]*SH_MAG[3] + P[5][0]*SK_MY[2] - P[5][2]*SK_MY[1] - P[5][16]*SK_MY[3]);
|
||||||
hal.util->perf_begin(_perf_test[4]);
|
Kfusion[6] = SK_MY[0]*(P[6][20] + P[6][17]*SH_MAG[0] + P[6][18]*SH_MAG[3] + P[6][0]*SK_MY[2] - P[6][2]*SK_MY[1] - P[6][16]*SK_MY[3]);
|
||||||
|
Kfusion[7] = SK_MY[0]*(P[7][20] + P[7][17]*SH_MAG[0] + P[7][18]*SH_MAG[3] + P[7][0]*SK_MY[2] - P[7][2]*SK_MY[1] - P[7][16]*SK_MY[3]);
|
||||||
// calculate observation jacobians
|
Kfusion[8] = SK_MY[0]*(P[8][20] + P[8][17]*SH_MAG[0] + P[8][18]*SH_MAG[3] + P[8][0]*SK_MY[2] - P[8][2]*SK_MY[1] - P[8][16]*SK_MY[3]);
|
||||||
for (uint8_t i = 0; i<=stateIndexLim; i++) H_MAG[i] = 0.0f;
|
Kfusion[9] = SK_MY[0]*(P[9][20] + P[9][17]*SH_MAG[0] + P[9][18]*SH_MAG[3] + P[9][0]*SK_MY[2] - P[9][2]*SK_MY[1] - P[9][16]*SK_MY[3]);
|
||||||
H_MAG[0] = magN*(SH_MAG[8] - 2.0f*q1*q2) - magD*SH_MAG[3] - magE*SH_MAG[0];
|
Kfusion[10] = SK_MY[0]*(P[10][20] + P[10][17]*SH_MAG[0] + P[10][18]*SH_MAG[3] + P[10][0]*SK_MY[2] - P[10][2]*SK_MY[1] - P[10][16]*SK_MY[3]);
|
||||||
H_MAG[1] = magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1];
|
Kfusion[11] = SK_MY[0]*(P[11][20] + P[11][17]*SH_MAG[0] + P[11][18]*SH_MAG[3] + P[11][0]*SK_MY[2] - P[11][2]*SK_MY[1] - P[11][16]*SK_MY[3]);
|
||||||
H_MAG[16] = SH_MAG[5];
|
Kfusion[12] = SK_MY[0]*(P[12][20] + P[12][17]*SH_MAG[0] + P[12][18]*SH_MAG[3] + P[12][0]*SK_MY[2] - P[12][2]*SK_MY[1] - P[12][16]*SK_MY[3]);
|
||||||
H_MAG[17] = 2.0f*q2*q3 - 2.0f*q0*q1;
|
Kfusion[13] = SK_MY[0]*(P[13][20] + P[13][17]*SH_MAG[0] + P[13][18]*SH_MAG[3] + P[13][0]*SK_MY[2] - P[13][2]*SK_MY[1] - P[13][16]*SK_MY[3]);
|
||||||
H_MAG[18] = SH_MAG[2];
|
Kfusion[14] = SK_MY[0]*(P[14][20] + P[14][17]*SH_MAG[0] + P[14][18]*SH_MAG[3] + P[14][0]*SK_MY[2] - P[14][2]*SK_MY[1] - P[14][16]*SK_MY[3]);
|
||||||
H_MAG[21] = 1.0f;
|
Kfusion[15] = SK_MY[0]*(P[15][20] + P[15][17]*SH_MAG[0] + P[15][18]*SH_MAG[3] + P[15][0]*SK_MY[2] - P[15][2]*SK_MY[1] - P[15][16]*SK_MY[3]);
|
||||||
|
// zero Kalman gains to inhibit wind state estimation
|
||||||
// calculate Kalman gain
|
if (!inhibitWindStates) {
|
||||||
SK_MZ[0] = 1.0f / varInnovMag[2];
|
Kfusion[22] = SK_MY[0]*(P[22][20] + P[22][17]*SH_MAG[0] + P[22][18]*SH_MAG[3] + P[22][0]*SK_MY[2] - P[22][2]*SK_MY[1] - P[22][16]*SK_MY[3]);
|
||||||
SK_MZ[1] = magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2);
|
Kfusion[23] = SK_MY[0]*(P[23][20] + P[23][17]*SH_MAG[0] + P[23][18]*SH_MAG[3] + P[23][0]*SK_MY[2] - P[23][2]*SK_MY[1] - P[23][16]*SK_MY[3]);
|
||||||
SK_MZ[2] = magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1];
|
} else {
|
||||||
SK_MZ[3] = 2.0f*q0*q1 - 2.0f*q2*q3;
|
Kfusion[22] = 0.0f;
|
||||||
Kfusion[0] = SK_MZ[0]*(P[0][21] + P[0][18]*SH_MAG[2] + P[0][16]*SH_MAG[5] - P[0][0]*SK_MZ[1] + P[0][1]*SK_MZ[2] - P[0][17]*SK_MZ[3]);
|
Kfusion[23] = 0.0f;
|
||||||
Kfusion[1] = SK_MZ[0]*(P[1][21] + P[1][18]*SH_MAG[2] + P[1][16]*SH_MAG[5] - P[1][0]*SK_MZ[1] + P[1][1]*SK_MZ[2] - P[1][17]*SK_MZ[3]);
|
|
||||||
Kfusion[2] = SK_MZ[0]*(P[2][21] + P[2][18]*SH_MAG[2] + P[2][16]*SH_MAG[5] - P[2][0]*SK_MZ[1] + P[2][1]*SK_MZ[2] - P[2][17]*SK_MZ[3]);
|
|
||||||
Kfusion[3] = SK_MZ[0]*(P[3][21] + P[3][18]*SH_MAG[2] + P[3][16]*SH_MAG[5] - P[3][0]*SK_MZ[1] + P[3][1]*SK_MZ[2] - P[3][17]*SK_MZ[3]);
|
|
||||||
Kfusion[4] = SK_MZ[0]*(P[4][21] + P[4][18]*SH_MAG[2] + P[4][16]*SH_MAG[5] - P[4][0]*SK_MZ[1] + P[4][1]*SK_MZ[2] - P[4][17]*SK_MZ[3]);
|
|
||||||
Kfusion[5] = SK_MZ[0]*(P[5][21] + P[5][18]*SH_MAG[2] + P[5][16]*SH_MAG[5] - P[5][0]*SK_MZ[1] + P[5][1]*SK_MZ[2] - P[5][17]*SK_MZ[3]);
|
|
||||||
Kfusion[6] = SK_MZ[0]*(P[6][21] + P[6][18]*SH_MAG[2] + P[6][16]*SH_MAG[5] - P[6][0]*SK_MZ[1] + P[6][1]*SK_MZ[2] - P[6][17]*SK_MZ[3]);
|
|
||||||
Kfusion[7] = SK_MZ[0]*(P[7][21] + P[7][18]*SH_MAG[2] + P[7][16]*SH_MAG[5] - P[7][0]*SK_MZ[1] + P[7][1]*SK_MZ[2] - P[7][17]*SK_MZ[3]);
|
|
||||||
Kfusion[8] = SK_MZ[0]*(P[8][21] + P[8][18]*SH_MAG[2] + P[8][16]*SH_MAG[5] - P[8][0]*SK_MZ[1] + P[8][1]*SK_MZ[2] - P[8][17]*SK_MZ[3]);
|
|
||||||
Kfusion[9] = SK_MZ[0]*(P[9][21] + P[9][18]*SH_MAG[2] + P[9][16]*SH_MAG[5] - P[9][0]*SK_MZ[1] + P[9][1]*SK_MZ[2] - P[9][17]*SK_MZ[3]);
|
|
||||||
Kfusion[10] = SK_MZ[0]*(P[10][21] + P[10][18]*SH_MAG[2] + P[10][16]*SH_MAG[5] - P[10][0]*SK_MZ[1] + P[10][1]*SK_MZ[2] - P[10][17]*SK_MZ[3]);
|
|
||||||
Kfusion[11] = SK_MZ[0]*(P[11][21] + P[11][18]*SH_MAG[2] + P[11][16]*SH_MAG[5] - P[11][0]*SK_MZ[1] + P[11][1]*SK_MZ[2] - P[11][17]*SK_MZ[3]);
|
|
||||||
Kfusion[12] = SK_MZ[0]*(P[12][21] + P[12][18]*SH_MAG[2] + P[12][16]*SH_MAG[5] - P[12][0]*SK_MZ[1] + P[12][1]*SK_MZ[2] - P[12][17]*SK_MZ[3]);
|
|
||||||
Kfusion[13] = SK_MZ[0]*(P[13][21] + P[13][18]*SH_MAG[2] + P[13][16]*SH_MAG[5] - P[13][0]*SK_MZ[1] + P[13][1]*SK_MZ[2] - P[13][17]*SK_MZ[3]);
|
|
||||||
Kfusion[14] = SK_MZ[0]*(P[14][21] + P[14][18]*SH_MAG[2] + P[14][16]*SH_MAG[5] - P[14][0]*SK_MZ[1] + P[14][1]*SK_MZ[2] - P[14][17]*SK_MZ[3]);
|
|
||||||
Kfusion[15] = SK_MZ[0]*(P[15][21] + P[15][18]*SH_MAG[2] + P[15][16]*SH_MAG[5] - P[15][0]*SK_MZ[1] + P[15][1]*SK_MZ[2] - P[15][17]*SK_MZ[3]);
|
|
||||||
// zero Kalman gains to inhibit wind state estimation
|
|
||||||
if (!inhibitWindStates) {
|
|
||||||
Kfusion[22] = SK_MZ[0]*(P[22][21] + P[22][18]*SH_MAG[2] + P[22][16]*SH_MAG[5] - P[22][0]*SK_MZ[1] + P[22][1]*SK_MZ[2] - P[22][17]*SK_MZ[3]);
|
|
||||||
Kfusion[23] = SK_MZ[0]*(P[23][21] + P[23][18]*SH_MAG[2] + P[23][16]*SH_MAG[5] - P[23][0]*SK_MZ[1] + P[23][1]*SK_MZ[2] - P[23][17]*SK_MZ[3]);
|
|
||||||
} else {
|
|
||||||
Kfusion[22] = 0.0f;
|
|
||||||
Kfusion[23] = 0.0f;
|
|
||||||
}
|
|
||||||
// zero Kalman gains to inhibit magnetic field state estimation
|
|
||||||
if (!inhibitMagStates) {
|
|
||||||
Kfusion[16] = SK_MZ[0]*(P[16][21] + P[16][18]*SH_MAG[2] + P[16][16]*SH_MAG[5] - P[16][0]*SK_MZ[1] + P[16][1]*SK_MZ[2] - P[16][17]*SK_MZ[3]);
|
|
||||||
Kfusion[17] = SK_MZ[0]*(P[17][21] + P[17][18]*SH_MAG[2] + P[17][16]*SH_MAG[5] - P[17][0]*SK_MZ[1] + P[17][1]*SK_MZ[2] - P[17][17]*SK_MZ[3]);
|
|
||||||
Kfusion[18] = SK_MZ[0]*(P[18][21] + P[18][18]*SH_MAG[2] + P[18][16]*SH_MAG[5] - P[18][0]*SK_MZ[1] + P[18][1]*SK_MZ[2] - P[18][17]*SK_MZ[3]);
|
|
||||||
Kfusion[19] = SK_MZ[0]*(P[19][21] + P[19][18]*SH_MAG[2] + P[19][16]*SH_MAG[5] - P[19][0]*SK_MZ[1] + P[19][1]*SK_MZ[2] - P[19][17]*SK_MZ[3]);
|
|
||||||
Kfusion[20] = SK_MZ[0]*(P[20][21] + P[20][18]*SH_MAG[2] + P[20][16]*SH_MAG[5] - P[20][0]*SK_MZ[1] + P[20][1]*SK_MZ[2] - P[20][17]*SK_MZ[3]);
|
|
||||||
Kfusion[21] = SK_MZ[0]*(P[21][21] + P[21][18]*SH_MAG[2] + P[21][16]*SH_MAG[5] - P[21][0]*SK_MZ[1] + P[21][1]*SK_MZ[2] - P[21][17]*SK_MZ[3]);
|
|
||||||
} else {
|
|
||||||
for (uint8_t i=16; i<=21; i++) {
|
|
||||||
Kfusion[i] = 0.0f;
|
|
||||||
}
|
}
|
||||||
|
// zero Kalman gains to inhibit magnetic field state estimation
|
||||||
|
if (!inhibitMagStates) {
|
||||||
|
Kfusion[16] = SK_MY[0]*(P[16][20] + P[16][17]*SH_MAG[0] + P[16][18]*SH_MAG[3] + P[16][0]*SK_MY[2] - P[16][2]*SK_MY[1] - P[16][16]*SK_MY[3]);
|
||||||
|
Kfusion[17] = SK_MY[0]*(P[17][20] + P[17][17]*SH_MAG[0] + P[17][18]*SH_MAG[3] + P[17][0]*SK_MY[2] - P[17][2]*SK_MY[1] - P[17][16]*SK_MY[3]);
|
||||||
|
Kfusion[18] = SK_MY[0]*(P[18][20] + P[18][17]*SH_MAG[0] + P[18][18]*SH_MAG[3] + P[18][0]*SK_MY[2] - P[18][2]*SK_MY[1] - P[18][16]*SK_MY[3]);
|
||||||
|
Kfusion[19] = SK_MY[0]*(P[19][20] + P[19][17]*SH_MAG[0] + P[19][18]*SH_MAG[3] + P[19][0]*SK_MY[2] - P[19][2]*SK_MY[1] - P[19][16]*SK_MY[3]);
|
||||||
|
Kfusion[20] = SK_MY[0]*(P[20][20] + P[20][17]*SH_MAG[0] + P[20][18]*SH_MAG[3] + P[20][0]*SK_MY[2] - P[20][2]*SK_MY[1] - P[20][16]*SK_MY[3]);
|
||||||
|
Kfusion[21] = SK_MY[0]*(P[21][20] + P[21][17]*SH_MAG[0] + P[21][18]*SH_MAG[3] + P[21][0]*SK_MY[2] - P[21][2]*SK_MY[1] - P[21][16]*SK_MY[3]);
|
||||||
|
} else {
|
||||||
|
for (uint8_t i=16; i<=21; i++) {
|
||||||
|
Kfusion[i] = 0.0f;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// set flags to indicate to other processes that fusion has been performed
|
||||||
|
// this can be used by other fusion processes to avoid fusing on the same frame as this expensive step
|
||||||
|
magFusePerformed = true;
|
||||||
|
|
||||||
|
}
|
||||||
|
else if (obsIndex == 2) // we are now fusing the Z measurement
|
||||||
|
{
|
||||||
|
// calculate observation jacobians
|
||||||
|
memset(&H_MAG, 0, sizeof(H_MAG));
|
||||||
|
H_MAG[0] = magN*(SH_MAG[8] - 2.0f*q1*q2) - magD*SH_MAG[3] - magE*SH_MAG[0];
|
||||||
|
H_MAG[1] = magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1];
|
||||||
|
H_MAG[16] = SH_MAG[5];
|
||||||
|
H_MAG[17] = 2.0f*q2*q3 - 2.0f*q0*q1;
|
||||||
|
H_MAG[18] = SH_MAG[2];
|
||||||
|
H_MAG[21] = 1.0f;
|
||||||
|
|
||||||
|
// calculate Kalman gain
|
||||||
|
SK_MZ[0] = 1.0f / varInnovMag[2];
|
||||||
|
SK_MZ[1] = magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2);
|
||||||
|
SK_MZ[2] = magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1];
|
||||||
|
SK_MZ[3] = 2.0f*q0*q1 - 2.0f*q2*q3;
|
||||||
|
Kfusion[0] = SK_MZ[0]*(P[0][21] + P[0][18]*SH_MAG[2] + P[0][16]*SH_MAG[5] - P[0][0]*SK_MZ[1] + P[0][1]*SK_MZ[2] - P[0][17]*SK_MZ[3]);
|
||||||
|
Kfusion[1] = SK_MZ[0]*(P[1][21] + P[1][18]*SH_MAG[2] + P[1][16]*SH_MAG[5] - P[1][0]*SK_MZ[1] + P[1][1]*SK_MZ[2] - P[1][17]*SK_MZ[3]);
|
||||||
|
Kfusion[2] = SK_MZ[0]*(P[2][21] + P[2][18]*SH_MAG[2] + P[2][16]*SH_MAG[5] - P[2][0]*SK_MZ[1] + P[2][1]*SK_MZ[2] - P[2][17]*SK_MZ[3]);
|
||||||
|
Kfusion[3] = SK_MZ[0]*(P[3][21] + P[3][18]*SH_MAG[2] + P[3][16]*SH_MAG[5] - P[3][0]*SK_MZ[1] + P[3][1]*SK_MZ[2] - P[3][17]*SK_MZ[3]);
|
||||||
|
Kfusion[4] = SK_MZ[0]*(P[4][21] + P[4][18]*SH_MAG[2] + P[4][16]*SH_MAG[5] - P[4][0]*SK_MZ[1] + P[4][1]*SK_MZ[2] - P[4][17]*SK_MZ[3]);
|
||||||
|
Kfusion[5] = SK_MZ[0]*(P[5][21] + P[5][18]*SH_MAG[2] + P[5][16]*SH_MAG[5] - P[5][0]*SK_MZ[1] + P[5][1]*SK_MZ[2] - P[5][17]*SK_MZ[3]);
|
||||||
|
Kfusion[6] = SK_MZ[0]*(P[6][21] + P[6][18]*SH_MAG[2] + P[6][16]*SH_MAG[5] - P[6][0]*SK_MZ[1] + P[6][1]*SK_MZ[2] - P[6][17]*SK_MZ[3]);
|
||||||
|
Kfusion[7] = SK_MZ[0]*(P[7][21] + P[7][18]*SH_MAG[2] + P[7][16]*SH_MAG[5] - P[7][0]*SK_MZ[1] + P[7][1]*SK_MZ[2] - P[7][17]*SK_MZ[3]);
|
||||||
|
Kfusion[8] = SK_MZ[0]*(P[8][21] + P[8][18]*SH_MAG[2] + P[8][16]*SH_MAG[5] - P[8][0]*SK_MZ[1] + P[8][1]*SK_MZ[2] - P[8][17]*SK_MZ[3]);
|
||||||
|
Kfusion[9] = SK_MZ[0]*(P[9][21] + P[9][18]*SH_MAG[2] + P[9][16]*SH_MAG[5] - P[9][0]*SK_MZ[1] + P[9][1]*SK_MZ[2] - P[9][17]*SK_MZ[3]);
|
||||||
|
Kfusion[10] = SK_MZ[0]*(P[10][21] + P[10][18]*SH_MAG[2] + P[10][16]*SH_MAG[5] - P[10][0]*SK_MZ[1] + P[10][1]*SK_MZ[2] - P[10][17]*SK_MZ[3]);
|
||||||
|
Kfusion[11] = SK_MZ[0]*(P[11][21] + P[11][18]*SH_MAG[2] + P[11][16]*SH_MAG[5] - P[11][0]*SK_MZ[1] + P[11][1]*SK_MZ[2] - P[11][17]*SK_MZ[3]);
|
||||||
|
Kfusion[12] = SK_MZ[0]*(P[12][21] + P[12][18]*SH_MAG[2] + P[12][16]*SH_MAG[5] - P[12][0]*SK_MZ[1] + P[12][1]*SK_MZ[2] - P[12][17]*SK_MZ[3]);
|
||||||
|
Kfusion[13] = SK_MZ[0]*(P[13][21] + P[13][18]*SH_MAG[2] + P[13][16]*SH_MAG[5] - P[13][0]*SK_MZ[1] + P[13][1]*SK_MZ[2] - P[13][17]*SK_MZ[3]);
|
||||||
|
Kfusion[14] = SK_MZ[0]*(P[14][21] + P[14][18]*SH_MAG[2] + P[14][16]*SH_MAG[5] - P[14][0]*SK_MZ[1] + P[14][1]*SK_MZ[2] - P[14][17]*SK_MZ[3]);
|
||||||
|
Kfusion[15] = SK_MZ[0]*(P[15][21] + P[15][18]*SH_MAG[2] + P[15][16]*SH_MAG[5] - P[15][0]*SK_MZ[1] + P[15][1]*SK_MZ[2] - P[15][17]*SK_MZ[3]);
|
||||||
|
// zero Kalman gains to inhibit wind state estimation
|
||||||
|
if (!inhibitWindStates) {
|
||||||
|
Kfusion[22] = SK_MZ[0]*(P[22][21] + P[22][18]*SH_MAG[2] + P[22][16]*SH_MAG[5] - P[22][0]*SK_MZ[1] + P[22][1]*SK_MZ[2] - P[22][17]*SK_MZ[3]);
|
||||||
|
Kfusion[23] = SK_MZ[0]*(P[23][21] + P[23][18]*SH_MAG[2] + P[23][16]*SH_MAG[5] - P[23][0]*SK_MZ[1] + P[23][1]*SK_MZ[2] - P[23][17]*SK_MZ[3]);
|
||||||
|
} else {
|
||||||
|
Kfusion[22] = 0.0f;
|
||||||
|
Kfusion[23] = 0.0f;
|
||||||
|
}
|
||||||
|
// zero Kalman gains to inhibit magnetic field state estimation
|
||||||
|
if (!inhibitMagStates) {
|
||||||
|
Kfusion[16] = SK_MZ[0]*(P[16][21] + P[16][18]*SH_MAG[2] + P[16][16]*SH_MAG[5] - P[16][0]*SK_MZ[1] + P[16][1]*SK_MZ[2] - P[16][17]*SK_MZ[3]);
|
||||||
|
Kfusion[17] = SK_MZ[0]*(P[17][21] + P[17][18]*SH_MAG[2] + P[17][16]*SH_MAG[5] - P[17][0]*SK_MZ[1] + P[17][1]*SK_MZ[2] - P[17][17]*SK_MZ[3]);
|
||||||
|
Kfusion[18] = SK_MZ[0]*(P[18][21] + P[18][18]*SH_MAG[2] + P[18][16]*SH_MAG[5] - P[18][0]*SK_MZ[1] + P[18][1]*SK_MZ[2] - P[18][17]*SK_MZ[3]);
|
||||||
|
Kfusion[19] = SK_MZ[0]*(P[19][21] + P[19][18]*SH_MAG[2] + P[19][16]*SH_MAG[5] - P[19][0]*SK_MZ[1] + P[19][1]*SK_MZ[2] - P[19][17]*SK_MZ[3]);
|
||||||
|
Kfusion[20] = SK_MZ[0]*(P[20][21] + P[20][18]*SH_MAG[2] + P[20][16]*SH_MAG[5] - P[20][0]*SK_MZ[1] + P[20][1]*SK_MZ[2] - P[20][17]*SK_MZ[3]);
|
||||||
|
Kfusion[21] = SK_MZ[0]*(P[21][21] + P[21][18]*SH_MAG[2] + P[21][16]*SH_MAG[5] - P[21][0]*SK_MZ[1] + P[21][1]*SK_MZ[2] - P[21][17]*SK_MZ[3]);
|
||||||
|
} else {
|
||||||
|
for (uint8_t i=16; i<=21; i++) {
|
||||||
|
Kfusion[i] = 0.0f;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// set flags to indicate to other processes that fusion has been performed
|
||||||
|
// this can be used by other fusion processes to avoid fusing on the same frame as this expensive step
|
||||||
|
magFusePerformed = true;
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// set flags to indicate to other processes that fusion has been performed and is required on the next frame
|
// correct the covariance P = (I - K*H)*P
|
||||||
// this can be used by other fusion processes to avoid fusing on the same frame as this expensive step
|
// take advantage of the empty columns in KH to reduce the
|
||||||
magFusePerformed = true;
|
// number of operations
|
||||||
magFuseRequired = false;
|
|
||||||
|
|
||||||
hal.util->perf_end(_perf_test[4]);
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
hal.util->perf_begin(_perf_test[5]);
|
|
||||||
|
|
||||||
// correct the covariance P = (I - K*H)*P
|
|
||||||
// take advantage of the empty columns in KH to reduce the
|
|
||||||
// number of operations
|
|
||||||
for (unsigned i = 0; i<=stateIndexLim; i++) {
|
|
||||||
for (unsigned j = 0; j<=2; j++) {
|
|
||||||
KH[i][j] = Kfusion[i] * H_MAG[j];
|
|
||||||
}
|
|
||||||
for (unsigned j = 3; j<=15; j++) {
|
|
||||||
KH[i][j] = 0.0f;
|
|
||||||
}
|
|
||||||
for (unsigned j = 16; j<=21; j++) {
|
|
||||||
KH[i][j] = Kfusion[i] * H_MAG[j];
|
|
||||||
}
|
|
||||||
for (unsigned j = 22; j<=23; j++) {
|
|
||||||
KH[i][j] = 0.0f;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
for (unsigned j = 0; j<=stateIndexLim; j++) {
|
|
||||||
for (unsigned i = 0; i<=stateIndexLim; i++) {
|
for (unsigned i = 0; i<=stateIndexLim; i++) {
|
||||||
ftype res = 0;
|
for (unsigned j = 0; j<=2; j++) {
|
||||||
res += KH[i][0] * P[0][j];
|
KH[i][j] = Kfusion[i] * H_MAG[j];
|
||||||
res += KH[i][1] * P[1][j];
|
}
|
||||||
res += KH[i][2] * P[2][j];
|
for (unsigned j = 3; j<=15; j++) {
|
||||||
res += KH[i][16] * P[16][j];
|
KH[i][j] = 0.0f;
|
||||||
res += KH[i][17] * P[17][j];
|
}
|
||||||
res += KH[i][18] * P[18][j];
|
for (unsigned j = 16; j<=21; j++) {
|
||||||
res += KH[i][19] * P[19][j];
|
KH[i][j] = Kfusion[i] * H_MAG[j];
|
||||||
res += KH[i][20] * P[20][j];
|
}
|
||||||
res += KH[i][21] * P[21][j];
|
for (unsigned j = 22; j<=23; j++) {
|
||||||
KHP[i][j] = res;
|
KH[i][j] = 0.0f;
|
||||||
}
|
|
||||||
}
|
|
||||||
// Check that we are not going to drive any variances negative and skip the update if so
|
|
||||||
bool healthyFusion = true;
|
|
||||||
for (uint8_t i= 0; i<=stateIndexLim; i++) {
|
|
||||||
if (KHP[i][i] > P[i][i]) {
|
|
||||||
healthyFusion = false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
if (healthyFusion) {
|
|
||||||
// update the covariance matrix
|
|
||||||
for (uint8_t i= 0; i<=stateIndexLim; i++) {
|
|
||||||
for (uint8_t j= 0; j<=stateIndexLim; j++) {
|
|
||||||
P[i][j] = P[i][j] - KHP[i][j];
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
for (unsigned j = 0; j<=stateIndexLim; j++) {
|
||||||
// force the covariance matrix to be symmetrical and limit the variances to prevent ill-conditioning.
|
for (unsigned i = 0; i<=stateIndexLim; i++) {
|
||||||
ForceSymmetry();
|
ftype res = 0;
|
||||||
ConstrainVariances();
|
res += KH[i][0] * P[0][j];
|
||||||
|
res += KH[i][1] * P[1][j];
|
||||||
// update the states
|
res += KH[i][2] * P[2][j];
|
||||||
// zero the attitude error state - by definition it is assumed to be zero before each observation fusion
|
res += KH[i][16] * P[16][j];
|
||||||
stateStruct.angErr.zero();
|
res += KH[i][17] * P[17][j];
|
||||||
|
res += KH[i][18] * P[18][j];
|
||||||
// correct the state vector
|
res += KH[i][19] * P[19][j];
|
||||||
for (uint8_t j= 0; j<=stateIndexLim; j++) {
|
res += KH[i][20] * P[20][j];
|
||||||
statesArray[j] = statesArray[j] - Kfusion[j] * innovMag[obsIndex];
|
res += KH[i][21] * P[21][j];
|
||||||
|
KHP[i][j] = res;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
// Check that we are not going to drive any variances negative and skip the update if so
|
||||||
// add table constraint here for faster convergence
|
bool healthyFusion = true;
|
||||||
if (have_table_earth_field && frontend->_mag_ef_limit > 0) {
|
for (uint8_t i= 0; i<=stateIndexLim; i++) {
|
||||||
MagTableConstrain();
|
if (KHP[i][i] > P[i][i]) {
|
||||||
|
healthyFusion = false;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
if (healthyFusion) {
|
||||||
|
// update the covariance matrix
|
||||||
|
for (uint8_t i= 0; i<=stateIndexLim; i++) {
|
||||||
|
for (uint8_t j= 0; j<=stateIndexLim; j++) {
|
||||||
|
P[i][j] = P[i][j] - KHP[i][j];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
// the first 3 states represent the angular misalignment vector. This is
|
// force the covariance matrix to be symmetrical and limit the variances to prevent ill-conditioning.
|
||||||
// is used to correct the estimated quaternion on the current time step
|
ForceSymmetry();
|
||||||
stateStruct.quat.rotate(stateStruct.angErr);
|
ConstrainVariances();
|
||||||
|
|
||||||
} else {
|
// update the states
|
||||||
// record bad axis
|
// zero the attitude error state - by definition it is assumed to be zero before each observation fusion
|
||||||
if (obsIndex == 0) {
|
stateStruct.angErr.zero();
|
||||||
faultStatus.bad_xmag = true;
|
|
||||||
} else if (obsIndex == 1) {
|
// correct the state vector
|
||||||
faultStatus.bad_ymag = true;
|
for (uint8_t j= 0; j<=stateIndexLim; j++) {
|
||||||
} else if (obsIndex == 2) {
|
statesArray[j] = statesArray[j] - Kfusion[j] * innovMag[obsIndex];
|
||||||
faultStatus.bad_zmag = true;
|
}
|
||||||
|
|
||||||
|
// add table constraint here for faster convergence
|
||||||
|
if (have_table_earth_field && frontend->_mag_ef_limit > 0) {
|
||||||
|
MagTableConstrain();
|
||||||
|
}
|
||||||
|
|
||||||
|
// the first 3 states represent the angular misalignment vector. This is
|
||||||
|
// is used to correct the estimated quaternion on the current time step
|
||||||
|
stateStruct.quat.rotate(stateStruct.angErr);
|
||||||
|
|
||||||
|
} else {
|
||||||
|
// record bad axis
|
||||||
|
if (obsIndex == 0) {
|
||||||
|
faultStatus.bad_xmag = true;
|
||||||
|
} else if (obsIndex == 1) {
|
||||||
|
faultStatus.bad_ymag = true;
|
||||||
|
} else if (obsIndex == 2) {
|
||||||
|
faultStatus.bad_zmag = true;
|
||||||
|
}
|
||||||
|
CovarianceInit();
|
||||||
|
return;
|
||||||
}
|
}
|
||||||
CovarianceInit();
|
|
||||||
hal.util->perf_end(_perf_test[5]);
|
|
||||||
return;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
hal.util->perf_end(_perf_test[5]);
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@ -880,7 +880,6 @@ private:
|
|||||||
ftype innovVtas; // innovation output from fusion of airspeed measurements
|
ftype innovVtas; // innovation output from fusion of airspeed measurements
|
||||||
ftype varInnovVtas; // innovation variance output from fusion of airspeed measurements
|
ftype varInnovVtas; // innovation variance output from fusion of airspeed measurements
|
||||||
bool magFusePerformed; // boolean set to true when magnetometer fusion has been performed in that time step
|
bool magFusePerformed; // boolean set to true when magnetometer fusion has been performed in that time step
|
||||||
bool magFuseRequired; // boolean set to true when magnetometer fusion will be performed in the next time step
|
|
||||||
uint32_t prevTasStep_ms; // time stamp of last TAS fusion step
|
uint32_t prevTasStep_ms; // time stamp of last TAS fusion step
|
||||||
uint32_t prevBetaStep_ms; // time stamp of last synthetic sideslip fusion step
|
uint32_t prevBetaStep_ms; // time stamp of last synthetic sideslip fusion step
|
||||||
uint32_t lastMagUpdate_us; // last time compass was updated in usec
|
uint32_t lastMagUpdate_us; // last time compass was updated in usec
|
||||||
@ -1222,7 +1221,6 @@ private:
|
|||||||
ftype magXbias;
|
ftype magXbias;
|
||||||
ftype magYbias;
|
ftype magYbias;
|
||||||
ftype magZbias;
|
ftype magZbias;
|
||||||
uint8_t obsIndex;
|
|
||||||
Matrix3f DCM;
|
Matrix3f DCM;
|
||||||
Vector3f MagPred;
|
Vector3f MagPred;
|
||||||
ftype R_MAG;
|
ftype R_MAG;
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user