Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 19:07:45 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

EKF: Don't modify accel bias states and variances when inhibited

Browse Source
This commit is contained in:
Paul Riseborough
2021-01-07 19:28:09 +11:00
committed by Daniel Agar
parent 3e1f2d8674
commit dd259a288f
1 changed files with 7 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf.h
+7 -1
View File
@@ -700,8 +700,14 @@ private:
// measurement update with a single measurement // measurement update with a single measurement
// returns true if fusion is performed // returns true if fusion is performed
template <size_t ...Idxs> template <size_t ...Idxs>
bool measurementUpdate(const Vector24f &K, const SparseVector24f<Idxs...> &H, float innovation) bool measurementUpdate(Vector24f &K, const SparseVector24f<Idxs...> &H, float innovation)
{ {
for (unsigned i = 0; i < 3; i++) {
if (_accel_bias_inhibit[i]) {
K(13 + i) = 0.0f;
}
}
// apply covariance correction via P_new = (I -K*H)*P // apply covariance correction via P_new = (I -K*H)*P
// first calculate expression for KHP // first calculate expression for KHP
// then calculate P - KHP // then calculate P - KHP