From 83eb3260760bdb9441a78bd0e3a2eec1aa9ecdc8 Mon Sep 17 00:00:00 2001 From: kamilritz Date: Mon, 23 Sep 2019 11:40:51 +0200 Subject: [PATCH] Missing posNED --- EKF/control.cpp | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 7424107683..76e9443bfa 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -279,9 +279,9 @@ void Ekf::controlExternalVisionFusion() // correct position and height for offset relative to IMU Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body; Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; - _ev_sample_delayed.posNED(0) -= pos_offset_earth(0); - _ev_sample_delayed.posNED(1) -= pos_offset_earth(1); - _ev_sample_delayed.posNED(2) -= pos_offset_earth(2); + _ev_sample_delayed.pos(0) -= pos_offset_earth(0); + _ev_sample_delayed.pos(1) -= pos_offset_earth(1); + _ev_sample_delayed.pos(2) -= pos_offset_earth(2); // Use an incremental position fusion method for EV position data if GPS is also used if (_params.fusion_mode & MASK_USE_GPS) { @@ -298,7 +298,7 @@ void Ekf::controlExternalVisionFusion() } else { // calculate the change in position since the last measurement - Vector3f ev_delta_pos = _ev_sample_delayed.posNED - _pos_meas_prev; + Vector3f ev_delta_pos = _ev_sample_delayed.pos - _pos_meas_prev; // rotate measurement into body frame is required when fusing with GPS ev_delta_pos = _ev_rot_mat * ev_delta_pos; @@ -312,13 +312,13 @@ void Ekf::controlExternalVisionFusion() } // record observation and estimate for use next time - _pos_meas_prev = _ev_sample_delayed.posNED; + _pos_meas_prev = _ev_sample_delayed.pos; _hpos_pred_prev(0) = _state.pos(0); _hpos_pred_prev(1) = _state.pos(1); } else { // use the absolute position - Vector3f ev_pos_meas = _ev_sample_delayed.posNED; + Vector3f ev_pos_meas = _ev_sample_delayed.pos; if (_params.fusion_mode & MASK_ROTATE_EV) { ev_pos_meas = _ev_rot_mat * ev_pos_meas; } @@ -350,11 +350,11 @@ void Ekf::controlExternalVisionFusion() _fuse_hor_vel = true; _fuse_vert_vel = true; - Vector3f velNED_aligned{_ev_sample_delayed.velNED}; + Vector3f vel_aligned{_ev_sample_delayed.vel}; // rotate measurement into correct earth frame if required if (_params.fusion_mode & MASK_ROTATE_EV) { - velNED_aligned = _ev_rot_mat * _ev_sample_delayed.velNED; + vel_aligned = _ev_rot_mat * _ev_sample_delayed.vel; } // correct velocity for offset relative to IMU @@ -362,11 +362,11 @@ void Ekf::controlExternalVisionFusion() Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body; Vector3f vel_offset_body = cross_product(ang_rate, pos_offset_body); Vector3f vel_offset_earth = _R_to_earth * vel_offset_body; - velNED_aligned -= vel_offset_earth; + vel_aligned -= vel_offset_earth; - _vel_pos_innov[0] = _state.vel(0) - velNED_aligned(0); - _vel_pos_innov[1] = _state.vel(1) - velNED_aligned(1); - _vel_pos_innov[2] = _state.vel(2) - velNED_aligned(2); + _vel_pos_innov[0] = _state.vel(0) - vel_aligned(0); + _vel_pos_innov[1] = _state.vel(1) - vel_aligned(1); + _vel_pos_innov[2] = _state.vel(2) - vel_aligned(2); // check if we have been deadreckoning too long if ((_time_last_imu - _time_last_vel_fuse) > _params.reset_timeout_max) {