From fd849e0578200484f8e3ffd2d15acc78ae66e8fc Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 16 Aug 2020 16:57:58 +1000 Subject: [PATCH] EKF: Fix specific force observation noise variance Also add protection for out of range value. --- EKF/drag_fusion.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/EKF/drag_fusion.cpp b/EKF/drag_fusion.cpp index a0cc3a31ab..1f95728fcb 100644 --- a/EKF/drag_fusion.cpp +++ b/EKF/drag_fusion.cpp @@ -48,8 +48,8 @@ void Ekf::fuseDrag() { SparseVector24f<0,1,2,3,4,5,6,22,23> Hfusion; // Observation Jacobians Vector24f Kfusion; // Kalman gain vector - const float R_ACC = sq(_params.drag_noise); // observation noise variance in specific force drag (m/sec**2)**2 + const float R_ACC = fmaxf(_params.drag_noise, 0.5f); // observation noise variance in specific force drag (m/sec**2)**2 const float rho = fmaxf(_air_density, 0.1f); // air density (kg/m**3) // calculate inverse of ballistic coefficient