diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index b1964fcc19..a5dc93320e 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -700,6 +700,9 @@ bool NavEKF3::InitialiseFilter(void) // Set the primary initially to be the lowest index primary = 0; + // invalidate shared origin + common_origin_valid = false; + // initialise the cores. We return success only if all cores // initialise successfully bool ret = true; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index 8aca702994..337589d336 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -484,7 +484,6 @@ bool NavEKF3_core::setOriginLLH(const Location &loc) // Set the NED origin to be used until the next filter reset void NavEKF3_core::setOrigin(const Location &loc) { - // assume origin at current GPS location (no averaging) EKF_origin = loc; // if flying, correct for height change from takeoff so that the origin is at field elevation if (inFlight) {