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

AP_NavEKF3: Improve variable names and documentation

Browse Source 
Also remove unnecessary calculation of innov * gain
This commit is contained in:
priseborough 2017-06-22 10:14:32 +10:00 committed by Francisco Ferreira
parent 368983ed5a
commit a4d18696a7
3 changed files with 10 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp
View File

@ -562,14 +562,15 @@ void NavEKF3_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP
// calculate the Kalman gain // calculate the Kalman gain
gain = (bcnPosOffsetMaxVar * obsDeriv) / innovVar; gain = (bcnPosOffsetMaxVar * obsDeriv) / innovVar;
// calculate a filtered state change magnitude to be used to select between the high or low offset
float stateChange = innov * gain; float stateChange = innov * gain;
OffsetMaxInnovFilt = (1.0f - filtAlpha) * OffsetMaxInnovFilt + fminf(fabsf(filtAlpha * stateChange) , 1.0f); maxOffsetStateChangeFilt = (1.0f - filtAlpha) * maxOffsetStateChangeFilt + fminf(fabsf(filtAlpha * stateChange) , 1.0f);
// Reject range innovation spikes using a 5-sigma threshold unless aligning // Reject range innovation spikes using a 5-sigma threshold unless aligning
if ((sq(innov) < sq(innovGateWidth) * innovVar) || aligning) { if ((sq(innov) < sq(innovGateWidth) * innovVar) || aligning) {
// state update // state update
bcnPosDownOffsetMax -= innov * gain; bcnPosDownOffsetMax -= stateChange;
// covariance update // covariance update
bcnPosOffsetMaxVar -= gain * obsDeriv * bcnPosOffsetMaxVar; bcnPosOffsetMaxVar -= gain * obsDeriv * bcnPosOffsetMaxVar;
@ -602,7 +603,7 @@ void NavEKF3_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP
// calculate a filtered state change magnitude to be used to select between the high or low offset // calculate a filtered state change magnitude to be used to select between the high or low offset
float stateChange = innov * gain; float stateChange = innov * gain;
OffsetMinInnovFilt = (1.0f - filtAlpha) * OffsetMinInnovFilt + fminf(fabsf(filtAlpha * stateChange) , 1.0f); minOffsetStateChangeFilt = (1.0f - filtAlpha) * minOffsetStateChangeFilt + fminf(fabsf(filtAlpha * stateChange) , 1.0f);
// Reject range innovation spikes using a 5-sigma threshold unless aligning // Reject range innovation spikes using a 5-sigma threshold unless aligning
if ((sq(innov) < sq(innovGateWidth) * innovVar) || aligning) { if ((sq(innov) < sq(innovGateWidth) * innovVar) || aligning) {
@ -625,9 +626,9 @@ void NavEKF3_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP
// calculate the innovation for the main filter using the offset that is most stable // 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 && (minOffsetStateChangeFilt < (0.8f * maxOffsetStateChangeFilt))) {
usingMinHypothesis = true; usingMinHypothesis = true;
} else if (usingMinHypothesis && (OffsetMaxInnovFilt < (0.8f * OffsetMinInnovFilt))) { } else if (usingMinHypothesis && (maxOffsetStateChangeFilt < (0.8f * minOffsetStateChangeFilt))) {
usingMinHypothesis = false; usingMinHypothesis = false;
} }
if (usingMinHypothesis) { if (usingMinHypothesis) {

4
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -353,10 +353,10 @@ void NavEKF3_core::InitialiseVariables()
minBcnPosD = 0.0f; minBcnPosD = 0.0f;
bcnPosDownOffsetMax = 0.0f; bcnPosDownOffsetMax = 0.0f;
bcnPosOffsetMaxVar = 0.0f; bcnPosOffsetMaxVar = 0.0f;
OffsetMaxInnovFilt = 0.0f; maxOffsetStateChangeFilt = 0.0f;
bcnPosDownOffsetMin = 0.0f; bcnPosDownOffsetMin = 0.0f;
bcnPosOffsetMinVar = 0.0f; bcnPosOffsetMinVar = 0.0f;
OffsetMinInnovFilt = 0.0f; minOffsetStateChangeFilt = 0.0f;
rngBcnFuseDataReportIndex = 0; rngBcnFuseDataReportIndex = 0;
memset(&rngBcnFusionReport, 0, sizeof(rngBcnFusionReport)); memset(&rngBcnFusionReport, 0, sizeof(rngBcnFusionReport));
bcnPosOffsetNED.zero(); bcnPosOffsetNED.zero();

4
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -1091,11 +1091,11 @@ private:
float bcnPosDownOffsetMax; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m) float bcnPosDownOffsetMax; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m)
float bcnPosOffsetMaxVar; // Variance of the bcnPosDownOffsetMax state (m) float bcnPosOffsetMaxVar; // Variance of the bcnPosDownOffsetMax state (m)
float OffsetMaxInnovFilt; // Filtered magnitude of the range innovations using bcnPosOffsetHigh float maxOffsetStateChangeFilt; // Filtered magnitude of the change in bcnPosOffsetHigh
float bcnPosDownOffsetMin; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m) float bcnPosDownOffsetMin; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m)
float bcnPosOffsetMinVar; // Variance of the bcnPosDownOffsetMin state (m) float bcnPosOffsetMinVar; // Variance of the bcnPosDownOffsetMin state (m)
float OffsetMinInnovFilt; // Filtered magnitude of the range innovations using bcnPosOffsetLow float minOffsetStateChangeFilt; // Filtered magnitude of the change in bcnPosOffsetLow
Vector3f bcnPosOffsetNED; // NED position of the beacon origin in earth frame (m) Vector3f bcnPosOffsetNED; // NED position of the beacon origin in earth frame (m)
bool bcnOriginEstInit; // True when the beacon origin has been initialised bool bcnOriginEstInit; // True when the beacon origin has been initialised