AP_NavEKF3: Extended Kalman Filter State Estimator

NavEKF3 (Extended Kalman Filter, version 3) is the primary navigation filter in ArduPilot. It fuses data from the IMU, GPS, barometer, compass, airspeed sensor, optical flow, rangefinder, and external navigation systems to produce continuous estimates of vehicle attitude, velocity, position, wind velocity, and magnetic field. All navigation modes — position hold, waypoint following, auto-landing — ultimately depend on the outputs of this filter. The older NavEKF2 remains available as a fallback but NavEKF3 is enabled by default.

What the EKF estimates

EKF3 maintains a 24-element state vector updated at approximately 83 Hz (every EKF_TARGET_DT = 0.012 s). The states cover:

Attitude

Vehicle orientation as a quaternion, reported as Euler angles (roll, pitch, yaw) or a body-to-NED rotation matrix.

Velocity

3-D velocity in the NED (North-East-Down) frame in m/s, fused from GPS, optical flow, wheel odometry, and IMU dead reckoning.

Position

Horizontal NE position relative to the EKF origin (metres) and vertical D position (down, positive below origin).

Gyro and accel bias

Per-IMU gyroscope bias in rad/s and accelerometer bias in m/s², estimated continuously and used to correct raw IMU readings.

Wind velocity

Horizontal wind speed and direction in NED frame, estimated from airspeed and GPS velocity differences.

Magnetic field

Earth-frame and body-frame magnetic field vectors in milligauss, used for yaw estimation and in-flight compass calibration.

EKF3 vs EKF2

NavEKF3 supersedes NavEKF2 and should be used for all new vehicles. Key differences:

Feature	EKF2	EKF3
Number of IMU lanes	1	Up to `MAX_EKF_CORES` (controlled by `EK3_IMU_MASK`)
Lane switching	No	Yes — automatic and manual
Source sets	No	Yes — up to 3 switchable source sets
GSF yaw estimator	No	Yes — independent yaw fallback
Drag model	No	Yes — for multirotors with `EK3_DRAG_*`
Default	Previously default	Primary default since ArduPilot 4.1

Enable EKF3 by setting EK3_ENABLE 1 (default). Disable EKF2 by setting EK2_ENABLE 0 to save memory on constrained hardware.

Sensor fusion inputs

EKF3 accepts data from a wide range of sensors. The active source for horizontal position, velocity, and yaw is configured through the source set system (EK3_SRC*_* parameters).

GPS / GNSS

The primary source of absolute horizontal position and velocity. EKF3 performs pre-flight GPS quality checks (satellite count, HDOP, speed error, drift) before fusing GPS data. The checks can be selectively bypassed with the EK3_GPS_CHECK bitmask.

Barometer

The default source for vertical position (altitude). The EK3_ALT_SOURCE parameter selects alternatives such as rangefinder or GPS altitude. Baro noise is modelled by EK3_ALT_M_NSE.

Compass / magnetometer

Used for yaw estimation. EKF3 supports full 3-axis magnetometer fusion, simple heading fusion (EK3_MAG_MASK), and automatic in-flight calibration. The filter detects magnetic anomalies and resets yaw using the EKFGSF estimator when needed.

Optical flow

Fuses 2-D angular flow rates from a downward-facing optical flow sensor. Used for low-altitude position hold without GPS. Enable with EK3_FLOW_USE. The filter compensates for vehicle angular rates using the IMU gyro.

Rangefinder

Used for low-altitude height above ground. When enabled by EK3_RNG_USE_HGT, the filter blends from baro to rangefinder below a configurable height threshold and speed limit.

External navigation (visual odometry, mocap)

Airspeed

Fuses equivalent airspeed from a pitot-static sensor. Used on fixed-wing vehicles to improve wind estimation and to prevent large position errors when GPS is temporarily lost.

Key output methods

All outputs are accessed through the NavEKF3 instance held by AP_AHRS. In most ArduPilot code you interact with EKF3 through AP::ahrs(), but the methods below are available directly on the NavEKF3 object when lower-level access is needed.

getRotationBodyToNED() — attitude matrix

void getRotationBodyToNED(Matrix3f &mat) const;

Returns the 3×3 direction cosine matrix (DCM) that rotates vectors from the body frame to the NED earth frame. Used by the attitude controller to compute attitude error.

getEulerAngles() — roll, pitch, yaw

void getEulerAngles(Vector3f &eulers) const;

