MAVLINKHUD

The EKF Core: Fusion Architecture

Executive Summary

The Extended Kalman Filter (EKF) is the mathematical brain of the autopilot. Its job is to calculate a single, trusted "Truth" (Attitude, Position, Velocity) by fusing data from noisy, contradictory sensors. It doesn't just average them; it builds a physics-based model of the vehicle and constantly corrects it based on sensor feedback.

Theory & Concepts

1. The State Observer

In control theory, an Observer is a mathematical model that guesses the state of a system (like a drone) based on its inputs (motor commands) and outputs (sensor readings). The EKF is a "Stochastic" observer, meaning it doesn't just guess where the drone is, it also calculates how sure it is about that guess.

2. Sensor Fusion: The "Contradictory" Problem

  • The Gyro is fast but drifts.
  • The GPS is slow but absolute.
  • The Math: The Kalman filter weight (Kalman Gain) acts like a dynamic volume knob. When the drone is moving fast, it turns up the volume on the Gyro. When the drone is hovering, it turns up the volume on the GPS. This "Optimal Weighting" is the secret to ArduPilot's legendary stability.

The State Vector (The "Brain")

ArduPilot's EKF3 tracks 24 Internal States. It isn't just tracking "Where am I?", but "How broken are my sensors?".

  1. Attitude (4): Quaternion defining rotation (Body to Earth).
  2. Velocity (3): North, East, Down velocity.
  3. Position (3): North, East, Down position.
  4. Gyro Bias (3): The constant error drift of the gyroscopes.
  5. Accel Bias (3): The constant error of the accelerometers.
  6. Earth Mag (3): The magnetic field vector of the Earth (Declination/Dip).
  7. Body Mag (3): The magnetic interference of the vehicle itself.
  8. Wind (2): North, East wind velocity (estimated by comparing Airspeed vs Groundspeed).

Control Architecture (Engineer's View)

The EKF runs in a strict Prediction-Correction cycle.

1. Prediction (The Strapdown)

  • Trigger: New IMU data (Gyro/Accel).
  • Rate: Fast (~400Hz or IMU rate).
  • Logic: "If I was at location X and moving at velocity V, and I accelerated by A for 2.5ms, where am I now?"
  • Code Path: UpdateStrapdownEquationsNED().
  • Result: The EKF propagates the state forward in time. This provides the low-latency estimate used for stabilization loops.

2. Correction (The Fusion)

  • Trigger: Slow sensor data arrives (GPS, Baro, Mag).
  • Rate: Variable (GPS @ 5-10Hz, Baro @ 50Hz).
  • Logic: "My Prediction says I'm at X. The GPS says I'm at Y. The difference is the Innovation."
  • Update: The EKF calculates how much to trust the GPS vs its own Prediction (based on covariance). It then "nudges" the states (Position, Velocity, and Biases) to reduce the error.
  • Code Path: UpdateFilter().

Why is this better than Complementary Filters?

A simple filter blends Gyro + Accel. The EKF blends Gyro + Accel + GPS + Mag + Airspeed + Baro + Rangefinder + Optical Flow.
Crucially, it estimates Sensor Biases. If the EKF realizes the GPS is consistently 2m to the right of the prediction, it might realize the Compass is wrong (causing the prediction to drift), or it might realize the GPS has a glitch.

Source Code Reference