From a7caa91ceffd4f25dd982550f2c6f5aa07abe4fb Mon Sep 17 00:00:00 2001
From: priseborough <p_riseborough@live.com.au>
Date: Wed, 29 Oct 2014 12:21:42 +1100
Subject: [PATCH] AP_NavEKF : Add public method to reset gyro bias states
---
libraries/AP_NavEKF/AP_NavEKF.cpp | 12 ++++++++++++
libraries/AP_NavEKF/AP_NavEKF.h | 3 +++
2 files changed, 15 insertions(+)
diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index 5fde16ab4..42e03aaa3 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -2810,6 +2810,18 @@ void NavEKF::getGyroBias(Vector3f &gyroBias) const
gyroBias = state.gyro_bias / dtIMU;
}
+// reset the body axis gyro bias states to zero and re-initialise the corresponding covariances
+void NavEKF::resetGyroBias(void)
+{
+ state.gyro_bias.zero();
+ zeroRows(P,10,12);
+ zeroCols(P,10,12);
+ P[10][10] = sq(radians(0.1f * dtIMU));
+ P[11][11] = P[10][10];
+ P[12][12] = P[10][10];
+
+}
+
// return weighting of first IMU in blending function and the individual Z-accel bias estimates in m/s^2
void NavEKF::getAccelBias(Vector3f &accelBias) const
{
diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h
index ba27b1bfb..51b8a9a5e 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.h
+++ b/libraries/AP_NavEKF/AP_NavEKF.h
@@ -105,6 +105,9 @@ public:
// return body axis gyro bias estimates in rad/sec
void getGyroBias(Vector3f &gyroBias) const;
+ // reset body axis gyro bias estimates
+ void resetGyroBias(void);
+
// return weighting of first IMU in blending function and the individual Z-accel bias estimates in m/s^2
void getAccelBias(Vector3f &accelBias) const;
--
GitLab