Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-23 22:58:10 +08:00
Code Issues Actions 15 Packages Projects Releases Wiki Activity

EKF: Set dvel bias variance to stored value when state is inactive

Browse Source
This commit is contained in:
Paul Riseborough
2021-01-07 19:23:49 +11:00
committed by Daniel Agar
parent c212975abe
commit 3e1f2d8674
1 changed files with 3 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/covariance.cpp
+3 -3
View File
@@ -576,7 +576,7 @@ void Ekf::predictCovariance()
nextP(13,13) = kahanSummation(nextP(13,13), process_noise(13), _delta_vel_bias_var_accum(0));
} else {
nextP.uncorrelateCovarianceSetVariance<1>(13, 0.f);
nextP.uncorrelateCovarianceSetVariance<1>(13, _prev_dvel_bias_var(0));
_delta_vel_bias_var_accum(0) = 0.f;
}
@@ -605,7 +605,7 @@ void Ekf::predictCovariance()
nextP(14,14) = kahanSummation(nextP(14,14), process_noise(14), _delta_vel_bias_var_accum(1));
} else {
nextP.uncorrelateCovarianceSetVariance<1>(14, 0.f);
nextP.uncorrelateCovarianceSetVariance<1>(14, _prev_dvel_bias_var(1));
_delta_vel_bias_var_accum(1) = 0.f;
}
@@ -635,7 +635,7 @@ void Ekf::predictCovariance()
nextP(15,15) = kahanSummation(nextP(15,15), process_noise(15), _delta_vel_bias_var_accum(2));
} else {
nextP.uncorrelateCovarianceSetVariance<1>(15, 0.f);
nextP.uncorrelateCovarianceSetVariance<1>(15, _prev_dvel_bias_var(2));
_delta_vel_bias_var_accum(2) = 0.f;
}