EKF: Miscellaneous updates for EKF2 and EKF3 #6288

priseborough · 2017-05-21T12:26:49Z

Improves the internal use of GPS reference height and add parameter control over behaviour and reporting. This enables the GPS reference height to be updated in-flight to for drift in the EKF's pressure height datum if using Baro or changes in the terrain height datum if using the range finder as a primary height sensor. The change in GPS reference height can also be reported externally as a change in EKF origin height if desired.
The time required to pass tilt alignment checks on startup has been reduced by inhibiting the gyro bias learning until the initial tilt alignment is completed.
More reliable inhibiting of unwanted states
Efficiency improvements in the covariance prediction by significantly reducing the number of copy operations.
Prevents the accel bias states becoming badly conditioned during long periods without movement which can cause loss of height estimation during preflight. This is achieved by slightly increasing the target covariance prediction dt and raising the lower limit on the delta velocity bias state variances.
Adds low rate logging of covariance matrix diagonals to assist with debugging filter tuning problems.
Reverts micro-optimisations in the covariance prediction auto-code that should be left to the compiler.
Adds protection against the delta velocity bias state variances collapsing, which can happen if there are extended periods without movement and the process noise parameter is small. If variances fall below an equivalent of 1 cm/s/s of bias uncertainty, then the process noise is increased up to a maximum of 10x when variance has fallen to an equivalent of 1 mm/s/s of bias uncertainty. If this level is reached, the covariance (off diagonal) terms are zeroed.

TESTING

Has been bench tested for extended (>1hr) periods without stability issues.
Has been SITL tested for Copter.
Has been flight tested for Copter.

khancyr · 2017-05-22T12:18:51Z

libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp

-                // Set the height of the NED origin to ‘height of baro height datum relative to GPS height datum'
-                EKF_origin.alt = gpsloc.alt - baroDataNew.hgt;
+                // Set the height of the NED origin
+                ekfGpsRefHgt = (double)0.01 * (double)gpsloc.alt + (double)outputDataNew.position.z;


Is it obligatory to have ekfGpsRefHgt as double ? as it is only use for loc.alt (int32_t) and gpsDataNew.hgt (float), it should prevent unessary casting and double operation, no?

Is is necessary because the height could be 10000 metres above sea level for some applications, which introduce rounding errors that prevent a single precision variable converging when used recursively by the estimator.

ok ! thank for the explaination

khancyr · 2017-05-22T12:20:10Z

libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp

