EKF3 doing yaw realignment to GPS velocity with GPS in NO_FIX status

Recently I’ve noticed quite a strange behaviour with a plane - it sometimes re-aligns yaw to GPS velocity even without having GPS fix available.

I’ve been testing plane behaviour in a scenario of GPS signal absence by using the GPS disable AUX function. I’m aware it only changes the GPS fix status and does not wipe rest of the data like sat count, xDOP, etc

This is Matek H743 slim with Matek M10-L4-3100 periph (RM3100 compass + ublox M10) running Plane v4.5.2

On the image below I’m temporarily re-enabling GPS so the correct position is fetched after flying GPS-less for some time and, as expected, the yaw is re-aligned to GPS velocity

Then after flying for a bit I disable GPS via AUX function and it still re-aligns yaw to GPS velocity with GPS being in no fix state for couple of mins at that point and this causes the trajectory jump

I started checking code for the GPS data (location, speed etc) usage to find where the data maybe leaking into EKF and it seems like all of it is wrapped in checks for fix present.

Then I went looking specifically for yaw reset and noticed a potential issue in EKF3 code here:

A small excerpt from the EKF3 fusion code:

    // Read GPS data from the sensor
    readGpsData();
    readGpsYawData();

    // get data that has now fallen behind the fusion time horizon
    gpsDataToFuse = storedGPS.recall(gpsDataDelayed,imuDataDelayed.time_ms);
    if (gpsDataToFuse) {
    ...
    
    // we have GPS data to fuse and a request to align the yaw using the GPS course
    if (gpsYawResetRequest) {
        realignYawGPS(false);
    }
    ...

The EKF code reads fresh GPS data in readGpsData() and pushes it to the storedGPS ringbuffer for fusion afterwards, but only if GPS fix was OK.
Later it recalls the delayed GPS data from ringbuffer and all accesses to gpsDataDelayed are wrapped with a check for gpsDataToFuse.
Except for realignYawGPS(), which uses the gpsDataDelayed inside to check the yaw error, but there is no check if whatever stored in gpsDataDelayed is in fact a newly recalled data, and not an old sample still stored in that variable. This can possibly result in yaw re-alignment to some old GPS data (whenever the last sample with GPS fix ok was)

And I think this is what causes the behaviour I observe, which happened on several flights now in similar testing scenario. What do you guys think?

1 Like

Try adding gpsDataToFuse condition to run realignYawGPS. Should fix the problem.

@Paul_Riseborough @siddharth

1 Like

I often have the same problem. But I’m not a programmer and I don’t understand how to fix it in the code. Is there really no way?