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

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

Browse Source 
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
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf_helper.cpp
+6
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];
}