EKF Miscellaneous Improvements and Bug Fixes #5407

priseborough · 2016-12-19T07:31:07Z

Allows use of range beacons simultaneously with GPS and optical flow
Fixes an error in the way that the Pozyx beacon system output is rotated into the local NED earth frame.
Makes the sensor buffer lengths adapt to the time delays to reduce output predictor errors
Uses the averaged, but unfiltered airspeed to reduce the effective time delay.
Fixes a potential bug that could allow the filter to start before the required sensor buffers are filled
Uses the time delay published by the GPS driver, instead of a value specified by a EKF parameter.
Updates EKF3 to allow copters to fly in non position aiding modes without a magnetometer.

TODO:

Add GPS_DELAY and GPS_DELAY2 parameters to the GPS library as discussed in #5405

OXINARF · 2016-12-19T10:37:43Z

libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp

+                FuseRngBcn();
+            } else {
+                // If we are using GPS, then GPS is the primary reference, but we continue to use the beacon data
+                // to calculate an indpendant position that is used to update the beacon position offset if we need to


indpedant -> independent

OXINARF · 2016-12-19T10:45:22Z

libraries/AP_NavEKF3/AP_NavEKF3_core.h

+    enum resetDataSource {
+                    DEFAULT=0,      // Use best available data
+                    GPS=1,          // Use GPS if available
+                    RNGBCN=2,       // Use beacon range data if available


This says "Use X if available", but looking at code it will always use the selected source, available or not - I think the callers to resets are always taking tare of ensuring availability, but the comments here should reflect that, the responsibility is on the caller.

OXINARF · 2016-12-19T10:48:17Z

libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp

    rngBcnFuseDataReportIndex++;
+    printf("%6.2f\n",(double)receiverPos.x);


Left-over debug?

OXINARF · 2016-12-19T12:02:33Z

libraries/AP_NavEKF3/AP_NavEKF3_core.cpp

+    // Calculate the expected EKF time step
+    if (_ahrs->get_ins().get_sample_rate() > 0) {
+        dtEkfAvg = 1.0f / _ahrs->get_ins().get_sample_rate();
+        dtEkfAvg = fmaxf(dtEkfAvg,EKF_TARGET_DT);


Any reason to not use our MAX definition?

None, is there a benefit (other than consistency)?

I went through the EKF3 code and both fmaxf() and MAX() functions are being used for floating point constraints in different places. I want to be consistent, but before changing them all to one or the other, would like to know what the advantages and disadvantage are.

Code or performance wise I think there's no benefit. I would prefer MAX for two reasons:

fmaxf is a C function and I prefer to avoid them and use the C++ ones instead, as sometimes there are conflicts

although it isn't the case with fmaxf, there can be cases where our math library provides functions that aren't available in all platforms, so I like that we use what our own library provides

In the end it is up to the developer, there is no rule here. I was mainly curious if there was some reason you needed to use fmaxf.

OXINARF · 2016-12-19T12:49:01Z

libraries/AP_NavEKF3/AP_NavEKF3.cpp

@@ -219,7 +214,7 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {

    // @Param: HGT_DELAY
    // @DisplayName: Height measurement delay (msec)
-    // @Description: This is the number of msec that the Height measurements lag behind the inertial measurements.
+    // @Description: This is the number of msec that the Height measurements lag behind the inertial measurements. The autpilot should be restarted after adjusting this parameter.


Instead of all these comments in the description it should instead be added @RebootRequired: True

OXINARF · 2016-12-28T15:00:03Z

libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp

    if (!use_compass()) {
-        if (onGround && (imuSampleTime_ms - lastSynthYawTime_ms > 1000)) {
+        if ((onGround || assume_zero_sideslip()) && (imuSampleTime_ms - lastSynthYawTime_ms > 140)) {


According to the comment I think this should be:

onGround || !assume_zero_sideslip()

Either we are on the ground and do it for all vehicles or we aren't and we do it for non-plane vehicles.

WickedShell · 2016-12-28T15:34:11Z

libraries/AP_NavEKF3/AP_NavEKF3_core.cpp

        return false;
+    } else if (!_ahrs->get_gps().all_configured()) {


I can't see a reason to have this case. We should always succeed at configuring a GPS, unless the user requested to not configure it, in that case we should never opt out of the time delay, and instead should always be waiting until we succeed configuration.

Agree - provided the GPS library all_configured() function does not return false indefinitely for a GPS of an unknown type.

OK, will remove the timeout and also add the delay configuration parameters to the GPS library.

WickedShell · 2016-12-28T21:30:36Z

It doesn't, it will always say it's configured unless it's a ublox and working on the configuration.

…

On Dec 28, 2016 4:04 PM, "Paul Riseborough" ***@***.***> wrote: ***@***.**** commented on this pull request. ------------------------------ In libraries/AP_NavEKF3/AP_NavEKF3_core.cpp <#5407>: > return false; + } else if (!_ahrs->get_gps().all_configured()) { Agree - provided the GPS library all_configured() function does not return false indefinitely for a GPS of an unknown type. — You are receiving this because you commented. Reply to this email directly, view it on GitHub <#5407>, or mute the thread <https://github.com/notifications/unsubscribe-auth/AAipiNSd1-cEk6ZDiForKg7PJ_jf5VVuks5rMs7GgaJpZM4LQcWt> .

priseborough · 2016-12-28T22:53:27Z

I flight tested this on a copter with px4flow sensor enabled and all COMPASS_USE parameters set to 0.

Flight was stable with some heading drift at the start that disappeared as it did manage to learn the small Z gyro offset with some low speed manoeuvring. Here are the gyro bias estimates:

Flow innovations looked normal:

Magnetometer innovations were zero as expected:

OXINARF · 2016-12-29T00:32:48Z

libraries/AP_GPS/AP_GPS.cpp

+    // @Description: Controls the amount of GPS  measurement delay that the autopilot compensates for. Set to zero to use the default delay for the detected GPS type.
+    // @Units: msec
+    // @Min: 0
+    // @Max: 250


Not Min/Max but @Range: 0 250

OXINARF · 2016-12-29T00:32:55Z

libraries/AP_GPS/AP_GPS.cpp

+    // @Description: Controls the amount of GPS  measurement delay that the autopilot compensates for. Set to zero to use the default delay for the detected GPS type.
+    // @Units: msec
+    // @Min: 0
+    // @Max: 250


rmackay9 · 2016-12-30T00:55:48Z

libraries/AP_GPS/AP_GPS.cpp

+    // @Param: DELAY_MS
+    // @DisplayName: GPS delay in milliseconds
+    // @Description: Controls the amount of GPS  measurement delay that the autopilot compensates for. Set to zero to use the default delay for the detected GPS type.
+    // @Units: msec


@Units: milliseconds would be more consistent with what we call it in other libraries.

I did a search through the code found the following usages in different places

milli-seconds
milliseconds
Milliseconds
msec (EKF libraries)

I will add patches to change them all to milliseconds

roger-zhao

Hi, how this PR goes on? still not merge into master, right? I wonder that if or not I can fly copter in loiter mode with only GPS (no optical-flow) when this feature merged.

priseborough · 2017-01-19T01:04:01Z

@roger-zhao I believe I have addressed review comments, so there is nothing more I can do from my end to get this in.

This PR is not required to fly in loiter mode with only GPS. Ardupilot has been able to do that for years.

roger-zhao · 2017-01-19T07:33:07Z

@priseborough , Sorry, Paul, It's my fault not express clearly what I thought, actually I mean if I can fly copter without compass(mightbe just initialize the heading when starting) and only with a good GPS in Loiter mode according to this PR, :). Again, tks!

OXINARF

@priseborough we want to merge this soon so I made one last review and made two small comments.

Also, looking at #5603 I think there's a bug in:

Since it returns it should have a perf_end.

OXINARF · 2017-01-23T18:57:34Z

libraries/AP_GPS/AP_GPS.cpp

+    } else if (drivers[instance] == nullptr || state[instance].status == NO_GPS) {
+        // the user has not specified a value and we cannot determine it from the GPS type
+        // so return a default delay of 1 measurement interval
+        return 0.001f * (float)_rate_ms[instance];


Were you thinking of this for when a backend didn't have a lag value? Because this is going to be returned when there is no GPS connected to this instance (or it hasn't been properly detected yet), so basically should never be used.

Yes, this was written before those changes. Will fix.

OXINARF · 2017-01-23T19:01:40Z

libraries/AP_GPS/AP_GPS.h

@@ -310,7 +310,8 @@ class AP_GPS
    }

    // the expected lag (in seconds) in the position and velocity readings from the gps
-    float get_lag() const { return 0.2f; }
+    float get_lag(uint8_t instance) const;


Since the EKF wants this in msec and the only current user is the AHRS_DCM, I would change the function to return in msec instead.

I will have a look at it, but it would involve making changes in multiple files for no benefit other than a few bytes of storage.

I had a go at it, but ran into compiler errors associated with use of the override attribute on the get_lag function that I did not understand, so will tackle that separate to this PR.

The branch i attempted this with is here: https://github.com/priseborough/ardupilot/tree/temp

priseborough · 2017-01-25T20:53:30Z

@OXINARF the perf counter is in the calling function for https://github.com/ArduPilot/ardupilot/blob/master/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp#L437 , so no end required.
Will push a patch to fix the other.

Prevent bad values for GPS time delay pushing the GPS time stamp outside the range of IMU data contained in the buffer. If this occurs it can prevent the GPS measurements from being fused and cause loss of navigation.

Removes a leftover debug printf statement. Fixes documentation errors. Replace remaining fmaxf function calls with Ardupilot MAX function

Fixes bug that could leave it locked out of range finder use.

Copter operation without a magnetometer is limited to constant position and relative position modes only (no GPS or range beacon fusion permitted) Copter optical flow operation without a magnetometer is permitted. The ability of planes to takeoff/launch without a magnetometer and align the yaw using the GPS velocity is retained.

Do not time out and provide an escalating series of messages. We may need to adjust the time thresholds used for escalation. Do not wait if the EKF is not using the GPS.

If the user sets a non-zero value of the delay it will be used in preference over the default value for that GPS type. If the GPS type is unknown and the parameter is set to zero, then a default delay of 1 sample period will be used (eg 200ms for 5Hz).

…brary

The removed else if was going to be true when there is no GPS connected to this instance (or it hasn't been properly detected yet), so should never be used.

OXINARF · 2017-01-25T22:08:59Z

@OXINARF the perf counter is in the calling function for https://github.com/ArduPilot/ardupilot/blob/master/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp#L437 , so no end required.

Sorry, you are right, I probably looked a second time at the EKF2 instead of looking at the EKF3.

When you push the fixes, please make a comment and I'll merge it. Thanks!

priseborough · 2017-01-25T22:48:10Z

I've made changes for the review comments and rebased. The branch has been flown on a Pixhawk 2 with a bin/arducopter binary and px4fmu-v3 target

OXINARF · 2017-01-26T00:20:41Z

libraries/AP_GPS/AP_GPS.cpp

-    } else if (drivers[instance] == nullptr || state[instance].status == NO_GPS) {
-        // the user has not specified a value and we cannot determine it from the GPS type
-        // so return a default delay of 1 measurement interval
-        return 0.001f * (float)_rate_ms[instance];


@priseborough This is actually wrong, we should check for a non-existent driver in the requested instance (it would be a wrong use of the function from the caller but we should prevent it here). I think we should return a default 200 ms in that case.
Would you prefer to use the _rate_ms in case a backend exists but doesn't have it's own defined lag (so every GPS except Ublox)? I can do that if you want.

If the user has specified a delay for that instance, then we should use it, otherwise, given we don't know what type of GPs it is, we should set the delay to one measurement interval (200msec for a 5Hz GPS).

OXINARF · 2017-01-26T23:06:41Z

@priseborough Can you check that you agree with https://github.com/ArduPilot/ardupilot/compare/master...OXINARF:review/5407?expand=1? It is basically this PR plus your changes to return lag in msec and my last commit changing the backend to return 1 measurement interval when the driver doesn't have a specific lag.

priseborough · 2017-01-27T06:12:21Z

@OXINARF How about we get this PR in with the change required to make it work and then deal with 'nice to haves' like changing the units of lag from sec to msec in a later PR. I don't have time at the moment to deal with any more changes.

rmackay9 · 2017-01-27T07:59:53Z

Merged, thanks for the code and reviews!
I think @OXINARF and @priseborough are going to handle the remaining questions/comments as separate PRs.

priseborough added Logging EKF Enhancement GPS Library Tools labels Dec 19, 2016

priseborough mentioned this pull request Dec 19, 2016

AP_GPS: added backend specific get_lag() function #5405

Closed

OXINARF reviewed Dec 19, 2016

View reviewed changes

priseborough force-pushed the pr-ekf3Updates branch from ffb33fc to b1946b1 Compare December 19, 2016 23:24

priseborough changed the title ~~EKF3 Miscellaneous Improvements~~ EKF Miscellaneous Improvements and Bug Fixes Dec 21, 2016

priseborough force-pushed the pr-ekf3Updates branch from 8d49b0a to 0a67f4c Compare December 21, 2016 11:18

priseborough mentioned this pull request Dec 24, 2016

AP_NavEKF3: Changed if statements to switch statement. #5439

Closed

priseborough force-pushed the pr-ekf3Updates branch 3 times, most recently from c6e9250 to 52e9e42 Compare December 28, 2016 11:31

OXINARF reviewed Dec 28, 2016

View reviewed changes

WickedShell requested changes Dec 28, 2016

View reviewed changes

OXINARF requested changes Dec 29, 2016

View reviewed changes

priseborough force-pushed the pr-ekf3Updates branch from af8186d to 7735de9 Compare December 29, 2016 09:51

rmackay9 reviewed Dec 30, 2016

View reviewed changes

rmackay9 added this to the AC 3.5.0 milestone Jan 9, 2017

WickedShell approved these changes Jan 13, 2017

View reviewed changes

roger-zhao reviewed Jan 18, 2017

View reviewed changes

OXINARF reviewed Jan 23, 2017

View reviewed changes

AP_NavEKF3: improve variable name

55a359d

priseborough added 17 commits January 26, 2017 09:02

AP_AHRS_NavEKF: fix ekf variable naming and initialisation

44aea73

AP_NavEKF2: Add protection against bad GPS time delay values

329a58d

Prevent bad values for GPS time delay pushing the GPS time stamp outside the range of IMU data contained in the buffer. If this occurs it can prevent the GPS measurements from being fused and cause loss of navigation.

AP_NavEKF3: Address review comments

3acec88

Removes a leftover debug printf statement. Fixes documentation errors. Replace remaining fmaxf function calls with Ardupilot MAX function

AP_NavEKF3: Improve range finder selection logic

4f6544f

Fixes bug that could leave it locked out of range finder use.

AP_NavEKF3: remove unnecessary stdio includes

8e6378d

AP_NavEKF3: fix logic bug introduced by magnetometer use changes

a761293

AP_NavEKF3: Improve waiting for GPS configuration

bf98e6a

Do not time out and provide an escalating series of messages. We may need to adjust the time thresholds used for escalation. Do not wait if the EKF is not using the GPS.

AP_NavEKF3: protect against large delay values returned by the GPS li…

f477414

…brary

Plane: use standard unit descriptor for milliseconds

0a857e1

AP_GPS: use standard unit descriptor for milliseconds

9ccaf50

AP_NavEKF2: use standard unit descriptor for milliseconds

ab665cd

AP_NavEKF3: use standard unit descriptor for milliseconds

1491ed3

AP_NavEKF2: Add missing perf_end

7c0ac91

AP_NavEKF3: relocate perf monitor to count all three mag components

3c58439

AP_GPS: remove unnecessary else if

f553113

The removed else if was going to be true when there is no GPS connected to this instance (or it hasn't been properly detected yet), so should never be used.

AP_NavEKF3: fix compiler error

d270b11

priseborough force-pushed the pr-ekf3Updates branch from 61bbe7f to d270b11 Compare January 25, 2017 22:44

OXINARF self-assigned this Jan 25, 2017

OXINARF reviewed Jan 26, 2017

View reviewed changes

AP_GPS: change backend default lag to 1 measurement interval

3245ea3

rmackay9 closed this Jan 27, 2017

WickedShell mentioned this pull request Jan 29, 2017

move EKF_GPS_DELAY to GPS library #2850

Closed

priseborough deleted the pr-ekf3Updates branch May 6, 2017 23:15

		rngBcnFuseDataReportIndex++;
		printf("%6.2f\n",(double)receiverPos.x);

		return false;
		} else if (!_ahrs->get_gps().all_configured()) {

EKF Miscellaneous Improvements and Bug Fixes #5407

EKF Miscellaneous Improvements and Bug Fixes #5407

Conversation

priseborough commented Dec 19, 2016 • edited

Choose a reason for hiding this comment

priseborough Dec 19, 2016 • edited

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 Dec 19, 2016 • edited

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

WickedShell commented Dec 28, 2016 via email

priseborough commented Dec 28, 2016 • edited

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

roger-zhao left a comment

Choose a reason for hiding this comment

priseborough commented Jan 19, 2017

roger-zhao commented Jan 19, 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

priseborough commented Jan 25, 2017

OXINARF commented Jan 25, 2017

priseborough commented Jan 25, 2017 • edited

Choose a reason for hiding this comment

Choose a reason for hiding this comment

OXINARF commented Jan 26, 2017

priseborough commented Jan 27, 2017 • edited

rmackay9 commented Jan 27, 2017

priseborough commented Dec 19, 2016 •

edited

priseborough Dec 19, 2016 •

edited

priseborough Dec 19, 2016 •

edited

priseborough commented Dec 28, 2016 •

edited

priseborough commented Jan 25, 2017 •

edited

priseborough commented Jan 27, 2017 •

edited