From be10ce8b50fd95d27297c3bf971afaae37c07366 Mon Sep 17 00:00:00 2001
From: Andy Piper <github@andypiper.com>
Date: Thu, 16 Apr 2026 11:40:29 +0100
Subject: [PATCH] AP_NavEKF3: preserve origin across bootstrap reset

Save validOrigin and EKF_origin before InitialiseVariables() in
InitialiseFilterBootstrap() and restore them after, so an externally
commanded reset does not shift the local NED frame.  Without this the
per-core EKF_origin gets re-set from the next GPS fix, diverging from
the shared frontend common_EKF_origin.  On first boot validOrigin is
false and the restore is a no-op.
---
 libraries/AP_NavEKF3/AP_NavEKF3.cpp      |  7 +++----
 libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 14 ++++++++++++++
 2 files changed, 17 insertions(+), 4 deletions(-)

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;