From 3e8139ff9f6af33f99387466828f461c17500cff Mon Sep 17 00:00:00 2001 From: kamilritz Date: Tue, 17 Sep 2019 09:12:06 +0200 Subject: [PATCH] Enable vision velocity fusion --- EKF/common.h | 8 +++- EKF/control.cpp | 101 +++++++++++++++++++++++++++-------------- EKF/ekf.h | 12 +++-- EKF/ekf_helper.cpp | 2 +- EKF/vel_pos_fusion.cpp | 10 ++-- 5 files changed, 87 insertions(+), 46 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index c559441f8f..21266ec4b6 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -189,11 +189,12 @@ struct auxVelSample { #define MASK_USE_GPS (1<<0) ///< set to true to use GPS data #define MASK_USE_OF (1<<1) ///< set to true to use optical flow data #define MASK_INHIBIT_ACC_BIAS (1<<2) ///< set to true to inhibit estimation of accelerometer delta velocity bias -#define MASK_USE_EVPOS (1<<3) ///< set to true to use external vision NED position data +#define MASK_USE_EVPOS (1<<3) ///< set to true to use external vision position data #define MASK_USE_EVYAW (1<<4) ///< set to true to use external vision quaternion data for yaw #define MASK_USE_DRAG (1<<5) ///< set to true to use the multi-rotor drag model to estimate wind #define MASK_ROTATE_EV (1<<6) ///< set to true to if the EV observations are in a non NED reference frame and need to be rotated before being used #define MASK_USE_GPSYAW (1<<7) ///< set to true to use GPS yaw data if available +#define MASK_USE_EVVEL (1<<8) ///< sset to true to use external vision velocity data // Integer definitions for mag_fusion_type #define MAG_FUSE_TYPE_AUTO 0 ///< The selection of either heading or 3D magnetometer fusion will be automatic @@ -299,7 +300,9 @@ struct parameters { float range_stuck_threshold{0.1f}; ///< minimum variation in range finder reading required to declare a range finder 'unstuck' when readings recommence after being out of range (m) // vision position fusion - float ev_innov_gate{5.0f}; ///< vision estimator fusion innovation consistency gate size (STD) + float ev_innov_gate{5.0f}; ///< vision estimator fusion innovation consistency gate size (STD) + float ev_vel_innov_gate{3.0f}; ///< vision velocity fusion innovation consistency gate size (STD) + float ev_pos_innov_gate{5.0f}; ///< vision position fusion innovation consistency gate size (STD) // optical flow fusion float flow_noise{0.15f}; ///< observation noise for optical flow LOS rate measurements (rad/sec) @@ -455,6 +458,7 @@ union filter_control_status_u { uint32_t rng_stuck : 1; ///< 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough uint32_t gps_yaw : 1; ///< 22 - true when yaw (not ground course) data from a GPS receiver is being fused uint32_t mag_align_complete : 1; ///< 23 - true when the in-flight mag field alignment has been completed + uint32_t ev_vel : 1; ///< 24 - true when local earth frame velocity data from external vision measurements are being fused } flags; uint32_t value; }; diff --git a/EKF/control.cpp b/EKF/control.cpp index 4c2ab0d986..ac753e245f 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -167,43 +167,36 @@ void Ekf::controlExternalVisionFusion() // if the ev data is not in a NED reference frame, then the transformation between EV and EKF navigation frames // needs to be calculated and the observations rotated into the EKF frame of reference - if ((_params.fusion_mode & MASK_ROTATE_EV) && !_control_status.flags.ev_yaw) { + if ((_params.fusion_mode & MASK_ROTATE_EV) && ((_params.fusion_mode & MASK_USE_EVPOS) || (_params.fusion_mode & MASK_USE_EVVEL)) && !_control_status.flags.ev_yaw) { // rotate EV measurements into the EKF Navigation frame calcExtVisRotMat(); } - // reset external vision rotation matrix independent if is actually used for debugging purposes - if ((_time_last_imu - _time_last_ext_vision) < (2 * EV_MAX_INTERVAL) - && (_params.fusion_mode & MASK_ROTATE_EV) && !(_params.fusion_mode & MASK_USE_EVYAW)){ - // Reset transformation between EV and EKF navigation frames - resetExtVisRotMat(); - } - - // external vision position aiding selection logic - if ((_params.fusion_mode & MASK_USE_EVPOS) && !_control_status.flags.ev_pos && _control_status.flags.tilt_align - && _control_status.flags.yaw_align) { + // external vision aiding selection logic + if (_control_status.flags.tilt_align && _control_status.flags.yaw_align) { // check for a external vision measurement that has fallen behind the fusion time horizon if ((_time_last_imu - _time_last_ext_vision) < (2 * EV_MAX_INTERVAL)) { // turn on use of external vision measurements for position - _control_status.flags.ev_pos = true; - ECL_INFO("EKF commencing external vision position fusion"); - - if ((_params.fusion_mode & MASK_ROTATE_EV) && !(_params.fusion_mode & MASK_USE_EVYAW)) { - // Reset transformation between EV and EKF navigation frames when starting fusion - resetExtVisRotMat(); + if (_params.fusion_mode & MASK_USE_EVPOS && !_control_status.flags.ev_pos) { + _control_status.flags.ev_pos = true; + resetPosition(); + ECL_INFO("EKF commencing external vision position fusion"); } - // reset the position if we are not already aiding using GPS, else use a relative position - // method for fusing the position data - if (_control_status.flags.gps) { - _fuse_hpos_as_odom = true; - - } else { - resetPosition(); - resetVelocity(); - + // turn on use of external vision measurements for velocity + if (_params.fusion_mode & MASK_USE_EVVEL && !_control_status.flags.ev_vel) { + _control_status.flags.ev_vel = true; + resetVelocity(); + ECL_INFO("EKF commencing external vision velocity fusion"); } + + if ((_params.fusion_mode & MASK_ROTATE_EV) && !(_params.fusion_mode & MASK_USE_EVYAW) + && !_ev_rot_mat_initialised) { + // Reset transformation between EV and EKF navigation frames when starting fusion + resetExtVisRotMat(); + _ev_rot_mat_initialised = true; + } } } @@ -332,7 +325,7 @@ void Ekf::controlExternalVisionFusion() // check if we have been deadreckoning too long if ((_time_last_imu - _time_last_pos_fuse) > _params.reset_timeout_max) { // don't reset velocity if we have another source of aiding constraining it - if ((_time_last_imu - _time_last_of_fuse) > (uint64_t)1E6) { + if (((_time_last_imu - _time_last_of_fuse) > (uint64_t)1E6) && ((_time_last_imu - _time_last_vel_fuse) > (uint64_t)1E6)) { resetVelocity(); } @@ -340,16 +333,55 @@ void Ekf::controlExternalVisionFusion() } } + // innovation gate size + _posInnovGateNE = fmaxf(_params.ev_pos_innov_gate, 1.0f); + }else{ + _vel_pos_innov[3] = 0.0f; + _vel_pos_innov[4] = 0.0f; + } + + // determine if we should use the velocity observations + if (_control_status.flags.ev_vel) { + _fuse_hor_vel = true; + _fuse_vert_vel = true; + + Vector3f velNED_aligned{_ev_sample_delayed.velNED}; + + // rotate measurement into correct earth frame if required + if (_params.fusion_mode & MASK_ROTATE_EV) { + velNED_aligned = _ev_rot_mat * _ev_sample_delayed.velNED; + } + + // correct velocity for offset relative to IMU + Vector3f ang_rate = _imu_sample_delayed.delta_ang * (1.0f / _imu_sample_delayed.delta_ang_dt); + 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_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); + + // check if we have been deadreckoning too long + if ((_time_last_imu - _time_last_vel_fuse) > _params.reset_timeout_max) { + // don't reset velocity if we have another source of aiding constraining it + if (((_time_last_imu - _time_last_of_fuse) > (uint64_t)1E6) && ((_time_last_imu - _time_last_pos_fuse) > (uint64_t)1E6)) { + resetVelocity(); + } + } + // observation 1-STD error - _posObsNoiseNE = fmaxf(_ev_sample_delayed.posErr, 0.01f); + _velObsVarNED(2) = _velObsVarNED(1) = _velObsVarNED(0) = fmaxf(_ev_sample_delayed.velErr, 0.01f); // innovation gate size - _posInnovGateNE = fmaxf(_params.ev_innov_gate, 1.0f); + _vvelInnovGate = _hvelInnovGate = fmaxf(_params.ev_vel_innov_gate, 1.0f); } // Fuse available NED position data into the main filter - if (_fuse_height || _fuse_pos) { + if (_fuse_height || _fuse_pos || _fuse_hor_vel || _fuse_vert_vel) { fuseVelPosHeight(); + _fuse_vert_vel = _fuse_hor_vel = false; _fuse_pos = _fuse_height = false; _fuse_hpos_as_odom = false; @@ -361,12 +393,13 @@ void Ekf::controlExternalVisionFusion() } - } else if (_control_status.flags.ev_pos + } else if ((_control_status.flags.ev_pos || _control_status.flags.ev_vel) && (_time_last_imu >= _time_last_ext_vision) && ((_time_last_imu - _time_last_ext_vision) > (uint64_t)_params.reset_timeout_max)) { // Turn off EV fusion mode if no data has been received _control_status.flags.ev_pos = false; + _control_status.flags.ev_vel = false; _control_status.flags.ev_yaw = false; ECL_INFO("EKF External Vision Data Stopped"); @@ -688,7 +721,7 @@ void Ekf::controlGpsFusion() _posObsNoiseNE = math::constrain(_gps_sample_delayed.hacc, lower_limit, upper_limit); } - _velObsVarNE(1) = _velObsVarNE(0) = sq(fmaxf(_gps_sample_delayed.sacc, _params.gps_vel_noise)); + _velObsVarNED(2) = _velObsVarNED(1) = _velObsVarNED(0) = sq(fmaxf(_gps_sample_delayed.sacc, _params.gps_vel_noise)); // calculate innovations _vel_pos_innov[0] = _state.vel(0) - _gps_sample_delayed.vel(0); @@ -1627,6 +1660,7 @@ void Ekf::controlVelPosFusion() if (!_control_status.flags.gps && !_control_status.flags.opt_flow && !_control_status.flags.ev_pos && + !_control_status.flags.ev_vel && !(_control_status.flags.fuse_aspd && _control_status.flags.fuse_beta)) { // We now need to use a synthetic position observation to prevent unconstrained drift of the INS states. @@ -1690,7 +1724,8 @@ void Ekf::controlAuxVelFusion() _fuse_hor_vel_aux = true; _aux_vel_innov[0] = _state.vel(0) - _auxvel_sample_delayed.velNE(0); _aux_vel_innov[1] = _state.vel(1) - _auxvel_sample_delayed.velNE(1); - _velObsVarNE = _auxvel_sample_delayed.velVarNE; + _velObsVarNED(0) = _auxvel_sample_delayed.velVarNE(0); + _velObsVarNED(1) = _auxvel_sample_delayed.velVarNE(1); _hvelInnovGate = _params.auxvel_gate; fuseVelPosHeight(); } diff --git a/EKF/ekf.h b/EKF/ekf.h index bcc8e25c4b..d90bf6ce21 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -298,17 +298,19 @@ private: float _posObsNoiseNE{0.0f}; ///< 1-STD observation noise used for the fusion of NE position data (m) float _posInnovGateNE{1.0f}; ///< Number of standard deviations used for the NE position fusion innovation consistency check - Vector2f _velObsVarNE; ///< 1-STD observation noise variance used for the fusion of NE velocity data (m/sec)**2 - float _hvelInnovGate{1.0f}; ///< Number of standard deviations used for the horizontal velocity fusion innovation consistency check + Vector3f _velObsVarNED; ///< 1-STD observation noise variance used for the fusion of NED velocity data (m/sec)**2 + float _hvelInnovGate{1.0f}; ///< Number of standard deviations used for the horizontal velocity fusion innovation consistency check + float _vvelInnovGate{1.0f}; ///< Number of standard deviations used for the vertical velocity fusion innovation consistency check // variables used when position data is being fused using a relative position odometry model bool _fuse_hpos_as_odom{false}; ///< true when the NE position data is being fused using an odometry assumption 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 - Vector3f _ev_rot_vec_filt; ///< filtered rotation vector defining the rotation from EKF to EV reference (rad) - Dcmf _ev_rot_mat; ///< transformation matrix that rotates observations from the EV to the EKF navigation frame - uint64_t _ev_rot_last_time_us{0}; ///< previous time that the calculation of the ekf to ev rotation matrix was updated (uSec) + Vector3f _ev_rot_vec_filt; ///< filtered rotation vector defining the rotation EV to EKF reference, initiliazied to zero rotation (rad) + Dcmf _ev_rot_mat; ///< 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 _ev_rot_mat_initialised{0}; ///< _ev_rot_mat 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 diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 9c286e5a24..2431274848 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -1336,7 +1336,7 @@ bool Ekf::global_position_is_valid() // return true if we are totally reliant on inertial dead-reckoning for position void Ekf::update_deadreckoning_status() { - bool velPosAiding = (_control_status.flags.gps || _control_status.flags.ev_pos) + bool velPosAiding = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel) && (((_time_last_imu - _time_last_pos_fuse) <= _params.no_aid_timeout_max) || ((_time_last_imu - _time_last_vel_fuse) <= _params.no_aid_timeout_max) || ((_time_last_imu - _time_last_delpos_fuse) <= _params.no_aid_timeout_max)); diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index 8b27442d5b..c4650b629a 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -67,8 +67,8 @@ void Ekf::fuseVelPosHeight() } // Set observation noise variance and innovation consistency check gate size for the NE position observations - R[0] = _velObsVarNE(0); - R[1] = _velObsVarNE(1); + R[0] = _velObsVarNED(0); + R[1] = _velObsVarNED(1); gate_size[1] = gate_size[0] = _hvelInnovGate; } @@ -76,12 +76,12 @@ void Ekf::fuseVelPosHeight() if (_fuse_vert_vel) { fuse_map[2] = true; // observation variance - use receiver reported accuracy with parameter setting the minimum value - R[2] = fmaxf(_params.gps_vel_noise, 0.01f); + R[2] = _velObsVarNED(2); // use scaled horizontal speed accuracy assuming typical ratio of VDOP/HDOP R[2] = 1.5f * fmaxf(R[2], _gps_sample_delayed.sacc); R[2] = R[2] * R[2]; // innovation gate size - gate_size[2] = fmaxf(_params.vel_innov_gate, 1.0f); + gate_size[2] = _vvelInnovGate; } if (_fuse_pos) { @@ -152,7 +152,7 @@ void Ekf::fuseVelPosHeight() R[5] = fmaxf(_ev_sample_delayed.hgtErr, 0.01f); R[5] = R[5] * R[5]; // innovation gate size - gate_size[5] = fmaxf(_params.ev_innov_gate, 1.0f); + gate_size[5] = fmaxf(_params.ev_pos_innov_gate, 1.0f); } // update innovation class variable for logging purposes