In this article, which is FC13 of the Navigation series, we will cover ArduPilot's EKF (Extended Kalman Filter). While we explained the mathematical theory of EKF in NAV8, this article focuses on ArduPilot's implementation, explaining the EKF operations and design decisions that FC designers and operators should know.
Why ArduPilot needs EKF
The information required for UAV flight control consists of a total of 9 degrees of freedom: attitude (roll, pitch, yaw), position (NED coordinates), and velocity (NED coordinate system). These cannot be obtained directly from a single sensor.
Gyros measure angular velocity but drift due to integration errors. Accelerometers cannot distinguish between gravity and vehicle acceleration. GNSS provides position and velocity, but the update rate is low (10–20 Hz) and accuracy is environment-dependent. Barometers provide an absolute altitude reference but are susceptible to airflow disturbances. Compasses provide an absolute yaw reference but are vulnerable to electromagnetic interference.
EKF integrates these multiple sensors in a statistically optimal way, continuously outputting the best state estimates while accounting for noise and uncertainty. Without EKF, stable flight of modern multicopters would be theoretically impossible.
Evolution from EKF1 to EKF3
ArduPilot's EKF implementation has evolved over three generations.
EKF1 (DCM-based) was the predecessor to EKF, using DCM (Direction Cosine Matrix) for attitude estimation. It processed GPS position and barometer altitude with independent filters. It is no longer in use.
EKF2 was ArduPilot's first full EKF implementation and has a long track record in production. It has a 22-dimensional state vector and integrates GPS, barometer, compass, and optical flow. Its support for GPS-denied environments was limited.
EKF3 is the current standard implementation. It adds a 24-dimensional state vector, redundancy through multiple EKF instances, support for external position sources (VIO, LiDAR SLAM), selective use of GPS velocity/position, and improved support for GPS-denied environments.
From ArduPilot 4.1 onwards, EKF3 is the default, and it can be enabled or disabled via the EK3_ENABLE parameter. This article assumes the use of EKF3.
EKF3 State Vector
We will organize the 24-dimensional state vector estimated by EKF3.
Position (3D) is the coordinate in the NED frame, measured in meters. It is managed as a value converted to local orthogonal GNSS coordinates.
Velocity (3D) is the velocity in the NED frame, measured in m/s. It is estimated from both GPS velocity observations and IMU integration.
Attitude is represented by a quaternion (4D). In the error-state EKF, it is managed as a 3D rotation vector, separated from the nominal quaternion to ensure numerical stability.
Gyro bias (3D) is the steady-state offset of the gyro on each X, Y, and Z axis. The EKF estimates this in real-time during flight, continuously correcting gyro integration errors.
Accelerometer bias (3D) is the accelerometer offset on each X, Y, and Z axis. It directly affects the accuracy of gravity compensation.
Geomagnetic field (NED frame, 3D) is the estimated value of the geomagnetic vector at the current position. It corrects compass calibration errors during flight.
Vehicle magnetic field bias (3D) is the estimated value of the vehicle's inherent residual magnetism. It corrects constant magnetic field interference caused by ESCs and batteries.
Wind speed (horizontal 2D) is the horizontal wind speed estimated by the EKF during flight. Even without an airspeed sensor, it can be estimated from the difference between GPS velocity and aircraft attitude.
Correspondence between sensors and observations
We will organize the observations processed by EKF3 and their data sources.
IMU data is the input for the EKF prediction step (propagation step). Gyro and accelerometer data are processed at 400Hz to 1kHz to perform state propagation and P-matrix updates. The IMU is the EKF's "engine" and is used at the highest frequency.
GPS position and velocity are used for observation updates at 10-20Hz via loose coupling. EKF3 can be configured to trust GPS velocity more than position (EK3_GPS_TYPE=3), and there is also a lightweight mode that uses only velocity.
The barometer is used for vertical position observation updates. ArduPilot defaults to prioritizing the barometer over GPS altitude, and the absolute error of the barometer is corrected by the GPS altitude at the start of the flight.
The compass is used as an absolute reference for yaw. The compass observation update uses a method that compares the entire magnetic field vector to the model, which is more accurate than simple yaw angle comparison.
GPS Yaw determines yaw from the azimuth between two GNSS antennas. It can be used with u-blox F9P-based Moving Baseline functionality or dedicated yaw GNSS, eliminating reliance on the compass.
External navigation (ExternalNav) is position, velocity, and attitude input from VIO or LiDAR SLAM. It is used as a substitute or supplement to GPS via the EK3_SRC setting.
Optical flow is used for observing relative velocity to the ground. It is used for position holding indoors or at low altitudes in combination with a distance sensor (such as LIDAR-Lite).
Redundant EKF and failover
EKF3 has a redundant design that allows multiple instances to operate in parallel.
ArduPilot runs three EKF instances in parallel by default. Each instance uses independent IMU, GPS, and compass data (in combination with sensor multiplexing).
Instance selection is performed based on the health metrics of the EKF's internal state. The health metric is calculated using the normalized innovation squared (NIS) of each observation:
NIS = yᵀ × (HPHᵀ+R)⁻¹ × y
Instances with a large NIS indicate sensor failure or estimation divergence, and instances with a small NIS are prioritized.
Failover is managed by flags displayed in the GCS EKF Status (Mission Planner HUD). Flag bits in EKF_STATUS_REPORT indicate the status of each sensor, independently evaluating the reliability of attitude estimation, velocity estimation, and position estimation.
In flight controllers equipped with multiple IMUs, different IMUs are assigned to each EKF instance (EK3_IMU_MASK), so that if one IMU fails, other instances continue to operate. This is the basis for the redundant design in flight controllers equipped with three IMUs, such as the Cube Orange+.
Observability and initialization
For the EKF to function correctly, each state variable must be observable.
Initialization in a completely stationary state is problematic. When the aircraft is stationary, the velocity is zero, making it impossible to observe wind speed. GPS velocity also consists only of noise near zero. For this reason, initial estimation of certain states (wind speed, aircraft magnetic field bias) is performed through maneuvers after flight begins.
Yaw observability decreases without GPS signals. If GPS is denied when the compass cannot be used (in a strong electromagnetic environment), the uncertainty in yaw estimation increases rapidly. This is why ArduPilot recognizes high uncertainty in yaw estimation during GPS denial and restricts certain modes.
The EKF initialization sequence is executed immediately after ArduPilot starts. It calibrates gyro bias in a stationary state (3-second average), estimates initial attitude (roll/pitch) using accelerometers, and initializes position upon acquiring a GNSS fix. Yaw is determined after compass calibration is complete.
Completion of EKF initialization is a condition for ArduPilot's arming check. If messages like 'PreArm: EKF not started' appear on the GCS, it means initialization is incomplete.
Monitoring EKF Status
We will organize methods for monitoring the state of the EKF during flight.
The Mission Planner flight data screen displays 'EKF Lane,' which shows the currently used EKF instance and any switching that occurs. Frequent switching of instances is an early warning of sensor issues.
The EKF Status Flag uses a bitmask to indicate the health of each subsystem. There are flags for attitude, velocity_horiz (horizontal velocity), velocity_vert (vertical velocity), pos_horiz_rel (relative horizontal position), pos_horiz_abs (absolute horizontal position), pos_vert_abs (absolute vertical position), pos_vert_agl (altitude above ground level), const_pos_mode (position estimation stopped), pred_pos_horiz_rel (predicted horizontal position reliability), and pred_pos_horiz_abs (predicted absolute horizontal position).
ArduPilot log analysis is performed using the 'Review a log' feature in Mission Planner or via PyFlightAnalysis. By looking at NKF5 messages (EKF3 innovation statistics), you can identify which sensor is causing the EKF to become unstable.
Impact of EKF Design on FC Hardware
We will organize how EKF operational requirements affect FC hardware design.
Equip at least two redundant IMUs, or three for industrial applications. Place each IMU on a different SPI bus and design the system so that noise on one bus does not affect multiple EKF instances.
Regarding computational resources, EKF3's 24-dimensional matrix operations are the most computationally intensive. On an STM32F7, a 400Hz EKF loop is near the limit, while an STM32H7 (480MHz, FPU) operates with plenty of headroom.
Regarding memory, EKF3 consumes significant RAM due to multiple instances multiplied by the P matrix (24x24xfloat). Three instances of EKF3 require approximately 70KB of RAM, which is not an issue for the 1MB of RAM on an STM32H7.
Regarding the real-time nature of sensor data, EKF IMU data latency directly affects attitude control precision. Low-latency IMU data acquisition using SPI DMA transfers and DRDY interrupts is a key element of FC design.
Summary
ArduPilot's EKF3 is an error-state EKF that simultaneously estimates attitude, velocity, position, sensor bias, geomagnetism, and wind speed using a 24-dimensional state vector. Parallel operation of multiple instances and NIS-based failover are the foundation of industrial FC reliability. Hardware design that understands sensor observability (redundant IMUs, GPS Yaw support) and tuning of EKF parameters determine the accuracy of the entire system. In the next installment, FC14, we will explain EKF tuning—practical procedures for log analysis and parameter adjustment.