-    // correct the EKF origin and variance estimate if the innovation variance ratio is < 5-sigma
-    if (ratio < 5.0f) {
-        EKF_origin.alt -= (int)(100.0f * gain * innovation);
+    // correct the EKF origin and variance estimate if the innovation is less than 5-sigma


It shouldn't be 25-sigma ?

No, because we are comparing the ratio of variances which have units of sigma^2

khancyr · 2017-05-22T12:24:01Z

libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp

@@ -451,6 +451,13 @@ void  NavEKF3_core::getVariances(float &velVar, float &posVar, float &hgtVar, Ve
    offset   = posResetNE;
 }

+// return the diagonals from the covariance matrix
+void  NavEKF3_core::getStateVariances(float stateVar[24])


shouldn't stateVar be a pointer ? otherwith it will create the table on function call and destroy it on exit, so we return anything

it is a pointer. The [24] is really just a hint to the programmer of the size

khancyr · 2017-05-22T12:28:01Z

libraries/AP_NavEKF3/AP_NavEKF3_core.h

@@ -58,8 +58,8 @@
 #define ACCEL_BIAS_LIM_SCALER 0.2f

 // target update time for the EKF in msec and sec
-#define EKF_TARGET_DT_MS 10.0
-#define EKF_TARGET_DT    0.01
+#define EKF_TARGET_DT_MS 12.5


EKF_TARGET_DT_MS is use with cast on uint16_t and uint8_t so we lost the 0.5

khancyr · 2017-05-22T12:28:39Z

libraries/AP_NavEKF3/AP_NavEKF3_core.h

-#define EKF_TARGET_DT_MS 10.0
-#define EKF_TARGET_DT    0.01
+#define EKF_TARGET_DT_MS 12.5
+#define EKF_TARGET_DT    0.0125


EKF_TARGET_DT is a float on NavEKF2, isn't the f califier missing ?

khancyr · 2017-05-22T12:32:45Z

libraries/AP_NavEKF3/AP_NavEKF3_core.cpp

@@ -1417,7 +1417,7 @@ void NavEKF3_core::ConstrainVariances()
    }

    if (!inhibitDelVelBiasStates) {
-        for (uint8_t i=13; i<=15; i++) P[i][i] = constrain_float(P[i][i],0.0f,sq(10.0f * dtEkfAvg));
+        for (uint8_t i=13; i<=15; i++) P[i][i] = constrain_float(P[i][i],1e-9f,sq(10.0f * dtEkfAvg));


We are on float using FLT_EPSILON will be better no ?

No, the 1e-9 was determined from testing and is related to overall stability of the covariance matrix prediction and updates.

khancyr · 2017-05-23T09:38:20Z

looks good to me, but I don't have the knowledge to review the logical changes deeply

Fix rounding error bug preventing state from updating after initial convergence. Decouple GPS reference height from published EKf origin height. Add bitmask parameter to control update and publishing of GPS reference height.

…ation Inhibiting gyro bias estimation during the initial tilt alignment speeds alignment. The calculation of the maxmum state index required has been modified so that it can handle all combinations of inhibited states. Limiting the maximum state index accessed by all EKF operations result in significant processing reductions when higher index states are not being used.

All Kalman gain calculations now explicity set gains for deactivated states to zero. Previous use of loops to set gains to zero have been replaced with more efficient memset operations.

…diction This patch ensures that covariance matrix entries for inactive states are always set to zero. It also halves the number of copy operations from the updated to stored matrix.

…sations To eliminate the possibility of editing errors, revert the covariance prediction auto-code to the original auto-code without the replacement fo the /2 and /4 operations. The compiler optimisations are able to correctly handle the /2 and /4 operations. Also use local variables for intermediate covariance calculations. The use of calss varaibles for these small arrays was unnecessary.

The target covariance time step has been increased from 10 to 12.5 msec to improve conditioning of the covariance prediction calculation.

This is required to prevent co-variance matrix errors due to long periods without movement causing height divergence.

…ances If variance falls below desired minimum, set state noise to a larger value. If variance drops below safe value, set to desired minimum and reset off-diagonals to zero.

OXINARF

Made a few comments, some are just format/spelling mistakes.

OXINARF · 2017-05-29T14:44:54Z

libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp

@@ -306,7 +306,7 @@ bool NavEKF3_core::getLLH(struct Location &loc) const
 {
    if(validOrigin) {
        // Altitude returned is an absolute altitude relative to the WGS-84 spherioid
-        loc.alt = EKF_origin.alt - outputDataNew.position.z*100;
+        loc.alt =  100 * (int32_t)(ekfGpsRefHgt - (double)outputDataNew.position.z);


I wonder if we shouldn't use EKF_origin.alt when the update origin bit is turned off, otherwise I think that, when that is configured, the relative altitude (getPosD) can be different than getLLH.alt - getOriginLLH.alt.

That makes sense.

Thinking about it some more, we should also adjust the altitude reported by getPosD for the difference between EKF_Origin.alt and ekfGpsRefHgt if internal reference height adjustments have been requested by the setting of bit positions 0 and 1.

OXINARF · 2017-05-29T14:52:31Z

libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp

-    // correct the EKF origin and variance estimate if the innovation variance ratio is < 5-sigma
-    if (ratio < 5.0f) {
-        EKF_origin.alt -= (int)(100.0f * gain * innovation);
-        ekfOriginHgtVar -= fmaxf(gain * ekfOriginHgtVar , 0.0f);


Not sure if it is an error or not, but the ekfOriginHgtVar subtraction is removed in EKF2 but not in EKF3.

Good catch - it was not supposed to be removed.

OXINARF · 2017-05-29T15:25:54Z

libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp

+                memset(&Kfusion[10], 0, 12);
+            }
+
+            if (!inhibitDelVelBiasStates) {
            Kfusion[13] = SK_MZ[0]*(P[13][21] + P[13][0]*SH_MAG[1] - P[13][1]*SH_MAG[2] + P[13][3]*SH_MAG[0] + P[13][2]*SK_MZ[2] + P[13][18]*SK_MZ[1] + P[13][16]*SK_MZ[4] - P[13][17]*SK_MZ[3]);
            Kfusion[14] = SK_MZ[0]*(P[14][21] + P[14][0]*SH_MAG[1] - P[14][1]*SH_MAG[2] + P[14][3]*SH_MAG[0] + P[14][2]*SK_MZ[2] + P[14][18]*SK_MZ[1] + P[14][16]*SK_MZ[4] - P[14][17]*SK_MZ[3]);
            Kfusion[15] = SK_MZ[0]*(P[15][21] + P[15][0]*SH_MAG[1] - P[15][1]*SH_MAG[2] + P[15][3]*SH_MAG[0] + P[15][2]*SK_MZ[2] + P[15][18]*SK_MZ[1] + P[15][16]*SK_MZ[4] - P[15][17]*SK_MZ[3]);


Indentation should be changed.

OXINARF · 2017-05-29T15:29:53Z

libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp

@@ -614,19 +627,25 @@ void NavEKF3_core::FuseOptFlow()
            Kfusion[7] = -t78*(P[7][0]*t2*t5+P[7][5]*t2*t8-P[7][6]*t2*t10+P[7][1]*t2*t16-P[7][2]*t2*t19+P[7][3]*t2*t22+P[7][4]*t2*t27);
            Kfusion[8] = -t78*(P[8][0]*t2*t5+P[8][5]*t2*t8-P[8][6]*t2*t10+P[8][1]*t2*t16-P[8][2]*t2*t19+P[8][3]*t2*t22+P[8][4]*t2*t27);
            Kfusion[9] = -t78*(P[9][0]*t2*t5+P[9][5]*t2*t8-P[9][6]*t2*t10+P[9][1]*t2*t16-P[9][2]*t2*t19+P[9][3]*t2*t22+P[9][4]*t2*t27);
+
+            if (!inhibitDelAngBiasStates) {
            Kfusion[10] = -t78*(P[10][0]*t2*t5+P[10][5]*t2*t8-P[10][6]*t2*t10+P[10][1]*t2*t16-P[10][2]*t2*t19+P[10][3]*t2*t22+P[10][4]*t2*t27);
            Kfusion[11] = -t78*(P[11][0]*t2*t5+P[11][5]*t2*t8-P[11][6]*t2*t10+P[11][1]*t2*t16-P[11][2]*t2*t19+P[11][3]*t2*t22+P[11][4]*t2*t27);
            Kfusion[12] = -t78*(P[12][0]*t2*t5+P[12][5]*t2*t8-P[12][6]*t2*t10+P[12][1]*t2*t16-P[12][2]*t2*t19+P[12][3]*t2*t22+P[12][4]*t2*t27);


Wrong indentation

OXINARF · 2017-05-29T15:38:37Z

libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp

+            memset(&Kfusion[10], 0, 12);
+        }
+
+        if (!inhibitDelVelBiasStates) {
        Kfusion[13] = -t26*(P[13][7]*t4*t9+P[13][8]*t3*t9+P[13][9]*t2*t9);
        Kfusion[14] = -t26*(P[14][7]*t4*t9+P[14][8]*t3*t9+P[14][9]*t2*t9);
        Kfusion[15] = -t26*(P[15][7]*t4*t9+P[15][8]*t3*t9+P[15][9]*t2*t9);


Indentation

OXINARF · 2017-05-29T16:26:11Z

libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp

+// Adjust the indexing limits used to address the covariance, states and other EKF arrays to avoid unnecessary operations
+// if we are not using those states
+void NavEKF3_core::updateStateIndexLim()
+{
    // Adjust the indexing limits used to address the covariance, states and other EKF arrays to avoid unnecessary operations
    // if we are not using those states


Remove this comment since it was added to the function above.

OXINARF · 2017-05-29T16:26:24Z

libraries/AP_NavEKF3/AP_NavEKF3_core.h

@@ -766,6 +766,9 @@ class NavEKF3_core

    // update timing statistics structure
    void updateTimingStatistics(void);
+
+    // Update the stte index limit based on which states are active


stte -> state

OXINARF · 2017-05-29T16:28:46Z

libraries/AP_NavEKF3/AP_NavEKF3_core.cpp

+    // we calculate the lower diagonal and copy to take advantage of symmetry
+
+    // intermediate calculations
+    float SF[21];


Shouldn't this be a Vector21? Also applies to all new variables below.

OXINARF · 2017-05-29T17:45:18Z

libraries/AP_NavEKF3/AP_NavEKF3_core.cpp

+                // restore variances
+                for (uint8_t j=13; j<=15; j++) {
+                    P[j][j] = delVelBiasVar[j-13];
+                }


I don't really understand the piece of code above, I think you missed that this is already inside a loop. It looks like either a break is missing in the end or this code should simply be:

float delVelBiasVar = P[i][i]; zeroCols(P, i, i); P[i][i] = delVelBiasVar;

But in that case I don't know where the P[i][i] = 1E-8f; would belong, as that would remove the need to save the variance.

I will rework this section of code as it is not doing what I intended.

OXINARF · 2017-05-29T17:48:11Z

libraries/AP_NavEKF3/AP_NavEKF3_core.cpp

-    // constrain diagonals to prevent ill-conditioning
+    // copy variances (diagonals)
+    for (uint8_t i = 0; i <= stateIndexLim; i++) {
+        P[i][i] = nextP[i][i];


This can be further improved by doing this in the previous loop in one of two ways:

change column < row to column <= row

put this line before or after the inner loop

For either solution row = 1 needs to be changed to row = 0.

priseborough · 2017-05-31T02:08:48Z

I think turning off the corrections by setting the control parameter to 0 is the best thing to do at the moment until the lead developers for each platform type decide which default behaviour they want. We can then set a different default for each platform type.

Turn off by default. Update parameter description.

Turn off by default. Update parameter description

priseborough · 2017-05-31T02:50:37Z

Here is a test with bit position 2 set to 1 so that the corrections are applied to the origin instead of the local vertical position.

OXINARF · 2017-05-31T03:23:34Z

@priseborough Awesome! My math may be wrong at 4AM, but I think that by correcting posD, LLH shouldn't care about bit position 2 and always use the corrected origin height - more specifically commits 2343887 and b2a3584 should be removed.

I think your graphs don't show this as it isn't logging the altitude from LLH. I thought the POS message would log it but apparently we are fixing something in AHRS: https://github.com/ArduPilot/ardupilot/blob/master/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp#L384 (I don't know what this code here is supposed to fix).

Regarding the default value of the parameter, I think we should discuss in the next dev call. I'm actually inclined to say turn on the internal correction but without the corrected origin.

amilcarlucas · 2017-06-01T15:52:38Z

Tested with EKF2 on a copter today. It had no issues.

priseborough · 2017-06-06T01:04:14Z

@OXINARF I need to do do a more thorough review to see if we can remove those patches without affecting behaviour. As for turning on internal correction for copter and apply corrections to the local position, I think that may cause problems with copters hovering in alt_hold with bad GPS. My preference for copter is to either turn the corrections off or to apply the correction to the origin and leave the local position alone so that that users with bad GPS are not affected.

OXINARF · 2017-06-06T02:53:56Z

@priseborough If you look at the current math between posD, posLLH and originLLH you'll see that when bit position 2 is turned off, it isn't correct. By removing those two patches everything is correct again. It's possible my math is wrong, but I checked it at a more sane time (after that 4AM comment) and came to the same conclusion. I can go into more detail where I see it wrong if needed.

Regarding the default flags, I totally understand your reason for concern. I agree that serious testing needs to happen with corrections applied to local position before enabling it by default.

This reverts commit b2a3584.

This reverts commit 2343887.

priseborough · 2017-06-06T08:26:37Z

@OXINARF My review and bench testing confirmed that removing those patches produces correct behaviour. I have reverted them.

OXINARF · 2017-06-13T20:44:21Z

I've squashed the PR: master...OXINARF:review/6288
I've checked that the final diff is the same (with the exception of a small correction to the parameter description, it was missing an or).

@priseborough Should we talk to vehicle maintainers before merging or are you confident in merging it with parameter turned off and discussing it later?

priseborough · 2017-06-13T23:12:50Z

Let's merge with the height corrections turned off (OGN_HGT_MASK = 0) and set the platform specific defaults after @tridge and @rmackay9 have had a chance to decide what they want.

OXINARF · 2017-06-14T11:47:22Z

Merged, thanks!

EShamaev · 2017-06-18T14:36:27Z

What will happen if during flight we loose GPS and revert to DCM? Will corrections for baro that got accumulated be valid for DCM at that point or a sudden alt change will happen?

priseborough · 2017-06-18T22:41:49Z

I assume you are talking about planes. The TECS height controller for that platform type uses take height from the AHRS get_relative_position_D_home(height).

If DCM is being used, that quantity comes directly from the baro.
If the EKF is being used it comes from the EKF's local vertical position corrected for the offset between the heights of the EKF origin and home position.

That means there will be a step in height if planes revert back to DCM unless AP_AHRS_DCM::get_relative_position_D_home() is modified to correct using the difference between the HOME altitude and last known good EKF origin altitude. Bit position 2 in the _OGN_HGT_MASK parameter would need to be set to 2 so that the drift correction was applied to the EKF origin for this to work.

EShamaev · 2017-06-19T05:00:18Z

Is it possible to make the changes to AP_AHRS_DCM::get_relative_position_D_home() in this PR or a separate one is needed?

priseborough · 2017-06-19T07:12:27Z

A separate one is needed. This has already been merged.

This feature is off by default for both platforms.

EShamaev · 2017-06-19T08:43:35Z

OK, I will try to make it as I am afraid to test this feature on plane without the discussed changes to DCM.

EShamaev · 2017-06-25T14:22:10Z

Flight tested today on a plane.
Works fine.

priseborough added BUG EKF Enhancement labels May 21, 2017

khancyr reviewed May 22, 2017

View reviewed changes

priseborough added 14 commits May 24, 2017 21:21

AP_NavEKF3: Improve GPS reference height estimator

43622b2

Fix rounding error bug preventing state from updating after initial convergence. Decouple GPS reference height from published EKf origin height. Add bitmask parameter to control update and publishing of GPS reference height.

AP_NavEKF2: Improve GPS reference height estimator

256b3ac

Fix rounding error bug preventing state from updating after initial convergence. Decouple GPS reference height from published EKf origin height. Add bitmask parameter to control update and publishing of GPS reference height.

AP_NavEKF3: Fix initialisation of state inhibit flags

e93e422

AP_NavEKF3: Ensure Kalman gain calculatons respect deactivated states

804bdfe

All Kalman gain calculations now explicity set gains for deactivated states to zero. Previous use of loops to set gains to zero have been replaced with more efficient memset operations.

AP_NavEKF3: Improve partitioning and efficiency of the covariance pre…

ba5f6aa

…diction This patch ensures that covariance matrix entries for inactive states are always set to zero. It also halves the number of copy operations from the updated to stored matrix.

AP_NavEKF3: Prevent possible race condition re-zeroing state variances

bc210ef

AP_NavEKF3: Add public accessor for state variances

366e1f4

DataFlash: Add logging for EKF3 state variances

6bab31e

DataFlash: reduce logging rate of ekf state variances

31cd09b

AP_NavEKF3: Make target covariance time step larger

60d0fc7

The target covariance time step has been increased from 10 to 12.5 msec to improve conditioning of the covariance prediction calculation.

AP_NavEKF3: Raise lower limit on del vel bias state variances

1651360

This is required to prevent co-variance matrix errors due to long periods without movement causing height divergence.

AP_NavEKF3: fixes arising from review comments

700ba4a

priseborough force-pushed the pr-ekfUpdates branch 3 times, most recently from d13057a to fc4d181 Compare May 26, 2017 02:26

AP_NavEKF3: Improve protection for badly conditioned dVel bias covari…

280876c

…ances If variance falls below desired minimum, set state noise to a larger value. If variance drops below safe value, set to desired minimum and reset off-diagonals to zero.

priseborough force-pushed the pr-ekfUpdates branch from fc4d181 to 280876c Compare May 26, 2017 07:07

OXINARF reviewed May 29, 2017

View reviewed changes

priseborough added 2 commits May 31, 2017 12:39

AP_NavEKF3: Change default value of EK3_OGN_HGT_MASK

ef5f0fd

Turn off by default. Update parameter description.

AP_NavEKF2: Change default value of EK2_OGN_HGT_MASK

f79a38e

Turn off by default. Update parameter description

priseborough mentioned this pull request Jun 2, 2017

EKF3 has an error in NavEKF3_core::setWindMagStateLearningMode() #6366

Closed

OXINARF added the DevCallTopic label Jun 5, 2017

OXINARF removed the DevCallTopic label Jun 6, 2017

priseborough added 2 commits June 6, 2017 16:30

Revert "AP_NavEKF2: Report consistent origin, global and local height"

2173665

This reverts commit b2a3584.

Revert "AP_NavEKF3: Report consistent origin, global and local height"

6994e62

This reverts commit 2343887.

OXINARF closed this Jun 14, 2017

OXINARF mentioned this pull request Jun 14, 2017

AP_Baro: Long term correction of Baro based on GPS #6228

Closed

EShamaev mentioned this pull request Jun 24, 2017

DCM: Home correction referenced to EKF #6497

Closed

This was referenced Jun 26, 2017

AP_AHRS: Do not revert to DCM if GPS is lost but we have healthy airspeed sensor #6502

Closed

AP_AHRS: Do not revert to DCM if GPS is lost but we have healthy airspeed sensor #6503

Closed

tridge added this to PRs in Plane Backports Oct 26, 2017

tridge removed this from PRs in Plane Backports Oct 30, 2017

EKF: Miscellaneous updates for EKF2 and EKF3 #6288

EKF: Miscellaneous updates for EKF2 and EKF3 #6288

Conversation

priseborough commented May 21, 2017 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

khancyr commented May 23, 2017

OXINARF left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

priseborough commented May 31, 2017

priseborough commented May 31, 2017

OXINARF commented May 31, 2017

amilcarlucas commented Jun 1, 2017

priseborough commented Jun 6, 2017 • edited Loading

OXINARF commented Jun 6, 2017 • edited Loading

priseborough commented Jun 6, 2017

OXINARF commented Jun 13, 2017

priseborough commented Jun 13, 2017

OXINARF commented Jun 14, 2017

EShamaev commented Jun 18, 2017

priseborough commented Jun 18, 2017

EShamaev commented Jun 19, 2017

priseborough commented Jun 19, 2017

EShamaev commented Jun 19, 2017

EShamaev commented Jun 25, 2017

priseborough commented May 21, 2017 •

edited

Loading

priseborough commented Jun 6, 2017 •

edited

Loading

OXINARF commented Jun 6, 2017 •

edited

Loading