fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-12 04:27:35 +08:00
Code Issues 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
// returns true if fusion is performed
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
// first calculate expression for KHP
// then calculate P - KHP