aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF3: Update range beacon fusion

Browse Source 
Add innovation consistency check to alignment filter and publish alignment filter status data
This commit is contained in:
priseborough 2016-12-17 22:52:57 +11:00 committed by Randy Mackay
parent ed0a90ae5f
commit 2cc48f78a7
1 changed files with 56 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp
View File

@ -424,6 +424,15 @@ void NavEKF3_core::FuseRngBcnStatic()
deltaPosNED.z -= bcnPosOffsetNED.z;
innovRngBcn = deltaPosNED.length() - rngBcnDataDelayed.rng;
// calculate the innovation consistency test ratio
rngBcnTestRatio = sq(innovRngBcn) / (sq(MAX(0.01f * (float)frontend->_rngBcnInnovGate, 1.0f)) * varInnovRngBcn);
// fail if the ratio is > 1, but don't fail if bad IMU data
rngBcnHealth = ((rngBcnTestRatio < 1.0f) || badIMUdata || !rngBcnAlignmentCompleted);
// test the ratio before fusing data
if (rngBcnHealth) {
// update the state
receiverPos.x -= K_RNG[0] * innovRngBcn;
receiverPos.y -= K_RNG[1] * innovRngBcn;
@ -444,6 +453,7 @@ void NavEKF3_core::FuseRngBcnStatic()
KHP[i][j] = res;
}
}
// prevent negative variances
for (uint8_t i= 0; i<=2; i++) {
if (receiverPosCov[i][i] < 0.0f) {
@ -453,12 +463,14 @@ void NavEKF3_core::FuseRngBcnStatic()
KHP[i][i] = receiverPosCov[i][i];
}
}
// apply the covariance correction
for (uint8_t i= 0; i<=2; i++) {
for (uint8_t j= 0; j<=2; j++) {
receiverPosCov[i][j] -= KHP[i][j];
}
}
// ensure the covariance matrix is symmetric
for (uint8_t i=1; i<=2; i++) {
for (uint8_t j=0; j<=i-1; j++) {
@ -468,11 +480,19 @@ void NavEKF3_core::FuseRngBcnStatic()
}
}
}
if (numBcnMeas >= 100) {
// 100 observations is enough for a stable estimate under most conditions
// TODO monitor stability of the position estimate
rngBcnAlignmentCompleted = true;
}
// Update the fusion report
rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].beaconPosNED = rngBcnDataDelayed.beacon_posNED;
rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].innov = innovRngBcn;
rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].innovVar = varInnovRngBcn;
rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].rng = rngBcnDataDelayed.rng;
rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].testRatio = rngBcnTestRatio;
}
}