From 10cbd79db728fb80bbfba553d21eec825aedbcb3 Mon Sep 17 00:00:00 2001 From: kamilritz Date: Tue, 17 Sep 2019 14:30:34 +0200 Subject: [PATCH] Rotate external vision info in reset function if necessary --- EKF/ekf_helper.cpp | 21 ++++++++++++++++----- 1 file changed, 16 insertions(+), 5 deletions(-) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 44bda2b391..5094b3acc4 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -94,11 +94,18 @@ bool Ekf::resetVelocity() // reset the horizontal velocity variance using the optical flow noise variance P[5][5] = P[4][4] = sq(range) * calcOptFlowMeasVar(); - - } else if (_control_status.flags.ev_pos) { + } else if (_control_status.flags.ev_vel) { + Vector3f _ev_vel = _ev_sample_delayed.velNED; + if(_params.fusion_mode & MASK_ROTATE_EV){ + _ev_vel = _ev_rot_mat *_ev_sample_delayed.velNED; + } + _state.vel(0) = _ev_vel(0); + _state.vel(1) = _ev_vel(1); + _state.vel(2) = _ev_vel(2); + // TODO: to what should we set the covariances? + } else if (_control_status.flags.ev_pos) { _state.vel.setZero(); zeroOffDiag(P, 4, 6); - } else { // Used when falling back to non-aiding mode of operation _state.vel(0) = 0.0f; @@ -149,8 +156,12 @@ bool Ekf::resetPosition() } else if (_control_status.flags.ev_pos) { // this reset is only called if we have new ev data at the fusion time horizon - _state.pos(0) = _ev_sample_delayed.posNED(0); - _state.pos(1) = _ev_sample_delayed.posNED(1); + Vector3f _ev_pos = _ev_sample_delayed.posNED; + if(_params.fusion_mode & MASK_ROTATE_EV){ + _ev_pos = _ev_rot_mat *_ev_sample_delayed.posNED; + } + _state.pos(0) = _ev_pos(0); + _state.pos(1) = _ev_pos(1); // use EV accuracy to reset variances setDiag(P, 7, 8, sq(_ev_sample_delayed.posErr));