ekf2: use more efficient covariance update computation

KSK' is faster to compute than KHP and is mathematically equivalent
This commit is contained in:
bresch
2022-12-12 12:31:54 -05:00
committed by Daniel Agar
parent 61fa28c0d6
commit 41abf695a8
8 changed files with 21 additions and 49 deletions
+1 -1
View File
@@ -331,7 +331,7 @@ void Ekf::fuseDrag(const dragSample &drag_sample)
// if the innovation consistency check fails then don't fuse the sample
if (_drag_test_ratio(axis_index) <= 1.0f) {
measurementUpdate(Kfusion, Hfusion, _drag_innov(axis_index));
measurementUpdate(Kfusion, _drag_innov_var(axis_index), _drag_innov(axis_index));
}
}
}