Fixed GPS timestamping and EKF time horizon issues #5060

tridge · 2016-10-25T04:57:00Z

This fixes a problem with timing jitter on UARTs leading to higher EKF position innovations. It does 3 things:

adds simulation of UART timing jitter in SITL with SIM_GPS_JITTER
makes the fix time from AP_GPS automatically correct for UART timing jitter
fixes the EKF to propogate GPS data to the correct time horizon to remove IMU/GPS timing jitter

This work was done after 70ms timing jitter was seen in flight logs for a Disco, but it applies (to a smaller degree) to all systems.

WickedShell

I haven't reviewed the SITL part yet, or actually flashed it yet, still has a header problem.

WickedShell · 2016-10-25T05:30:23Z

libraries/AP_GPS/AP_GPS_UBLOX.cpp

@@ -23,6 +23,7 @@
 #include <AP_HAL/Util.h>
 #include <DataFlash/DataFlash.h>
 #include <GCS_MAVLink/GCS.h>
+#include <stdio.h>


we used to avoid adding stdio.h as it broke on AVR, but now it is fine on all platforms, and it makes adding debug code easier

Mostly pointed it out, as I don't actually see any new code that uses anything from it, not that worried either way though.

WickedShell · 2016-10-25T05:30:29Z

libraries/AP_GPS/AP_GPS.cpp

