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

EKF: limit yaw variance increase to 0.01 rad^2 to prevent a badly conditioned covariance matrix

Browse Source 
Signed-off-by: CarlOlsson <carlolsson.co@gmail.com>
This commit is contained in:
CarlOlsson
2019-01-31 09:53:57 -05:00
committed by Daniel Agar
parent 0896f7b9bd
commit 6e7c119b31
1 changed files with 3 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf_helper.cpp
+3
View File
@@ -1676,6 +1676,9 @@ void Ekf::increaseQuatYawErrVariance(float yaw_variance)
SQ[2] = 0.5f * ((_state.quat_nominal(3)*SG[0]) - (_state.quat_nominal(1)*SG[1]) + (_state.quat_nominal(2)*SG[2])); SQ[2] = 0.5f * ((_state.quat_nominal(3)*SG[0]) - (_state.quat_nominal(1)*SG[1]) + (_state.quat_nominal(2)*SG[2]));
SQ[3] = 0.5f * ((_state.quat_nominal(0)*SG[0]) + (_state.quat_nominal(1)*SG[2]) + (_state.quat_nominal(2)*SG[1])); SQ[3] = 0.5f * ((_state.quat_nominal(0)*SG[0]) + (_state.quat_nominal(1)*SG[2]) + (_state.quat_nominal(2)*SG[1]));
// Limit yaw variance increase to prevent a badly conditioned covariance matrix
yaw_variance = fminf(yaw_variance, 1.0e-2f);
// Add covariances for additonal yaw uncertainty to existing covariances. // Add covariances for additonal yaw uncertainty to existing covariances.
// This assumes that the additional yaw error is uncorrrelated to existing errors // This assumes that the additional yaw error is uncorrrelated to existing errors
P[0][0] += yaw_variance*sq(SQ[2]); P[0][0] += yaw_variance*sq(SQ[2]);