From 41744c884d8a02467b820ba4723c7528c249caae Mon Sep 17 00:00:00 2001
From: Paul Riseborough <gncsolns@gmail.com>
Date: Tue, 17 Mar 2026 10:27:09 +0000
Subject: [PATCH] AP_NavEKF3: Add function to do externally commanded reset

Add InitialiseFilterBootstrap() public method that clears statesInitialised
and re-runs bootstrap alignment on all cores.

Uses statesInitialised to check success rather than the per-core return
value which is false when the IMU delay buffer is not yet full.
---
 libraries/AP_NavEKF3/AP_NavEKF3.cpp    | 30 ++++++++++++++++++++++++++
 libraries/AP_NavEKF3/AP_NavEKF3.h      |  4 ++++
 libraries/AP_NavEKF3/AP_NavEKF3_core.h |  8 ++++++-
 3 files changed, 41 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp
index ef832e746af..e74c0395de9 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp
@@ -2180,3 +2180,33 @@ const EKFGSF_yaw *NavEKF3::get_yawEstimator(void) const
     }
     return nullptr;
 }
+
+// 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.
+bool NavEKF3::InitialiseFilterBootstrap()
+{
+    // ignore any data if the EKF is not started
+    if (!core) {
+        return false;
+    }
+
+    // initialise the cores. We return success only if all cores
+    // initialise successfully.  The per-core InitialiseFilterBootstrap()
+    // return value is false when the IMU delay buffer is not yet full,
+    // which is normal during a reset. Check statesInitialised directly
+    // to determine whether the bootstrap alignment succeeded.
+    bool ret = true;
+    for (uint8_t i=0; i<num_cores; i++) {
+        // clear the statesInitialised status to allow a bootstrap alignment
+        core[i].clearStatesInitialised();
+        // perform a bootstrap alignment
+        core[i].InitialiseFilterBootstrap();
+        if (!core[i].isStatesInitialised()) {
+            ret = false;
+        }
+    }
+    return ret;
+}
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h
index ec1cfd568ad..ddb89547a95 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3.h
+++ b/libraries/AP_NavEKF3/AP_NavEKF3.h
@@ -383,6 +383,10 @@ public:
     // get a yaw estimator instance
     const EKFGSF_yaw *get_yawEstimator(void) const;
 
+    // Do a reset and bootstrap alignment of all EKF cores
+    // return true if successful for all cores
+    bool InitialiseFilterBootstrap();
+
 private:
     class AP_DAL &dal;
 
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h
index 5efdb553bf4..7cc130a3a1f 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h
@@ -485,7 +485,13 @@ public:
     // failure message
     // requires_position should be true if horizontal position configuration should be checked
     bool pre_arm_check(bool requires_position, char *failure_msg, uint8_t failure_msg_len) const;
-    
+
+    // clear the statesInitialised status which allows a reset and bootstrap alignment
+    void clearStatesInitialised(void) { statesInitialised = false; }
+
+    // return true if states have been initialised by a bootstrap alignment
+    bool isStatesInitialised(void) const { return statesInitialised; }
+
 private:
     EKFGSF_yaw *yawEstimator;
     AP_DAL &dal;