Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 14:08:45 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: enable fine grained perf tuning for mag fusion

Browse Source
This commit is contained in:
Andrew Tridgell 2015-10-20 08:32:07 +11:00
parent b6c1352e4c
commit fc23be8025
1 changed files with 54 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

72
libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
View File

@ -149,7 +149,13 @@ void NavEKF2_core::SelectMagFusion()
FuseDeclination(); FuseDeclination();
} }
// fuse the three magnetometer componenents sequentially // fuse the three magnetometer componenents sequentially
for (mag_state.obsIndex = 0; mag_state.obsIndex <= 2; mag_state.obsIndex++) FuseMagnetometer(); for (mag_state.obsIndex = 0; mag_state.obsIndex <= 2; mag_state.obsIndex++) {
irqstate_t istate = irqsave();
perf_begin(_perf_test[0]);
FuseMagnetometer();
perf_end(_perf_test[0]);
irqrestore(istate);
}
} }
} }
@ -164,6 +170,8 @@ void NavEKF2_core::SelectMagFusion()
*/ */
void NavEKF2_core::FuseMagnetometer() void NavEKF2_core::FuseMagnetometer()
{ {
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;
@ -185,6 +193,8 @@ void NavEKF2_core::FuseMagnetometer()
Vector6 SK_MY; Vector6 SK_MY;
Vector6 SK_MZ; Vector6 SK_MZ;
perf_end(_perf_test[1]);
// 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
// uncorrelated which is not true, however in the absence of covariance // uncorrelated which is not true, however in the absence of covariance
@ -194,6 +204,7 @@ void NavEKF2_core::FuseMagnetometer()
// calculate observation jacobians and Kalman gains // calculate observation jacobians and Kalman gains
if (obsIndex == 0) if (obsIndex == 0)
{ {
perf_begin(_perf_test[2]);
// copy required states to local variable names // copy required states to local variable names
q0 = stateStruct.quat[0]; q0 = stateStruct.quat[0];
q1 = stateStruct.quat[1]; q1 = stateStruct.quat[1];
@ -253,6 +264,7 @@ void NavEKF2_core::FuseMagnetometer()
CovarianceInit(); CovarianceInit();
obsIndex = 1; obsIndex = 1;
faultStatus.bad_xmag = true; faultStatus.bad_xmag = true;
perf_end(_perf_test[2]);
return; return;
} }
SK_MX[1] = magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2); SK_MX[1] = magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2);
@ -274,6 +286,8 @@ void NavEKF2_core::FuseMagnetometer()
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[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[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]); 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 // zero Kalman gains to inhibit wind state estimation
if (!inhibitWindStates) { 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[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]);
@ -303,9 +317,11 @@ void NavEKF2_core::FuseMagnetometer()
// this can be used by other fusion processes to avoid fusing on the same frame as this expensive step // this can be used by other fusion processes to avoid fusing on the same frame as this expensive step
magFusePerformed = true; magFusePerformed = true;
magFuseRequired = true; magFuseRequired = true;
perf_end(_perf_test[2]);
} }
else if (obsIndex == 1) // we are now fusing the Y measurement else if (obsIndex == 1) // we are now fusing the Y measurement
{ {
perf_begin(_perf_test[3]);
// calculate observation jacobians // calculate observation jacobians
for (uint8_t i = 0; i<=stateIndexLim; i++) H_MAG[i] = 0.0f; 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[0] = magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5];
@ -326,6 +342,7 @@ void NavEKF2_core::FuseMagnetometer()
CovarianceInit(); CovarianceInit();
obsIndex = 2; obsIndex = 2;
faultStatus.bad_ymag = true; faultStatus.bad_ymag = true;
perf_end(_perf_test[3]);
return; return;
} }
SK_MY[1] = magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]; SK_MY[1] = magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1];
@ -373,9 +390,11 @@ void NavEKF2_core::FuseMagnetometer()
// this can be used by other fusion processes to avoid fusing on the same frame as this expensive step // this can be used by other fusion processes to avoid fusing on the same frame as this expensive step
magFusePerformed = true; magFusePerformed = true;
magFuseRequired = true; magFuseRequired = true;
perf_end(_perf_test[3]);
} }
else if (obsIndex == 2) // we are now fusing the Z measurement else if (obsIndex == 2) // we are now fusing the Z measurement
{ {
perf_begin(_perf_test[4]);
// calculate observation jacobians // calculate observation jacobians
for (uint8_t i = 0; i<=stateIndexLim; i++) H_MAG[i] = 0.0f; for (uint8_t i = 0; i<=stateIndexLim; i++) H_MAG[i] = 0.0f;
H_MAG[0] = magN*(SH_MAG[8] - 2.0f*q1*q2) - magD*SH_MAG[3] - magE*SH_MAG[0]; H_MAG[0] = magN*(SH_MAG[8] - 2.0f*q1*q2) - magD*SH_MAG[3] - magE*SH_MAG[0];
@ -396,6 +415,7 @@ void NavEKF2_core::FuseMagnetometer()
CovarianceInit(); CovarianceInit();
obsIndex = 3; obsIndex = 3;
faultStatus.bad_zmag = true; faultStatus.bad_zmag = true;
perf_end(_perf_test[4]);
return; return;
} }
SK_MZ[1] = magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2); SK_MZ[1] = magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2);
@ -443,7 +463,10 @@ void NavEKF2_core::FuseMagnetometer()
// this can be used by other fusion processes to avoid fusing on the same frame as this expensive step // this can be used by other fusion processes to avoid fusing on the same frame as this expensive step
magFusePerformed = true; magFusePerformed = true;
magFuseRequired = false; magFuseRequired = false;
perf_end(_perf_test[4]);
} }
perf_begin(_perf_test[5]);
// calculate the measurement innovation // calculate the measurement innovation
innovMag[obsIndex] = MagPred[obsIndex] - magDataDelayed.mag[obsIndex]; innovMag[obsIndex] = MagPred[obsIndex] - magDataDelayed.mag[obsIndex];
// calculate the innovation test ratio // calculate the innovation test ratio
@ -454,6 +477,8 @@ void NavEKF2_core::FuseMagnetometer()
// In this case we might as well try using the magnetometer, but with a reduced weighting // In this case we might as well try using the magnetometer, but with a reduced weighting
if (magHealth || ((magTestRatio[obsIndex] < 1.0f) && !assume_zero_sideslip() && magTimeout)) { if (magHealth || ((magTestRatio[obsIndex] < 1.0f) && !assume_zero_sideslip() && magTimeout)) {
perf_begin(_perf_test[8]);
// zero the attitude error state - by definition it is assumed to be zero before each observaton fusion // zero the attitude error state - by definition it is assumed to be zero before each observaton fusion
stateStruct.angErr.zero(); stateStruct.angErr.zero();
@ -496,30 +521,41 @@ void NavEKF2_core::FuseMagnetometer()
KH[i][j] = 0.0f; KH[i][j] = 0.0f;
} }
} }
for (uint8_t i = 0; i<=stateIndexLim; i++) { perf_end(_perf_test[8]);
for (uint8_t j = 0; j<=stateIndexLim; j++) { perf_begin(_perf_test[9]);
for (unsigned j = 0; j<=stateIndexLim; j++) {
for (unsigned i = 0; i<=stateIndexLim; i++) {
KHP[i][j] = 0; KHP[i][j] = 0;
for (uint8_t k = 0; k<=2; k++) { for (unsigned k=0; k<=2; k++) {
KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; KHP[i][j] += KH[i][k] * P[k][j];
} }
if (!inhibitMagStates) { if (!inhibitMagStates) {
for (uint8_t k = 16; k<=21; k++) { KHP[i][j] += KH[i][16] * P[16][j];
KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; KHP[i][j] += KH[i][17] * P[17][j];
} KHP[i][j] += KH[i][18] * P[18][j];
KHP[i][j] += KH[i][19] * P[19][j];
KHP[i][j] += KH[i][20] * P[20][j];
KHP[i][j] += KH[i][21] * P[21][j];
} }
} }
} }
for (uint8_t i = 0; i<=stateIndexLim; i++) { for (unsigned j = 0; j<=stateIndexLim; j++) {
for (uint8_t j = 0; j<=stateIndexLim; j++) { for (unsigned i = 0; i<=stateIndexLim; i++) {
P[i][j] = P[i][j] - KHP[i][j]; P[i][j] = P[i][j] - KHP[i][j];
} }
} }
perf_end(_perf_test[9]);
} }
perf_end(_perf_test[5]);
// force the covariance matrix to be symmetrical and limit the variances to prevent // force the covariance matrix to be symmetrical and limit the variances to prevent
// ill-condiioning. // ill-condiioning.
perf_begin(_perf_test[6]);
ForceSymmetry(); ForceSymmetry();
perf_end(_perf_test[6]);
perf_begin(_perf_test[7]);
ConstrainVariances(); ConstrainVariances();
perf_end(_perf_test[7]);
} }
@ -779,19 +815,19 @@ float NavEKF2_core::calcMagHeadingInnov()
float innovation = atan2f(magMeasNED.y,magMeasNED.x) - _ahrs->get_compass()->get_declination(); float innovation = atan2f(magMeasNED.y,magMeasNED.x) - _ahrs->get_compass()->get_declination();
// wrap the innovation so it sits on the range from +-pi // wrap the innovation so it sits on the range from +-pi
if (innovation > M_PI) { if (innovation > M_PI_F) {
innovation = innovation - 2.0f*M_PI; innovation = innovation - 2*M_PI_F;
} else if (innovation < -M_PI) { } else if (innovation < -M_PI_F) {
innovation = innovation + 2.0f*M_PI; innovation = innovation + 2*M_PI_F;
} }
// Unwrap so that a large yaw gyro bias offset that causes the heading to wrap does not lead to continual uncontrolled heading drift // Unwrap so that a large yaw gyro bias offset that causes the heading to wrap does not lead to continual uncontrolled heading drift
if (innovation - lastInnovation > M_PI) { if (innovation - lastInnovation > M_PI_F) {
// Angle has wrapped in the positive direction to subtract an additional 2*Pi // Angle has wrapped in the positive direction to subtract an additional 2*Pi
innovationIncrement -= 2.0f*M_PI; innovationIncrement -= 2*M_PI_F;
} else if (innovation -innovationIncrement < -M_PI) { } else if (innovation -innovationIncrement < -M_PI_F) {
// Angle has wrapped in the negative direction so add an additional 2*Pi // Angle has wrapped in the negative direction so add an additional 2*Pi
innovationIncrement += 2.0f*M_PI; innovationIncrement += 2*M_PI_F;
} }
lastInnovation = innovation; lastInnovation = innovation;