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

@@ -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;
     }
 

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;