diff --git a/EKF/control.cpp b/EKF/control.cpp index ac753e245f..2ea5692b0f 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -283,10 +283,9 @@ void Ekf::controlExternalVisionFusion() _ev_sample_delayed.posNED(1) -= pos_offset_earth(1); _ev_sample_delayed.posNED(2) -= pos_offset_earth(2); - // Use an incremental position fusion method for EV data if using GPS or if the observations are not in NED - if (_control_status.flags.gps || (_params.fusion_mode & MASK_ROTATE_EV)) { + // Use an incremental position fusion method for EV position data if GPS is also used + if (_params.fusion_mode & MASK_USE_GPS) { _fuse_hpos_as_odom = true; - } else { _fuse_hpos_as_odom = false; } @@ -301,15 +300,15 @@ void Ekf::controlExternalVisionFusion() // calculate the change in position since the last measurement Vector3f ev_delta_pos = _ev_sample_delayed.posNED - _pos_meas_prev; - // rotate measurement into body frame if required - if (_params.fusion_mode & MASK_ROTATE_EV) { - ev_delta_pos = _ev_rot_mat * ev_delta_pos; - } + // rotate measurement into body frame is required when fusing with GPS + ev_delta_pos = _ev_rot_mat * ev_delta_pos; // use the change in position since the last measurement _vel_pos_innov[3] = _state.pos(0) - _hpos_pred_prev(0) - ev_delta_pos(0); _vel_pos_innov[4] = _state.pos(1) - _hpos_pred_prev(1) - ev_delta_pos(1); + // observation 1-STD error, incremental pos observation is expected to have more uncertainty + _posObsNoiseNE = fmaxf(_ev_sample_delayed.posErr, 0.5f); } // record observation and estimate for use next time @@ -319,8 +318,14 @@ void Ekf::controlExternalVisionFusion() } else { // use the absolute position - _vel_pos_innov[3] = _state.pos(0) - _ev_sample_delayed.posNED(0); - _vel_pos_innov[4] = _state.pos(1) - _ev_sample_delayed.posNED(1); + Vector3f ev_pos_meas = _ev_sample_delayed.posNED; + if (_params.fusion_mode & MASK_ROTATE_EV) { + ev_pos_meas = _ev_rot_mat * ev_pos_meas; + } + _vel_pos_innov[3] = _state.pos(0) - ev_pos_meas(0); + _vel_pos_innov[4] = _state.pos(1) - ev_pos_meas(1); + // observation 1-STD error + _posObsNoiseNE = fmaxf(_ev_sample_delayed.posErr, 0.01f); // check if we have been deadreckoning too long if ((_time_last_imu - _time_last_pos_fuse) > _params.reset_timeout_max) {