Returns roll, pitch, and yaw in radians. Yaw is measured clockwise from true north. For flight control code, prefer getRotationBodyToNED() to avoid singularities at ±90° pitch.

getPosNE() and getPosD() — NED position

bool getPosNE(Vector2p &posNE) const;
bool getPosD(postype_t &posD) const;

Returns horizontal (North, East) and vertical (Down) position in metres relative to the EKF origin. Returns false if the solution is not valid enough for flight control — always check the return value.

getVelNED() — NED velocity

void getVelNED(Vector3f &vel) const;

Returns 3-D velocity in m/s in the NED frame. The Down component is positive downward; negate it to obtain a climb rate.

getWind() — wind velocity

bool getWind(Vector3f &wind) const;

Returns the estimated horizontal wind velocity in m/s (NED frame). Returns false if wind estimation is not active (requires airspeed sensor or sideslip fusion).

getMagNED() and getMagXYZ() — magnetic field

void getMagNED(Vector3f &magNED) const;
void getMagXYZ(Vector3f &magXYZ) const;

Returns the estimated earth-frame (getMagNED) and body-frame (getMagXYZ) magnetic field vectors in units of milligauss / 1000. Used for in-flight compass calibration feedback.

getGyroBias() and getAccelBias()

void getGyroBias(int8_t instance, Vector3f &gyroBias) const;
void getAccelBias(int8_t instance, Vector3f &accelBias) const;

Returns the estimated gyro bias (rad/s) and accelerometer bias (m/s²) for the specified EKF core instance. Pass -1 for the primary core. These values are logged and can be used to diagnose IMU quality.

getInnovations() — sensor residuals

bool getInnovations(Vector3f &velInnov, Vector3f &posInnov,
                    Vector3f &magInnov, float &tasInnov, float &yawInnov) const;

Returns the current innovation (measurement residual) for velocity, position, magnetometer, airspeed, and yaw observations. Small, zero-mean innovations indicate good sensor agreement; large or persistent innovations indicate a sensor fault.

getVariances() — innovation consistency ratios

bool getVariances(float &velVar, float &posVar, float &hgtVar,
                  Vector3f &magVar, float &tasVar, Vector2f &offset) const;

Returns normalised innovation variances for each fusion source. Values significantly greater than 1.0 indicate that a sensor’s readings are inconsistent with the filter prediction (innovation test failure).

healthy() and getFilterStatus()

bool healthy(void) const;
void getFilterStatus(nav_filter_status &status) const;

healthy() returns a consolidated pass/fail for the primary core. getFilterStatus() populates a detailed bitmask that reports individual flags such as attitude initialised, using GPS, velocity drift, etc. This bitmask is sent to the GCS as EKF_STATUS_REPORT.

Lane switching

EKF3 can run multiple independent prediction cores simultaneously, each using a different IMU (controlled by EK3_IMU_MASK). This is called a “lane”. Each lane accumulates a relative error score based on its innovation consistency. When a lane’s score becomes significantly better than the current primary, the filter automatically switches lanes.

// Check which core is currently primary
int8_t primary_core = EKF3.getPrimaryCoreIndex();

// Manually trigger a lane switch check (called by vehicle failsafe logic)
EKF3.checkLaneSwitch();

// Force switch to a specific lane
EKF3.switchLane(new_lane_index);

Lane switching is transparent to consumers of the EKF output. Position, velocity, and attitude jumps at switch time are tracked and compensated by AP_AHRS so that vehicle position does not suddenly change from the flight controller’s perspective.

Setting EK3_OPTIONS bit 1 (ManualLaneSwitch) disables automatic switching and requires explicit commands — useful for testing individual IMU performance.

Innovation testing and failsafe integration

EKF3 performs chi-squared innovation consistency checks on every sensor measurement before fusing it. The test gate widths (in standard deviations) are tunable:

Parameter	Sensor
`EK3_VEL_I_GATE`	GPS velocity
`EK3_POS_I_GATE`	GPS position
`EK3_HGT_I_GATE`	Barometric altitude
`EK3_MAG_I_GATE`	Magnetometer
`EK3_TAS_I_GATE`	True airspeed

When a sensor fails its innovation test consistently for longer than the retry timeout, EKF3 declares it failed and stops fusing it. The vehicle’s failsafe system monitors getFilterStatus() and healthy() to trigger RTL, land, or other protective actions when the navigation solution degrades.

