From 0896f7b9bdb914b65db8e176976063427b65c7b6 Mon Sep 17 00:00:00 2001 From: CarlOlsson Date: Wed, 16 Jan 2019 08:52:54 +0100 Subject: [PATCH] EKF: Also fill in lower part of covariance matrix in increaseQuatYawErrVariance() Signed-off-by: CarlOlsson --- EKF/ekf_helper.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 6b80b794f4..27463dd93b 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -1688,4 +1688,10 @@ void Ekf::increaseQuatYawErrVariance(float yaw_variance) P[1][3] -= yaw_variance*SQ[1]*SQ[3]; P[2][3] -= yaw_variance*SQ[0]*SQ[3]; P[3][3] += yaw_variance*sq(SQ[3]); + P[1][0] += yaw_variance*SQ[1]*SQ[2]; + P[2][0] += yaw_variance*SQ[0]*SQ[2]; + P[2][1] += yaw_variance*SQ[0]*SQ[1]; + P[3][0] -= yaw_variance*SQ[2]*SQ[3]; + P[3][1] -= yaw_variance*SQ[1]*SQ[3]; + P[3][2] -= yaw_variance*SQ[0]*SQ[3]; }