Servers by jDrones

GSoC 20 : EKF3 Affinity and Lane Switching (Merged !)

EKF3 Affinity and Lane Switching

What started as a student proposal earlier in May is now part of ArduPilot:Master and will be released officially with other upcoming features. It has been a remarkable journey to have been able to contribute towards improving the all-important EKF3. This blog summarizes the idea, technical details and development of the EKF3 Affinity and Lane Switching Feature. An effort towards making ArduPilot flight more safer.

The project brings forward 2 major developments - Sensor affinity and Relative Error Accumulation based Lane Switching. Both combined provide an enhanced and robust sensor selection and error handling mechanism which improves vehicle safety in not-so-uncommon failure scenarios.

Sensor Affinity
Affinity is a sensor allotment scheme for Airspeed - Barometer - GPS - Magnetometer sensors. Modern-day vehicles are installed with multiple good quality sensors such as planes with 2 airspeed sensors, copters with 2 GPS sensors. In a multi-sensor system, normally, only one of them which is set as the primary sensor, is used for measurement fusion with all the EKF3 lanes. The others remain redundant for fallback in case of a fault. With affinity, EKF3 lanes can use non-primary instances of these sensors. This is a statistically robust way of using multiple sensors. An EK3_AFFINITY bitmask parameter is available to select the sensors you want to enable Affinity for. With lane switching, we leverage affinity to form a better sensor selection system as discussed below.

Relative Error Accumulation and Lane Switching
An EKF Error Score is calculated for all active lanes which is a measure of fusion performance of GPS, Barometer, Airspeed and Magnetometer. A high error score indicates larger errors in that EKF lane. This error score is used by the EKF front-end to determine which lane should be the primary. Through this project, we have improved the handling of the error score for more efficient lane switching by accumulating error scores for lanes with respect to the primary lane. On initialization, lane 0 is always the primary lane. After arming the lane selection logic is available.

A relative error score is maintained for all alternative lanes which is simply the difference between its error score and the primary lane’s error score. The relative error score for the primary lane is always 0. A positive difference means the alternative lane is performing ‘worse’ than the active primary, hence should not be switched to. However, a negative difference means the alternative lane is performing ‘better’ than the active primary, and can be switched to. This difference is ‘accumulated’ with every EKF update cycle.

There are 2 important thresholds that actually control when to change the relative error scores and when to switch -

  1. EK3_ERR_THRESH - This parameter controls how much of an error score difference is considered to be significant to reduce the relative error of a lane. Note, this threshold is only for reducing the relative error score and not increasing. This is because of the ‘integrating’ nature of the relative error accumulation, we do not want insignificant differences to add up and trigger frequent lane switches. (Default value = 0.2)

  2. BETTER_THRESH - This is a defined threshold on the relative error score below which we can say an alternative lane is significantly better than the primary lane and we can switch to it, even if the active primary lane is healthy. (Default value = -0.5)

An example to visualize the above, with default settings for all parameters. Assume a vehicle with 4 EK3 lanes and starting with a 0 relative error score -

Lane Number 0 (primary lane) 1 2 3
Error Score 1.0 (Some sensor is showing high innovations for this lane) 0.9 (Slightly better than the primary lane) 1.4 (Worse than the primary lane) 0.1 (A good lane)
Relative Error Score 0 (Rel. Error is 0 for primary lane) 0 (0.1 error difference does not cross the EK3_ERR_THRESH) 0.4 (1.4 - 1.0) -0.9 (We will switch to this lane as the new primary from next update cycle)

Putting it together

Affinity with lane switching is a robust feature that provides a sensor selection mechanism that is statistically consistent and takes into account errors from Airspeed, GPS, Barometer and Magnetometer as well. Also the accumulation of the error score allows for identifying consistently better performing lanes and be able to switch to them in case a persistent error develops on any of the primary lane’s sensors. The thresholds serve as a check to avoid switching for transient, insignificant errors. Some scenarios where this can really be helpful are -

  • A plane with 2 airspeed sensors flying in the rain, and a water droplet clogs the primary lane’s airspeed sensor. When throttling speed, no change in pressure due to the clog could cause over-speeding or stalling of the vehicle. Second airspeed sensor is working fine.
  • A copter with 3 magnetometers, flying in an area with high tension wires which disturbs the offset of, unluckily, only the primary magnetometer, and creates vehicle orientation errors. Other 2 magnetometers are working fine.
  • A vehicle with 2 GPS sensors, where the primary GPS gets subjected to interference from RF emissions from nearby bands, causing spurious position measurements while the second is GPS working fine.

All the above scenarios can result in potential crashes, but can be saved by switching to the ‘right’ sensor in-time. EKF3 Affinity and Lane-Switching has been designed keeping in mind to improve handling of these situations. These cases are simulated and tested in SITL and inferences can be found in the PR linked below.

This entire project has been reviewed and merged with this PR -

Key Highlights -

  • Relative Error Accumulation and Lane Switching (as discussed above)
  • Sensor Affinity (as discussed above)
  • Logging for error scores and sensor indices used by lanes
  • Configure AHRS to use the airspeed sensor instance being used by the primary lane
  • Some GPS, Airspeed, and Barometer SITL parameters for testing
  • An autotest for Affinity and Lane Switching for all relevant sensors

Documentation available here -

And Test Logs -

In the end, I would like to extend my sincere thanks and gratitude towards my mentors for this project @Tridge and @Priseborough. Truly, it has been a privilege to have had your guidance. Also a shout out to @PeterBarker @PPoirier @rmackay9 @Khancyr @iampete for helping me out with reviews, technical help and adding finesse to my work. This project has indeed been a collective effort of a humble and passionate community of world-class developers. This summer has been an intellectual feast and I hope to continue working to bring more improvements to the EKF in the near future.

“Now this is not the end. It is not even the beginning of the end. But it is, perhaps, the end of the beginning.” - Winston Churchill


great write up! Many thanks for your work on this project, this feature is going to save many aircraft and gives us a great foundation for really robust failover. Thank you!


Thanks to you as well for mentoring me on this. I have learnt a lot from you this summer !

1 Like

It’s a bit late, but revisiting this post I can’t resist and have to add an additional kudos to you not only for your great work and the post, but for the top picture! Miss EKF! :grinning: :smiley: :smile:

1 Like

Very excited to try it with dual CAN GPS. What are the chances we could get flow sensor affinity as well? Would it make sense to enable it for compasses?

I have uploaded plane to this thrust vectored belly sitter

And I think I got a bad result.

From time to time the neutral position of tilt motors drift upward and the neutral position of control surface drift downward. This is very bad as the plane become unstable in hover. C1 and C4 are control surfaces, C2 and C3 tilt motors. To explain what I mean:

This is the beginning of the flight, hover mode, wing almost vertical, RCin C2 neutral, control surfaces and tilt motors are close to neutral position, the balance is good and PIQP_I stay around 0. Everything is fine.

But from time to time PIQP_I built, balance is broken and instability come. I have reviewed more than 20 logs and found this problem happen only when flying Qhover or qstabilise and only with plane 4.1.
The starting point of I building is always a EKF3 lane switch message. When I look at tilt motors output (C2-C3) this message is always immediately followed by a sudden jump of 100 to 300ms but no (immediate) jump of control surfaces (c1 and c4). The tilt motor shift is kept until a mode change to FW or manual. So control surfaces have to move a lot to balance the new trim C2-C3 position and this is why from time to time my wing becomes out of trim and fly badly. This is something I can see on the logs but also visually observed several time since 4.1.

Since that problem I came back to 4.0.7 and my TVBS fly well.

1 Like
Servers by jDrones