Algorithm Overview

This method operates without requiring magnetometers or external yaw reference sensors, and is designed to automatically recover navigation performance lost to yaw error after takeoff. Running alongside the main navigation extended Kalman filter (EKF), the approach uses an auxiliary multi-hypothesis framework consisting of multiple 3-state EKFs tracking north (N) velocity, east (E) velocity and yaw angle. Individual filter outputs are combined via a Gaussian Sum Filter (GSF) to produce the final fused yaw estimate. Each 3-state EKF consumes IMU and GPS horizontal velocity data (with optional airspeed input), and requires no prior yaw knowledge or magnetometer measurements.

As implemented in the ECL EKF2 library, this algorithm runs separately from the main 24-state EKF, and includes three core components:

• 5 parallel AHRS solutions using complementary filtering: These predict yaw angle as well as forward and right-frame acceleration. Measured or estimated airspeed is used to correct for centripetal acceleration during fixed-wing flight.
• 5 parallel 3-state extended Kalman filters: The state vector for each filter contains north velocity, east velocity, and yaw angle. Initial yaw hypotheses are spaced evenly across the full ±π angular range, and GPS north/east velocity measurements are used as filter observations.
• Gaussian Sum Filter fusion: Weights for each individual EKF are calculated based on the normalized innovation magnitude of GPS velocity measurements, with all weights normalized to sum to 1. The output yaw estimate is the weighted average of all individual EKF yaw estimates.

Analysis of real flight log (ulg) data shows: 1) Vehicle angular velocity is typically logged at 45~50Hz, 2) Vehicle attitude is logged at ~20Hz. Even with this relatively low IMU update rate, the algorithm delivers significant navigation performance gains.

Core Algorithm Intuition

EKF-GSF relies on the key observation that yaw angle error drives predictable changes in observed north-east velocity increments when combined with measured z-axis angular rate increments. Yaw estimation error is therefore directly observable from these three measurements, which eliminates the need for magnetometer input. Convergence speed scales with the magnitude of horizontal velocity changes, meaning the algorithm converges fastest during maneuvering flight.

Initialization and Alignment

EKF-GSF Hypothesis Initialization

The key initialization step is to pre-populate 5 evenly spaced initial yaw hypotheses across the full yaw range, with all models starting at equal weight. All other initialization follows standard EKF practice.

void EKFGSF_yaw::initHypotheses()
{
    fused_yaw = 0.0f;
    vel_fusion_active = false;
    fused_yaw_variance = M_PI * M_PI;
    hypo_weights.fill(1.0f / static_cast<float>(NUM_HYPOTHESES));

    memset(&ekf_hypotheses, 0, sizeof(ekf_hypotheses));
    const float yaw_step = 2.0f * M_PI / static_cast<float>(NUM_HYPOTHESES);
    for (uint8_t hypo_idx = 0; hypo_idx < NUM_HYPOTHESES; hypo_idx++) {
        // Evenly space initial yaw estimates between -π and +π
        ekf_hypotheses[hypo_idx].state(2) = -M_PI + (0.5f * yaw_step) + (static_cast<float>(hypo_idx) * yaw_step);

        // Initialize velocity states from current measured NE velocity
        ekf_hypotheses[hypo_idx].state(0) = curr_vel_NE(0);
        ekf_hypotheses[hypo_idx].state(1) = curr_vel_NE(1);
        ekf_hypotheses[hypo_idx].cov(0,0) = vel_measurement_std * vel_measurement_std;
        ekf_hypotheses[hypo_idx].cov(1,1) = ekf_hypotheses[hypo_idx].cov(0,0);

        // Set initial yaw uncertainty to half the spacing between adjacent hypotheses
        ekf_hypotheses[hypo_idx].cov(2,2) = (0.5f * yaw_step) * (0.5f * yaw_step);
    }
}

AHRS Tilt Alignment

Tilt alignment constructs the rotation matrix directly from accelerometer measurements. Since tilt is identical for all yaw hypotheses, this only needs to be computed once, under two core assumptions:

1. Yaw is initialized to zero for alignment; individual hypothesis yaw angles are updated once velocity fusion starts.
2. The vehicle has zero acceleration during alignment, so all measured acceleration is caused by gravity.

void EKFGSF_yaw::alignTiltFromAccel()
{
    // Get unit vector pointing downward in body frame from delta velocity
    const Vector3f gravity_bf = -_delta_vel.normalized();

    // Calculate north axis unit vector in body frame, orthogonal to gravity
    const Vector3f forward_body_vec(1.0f, 0.0f, 0.0f);
    Vector3f north_bf = forward_body_vec - gravity_bf * (forward_body_vec.dot(gravity_bf));
    north_bf.normalize();

    // Calculate east axis unit vector orthogonal to gravity and north
    const Vector3f east_bf = gravity_bf.cross(north_bf);

    // Build rotation matrix from earth frame to body frame
    Dcmf earth_to_body;
    earth_to_body.setRow(0, north_bf);
    earth_to_body.setRow(1, east_bf);
    earth_to_body.setRow(2, gravity_bf);

    // Assign the common rotation matrix to all hypotheses
    for (uint8_t hypo_idx = 0; hypo_idx < NUM_HYPOTHESES; hypo_idx++) {
        ahrs_hypotheses[hypo_idx].rotation = earth_to_body;
    }
}

After yaw alignment is completed, measurement updates can be performed for each hypothesis individually.