diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 5fde16ab4193638863292ee4e49da522025f69e9..42e03aaa38925b0552b748fcd9737f8d7d28ee92 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 ba27b1bfbe9d1e0c2ad4df96385bfc00f8f5a6eb..51b8a9a5e877e2fdfcd591bb176ed850b7e3f8fa 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;