Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: Fix bugs causing height drift when using range beacons

This commit is contained in:
priseborough 2017-06-21 08:56:41 +10:00 committed by Francisco Ferreira
parent 6cf9b870b5
commit 2a9eceaf10
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

@ -784,7 +784,8 @@ void NavEKF3_core::readRngBcnData()
// Correct the range beacon earth frame origin for estimated offset relative to the EKF earth frame origin
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

@ -531,7 +531,7 @@ void NavEKF3_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP
float obsDeriv; // derivative of observation relative to state
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
// estimate upper value for offset
@ -552,9 +552,6 @@ void NavEKF3_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP
// Calculate innovation
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
bcnPosOffsetMaxVar += stateNoiseVar;
@ -562,10 +559,14 @@ void NavEKF3_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP
innovVar = obsDeriv * bcnPosOffsetMaxVar * obsDeriv + obsVar;
innovVar = MAX(innovVar, obsVar);
// calculate the Kalman gain
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) {
// calculate the Kalman gain
gain = (bcnPosOffsetMaxVar * obsDeriv) / innovVar;
// state update
bcnPosDownOffsetMax -= innov * gain;
@ -589,9 +590,6 @@ void NavEKF3_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP
// Calculate innovation
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
bcnPosOffsetMinVar += stateNoiseVar;
@ -599,13 +597,18 @@ void NavEKF3_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP
innovVar = obsDeriv * bcnPosOffsetMinVar * obsDeriv + obsVar;
innovVar = MAX(innovVar, obsVar);
// calculate the Kalman gain
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) {
// calculate the Kalman gain
gain = (bcnPosOffsetMinVar * obsDeriv) / innovVar;
// state update
bcnPosDownOffsetMin -= innov * gain;
bcnPosDownOffsetMin -= stateChange;
// covariance update
bcnPosOffsetMinVar -= gain * obsDeriv * bcnPosOffsetMinVar;
@ -620,16 +623,21 @@ void NavEKF3_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP
bcnPosDownOffsetMax = MAX(bcnPosDownOffsetMax, 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
if (!usingMinHypothesis && OffsetMinInnovFilt < 0.8f * OffsetMaxInnovFilt) {
bcnPosOffsetNED.z = bcnPosDownOffsetMin;
if (!usingMinHypothesis && (OffsetMinInnovFilt < (0.8f * OffsetMaxInnovFilt))) {
usingMinHypothesis = true;
} else if (usingMinHypothesis && OffsetMaxInnovFilt < 0.8f * OffsetMinInnovFilt) {
bcnPosOffsetNED.z = bcnPosDownOffsetMax;
} else if (usingMinHypothesis && (OffsetMaxInnovFilt < (0.8f * OffsetMinInnovFilt))) {
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