From 9238e2f1a222195122028d01642ef26724b23cc8 Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Thu, 8 Feb 2018 10:20:59 +0100 Subject: [PATCH] EKF: fix spike handling in ev rotation calculation --- EKF/ekf_helper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 76a8958ffc..8ba637ef10 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -1540,7 +1540,7 @@ void Ekf::calcExtVisRotMat() Vector3f _input_delta_vec = rot_vec - _ev_rot_vec_filt; float input_delta_mag = _input_delta_vec.norm(); if (input_delta_mag > 0.1f) { - rot_vec = _ev_rot_vec_filt + rot_vec * (0.1f / input_delta_mag); + rot_vec = _ev_rot_vec_filt + _input_delta_vec * (0.1f / input_delta_mag); } // Apply a first order IIR low pass filter