Hello everyone, I have a question regarding the EKF2 algorithm. I’ve seen an innovation rejection mechanism that do not allow, for example, GPS fusion if it’s measurements was 5 SD away from the current estimated position or velocity state, I was wondering about this case: what if the gps didn’t fuse for lets say 10 seconds, this will case the states related drift and diverge, if they (the predicted states) diverged more than 5 SD and then a valid GPS data comes, it still won’t fuse it but now because the prediction is what has drifted not the invalid GPS data, which will make things even worse.
How is this case handled in the current EKF2 algorithm? I thought about mid air reset (re initialization and covariance reset) but this can be dangerous, even partial resting isn’t safe as this might harm other states due to sudden changes, more specifically the biases states.
Good question! EKF2 avoids the situation where valid GPS can never fuse again. If GPS innovations keep getting rejected, the filter increases covariance (reduces confidence in prediction), which widens the acceptance gate. That way, once GPS data is valid, it can fuse again.
Instead of a full mid-air reset (risky), EKF2 uses selective covariance resets/inflation on position/velocity states so they can realign smoothly, while leaving biases and attitude untouched. In practice, this lets the filter recover from drift without getting stuck or causing dangerous jumps.
Aha, thank you for your response.
Could you please point the finger on where is this logic implemented in the code? I’m really interested to see how they implemented this.
Sure — check EKF2::controlGpsFusion() and EKF2::controlExternalVisionFusion() in ekf2_main.cpp. The gating and covariance inflation logic lives in src/modules/ekf2/EKF/control.cpp (look for increaseVarianceIfNeeded() and resetVelocityToGps()). That’s where the filter decides to relax innovation gates and let GPS fuse again.