EKFGSF yaw estimator

EKF3 runs a companion Gaussian Sum Filter (GSF) yaw estimator on each active lane. The GSF uses GPS velocity and IMU data to maintain a yaw estimate that is independent of the compass. If compass-based yaw becomes unreliable (large innovations, magnetic anomaly), EKF3 can reset its yaw to the GSF estimate:

// Request a yaw reset to the GSF estimate
EKF3.requestYawReset();

// Check if yaw is aligned
bool aligned = EKF3.yawAlignmentComplete();

The number of allowed GSF-triggered resets per flight is limited by EK3_GSF_RST_MAX.

Usage example

In ArduPilot code, EKF3 is almost always accessed through AP_AHRS rather than directly. The following example shows how to read navigation state safely:

#include <AP_AHRS/AP_AHRS.h>

void example_ekf_read()
{
    AP_AHRS &ahrs = AP::ahrs();

    // Attitude — Euler angles in radians
    float roll, pitch, yaw;
    ahrs.get_roll_pitch_yaw(roll, pitch, yaw);

    // Attitude — rotation matrix (body to NED)
    Matrix3f dcm;
    ahrs.get_rotation_body_to_ned(dcm);

    // NED velocity in m/s
    Vector3f vel_ned;
    ahrs.get_velocity_NED(vel_ned);
    float climb_rate = -vel_ned.z; // positive = climbing

    // Location (lat/lng/alt)
    Location loc;
    if (ahrs.get_location(loc)) {
        // location is valid
    }

    // Wind estimate
    Vector3f wind;
    ahrs.wind_estimate(wind);

    // EKF health
    if (!ahrs.healthy()) {
        // Do not use navigation for flight control
    }
}

Symbolic derivation

The EKF equations were derived symbolically in MATLAB and the derivation scripts are stored in:

libraries/AP_NavEKF3/derivation/

This directory contains the Symforce/SymPy scripts that generate the C++ covariance prediction and measurement fusion code. Modifying the filter equations requires regenerating the C++ output from these scripts.

Key parameters

Parameter	Description
`EK3_ENABLE`	Enable EKF3 (1 = enabled, default)
`EK3_IMU_MASK`	Bitmask of IMUs to run EKF3 lanes on
`EK3_ALT_SOURCE`	Altitude measurement source (0 = baro, 1 = rangefinder, 2 = GPS, 3 = beacon)
`EK3_GPS_TYPE`	GPS usage (0 = auto, 1 = no GPS, 3 = no vel)
`EK3_GPS_CHECK`	Pre-arm GPS check bitmask
`EK3_MAG_CAL`	In-flight magnetometer calibration mode
`EK3_MAG_MASK`	Bitmask forcing simple compass fusion per lane
`EK3_FLOW_USE`	Optical flow fusion mode (0 = off, 1 = nav, 2 = terrain)
`EK3_RNG_USE_HGT`	Rangefinder altitude use threshold (% of max range)
`EK3_GSF_RUN_MASK`	Bitmask enabling the GSF yaw estimator per lane
`EK3_OPTIONS`	Processing option flags (jamming mode, manual lane switch, etc.)
`EK3_PRIMARY`	Initial primary core index at startup

Sensors & Navigation

Control & Planning

Communication & Peripherals

AP_NavEKF3: Extended Kalman Filter State Estimator

What the EKF estimates

Attitude

Velocity

Position

Gyro and accel bias

Wind velocity

Magnetic field

EKF3 vs EKF2

Sensor fusion inputs

Key output methods

Lane switching

Innovation testing and failsafe integration

EKFGSF yaw estimator

Usage example

Symbolic derivation

Key parameters

Build docs developers (and LLMs) love

Sensors & Navigation

Control & Planning

Communication & Peripherals

Documentation Index

​What the EKF estimates

Attitude

Velocity

Position

Gyro and accel bias

Wind velocity

Magnetic field

​EKF3 vs EKF2

​Sensor fusion inputs

​Key output methods

​Lane switching

​Innovation testing and failsafe integration

​EKFGSF yaw estimator

​Usage example

​Symbolic derivation

​Key parameters

Build docs developers (and LLMs) love

What the EKF estimates

EKF3 vs EKF2

Sensor fusion inputs

Key output methods

Lane switching

Innovation testing and failsafe integration

EKFGSF yaw estimator

Usage example

Symbolic derivation

Key parameters