vel gate and gps timeout in EKF for fixed wing

[Andrew Tridgell]

Hi Paul,

I've been looking a bit more at the EKF failure logs I showed you
yesterday:

http://uav.tridgell.net/Paul/EKF-fail-2014-12-07/

one of the main issues was the bug that was introduced with the optflow
merge, which has since been fixed:

https://github.com/diydrones/ardupilot/commit/cb9c518ab8f53d658438c3ef8edabdcb8ab2ecbc

but there are other issues that these logs show. I've added some new
features to Replay to help track those down, so I thought I'd send this
to the list so other developers could see the new debugging techniques.

The Replay tool now generates DF logs, as well as consuming them. So you
can run Replay over a log with different EKF or AHRS parameters and see
the resulting flight track. This allows you to use tools like
mavflightview.py to see what impact changing the EKF parameters has.

For example:

cd Tools/Replay
rm -rf logs
make linux -j8
./Replay.elf -- 2014-12-07/107.BIN -pEKF_VEL_GATE=6
mavflightview.py --ekf --ekf-sample=10 logs/1.BIN

what that does is:

1) remove the old logs directory, so the log generates as logs/1.BIN
every time (useful for fast re-graphing)

2) builds the Replay tool

3) runs Replay on the 107.BIN log with EKF_VEL_GATE=6

4) views the resulting DF log, showing the EKF and GPS track, but only
showing every 10th EKF point (to keep the UI fast, as otherwise
there are a huge number of points on the map)

now to look at some results. First here is the output using current
master on the 107.BIN logfile:

http://uav.tridgell.net/Paul/EKF-fail-2014-12-07/107_original.png

you can see that the GPS track (darker orange) and EKF track diverge for
quite a long time.

Now let's run the same log with a larger EKF_VEL_GATE:

http://uav.tridgell.net/Paul/EKF-fail-2014-12-07/107_vel_gate_12.png

so a EKF_VEL_GATE=12 does result in much better tracking. I'm not sure
it is the right approach though. Looking more closely at this log you
can see that it gets a gps velocity timeout. That timeout is 20 seconds,
which is an awfully long time for a fixed wing plane with high vibration
to not be using the GPS velocity. Lowering the _gpsRetryTimeUseTAS from
20000 (20 seconds) to 2000 (2 seconds) while leaving EKF_VEL_GATE=6
gives this:

http://uav.tridgell.net/Paul/EKF-fail-2014-12-07/107_gps_retry_2000.png

I suspect that 20s is just far too long to be trying to dead-reckon the
velocity on a plane with this much vibration without using the GPS.

The same thing happens with log 109:

http://uav.tridgell.net/Paul/EKF-fail-2014-12-07/109_original.png

in that log you can see that the EKF really mucks up the track on
landing. The plane was under DCM control, and did a very nice auto-land
on the runway. The EKF had the plane going over the road.

If we change the gps_retry time to 2000 then it gets this:

http://uav.tridgell.net/Paul/EKF-fail-2014-12-07/109_gps_retry_2000.png

which is much more accurate.

So do we have the GPS retry time set at the right level for fixed wing?
Or is there something else causing this?

Cheers, Tridge

[Paul Riseborough]

Tridge,

 

For a fixed wing with good quality inertial data and using airspeed sensing, early tests showed 20 seconds as a reasonable time for dead reckoning that covered the wort case GPS glitch duration (normally they last up to 10 seconds). With the poor quality inertial data in this log it is clearly too long and should be reduced, but I wouldn't go any lower than 5 seconds or else is it will start interfering with GPS glitch rejection. This is just a band-aid for the root casue which is the mis-match between the quaity of the inertial data and the filters expectation.

 

This can be rectified by increasing the acceleration and angular rate process nosie parameters EKF_ACC_PNOISE and EKF_GYRO_PNOISE. I would start by doubling and then quadrupling EKF_ACC_PNOISE and EKF_GYRO_PNOISE in turn and check the velocity innovations in the EKF3 message. As they are increased, the filter will start to 'trust' the accelerometer or gyro data less and put more weighting on the GPS and be less likely to reject the GPS. The downsides to increasing them too far are:

 

- For EKF_GYRO_PNOISE it will casue the attitude solution to become more reactive to GPS noise and errors, but you can probably safely double it

- for EKF_ACC_PNOISE it will make the velocity and position outputs noisier, which is probably not an issue for your application

 

For planes we can also increase the rate at which the GPS position offset is pulled back to zero from its current default of 1 m/s, which has been set primairly for copters where velocity errors due to GPS gliches are less acceptable. This is set in https://github.com/diydrones/ardupilot/blob/master/libraries/AP_NavEKF/AP_NavEKF.cpp#L4433 

The value of 1 m/s could be increased by up to 5 m/s when assume_zero_sideslip() is true .

 

The rejection gates probably shouldn't be touched, otherwise we run the risk of bad GPS corrupting attitude like can happen with DCM.

 

I may get time this week to try it in the replay for myself, but it will probably have to wait until next week.

 

Regards,

 

Paul

[Paul Riseborough]

I had another quick look and found a merge error which has been fixed here:

https://github.com/priseborough/ardupilot/commit/03fe31792a596785b10870b19eb132bfd2b064dd

which contributes to the bad behaviour.

The velocity measurements are being rejected at a lower innovation magnitude than I would expect given the tuning parameters, so I will check that the scaling used to set the normalised errors hasn't been accidentally changed during the optical flow integration.

-Paul

On Monday, 8 December 2014 13:02:47 UTC+11, Andrew Tridgell wrote:

[Paul Riseborough]

Ignore the merge error report. The merge error was in the ekfOptFlow branch and has been fixed in master. The lower value for the velocity innovation gate being used (2 vs the default of 6) explains the early rejection of velocity measurements.

The changes i have experimented with which increased the robustness/ reduced recovery time from the bad inertial data were:

1) Increasing gyro and accelerometer process noise parameters
2) Increasing the rate at which the GPS glitch offset is pulled back to zero
3) Reducing the period the filter will go free inertial before forcing use of GPS data

I'll create  a patch to implement these with values that handle Tridges logs and then we can check them with flying on setups with different data quality.

I think it is fair to say that plane users are more likely to have poor quality inertial data due to the fact that they have not historically had to worry about vibration until the recent introduction of an inertial navigation option. They are also less likely to be bothered by small position jumps due to GPS glitches than copter users, and fly higher and further away from buildings and other objects that can create multi-path effects. This means we should be able to move to a default EKF tune for plane users that weights inertial data less and GPS data more.

-Paul

[Paul Riseborough]

These proposed changes have been addressed as part of the following EKF patches:

https://github.com/diydrones/ardupilot/pull/1696
https://github.com/diydrones/ardupilot/pull/1697