From 4b0e7b2cffe43008f6ed3e274e0857e01907b892 Mon Sep 17 00:00:00 2001 From: kamilritz Date: Thu, 23 Jan 2020 15:36:48 +0100 Subject: [PATCH] Remove filtering on external vision alignment --- EKF/control.cpp | 8 ------ EKF/ekf.h | 11 +-------- EKF/ekf_helper.cpp | 55 ++---------------------------------------- EKF/gps_yaw_fusion.cpp | 5 ---- 4 files changed, 3 insertions(+), 76 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 00e1ee216d..06d9f45fcc 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -205,14 +205,6 @@ void Ekf::controlExternalVisionFusion() resetVelocity(); ECL_INFO_TIMESTAMPED("commencing external vision velocity fusion"); } - - if ((_params.fusion_mode & MASK_ROTATE_EV) && !(_params.fusion_mode & MASK_USE_EVYAW) - && !_R_ev_to_ekf_initialised) { - // Reset transformation between EV and EKF navigation frames when starting fusion - resetExtVisRotMat(); - _R_ev_to_ekf_initialised = true; - ECL_INFO_TIMESTAMPED("external vision aligned"); - } } } diff --git a/EKF/ekf.h b/EKF/ekf.h index 9d0a7c5c77..2f78dc46be 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -319,10 +319,7 @@ private: Vector3f _pos_meas_prev; ///< previous value of NED position measurement fused using odometry assumption (m) Vector2f _hpos_pred_prev; ///< previous value of NE position state used by odometry fusion (m) bool _hpos_prev_available{false}; ///< true when previous values of the estimate and measurement are available for use - AxisAnglef _ev_rot_vec_filt; ///< filtered rotation vector defining the rotation EV to EKF reference, initiliazied to zero rotation (rad) Dcmf _R_ev_to_ekf; ///< transformation matrix that rotates observations from the EV to the EKF navigation frame, initialized with Identity - uint64_t _ev_rot_last_time_us{0}; ///< previous time that the calculation of the EV to EKF rotation matrix was updated (uSec) - bool _R_ev_to_ekf_initialised{0}; ///< _R_ev_to_ekf should only be initialised once in the beginning through the reset function // booleans true when fresh sensor data is available at the fusion time horizon bool _gps_data_ready{false}; ///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused @@ -615,15 +612,9 @@ private: // modify output filter to match the the EKF state at the fusion time horizon void alignOutputFilter(); - // update the estimated angular misalignment vector between the EV naigration frame and the EKF navigation frame - // and update the rotation matrix which transforms EV navigation frame measurements into NED + // update the rotation matrix which transforms EV navigation frame measurements into NED void calcExtVisRotMat(); - - // reset the estimated angular misalignment vector between the EV naigration frame and the EKF navigation frame - // and reset the rotation matrix which transforms EV navigation frame measurements into NED - void resetExtVisRotMat(); - // limit the diagonal of the covariance matrix // force symmetry when the argument is true void fixCovarianceErrors(bool force_symmetry); diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 6b34bd6412..5633611c06 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -696,11 +696,6 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool // record the state change _state_reset_status.quat_change = q_error; - // reset the rotation from the EV to EKF frame of reference if it is being used - if ((_params.fusion_mode & MASK_ROTATE_EV) && !_control_status.flags.ev_yaw) { - resetExtVisRotMat(); - } - if (increase_yaw_var) { // update the yaw angle variance using the variance of the measurement if (_control_status.flags.ev_yaw) { @@ -1657,64 +1652,18 @@ void Ekf::startMag3DFusion() } } -// update the estimated misalignment between the EV navigation frame and the EKF navigation frame -// and calculate a rotation matrix which rotates EV measurements into the EKF's navigation frame +// update the rotation matrix which rotates EV measurements into the EKF's navigation frame void Ekf::calcExtVisRotMat() { // Calculate the quaternion delta that rotates from the EV to the EKF reference frame at the EKF fusion time horizon. const Quatf q_error((_state.quat_nominal * _ev_sample_delayed.quat.inversed()).normalized()); - - // convert to a delta angle and apply a spike and low pass filter - AxisAnglef rot_vec(q_error); - - if (rot_vec.norm() > 1e-6f) { - - // apply an input limiter to protect from spikes - const Vector3f _input_delta_vec = rot_vec - _ev_rot_vec_filt; - const float input_delta_len = _input_delta_vec.norm(); - - if (input_delta_len > 0.1f) { - rot_vec = _ev_rot_vec_filt + _input_delta_vec * (0.1f / input_delta_len); - } - - // Apply a first order IIR low pass filter - const float omega_lpf_us = 0.2e-6f; // cutoff frequency in rad/uSec - float alpha = math::constrain(omega_lpf_us * (float)(_time_last_imu - _ev_rot_last_time_us), 0.0f, 1.0f); - _ev_rot_last_time_us = _time_last_imu; - _ev_rot_vec_filt = _ev_rot_vec_filt * (1.0f - alpha) + rot_vec * alpha; - - } - - _R_ev_to_ekf = Dcmf(_ev_rot_vec_filt); - -} - -// reset the estimated misalignment between the EV navigation frame and the EKF navigation frame -// and update the rotation matrix which rotates EV measurements into the EKF's navigation frame -void Ekf::resetExtVisRotMat() -{ - // Calculate the quaternion delta that rotates from the EV to the EKF reference frame at the EKF fusion time horizon. - Quatf q_error = _state.quat_nominal * _ev_sample_delayed.quat.inversed(); - q_error.normalize(); - - AxisAnglef rot_vec(q_error); - - float rot_vec_norm = rot_vec.norm(); - - if (rot_vec_norm > 1e-9f) { - _ev_rot_vec_filt = rot_vec; - - } else { - _ev_rot_vec_filt.zero(); - } - _R_ev_to_ekf = Dcmf(q_error); } // return the quaternions for the rotation from External Vision system reference frame to the EKF reference frame void Ekf::get_ev2ekf_quaternion(float *quat) { - const Quatf quat_ev2ekf(_ev_rot_vec_filt); + const Quatf quat_ev2ekf(_R_ev_to_ekf); for (unsigned i = 0; i < 4; i++) { quat[i] = quat_ev2ekf(i); diff --git a/EKF/gps_yaw_fusion.cpp b/EKF/gps_yaw_fusion.cpp index c0f429fdc7..271d817420 100644 --- a/EKF/gps_yaw_fusion.cpp +++ b/EKF/gps_yaw_fusion.cpp @@ -393,11 +393,6 @@ bool Ekf::resetGpsAntYaw() // update transformation matrix from body to world frame using the current estimate _R_to_earth = Dcmf(_state.quat_nominal); - // reset the rotation from the EV to EKF frame of reference if it is being used - if ((_params.fusion_mode & MASK_ROTATE_EV) && (_params.fusion_mode & MASK_USE_EVPOS)) { - resetExtVisRotMat(); - } - // update the yaw angle variance using the variance of the measurement increaseQuatYawErrVariance(sq(fmaxf(_params.mag_heading_noise, 1.0e-2f)));