EKF development - Status Report

[Paul Riseborough]

This is a heads up to the community about ongoing development that is aimed at improving navigation accuracy and robustness

Development of the estimation software  has been a long and difficult process due to the wide range of use cases and sensor quality/user setups. What has been developed, starting with AP_NavEKF and then moving on to AP_NavEKF2 is a general purpose estimator. This inevitably means compromises have had to be made, which means that performance for some applications will be inferior to special purpose algorithms. The use cases that have driven development have been full inertial navigation with Barometer, GPS, and magnetometer with the ability to incorporate additional sensors such as range finders, optical flow and airspeed. Operation without GPS or optical flow to provide only an attitude and height estimate has never been a priority for development, and has only been developed to the extent required to provide a reversion mode. 

When we moved from the AP_NavEKF to the AP_NavEKF2 library, a number of changes to the underlying algorithms were made in an effort to improve robustness to bas sensor data, particularly the IMU which is much harder to recover from compared to other errors. An effort was also made to enable alignment when the vehicle was on a moving platform or being carried as a slung load which can generate large amplitude angular and translational movement.

1. Dual EKF instances with each instance using a different IMU.
   
2. Use of data buffers and delayed EKF time horizon to compensate for different sensor delays. The EKF runs 250 msec behind current time. This means that when measurements are fused, the measurements, EKF states and covariances all have consistent time stamps. The old EKF used the states from the measurement time (delayed) and the covariance matrix from the current time which made the calculation less accurate and stable for some measurements.
   
3. Use of an output predictor to predict the EKF states forward from the fusion time horizon to the current time. This also filters out the small steps in position, velocity and attitude that occur when measurements, eg GPS, are fused.
   
4. Attitude (angular orientation) in the filter states was represented as a 3D vector representing the growth in error since the last update, instead of using the quaternion states form the INS directly. This had the benefit that the EKF could self align from any orientation regardless of motion. This was also great for recovering from very bad in-flight data. Unfortunately it came at a cost in that if the yaw rate is high enough, the roll and pitch estimate suffers from temporary loss of accuracy. 
   
5. Estimation of gyro scale factors was added to try and improve accuracy during high angular rate manoeuvres. In practice this was of limited benefit, the problem being that the scale factor is slow to learn and appeared to vary with temperature.
   

After some early problems with tuning AP_NavEKF2 is now providing a robust solution for a wide range of platforms, but for enterprise use with good quality setups, and with the increased availability of higher precision position solutions, eg RTK GPS, I started investigating an alternative mathematical formulation which provided increased accuracy, but at the expense of being able to bootstrap with arbitrary movement. However the revised formulation is still capable of aligning from boats, so boat mode has not been affected. The effect on some of the more extreme slung load startup scenarios has not been assessed.

This formulation, which is also implemented in the PX4/ecl library, has a number of key differences to the Ardupilot EKF2:

1. Direct estimation of quaternions states instead of the attitude error to avoid the roll/pitch errors produced by yaw transients.
   
2. Removal of the gyro scale factor state estimation.
   
3. Addition of 3-axis accelerometer bias state estimation.

This has now been implemented in Ardupilot in a AP_NavEKF3 library and pushed to a branch that enables the three EKF estimators EKF (AP_NavEKF), EKF2 (AP_NavEKF2) and the new formulation EKF3 (AP_NavEKF3) to be compared by changing parameters and rebooting between tests. Here is the branch:

https://github.com/priseborough/ardupilot/commits/ekf3-wip

A downside is that adding the additional estimator option has pushed the code over the 1MB flash limit due to a hardware bug in the STM32 that affects the older pixhawks. That required modifying the PX4Firmware build configuration to bypass the 1MB limit applied during the build process. Note that if this is loaded onto a pixhawk with the older STM32 processor affected by the hardware bug, then the USB interface will not work for telemetry comms or log download, however the boot-loader will still work, so the usb can still be used to change the firmware.

• https://github.com/priseborough/Firmware/commit/9f94d3fc3f19f84027b05d9e9bcf45bfb8e91122

I have tested the ekf3-wip branch in GPS and optical flow aiding using an IRIS+ quadcopter so it is now at a point where getting some feedback from other testers would be useful. If you want to try EKF3 for yourself, the following parameter changes are required.

1) Set AHRS_EKF_TYPE to the number corresponding to the estimator you wish to test, eg set AHRS_EKF_TYPE = 3

2) Disable the other estimators, eg set EKF_ENABLE = 0 and  EK2_ENABLE = 0

3) Enable the estimator you wish to use, eg set EK3_ENABLE = 1

4) Reboot

The log data is in XKF1, ... , XKF9

[Jesus Alvarez]

Many thanks Paul for the effort.

Is the intention to replace EKF2 with this new implementation in the future? Or will it coexist and be selectable depending on type of UAV usage?

[Tom Pittenger]

Thanks for the report, Paul. Amazing work as usual.

@Jesus 

>> Is the intention to replace EKF2 with this new implementation in the future? Or will it coexist and be selectable depending on type of UAV usage?

>>This has now been implemented in Ardupilot in a AP_NavEKF3 library and pushed to a branch that enables the three EKF estimators EKF (AP_NavEKF), EKF2 (AP_NavEKF2) and the new formulation EKF3 (AP_NavEKF3) to be compared by changing parameters and rebooting between tests

--
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-discuss+unsubscribe@googlegroups.com.
For more options, visit https://groups.google.com/d/optout.

[Paul Riseborough]

