Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 01:48:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: Fix bugs causing height drift when using range beacons

Browse Source
This commit is contained in:
priseborough 2017-06-21 08:56:41 +10:00 committed by Randy Mackay
parent 618644addc
commit b847a6e38c
2 changed files with 27 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -788,7 +788,8 @@ void NavEKF3_core::readRngBcnData()
// Correct the range beacon earth frame origin for estimated offset relative to the EKF earth frame origin // Correct the range beacon earth frame origin for estimated offset relative to the EKF earth frame origin
if (rngBcnDataToFuse) { if (rngBcnDataToFuse) {
rngBcnDataDelayed.beacon_posNED += bcnPosOffsetNED; rngBcnDataDelayed.beacon_posNED.x += bcnPosOffsetNED.x;
rngBcnDataDelayed.beacon_posNED.y += bcnPosOffsetNED.y;
} }
} }

42
libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp
View File

@ -512,7 +512,7 @@ void NavEKF3_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP
float obsDeriv; // derivative of observation relative to state float obsDeriv; // derivative of observation relative to state
const float stateNoiseVar = 0.1f; // State process noise variance const float stateNoiseVar = 0.1f; // State process noise variance
const float filtAlpha = 0.01f; // LPF constant const float filtAlpha = 0.1f; // LPF constant
const float innovGateWidth = 5.0f; // width of innovation consistency check gate in std const float innovGateWidth = 5.0f; // width of innovation consistency check gate in std
// estimate upper value for offset // estimate upper value for offset
@ -533,9 +533,6 @@ void NavEKF3_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP
// Calculate innovation // Calculate innovation
innov = sqrtf(t8) - rngBcnDataDelayed.rng; innov = sqrtf(t8) - rngBcnDataDelayed.rng;
// calculate a filtered innovation magnitude to be used to select between the high or low offset
OffsetMaxInnovFilt = (1.0f - filtAlpha) * bcnPosOffsetMaxVar + filtAlpha * fabsf(innov);
// covariance prediction // covariance prediction
bcnPosOffsetMaxVar += stateNoiseVar; bcnPosOffsetMaxVar += stateNoiseVar;
@ -543,11 +540,15 @@ void NavEKF3_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP
innovVar = obsDeriv * bcnPosOffsetMaxVar * obsDeriv + obsVar; innovVar = obsDeriv * bcnPosOffsetMaxVar * obsDeriv + obsVar;
innovVar = MAX(innovVar, obsVar); innovVar = MAX(innovVar, obsVar);
// Reject range innovation spikes using a 5-sigma threshold unless aligning
if ((sq(innov) < sq(innovGateWidth) * innovVar) || aligning) {
// calculate the Kalman gain // calculate the Kalman gain
gain = (bcnPosOffsetMaxVar * obsDeriv) / innovVar; gain = (bcnPosOffsetMaxVar * obsDeriv) / innovVar;
float stateChange = innov * gain;
OffsetMaxInnovFilt = (1.0f - filtAlpha) * OffsetMaxInnovFilt + fminf(fabsf(filtAlpha * stateChange) , 1.0f);
// Reject range innovation spikes using a 5-sigma threshold unless aligning
if ((sq(innov) < sq(innovGateWidth) * innovVar) || aligning) {
// state update // state update
bcnPosDownOffsetMax -= innov * gain; bcnPosDownOffsetMax -= innov * gain;
@ -570,9 +571,6 @@ void NavEKF3_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP
// Calculate innovation // Calculate innovation
innov = sqrtf(t8) - rngBcnDataDelayed.rng; innov = sqrtf(t8) - rngBcnDataDelayed.rng;
// calculate a filtered innovation magnitude to be used to select between the high or low offset
OffsetMinInnovFilt = (1.0f - filtAlpha) * OffsetMinInnovFilt + filtAlpha * fabsf(innov);
// covariance prediction // covariance prediction
bcnPosOffsetMinVar += stateNoiseVar; bcnPosOffsetMinVar += stateNoiseVar;
@ -580,13 +578,18 @@ void NavEKF3_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP
innovVar = obsDeriv * bcnPosOffsetMinVar * obsDeriv + obsVar; innovVar = obsDeriv * bcnPosOffsetMinVar * obsDeriv + obsVar;
innovVar = MAX(innovVar, obsVar); innovVar = MAX(innovVar, obsVar);
// Reject range innovation spikes using a 5-sigma threshold unless aligning
if ((sq(innov) < sq(innovGateWidth) * innovVar) || aligning) {
// calculate the Kalman gain // calculate the Kalman gain
gain = (bcnPosOffsetMinVar * obsDeriv) / innovVar; gain = (bcnPosOffsetMinVar * obsDeriv) / innovVar;
// calculate a filtered state change magnitude to be used to select between the high or low offset
float stateChange = innov * gain;
OffsetMinInnovFilt = (1.0f - filtAlpha) * OffsetMinInnovFilt + fminf(fabsf(filtAlpha * stateChange) , 1.0f);
// Reject range innovation spikes using a 5-sigma threshold unless aligning
if ((sq(innov) < sq(innovGateWidth) * innovVar) || aligning) {
// state update // state update
bcnPosDownOffsetMin -= innov * gain; bcnPosDownOffsetMin -= stateChange;
// covariance update // covariance update
bcnPosOffsetMinVar -= gain * obsDeriv * bcnPosOffsetMinVar; bcnPosOffsetMinVar -= gain * obsDeriv * bcnPosOffsetMinVar;
@ -601,16 +604,21 @@ void NavEKF3_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP
bcnPosDownOffsetMax = MAX(bcnPosDownOffsetMax, vehiclePosNED.z - bcnMidPosD + 0.5f); bcnPosDownOffsetMax = MAX(bcnPosDownOffsetMax, vehiclePosNED.z - bcnMidPosD + 0.5f);
bcnPosDownOffsetMin = MIN(bcnPosDownOffsetMin, vehiclePosNED.z - bcnMidPosD - 0.5f); bcnPosDownOffsetMin = MIN(bcnPosDownOffsetMin, vehiclePosNED.z - bcnMidPosD - 0.5f);
// calculate the innovation for the main filter using the offset with the smallest innovation history // calculate the innovation for the main filter using the offset that is most stable
// apply hysteresis to prevent rapid switching // apply hysteresis to prevent rapid switching
if (!usingMinHypothesis && OffsetMinInnovFilt < 0.8f * OffsetMaxInnovFilt) { if (!usingMinHypothesis && (OffsetMinInnovFilt < (0.8f * OffsetMaxInnovFilt))) {
bcnPosOffsetNED.z = bcnPosDownOffsetMin;
usingMinHypothesis = true; usingMinHypothesis = true;
} else if (usingMinHypothesis && OffsetMaxInnovFilt < 0.8f * OffsetMinInnovFilt) { } else if (usingMinHypothesis && (OffsetMaxInnovFilt < (0.8f * OffsetMinInnovFilt))) {
bcnPosOffsetNED.z = bcnPosDownOffsetMax;
usingMinHypothesis = false; usingMinHypothesis = false;
} }
if (usingMinHypothesis) {
bcnPosOffsetNED.z = bcnPosDownOffsetMin;
} else {
bcnPosOffsetNED.z = bcnPosDownOffsetMax;
}
// apply the vertical offset to the beacon positions
rngBcnDataDelayed.beacon_posNED.z += bcnPosOffsetNED.z;
} }
#endif // HAL_CPU_CLASS #endif // HAL_CPU_CLASS