Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-23 09:08:30 -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();
}
// 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()
{
perf_begin(_perf_test[1]);
// declarations
ftype &q0 = mag_state.q0;
ftype &q1 = mag_state.q1;
@ -185,6 +193,8 @@ void NavEKF2_core::FuseMagnetometer()
Vector6 SK_MY;
Vector6 SK_MZ;
perf_end(_perf_test[1]);
// perform sequential fusion of magnetometer measurements.
// this assumes that the errors in the different components are
// 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
if (obsIndex == 0)
{
perf_begin(_perf_test[2]);
// copy required states to local variable names
q0 = stateStruct.quat[0];
q1 = stateStruct.quat[1];
@ -253,6 +264,7 @@ void NavEKF2_core::FuseMagnetometer()
CovarianceInit();
obsIndex = 1;
faultStatus.bad_xmag = true;
perf_end(_perf_test[2]);
return;
}
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[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]);
@ -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
magFusePerformed = true;
magFuseRequired = true;
perf_end(_perf_test[2]);
}
else if (obsIndex == 1) // we are now fusing the Y measurement
{
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];
@ -326,6 +342,7 @@ void NavEKF2_core::FuseMagnetometer()
CovarianceInit();
obsIndex = 2;
faultStatus.bad_ymag = true;
perf_end(_perf_test[3]);
return;
}
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
magFusePerformed = true;
magFuseRequired = true;
perf_end(_perf_test[3]);
}
else if (obsIndex == 2) // we are now fusing the Z measurement
{
perf_begin(_perf_test[4]);
// calculate observation jacobians
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];
@ -396,6 +415,7 @@ void NavEKF2_core::FuseMagnetometer()
CovarianceInit();
obsIndex = 3;
faultStatus.bad_zmag = true;
perf_end(_perf_test[4]);
return;
}
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
magFusePerformed = true;
magFuseRequired = false;
perf_end(_perf_test[4]);
}
perf_begin(_perf_test[5]);
// calculate the measurement innovation
innovMag[obsIndex] = MagPred[obsIndex] - magDataDelayed.mag[obsIndex];
// 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
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
stateStruct.angErr.zero();
@ -496,30 +521,41 @@ void NavEKF2_core::FuseMagnetometer()
KH[i][j] = 0.0f;
}
}
for (uint8_t i = 0; i<=stateIndexLim; i++) {
for (uint8_t j = 0; j<=stateIndexLim; j++) {
perf_end(_perf_test[8]);
perf_begin(_perf_test[9]);
for (unsigned j = 0; j<=stateIndexLim; j++) {
for (unsigned i = 0; i<=stateIndexLim; i++) {
KHP[i][j] = 0;
for (uint8_t k = 0; k<=2; k++) {
KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
for (unsigned k=0; k<=2; k++) {
KHP[i][j] += KH[i][k] * P[k][j];
}
if (!inhibitMagStates) {
for (uint8_t k = 16; k<=21; k++) {
KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
}
KHP[i][j] += KH[i][16] * P[16][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 (uint8_t j = 0; j<=stateIndexLim; j++) {
for (unsigned j = 0; j<=stateIndexLim; j++) {
for (unsigned i = 0; i<=stateIndexLim; i++) {
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
// ill-condiioning.
perf_begin(_perf_test[6]);
ForceSymmetry();
perf_end(_perf_test[6]);
perf_begin(_perf_test[7]);
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();
// wrap the innovation so it sits on the range from +-pi
if (innovation > M_PI) {
innovation = innovation - 2.0f*M_PI;
} else if (innovation < -M_PI) {
innovation = innovation + 2.0f*M_PI;
if (innovation > M_PI_F) {
innovation = innovation - 2*M_PI_F;
} else if (innovation < -M_PI_F) {
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
if (innovation - lastInnovation > M_PI) {
if (innovation - lastInnovation > M_PI_F) {
// Angle has wrapped in the positive direction to subtract an additional 2*Pi
innovationIncrement -= 2.0f*M_PI;
} else if (innovation -innovationIncrement < -M_PI) {
innovationIncrement -= 2*M_PI_F;
} else if (innovation -innovationIncrement < -M_PI_F) {
// 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;