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

EKF: Also fill in lower part of covariance matrix in increaseQuatYawErrVariance() 

Signed-off-by: CarlOlsson <carlolsson.co@gmail.com>
This commit is contained in:
CarlOlsson 2019-01-16 08:52:54 +01:00 committed by Daniel Agar
parent 4b3140e5f7
commit 0896f7b9bd
1 changed files with 6 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
EKF/ekf_helper.cpp
View File

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