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 -
-
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)
-
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 -
https://ardupilot.org/dev/docs/ek3-affinity-lane-switching.html
And Test Logs -
https://drive.google.com/drive/folders/18QE6SW5YkPaQ9a1JJhZDb1iTu7Ph4Zh0?usp=sharing
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
