fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 15:18:54 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Fix sign error in increaseQuatYawErrVariance function

Browse Source
This commit is contained in:
Paul Riseborough
2019-01-31 09:53:57 -05:00
committed by Daniel Agar
parent a0b9cb002e
commit 911d4d8f49
1 changed files with 5 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf_helper.cpp
+5 -4
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]);
}