Gps noise and delay

[jgoppert]

In the process of debugging this I figured out that all of the timestamps were wrong. We were using the public attribute .nsec directly. But this is just the remainder after the seconds have been removed :-).

[jgoppert]

This currently doesn't work with ekf2 at all, odd. Trying to figure it out.. seems to be timestamp related.

[Dbg] [gazebo_mavlink_interface.cpp:743] hil_state_quat usec: 29696000
[Dbg] [gazebo_mavlink_interface.cpp:535] gps usec: 29698000
[Dbg] [gazebo_mavlink_interface.cpp:701] sensor usec: 29698000

This patch

pxh> gpssim status
INFO  [gpssim] protocol: SIM
INFO  [gpssim] port: , baudrate: 0, status: NOT OK
INFO  [gpssim] sat info: disabled, noise: 0, jamming detected: NO
INFO  [gpssim] position lock: 3D, satellites: 10, last update:   5.0760ms ago
INFO  [gpssim] lat: 473977464, lon: 85455727, alt: 488104
INFO  [gpssim] vel: 0.00m/s, 0.00m/s, 0.00m/s
INFO  [gpssim] eph: 1.00m, epv: 1.00m
INFO  [gpssim] rate publication:	  0.00 Hz
pxh> 

Master

INFO  [gpssim] protocol: SIM
INFO  [gpssim] port: , baudrate: 0, status: NOT OK
INFO  [gpssim] sat info: disabled, noise: 0, jamming detected: NO
INFO  [gpssim] position lock: 3D, satellites: 10, last update: 157.7780ms ago
INFO  [gpssim] lat: 473977418, lon: 85455938, alt: 488104
INFO  [gpssim] vel: 0.00m/s, 0.00m/s, 0.00m/s
INFO  [gpssim] eph: 1.00m, epv: 1.00m
INFO  [gpssim] rate publication:	  0.00 Hz
pxh> 

This look correct to me, @julianoes @Tumbili @priseborough have any ideas. Works fine with LPE and IEKF.

[jgoppert]

We probably should add a gps bias state, just modeling as white noise with std. dev. of 1m doesn't look too realistic. Both LPE and IEKF were able to complete the mission. LPE accounts for delay, but not as much GPS alt noise as I was pumping in. IEKF doesn't account for delay yet but still did pretty well.

IEKF results:

http://logs.px4.io/plot_app?log=b3dc79d6-6c5a-4178-a459-fbc8291d62b8

[nicolaerosia]

Hi, maybe it would be a good idea to use Gazebo GPS Plugin?
I tried working on this here #19 but I did not have the time to complete it.

[jgoppert]

@nicolaerosia I took a look, the rotors/gazebo gps model is very low fidelity, I've already surpassed it here. They have a white noise model with no delay. Really the signal is correlated with a time constant of about 1-10 min and has random walk of amplitude around 1-2 m with low regular noise density on the measurement. See the issue here about the gazebo gps model that rotors is inheriting from. https://bitbucket.org/osrf/gazebo/issues/1285/improve-sensor-noise-modeling.

I think we just need a nice long ulog log of gps data for around 1-3 hrs of a pixhawk outside with GPS plugged in, not moving. Then I can run my analysis script to identify the noise coefficients for the random walk as I did here: https://github.com/jgoppert/iekf_analysis/blob/master/Pixhawk%20Noise%20Identification.ipynb

@bkueng @LorenzMeier @mhkabir do you guys happen to have such a data set handy?

[jgoppert]

I added gps noise random walk now., looks reasonable, waiting for data for exact numbers:

http://logs.px4.io/plot_app?log=3352e517-c676-4aa7-b686-452e2556b8eb

@bkueng Really need ground truth plotting on flight review now.

[LorenzMeier]

@jgoppert Could you compare to current state and see if we should rebase and bring this in?

[TSC21]

@LorenzMeier I rebased this on master. Still have to do some style corrections here and there and still testing remaining.

[TSC21]

Everything now looks good. @jgoppert @LorenzMeier FYI:

Log: https://review.px4.io/plot_app?log=65d467fc-3473-4c6a-9bb2-1e03f52d694f

[LorenzMeier]

Needs to be rebased, but otherwise looks like its good to go.

[TSC21]

Now with the appropriate delay https://review.px4.io/plot_app?log=984d6d45-fa4c-4fd4-9e5c-ca78bcb9f4d3

[LorenzMeier]

Delay should be 120 ms. Have you set that?

[TSC21]

Delay should be 120 ms. Have you set that?

No actually I have at 300 ms. Let me fix that

[LorenzMeier]

Could you please do the same mission with current sitl_gazebo master? I'd like to understand if EKF2 is also way off there.

[TSC21]

Could you please do the same mission with current sitl_gazebo master? I'd like to understand if EKF2 is also way off there.

Sure.

[TSC21]

current PR with 120 ms delay: https://review.px4.io/plot_app?log=cbf926ad-a28d-4d68-93af-fda9afd99d0a
master: https://logs.px4.io/plot_app?log=52a21ebc-bee5-418e-91ca-fbec161b4385
The estimation looks pretty the same to me. Now I feel strange that the GPS projected is almost as the same as the ground truth. Probably we could increase the noise a little?

[TSC21]

with the random walk quadrupled:

static constexpr double gps_xy_random_walk = 8.0; // (m/s) / sqrt(hz)
static constexpr double gps_z_random_walk = 16.0; // (m/s) / sqrt(hz)

https://logs.px4.io/plot_app?log=13181289-e8bb-4f31-adff-e0dcdb5f55bf

[TSC21]

instead of increasing the random walk, multiply the noise by a thousand factor:

static constexpr double gps_xy_noise_density = 2e-3; // (m) / sqrt(hz)
static constexpr double gps_z_noise_density = 4e-3; // (m) / sqrt(hz)

https://logs.px4.io/plot_app?log=d81d19bb-0e59-49d2-ad2e-7e31e843dda5

[LorenzMeier]

Can you trace how the ground truth is being generated? It looks as if it has the noise on it as well.

[TSC21]

@LorenzMeier I think you are right: https://github.com/PX4/sitl_gazebo/blob/master/src/gazebo_mavlink_interface.cpp#L750-L751, and they are updated with the noise here https://github.com/PX4/sitl_gazebo/blob/master/src/gazebo_mavlink_interface.cpp#L772-L773. Let me try do this differently.

[LorenzMeier]

Cool, looking forward to it!

[TSC21]

@LorenzMeier the groundtruth LLA is now propagated in a specific topic at full rate and that data also doesn't have noise associated. Here's the log: https://logs.px4.io/plot_app?log=6957dd2f-533f-4f69-8a27-2b9abe6fabb8

[LorenzMeier]

I think this is now good to go when it passes CI.

[TSC21]

I think this is now good to go when it passes CI.

Yeah just doing some style cleanup.

[TSC21]

@LorenzMeier CI passed. Good to go now.

[LorenzMeier]

Can you rebase to resolve conflicts?

[TSC21]

Rebased in #155.