MAVLINKHUD

EKF Innovations & Lane Switching

Executive Summary

The Extended Kalman Filter (EKF) is the brain of the autopilot. It fuses data from IMUs, GPS, Compass, and Barometer to estimate position and attitude.

Innovation is the difference between what the EKF predicted a sensor would read and what it actually read.

  • Low Innovation: Sensors agree with the model. All good.
  • High Innovation: Something is wrong (GPS glitch, compass interference, vibration).

If innovations get too high on the primary core (Lane), the EKF will switch to a healthy backup lane ("Lane Switch").

Theory & Concepts

1. Innovation vs. Variance

  • Innovation: $Measurement - Prediction$.
  • Test Ratio (Variance): $\frac{Innovation^2}{Uncertainty}$.
    • This normalizes the error. A 1 meter GPS error is huge if accuracy is 0.1m, but fine if accuracy is 5m.
    • ArduPilot logs the Test Ratio (normalized to $<1.0$ is good).

2. EKF Lanes

Modern ArduPilot runs multiple EKF instances ("Lanes") in parallel, typically one per IMU.

  • Primary Lane: The one controlling the vehicle.
  • Lane Switching: If the Primary Lane's "Error Score" exceeds the others, the system swaps control to the better lane.

Codebase Investigation

1. The Switch Logic: checkLaneSwitch()

Located in libraries/AP_NavEKF3/AP_NavEKF3.cpp.

  • It calculates an errorScore for each lane based on velocity, position, and magnetometer innovations.
  • Threshold: It switches if altErrorScore < lowestErrorScore.
  • Hysteresis: It won't switch more than once every 5 seconds (to prevent thrashing).

2. Error Score Calculation

The score is a weighted sum of test ratios:

  • velTestRatio (Velocity)
  • posTestRatio (Position)
  • hgtTestRatio (Height - Baro/GPS)
  • magTestRatio (Compass)

Source Code Reference

Practical Guide: Analyzing Logs

1. Key Messages

  • XKF1: States (Roll, Pitch, Yaw, Velocity).
  • XKF4: Innovations (The most important message for diagnostics).
    • IV: Velocity Innovation (GPS speed vs IMU prediction).
    • IP: Position Innovation.
    • SM: Magnetometer Innovation.
  • XKF3: Lane Selection (C field tells you the active core: 0, 1, or 2).

2. Identifying a "Lane Switch"

  • Look at XKF3.C. If it changes from 0 to 1 mid-flight, a switch occurred.
  • Correlate with XKF4. Did IV or SM spike on Core 0 just before the switch?

3. Common Causes

  • Compass Variance: High SM (Compass Innovation). Usually magnetic interference from motors.
  • Velocity Variance: High IV. Often vibration (aliasing) or bad GPS signal (multipath).