At this stage I don't know the answer to that, but we will have a better idea when it gets more testing. I think we should sunset AP_NavEKF which will allow compilation of both AP_NavEKF2 and AP_NavEKF3 under the 1MB flash limit.

[Robert Lefebvre]

Hi Paul,

Thanks for all your hard work on this and the updates.

I have no problem with sunsetting AP_NavEKF after Copter 3.4 is released (or forked).  So far EKF2 is looking pretty good to me and I'm really pushing it.

Rob

--

[Al B]

Is the 1MB flash limit also an issue with the Pixhawk2.1?

[Robert Lefebvre]

No, I don't think so.  Even recently manufactured Pixhawk 1 should be good.  

--

[Tom Pittenger]

All Pixhawk 2.1 are not effected. Also, as Rob stated, newer Pixhawk 1.x are good too. Newer meaning <2.5 years old from 3dr.

[Andrew Tridgell]

Hi Paul,

I did a flight today with EKF, EKF2 and EKF3 all enabled on the Disco,
running the main loop at 100Hz:

http://uav.tridgell.net/Disco/disco-15-ekf3.bin

Seemed to fly well! It also had Ryans new adaptive controllers and the
new servo output code. The Disco really is a nice test platform.

Cheers, Tridge

[Tom Pittenger]

@Tridge Note, the Disco's scheduler rate was just pushed to 200 Hz via https://github.com/ArduPilot/ardupilot/pull/5015/files.

[Mark Whitehorn]

Although the problem was identified in May 2014, I don't think rev3 silicon became widely available till mid 2015.
Of course I wouldn't know whether 3DR had earlier access to it :)

On Saturday, October 22, 2016 at 3:15:17 PM UTC-6, Tom Pittenger wrote:

All Pixhawk 2.1 are not effected. Also, as Rob stated, newer Pixhawk 1.x are good too. Newer meaning <2.5 years old from 3dr.

On Oct 22, 2016 12:19 PM, "Robert Lefebvre" <robert....@gmail.com> wrote:

No, I don't think so.  Even recently manufactured Pixhawk 1 should be good.  

On 22 October 2016 at 12:45, Al B <cagi...@gmail.com> wrote:

Is the 1MB flash limit also an issue with the Pixhawk2.1?

On Thursday, October 20, 2016 at 6:40:12 PM UTC-7, Paul Riseborough wrote:

A downside is that adding the additional estimator option has pushed the code over the 1MB flash limit due to a hardware bug in the STM32 that affects the older pixhawks. That required modifying the PX4Firmware build configuration to bypass the 1MB limit applied during the build process.

--

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.

--
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.

[Tom Pittenger]

mid/late 2015 is around when 3dr stopped making pixhawks but they have made a ton of rev3 Pixhawks.

To unsubscribe from this group and stop receiving emails from it, send an email to drones-discuss+unsubscribe@googlegroups.com.

[Jeff Heiss]

The errata datasheet for the STM32 is DM00068628.pdf.  Which errata in the datasheet is the bug?

 

http://www.st.com/content/ccc/resource/technical/document/errata_sheet/38/e6/37/64/08/38/45/67/DM00068628.pdf/files/DM00068628.pdf/jcr:content/translations/en.DM00068628.pdf

[Alan Carvalho de Assis]

Hi Jeff,

The issue is the PA12 corrupting the flash bank#2, see the discussion here:

https://groups.google.com/forum/#!topic/px4users/lu46IbBg7wA

BR,

Alan

On 10/23/16, Jeff Heiss <jeff....@comcast.net> wrote:
> The errata datasheet for the STM32 is DM00068628.pdf. Which errata in the
> datasheet is the bug?
>
>
>
> http://www.st.com/content/ccc/resource/technical/document/errata_sheet/38/e6/37/64/08/38/45/67/DM00068628.pdf/files/DM00068628.pdf/jcr:content/translations/en.DM00068628.pdf
>
>
>
> From: drones-...@googlegroups.com
> [mailto:drones-...@googlegroups.com] On Behalf Of Mark Whitehorn
> Sent: Sunday, October 23, 2016 10:58 AM
> To: drones-discuss
> Subject: Re: [drones-discuss] Re: EKF development - Status Report
>
>
>
> Although the problem was identified in May 2014, I don't think rev3 silicon
> became widely available till mid 2015.
> Of course I wouldn't know whether 3DR had earlier access to it :)
>
> On Saturday, October 22, 2016 at 3:15:17 PM UTC-6, Tom Pittenger wrote:
>
> All Pixhawk 2.1 are not effected. Also, as Rob stated, newer Pixhawk 1.x are
> good too. Newer meaning <2.5 years old from 3dr.
>
>
>
> On Oct 22, 2016 12:19 PM, "Robert Lefebvre" <robert....@gmail.com

> <javascript:> > wrote:
>
> No, I don't think so. Even recently manufactured Pixhawk 1 should be good.
>
>
>
>
> On 22 October 2016 at 12:45, Al B <cagi...@gmail.com <javascript:> > wrote:
>
> Is the 1MB flash limit also an issue with the Pixhawk2.1

> <http://www.proficnc.com/> ?

>
>
>
> On Thursday, October 20, 2016 at 6:40:12 PM UTC-7, Paul Riseborough wrote:
>
>
>
> A downside is that adding the additional estimator option has pushed the
> code over the 1MB flash limit due to a hardware bug in the STM32 that
> affects the older pixhawks. That required modifying the PX4Firmware build
> configuration to bypass the 1MB limit applied during the build process.
>
> --
>
> 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 <javascript:> .

> For more options, visit https://groups.google.com/d/optout.
>
>
>
> --
> 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 <javascript:> .