diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index e74c0395de9..38d98d97929 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -2183,13 +2183,12 @@ const EKFGSF_yaw *NavEKF3::get_yawEstimator(void) const // Do a reset and bootstrap alignment of all EKF cores // return true if successful for all cores -// When on the ground and stationary, gyros are recalibrated first so -// the filter bootstraps with clean offsets. In flight the gyro -// calibration is skipped and the filter resets with existing biases. +// The per-core InitialiseFilterBootstrap() +// (Different to the core method where the return value is false when the IMU delay buffer is not yet full) bool NavEKF3::InitialiseFilterBootstrap() { // ignore any data if the EKF is not started - if (!core) { + if (core == nullptr) { return false; } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index 6e6c78f1cc9..2dc40875849 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -514,9 +514,23 @@ bool NavEKF3_core::InitialiseFilterBootstrap(void) return false; } + // preserve origin across re-initialisation so a bootstrap reset + // does not shift the local NED frame. On first boot validOrigin + // is false and the restore below is a no-op. + const bool hadValidOrigin = validOrigin; + const Location savedEKFOrigin = EKF_origin; + // set re-used variables to zero InitialiseVariables(); + // restore origin so per-core EKF_origin stays consistent with the + // shared frontend public_origin + if (hadValidOrigin) { + EKF_origin = savedEKFOrigin; + ekfGpsRefHgt = (double)0.01 * (double)EKF_origin.alt; + validOrigin = true; + } + // acceleration vector in XYZ body axes measured by the IMU (m/s^2) Vector3F initAccVec;