+ */
+void AP_GPS::estimate_fix_time(uint8_t instance, uint32_t now)
+{
+    const uint64_t ms_per_week = 604800000ULL;


I'd prefer to just see this as the math to get the ms_per_week just for auditing reasons, unless someone objects to it.

I don't understand what you mean by that, sorry

In general I'd rather see 7 * 24 * 60 * 60 * 1000 for magic constants like this as it makes it a bit more apparent where the number came from in general (this one is fairly self explanatory and is obviously correct)

There is now AP_MSEC_PER_WEEK in definitions.h

WickedShell · 2016-10-25T05:32:33Z

libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp

+void NavEKF2_core::propogateGpsData(gps_elements &gpsData, uint32_t new_time_ms)
+{
+    float dtime = (new_time_ms - gpsData.time_ms) * 1.0e-3f;
+    gpsData.pos.x += dtime * gpsData.vel.x;


Isn't the EKF already accounting for this for you when it rewinds the data? As I understand it this should only be needed if the GPS data is more delayed then the IMU history. (Highly possible I'm missing something about the EKF here)

the EKF wasn't coping with it fully. I spent a lot of time discussing this with Paul.

The EKF aligns the data to within the granularity of the prediction/update step which is every 10msec for copter and every 20msec for plane. This still leaves some residual jitter which this change compensates for.

libraries/AP_HAL_SITL/UARTDriver.h

magicrub · 2016-10-25T23:13:16Z

Ahh, didn't notice that was a SITL file.

On Oct 25, 2016 3:56 PM, "Andrew Tridgell" notifications@github.com wrote:

@tridge commented on this pull request.

In libraries/AP_HAL_SITL/UARTDriver.h
#5060:

@@ -75,6 +75,12 @@ class HALSITL::UARTDriver : public AP_HAL::UARTDriver {
const char *_uart_path;
uint32_t _uart_baudrate;
struct {
   bool enabled;
   uint64_t delay_till_us;
   uint16_t promised;
} _jitter;
in SITL all vars are aligned and packing is really not relevant

—
You are receiving this because you commented.
Reply to this email directly, view it on GitHub
#5060, or mute the thread
https://github.com/notifications/unsubscribe-auth/AEj7GzdPBfeFn8jFTBdpvXXKiQ1t9Qa6ks5q3okwgaJpZM4KfkAJ
.

priseborough

I have reviewed the AP_NavEKF2 changes and they look OK. If the GPS library changes have been reviewed and it has been flown on the Disco, then it should be good to merge.

tridge · 2016-10-26T08:51:06Z

thanks Paul, I haven't flown it yet, although Craig plans to fly it tomorrow. He's using a Trimble GPS and his logs were showing GPS timing jitter of around 50ms in the last flights.

lvale · 2016-10-29T11:43:11Z

@rmackay9 Do we want this on Copter 3.4.1 ??

OXINARF · 2016-11-05T07:16:14Z

It generally looks OK but I have four questions:

wouldn't the offset be better if it was an average? I mean, you can stay with a 4 second offset forever even if it only happened once
the use of last_message_time stays in the EKF code to check if GPS data is current or timed out, but isn't the use of last_fix_time more appropriate for this?
in readGpsData() the prevention for GPS time not being older than IMU was changed to only be done if origin is valid, is this correct? If it is then probably more code could be changed to be under this if
also if the previous point is correct, why not doing the propagate (isn't that spelled wrong in code?) just when doing the recall? When would the propagate in the GPS read be useful?

amilcarlucas · 2017-01-27T01:36:40Z

Any progress on this ?

magicrub · 2017-02-27T03:08:24Z

This needs a rebase

this ensures the time_week_ms stays in sync with the rest of the data

this allows us to simulate the jitter that happens on real UARTs, which affects the GPS data Settable in milliseconds using SIM_GPS_JITTER

this copes with large UART timing jitter without discarding samples by forward propogating GPS position and height to the IMU delayed time horizon.

this removes any remaining timing difference, which will be on the order of 10 to 20ms (depending on loop rate). At typical fixed wing flying speeds this removes about 20cm or so of position jitter, which should be worthwhile for RTK based systems

not needed with new GPS jitter correction

WickedShell · 2017-09-15T07:13:04Z

@tridge is this still on the radar/something you are looking to bring into master?

karthikdesai · 2018-05-24T09:06:29Z

@tridge friendly ping

amilcarlucas · 2019-02-24T21:44:00Z

This is an interesting PR, especially for people that are using MAVROS.
Can I help?

tridge · 2019-07-01T22:32:43Z

I have done some work on this last week, it is a bit complex, so not ready yet

amilcarlucas · 2019-10-21T10:36:40Z

Needs a rebase. Whould be great having this.

davidbuzz · 2021-07-21T23:13:41Z

last update more than 2 years ago, last comment from @tridge was 'not ready yet', so i propose closing it.

amilcarlucas · 2021-07-22T07:39:53Z

Would be great to have for precise navigation :)

tridge assigned WickedShell and priseborough Oct 25, 2016

tridge added this to the Plane v3.8.0 milestone Oct 25, 2016

tridge added EKF GPS labels Oct 25, 2016

WickedShell requested changes Oct 25, 2016

View reviewed changes

magicrub reviewed Oct 25, 2016

View reviewed changes

libraries/AP_HAL_SITL/UARTDriver.h Show resolved Hide resolved

priseborough reviewed Oct 26, 2016

View reviewed changes

tridge added 9 commits June 7, 2017 20:42

AP_GPS: only consider a ublox msg complete when we have a new timestamp

2cd1caa

this ensures the time_week_ms stays in sync with the rest of the data

HAL_SITL: use consistent timestamps in all of GPS emulator

480c43e

SITL: added SIM_GPS_JITTER parameter

cec7602

HAL_SITL: added configurable timing jitter on SITL GPS UARTs

e76f471

this allows us to simulate the jitter that happens on real UARTs, which affects the GPS data Settable in milliseconds using SIM_GPS_JITTER

AP_GPS: use estimated uart delay to correct fix time

984aec5

AP_NavEKF2: use GPS fix time for GPS timestamps

fde69eb

AP_NavEKF2: forward propopgate GPS data on large timing jitter

eff2f98

this copes with large UART timing jitter without discarding samples by forward propogating GPS position and height to the IMU delayed time horizon.

AP_NavEKF2: removed half of sample time adjustment

a9278b9

not needed with new GPS jitter correction

tridge force-pushed the pr-gps-timestamp branch from 824fbe0 to a9278b9 Compare June 7, 2017 10:46

WickedShell modified the milestones: Plane v3.9.0, Plane v3.8.0 Jun 19, 2017

WickedShell added the DevCallTopic label Jun 26, 2019

WickedShell removed the DevCallTopic label Jul 1, 2019

peterbarker added DevCallTopic and removed DevCallTopic labels Dec 4, 2020

amilcarlucas mentioned this pull request Feb 22, 2021

AP_GPS: Fix negative message delta time. #16705

Open

davidbuzz added the ClosePending label Jul 21, 2021

davidbuzz closed this Aug 16, 2021

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Fixed GPS timestamping and EKF time horizon issues #5060

Fixed GPS timestamping and EKF time horizon issues #5060

tridge commented Oct 25, 2016

WickedShell left a comment

WickedShell Oct 25, 2016

tridge Oct 25, 2016

WickedShell Oct 25, 2016

WickedShell Oct 25, 2016

tridge Oct 25, 2016

WickedShell Oct 25, 2016

amilcarlucas May 8, 2017

WickedShell Oct 25, 2016

tridge Oct 25, 2016

priseborough Oct 26, 2016

magicrub commented Oct 25, 2016

@tridge commented on this pull request.

priseborough left a comment

tridge commented Oct 26, 2016

lvale commented Oct 29, 2016 •

edited

Loading

OXINARF commented Nov 5, 2016

amilcarlucas commented Jan 27, 2017

magicrub commented Feb 27, 2017

WickedShell commented Sep 15, 2017

karthikdesai commented May 24, 2018

amilcarlucas commented Feb 24, 2019

tridge commented Jul 1, 2019

amilcarlucas commented Oct 21, 2019

davidbuzz commented Jul 21, 2021

amilcarlucas commented Jul 22, 2021

Fixed GPS timestamping and EKF time horizon issues #5060

Fixed GPS timestamping and EKF time horizon issues #5060

Conversation

tridge commented Oct 25, 2016

WickedShell 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

magicrub commented Oct 25, 2016

@tridge commented on this pull request.

priseborough left a comment

Choose a reason for hiding this comment

tridge commented Oct 26, 2016

lvale commented Oct 29, 2016 • edited Loading

OXINARF commented Nov 5, 2016

amilcarlucas commented Jan 27, 2017

magicrub commented Feb 27, 2017

WickedShell commented Sep 15, 2017

karthikdesai commented May 24, 2018

amilcarlucas commented Feb 24, 2019

tridge commented Jul 1, 2019

amilcarlucas commented Oct 21, 2019

davidbuzz commented Jul 21, 2021

amilcarlucas commented Jul 22, 2021

lvale commented Oct 29, 2016 •

edited

Loading