EKF: Also fill in lower part of covariance matrix in increaseQuatYawE…

…rrVariance() Signed-off-by: CarlOlsson <[email protected]>
PX4 · Jan 16, 2019 · 3d716c4 · 3d716c4
1 parent 47cf16f
commit 3d716c4
Showing 1 changed file with 6 additions and 0 deletions.
diff --git 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];
 }