AP_NavEKF3: Improve variable names and documentation

Also remove unnecessary calculation of innov * gain

libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp

@@ -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) {

libraries/AP_NavEKF3/AP_NavEKF3_core.cpp

@@ -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();

libraries/AP_NavEKF3/AP_NavEKF3_core.h

@@ -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