From 90ca01270595338e018af7f2c29eb61163627f2e Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 12 Feb 2020 21:31:49 +0100 Subject: [PATCH] ekf_helper: restore oridinal quaternion decorrelation The intent of the function "uncorrelateQuatStates()" is to uncorrelathe quaternions from the other states but not between each other as they are and should remain linked alltogether. Clearing the quaternions to quaternion covariances introduced unstabilities in other states (especially accel biases). --- EKF/ekf_helper.cpp | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index b8a819cafd..a626c83182 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -1372,7 +1372,25 @@ void Ekf::fuse(float *K, float innovation) void Ekf::uncorrelateQuatStates() { + // save 4x4 elements + uint32_t row; + uint32_t col; + float variances[4][4]; + for (row = 0; row < 4; row++) { + for (col = 0; col < 4; col++) { + variances[row][col] = P(row, col); + } + } + + // zero rows and columns P.uncorrelateCovariance<4>(0); + + // restore 4x4 elements + for (row = 0; row < 4; row++) { + for (col = 0; col < 4; col++) { + P(row, col) = variances[row][col]; + } + } } bool Ekf::global_position_is_valid()