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
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;
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
if ((sq(innov) < sq(innovGateWidth) * innovVar) || aligning) {
// state update
bcnPosDownOffsetMax -= innov * gain;
bcnPosDownOffsetMax -= stateChange;
// covariance update
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
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
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
// apply hysteresis to prevent rapid switching
if (!usingMinHypothesis && (OffsetMinInnovFilt < (0.8f * OffsetMaxInnovFilt))) {
if (!usingMinHypothesis && (minOffsetStateChangeFilt < (0.8f * maxOffsetStateChangeFilt))) {
usingMinHypothesis = true;
} else if (usingMinHypothesis && (OffsetMaxInnovFilt < (0.8f * OffsetMinInnovFilt))) {
} else if (usingMinHypothesis && (maxOffsetStateChangeFilt < (0.8f * minOffsetStateChangeFilt))) {
usingMinHypothesis = false;
}
if (usingMinHypothesis) {

4
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -353,10 +353,10 @@ void NavEKF3_core::InitialiseVariables()
minBcnPosD = 0.0f;
bcnPosDownOffsetMax = 0.0f;
bcnPosOffsetMaxVar = 0.0f;
OffsetMaxInnovFilt = 0.0f;
maxOffsetStateChangeFilt = 0.0f;
bcnPosDownOffsetMin = 0.0f;
bcnPosOffsetMinVar = 0.0f;
OffsetMinInnovFilt = 0.0f;
minOffsetStateChangeFilt = 0.0f;
rngBcnFuseDataReportIndex = 0;
memset(&rngBcnFusionReport, 0, sizeof(rngBcnFusionReport));
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 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 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)
bool bcnOriginEstInit; // True when the beacon origin has been initialised