From 911d4d8f491579c29046be752a4b429beaab0418 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 16 Jan 2019 17:28:55 +1100 Subject: [PATCH] EKF: Fix sign error in increaseQuatYawErrVariance function --- EKF/ekf_helper.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index c521b6b411..6b80b794f4 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -1661,7 +1661,8 @@ void Ekf::get_ekf2ev_quaternion(float *quat) // Argument is additional yaw variance in rad**2 void Ekf::increaseQuatYawErrVariance(float yaw_variance) { - // See DeriveYawResetEquations.m for derivation + // See DeriveYawResetEquations.m for derivation which produces code fragments in C_code4.txt file + // The auto-code was cleaned up and had terms multiplied by zero removed to give the following: // Intermediate variables float SG[3]; @@ -1683,8 +1684,8 @@ void Ekf::increaseQuatYawErrVariance(float yaw_variance) P[0][2] += yaw_variance*SQ[0]*SQ[2]; P[1][2] += yaw_variance*SQ[0]*SQ[1]; P[2][2] += yaw_variance*sq(SQ[0]); - P[0][3] += yaw_variance*SQ[2]*SQ[3]; - P[1][3] += yaw_variance*SQ[1]*SQ[3]; - P[2][3] += yaw_variance*SQ[0]*SQ[3]; + P[0][3] -= yaw_variance*SQ[2]*SQ[3]; + 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]); }