EKF improvements - progress report

[Paul Riseborough]

At the Embedded Linux Conference http://elinux.org/images/1/11/Application_of_Data_Fusion_to_Aerial_Robotics.pdf and https://www.youtube.com/watch?v=Z3Qpi1Rx6HM one of the topics discussed was improvements to the underlying maths used by the EKF to improve performance and robustness. I was finally able to  get some uninterrupted time and  to get an APM prototype running this weekend https://github.com/priseborough/ardupilot/tree/ekfNewMaths-wip

This is very much a quick hack prototype (please no-one fly it yet), but so far it has demonstrated 

1) Ability to self align from any orientation and self calibrate large gyro bias errors whilst moving, making the need for static alignment a thing of the past. 

2) Ability to estimate gyro scale factors given sufficient rotation.

3) Improved stability

4) Reduced computational load compared to the current NavEKF (if used with an output observer)

New design features include:

1) Use of a 3x1 vector to estimate attitude errors instead of estimating the 4x1 quaternion state directly.  This uses the technique outlined in this paper. Mark E. Pittelkau. "Rotation Vector in Attitude Estimation", Journal of Guidance, Control, and Dynamics, Vol. 26, No. 6 (2003),  pp. 855-860. This means the filter is able to converge reliably from any orientation unlike the current EKF, that needs to be reset if the angle errors become too large

2) Addition of states to estimate gyro scale factors

3) More efficient covariance prediction that takes half the time

4) Ability to run in different state configurations from 16 states to 24 states depending on the application. In its 16 state configuration the filter does not estimate magnetic field or wind.

5) The new EKF handles time delays by running the whole filter about 250msec behind time using delayed IMU data. and fusing measurements when the delayed filter time catches up to the time that each measurement was taken. The delayed states (position, velocity and attitude) are then would forward to the current time using either the buffered IMU data (less memory required / more accuracy / more calculations required) or an output observer (more memory required /much less calculations required / less accuracy) http://users.cecs.anu.edu.au/~trumpf/pubs/khosravian_trumpf_mahony_hamel_ACC2015.pdf . 

This significantly improves filter stability as measurements are being fused with consistent state and covariance data.

6) Addition of a simple compass fusion model that doesn't require magnetic field states. This is used for initial alignment and ground operation. It can also be used in flight if robustness is more important than accuracy.

See the following plots of showing what happens to the new EKF and DCM  (logged as AHR2) when the board is started without a gyro calibration whilst being moved around. 

The EKF3.TE is a tilt error convergence metric for the new filter, analagous to the ErrorRP for DCM. 

DCM does well for the first 10 seconds. but as soon as the initial startup gain is relaxed, the bias errors cause it do develop large offsets. 

The new EKF was able to converge in less than 10 seconds after startup and switch to full GPS mode.

The roll gyro error in this instance was almost 6 deg/sec, so a pretty good test. 

This start-up test was conducted indoors with noisy GPS, hence the noise that can be seen on the EKF angles. DCM had not started using GPS before logging was stopped.

Here's the gyro bias estimates in deg/sec

The TO DO list includes:

Airspeed, sideslip and optical flow fusion methods need to be added (currently it can only handle GPS, baro, range finder and magnetoemeter)

Addition of a simple observer to provide a smoothed output that doesn't have the small steps in output at each measurement fusion.

The code that stores and extracts measurements when they fall behind the filter tie horizon is inefficient and needs to be re-worked

It needs to be refactored and pulled into separate files

-Paul

[Jesus Alvarez]

Bravo Paul!

Thanks to the devs because APM is becoming a great autopilot bit by bit and effort by effort.

[OlliW]

Big THANKS especially also for bringing these two references to the attention of the RC community!

I got to know them only thanks to the pdf of your talk, and they're affecting my thinking, thus my sincere thanks! (a bit late maybe, but better too late than never :))
Olli

[Marco Robustini]

Thanks Paul for your progress with EKF!

-Marco

[Luis Vale Gonçalves]

Great work Paul :)

A small question. Given that some GPS units (M8N comes to mind) can optionally do some kind of EKF processing onboard, wouldn't it make sense to take advantage of it ?

From your talk you say that for IMU's a "small" EKF is better, so extending that to the GPS would feed "better" data do the main EKF model?

brgds

Luis

[Paul Riseborough]

Luis,

Yes, UBlox probably do use an EKF or similar type of state estimator to do motion  filtering, but their receivers are not  able to use 3 axis IMU data. 

-Paul 

[Tom Pittenger]

Great work, as usual, Paul. Keep up the good work!

Here are a few more wishlist TODOs:

- rangefinder without flow (stand alone AP_Rangefinder support)

- sensor switch step input handling: thinking GND_ALT_OFFSET and dual GPS and dual airspeed

--
You received this message because you are subscribed to the Google Groups "drones-discuss" group.
To unsubscribe from this group and stop receiving emails from it, send an email to drones-discus...@googlegroups.com.
For more options, visit https://groups.google.com/d/optout.

[Luis Vale Gonçalves]

I've playing with a M8N these days, but could not find any relevant info on how their EKF is "processed".

Second Tom's request for dual GPS support :)

