EKFGSF_yaw.cpp error

[triplez23]

Bug report

Issue details
In the EKFGSF_yaw::update function of libraries/AP_NavEKF/EKFGSF_yaw.cpp, there is a critical logical error in the calculation of the acceleration fusion gain (accel_gain). The code incorrectly uses the output variable accel_gain (historical gain value) instead of the input state variable EKFGSF_ahrs_ng (current acceleration multiple relative to gravity, i.e., "ng") for conditional judgment. This causes the accel_gain calculation to be decoupled from the current acceleration state of the vehicle, leading to incorrect weighting of AHRS tilt angle correction. Ultimately, this results in degraded accuracy or even divergence of the yaw angle estimation (especially for fixed-wing aircraft using EKFGSF yaw estimation without a magnetometer).

Key problematic code lines (in update function):

if (EKFGSF_ahrs_ng > 1.0f) {
    if (is_positive(true_airspeed)) {
        accel_gain = EKFGSF_tiltGain * sq(2.0f - EKFGSF_ahrs_ng);
    } else if (accel_gain <= 1.5f) { // ❌ Wrong: should use EKFGSF_ahrs_ng instead of accel_gain
        accel_gain = EKFGSF_tiltGain * sq(3.0f - 2.0f * EKFGSF_ahrs_ng);
    } else {
        accel_gain = 0.0f;
    }
} else if (accel_gain > 0.5f) { // ❌ Wrong: should use EKFGSF_ahrs_ng instead of accel_gain
    accel_gain = EKFGSF_tiltGain * sq(2.0f * EKFGSF_ahrs_ng - 1.0f);
} else {
    accel_gain = 0.0f;
}

The correct logic should replace accel_gain with EKFGSF_ahrs_ng in the conditional checks (e.g., else if (EKFGSF_ahrs_ng <= 1.5f) and else if (EKFGSF_ahrs_ng > 0.5f)), ensuring the gain is determined by the current acceleration magnitude relative to gravity, not historical gain values.

Version
All versions of ArduPilot that include the EKFGSF_yaw module (e.g., ArduPlane 3.5+, ArduCopter 4.0+, and subsequent stable/beta releases).

Platform
[ ] All
[ ] AntennaTracker
[ ] Copter
[X] Plane
[ ] Rover
[ ] Submarine

Airframe type
All fixed-wing airframes (flying wing, conventional plane, glider, etc.) that rely on EKFGSF for yaw angle estimation without a magnetometer.

Hardware type
All autopilot hardware running the EKFGSF_yaw module (Pixhawk, Cube Orange/Black, Pixracer, Navio2, etc.).

Logs
A sample log demonstrating the issue (showing inconsistent accel_gain values despite varying acceleration magnitudes) can be provided upon request. The log captures fixed-wing flight data with MAG_ENABLE=0 (no magnetometer), showing drift in yaw angle estimation that correlates with incorrect accel_gain calculations. A temporary link to the log file: [insert log link here, e.g., https://drive.google.com/file/d/XXXXXX/view?usp=sharing]