dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

EKF: Fix sign error in increaseQuatYawErrVariance function

This commit is contained in:
Paul Riseborough 2019-01-16 17:28:55 +11:00 committed by Daniel Agar
parent a0b9cb002e
commit 911d4d8f49
1 changed files with 5 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
EKF/ekf_helper.cpp
View File

@ -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]);
}