diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index 4c511196d7..e333e96815 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp @@ -406,6 +406,9 @@ void NavEKF3_core::SelectVelPosFusion() // get data that has now fallen behind the fusion time horizon gpsDataToFuse = storedGPS.recall(gpsDataDelayed,imuDataDelayed.time_ms); + if (gpsDataToFuse) { + CorrectGPSForAntennaOffset(gpsDataDelayed); + } // initialise all possible data we may fuse fusePosData = false; @@ -423,7 +426,6 @@ void NavEKF3_core::SelectVelPosFusion() fusePosData = true; extNavUsedForPos = false; - CorrectGPSForAntennaOffset(gpsDataDelayed); // copy corrected GPS data to observation vector if (fuseVelData) {