forked from Archive/PX4-Autopilot
EKF: Fix sign error in increaseQuatYawErrVariance function
This commit is contained in:
parent
a0b9cb002e
commit
911d4d8f49
|
@ -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]);
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue