From de02aebafdd5634fa3e9d0c84d5856b61b30e72c Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 12 Feb 2016 15:14:36 +1100 Subject: [PATCH] EKF: Reset covariance matrix when doing a yaw and magnetic field reset The correlation terms in the covariance matrix will be incorrect after a reset, so should be zeroed --- EKF/ekf.h | 6 ++++++ EKF/ekf_helper.cpp | 28 ++++++++++++++++++++++++++++ 2 files changed, 34 insertions(+) diff --git a/EKF/ekf.h b/EKF/ekf.h index 770887a348..de693135e9 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -242,4 +242,10 @@ private: { return var * var; } + + // zero the specified range of rows in the state covariance matricx + void zeroRows(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last); + + // zero the specified range of columns in the state covariance matricx + void zeroCols(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last); }; diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 8b82b2b501..a799f6e5e3 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -121,6 +121,14 @@ bool Ekf::resetMagHeading(Vector3f &mag_init) matrix::Dcm R_to_earth(euler_init); _state.mag_I = R_to_earth * mag_init; + // reset the corresponding rows and columns in the covariance matrix and set the variances on the magnetic field states to the measurement variance + zeroRows(P, 16, 21); + zeroCols(P, 16, 21); + + for (uint8_t index = 16; index <= 21; index ++) { + P[index][index] = sq(_params.mag_noise); + } + return true; } @@ -332,3 +340,23 @@ void Ekf::fuse(float *K, float innovation) _state.wind_vel(i) = _state.wind_vel(i) - K[i + 22] * innovation; } } + +// zero specified range of rows in the state covariance matrix +void Ekf::zeroRows(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last) +{ + uint8_t row; + + for (row = first; row <= last; row++) { + memset(&cov_mat[row][0], 0, sizeof(cov_mat[0][0]) * 24); + } +} + +// zero specified range of columns in the state covariance matrix +void Ekf::zeroCols(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last) +{ + uint8_t row; + + for (row = 0; row <= 23; row++) { + memset(&cov_mat[row][first], 0, sizeof(cov_mat[0][0]) * (1 + last - first)); + } +}