Luis

[Tom Pittenger]

Dual GPS is already supported, it could just use better blending.

[Andrew Tridgell]

Thanks Paul, I'm really glad to see this rework taking shape! It will be
a huge step forward when we get this merged.

> This is very much a quick hack prototype (please no-one fly it yet), but so
> far it has demonstrated

I'd like to echo the "don't fly it yet" recommendation. I get a lot of
floating point exceptions running this against existing logs with
Replay.

Cheers, Tridge

[Tom Coyle]

Hi Paul,

Excellent work and improvements!

Your original implementation of EKF was a quantum leap over DCM in relation to my rovers' navigation performance.

Regards,

Tom C AVD

[Paul Riseborough]

New EKF had its first flight today in an Iris flying in loiter mode for the entire flight. Here are some of the graphs.

Normalised innovations were all low. Magnetometer innovations rise briefly when I fly it past a steel object just before landing.

Position  and height innovations were low all the way through. The weighting on GPS is a bit higher than it needs to be, but I haven't yet tested it in replay with GPS glitching to see how much the weighting on GPS needs to be reduced.

Loiter was smooth with no audible motor pulsing so the output predictor/corrector is doing a good job of smoothing the corrections

The gyro biases converged quickly and were stable

There wasn't enough angular motion in the flight to learn new gyro scale factors, so they did not change. I need to fly it in something capable of aggressive manoeuvres.

It still needs the airspeed and optical flow fusion methods added.

-Paul

[Jonathan Challinger]

It should be able to learn the z axis gyro scale factor if you just hold the yaw stick over, right?

[Paul Riseborough]

Yes, assuming you do it for long enough. There probably isn't enough process noise on the scale factor states at the moment, but when I get a good 400Hz log without time slips, it can be tuned in on replay.

[Aleksandr Buyval]

Hello Paul,

I am very impressed your job. I would like to test new EKF after you add a optical flow.

Now I have some question:

1. As you mentioned, In its 16 state configuration the filter does not estimate magnetic field. Does it mean that the EKF won't use NED frame? If so, what will be initial directions? For my indoor flight I need local 'room' frame.

2. Are you planning to add into the EKF a additional source of data like vicon positions ? In my task I would like to use information about position of visual markers to correct the drone's position. I could help to add it  if you give me a short instruction how is better do it.
3. Can I get linear velocities from new/old EKF in body frame?  

Best Regards,

Alex


воскресенье, 5 июля 2015 г., 15:30:35 UTC+3 пользователь Paul Riseborough написал:

[Paul Riseborough]

Good news! The new EKF has now been pulled into master (a thank you to Andrew Tridgell and Siddharth Purohit for their contributions). It has been flown successfully during development on Plane, Copter and Heli. It has had some testing with optical flow on Copter.

https://github.com/diydrones/ardupilot/tree/master/libraries/AP_NavEKF2

To run the new EKF, you must first turn off terrain as we don't have enough memory to run two EKF's and the terrain library together.

TERRAIN_ENABLE = 0

You then enable the new EKF by setting 

EK2_ENABLE = 1

The flight control loops can use either the legacy EKF or the new EKF. This is controlled by the AHRS_EKF_TYPE parameter.

To use the legacy EKF in the control loops, set AHRS_EKF_TYPE = 1 (default value and recommended until you have verified correct operation using the data logs)

To use the new EKF in the control loops, set AHRS_EKF_TYPE = 2

The flight computer must be rebooted for the parameter changes to take proper effect.

You can compare the performance of the legacy and new EKF by comparing log data. The new EKF has its logs in the NKF1, NKF2, NKF3, NKF4 and NKF5 messages. The legacy EKF data is contained in the EKF1, EKF2, EKF3, EKF4 and EKF5 messages. Here are some suggested data to plot to compare the behaviour of the two filters. 

Roll (deg): EKF1.Roll and NKF1.Roll

Pitch (deg): EKF1.Pitch and NKF1.Pitch

Yaw (deg): EKF1.Yaw and NKF1.Yaw

NE Velocity (m/s): EKF1.VN,VE and NKF1.VN,VE

Down Position (m): EKF1.PD, NKF1.PD

Gyro bias (deg/s): EKF1.GX,GY,GZ and NKF1.GX,GY,GZ

GPS position innovations (m): EKF3.IPN,IPE and NKF3.IPN,IPE

GPS velocity innovations (m/s): EKF3.IVN,IVE and NKF3.IVN,IVE

altimeter innovations (m): EKF3.IPD and NKF3.IPD

Magnetometer innovations: EKF3.IMX,IMY,IMZ and NKF3.IMX,IMY,IMZ

Note that the new EKF starts using GPS when checks pass rather than waiting for the vehicle motors to arm.

The parameters for the new EKF are labelled EK2_* and where the suffix text is the same, they have the same definition as the old EKF parameters, e.g. EK2_ALT_NOISE has the same definition for EKF2 as EKF_ALT_NOISE does for the legacy EKF.

Please be careful - this is new code with limited testing so if you are going to fly with AHRS_EKF_TYPE = 2 make sure you have stabilise mode available if flying a copter or manual mode available for planes.