From e08da1c599dafcc4b7a102024cab64c8e432db3a Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 26 Jul 2017 17:10:52 +1000 Subject: [PATCH] EKF: Add ability to use EV and GPS data together Fuse external vision data using a relative position odometry method when GPS data is also being used and enable both GPOS and EV data to be fused on the same time step. --- EKF/control.cpp | 58 +++++++++++++++++++++++++++++++----------- EKF/ekf.h | 6 +++++ EKF/ekf_helper.cpp | 3 +++ EKF/vel_pos_fusion.cpp | 30 +++++++++++++++++++--- 4 files changed, 79 insertions(+), 18 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 825f97a809..23ed272c68 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -115,7 +115,6 @@ void Ekf::controlFusionModes() // control use of observations for aiding controlMagFusion(); - controlExternalVisionFusion(); controlOpticalFlowFusion(); controlGpsFusion(); controlAirDataFusion(); @@ -123,10 +122,13 @@ void Ekf::controlFusionModes() controlDragFusion(); controlHeightFusion(); - // for efficiency, fusion of direct state observations for position and velocity is performed sequentially - // in a single function using sensor data from multiple sources (GPS, external vision, baro, range finder, etc) + // For efficiency, fusion of direct state observations for position and velocity is performed sequentially + // in a single function using sensor data from multiple sources (GPS, baro, range finder, etc) controlVelPosFusion(); + // Additional data from an external vision sensor can also be fused. + controlExternalVisionFusion(); + // report dead reckoning if we are no longer fusing measurements that constrain velocity drift _is_dead_reckoning = (_time_last_imu - _time_last_pos_fuse > _params.no_aid_timeout_max) && (_time_last_imu - _time_last_vel_fuse > _params.no_aid_timeout_max) @@ -143,20 +145,27 @@ void Ekf::controlExternalVisionFusion() if ((_params.fusion_mode & MASK_USE_EVPOS) && !_control_status.flags.ev_pos && _control_status.flags.tilt_align && _control_status.flags.yaw_align) { // check for a exernal 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 and height - setControlEVHeight(); + // turn on use of external vision measurements for position + _control_status.flags.ev_pos = true; ECL_INFO("EKF commencing external vision position fusion"); - // reset the position, height and velocity - resetPosition(); - resetVelocity(); - resetHeight(); - _control_status.flags.ev_pos=true; + + // 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) { + _hpos_odometry = true; + + } else { + resetPosition(); + resetVelocity(); + _hpos_odometry = false; + + } } } // external vision yaw aiding selection logic if ((_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw && _control_status.flags.tilt_align) { - // check for a exernal vision measurement that has fallen behind the fusion time horizon + // don't start using EV data unless daa is arriving frequently if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) { // reset the yaw angle to the value from the observaton quaternion // get the roll, pitch, yaw estimates from the quaternion states @@ -207,11 +216,18 @@ void Ekf::controlExternalVisionFusion() } } - // determine if we should use the height observation + // determine if we should start using the height observations if (_params.vdist_sensor_type == VDIST_SENSOR_EV) { - setControlEVHeight(); - _fuse_height = true; + // don't start using EV data unless daa is arriving frequently + if (!_control_status.flags.ev_hgt && (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL)) { + setControlEVHeight(); + resetHeight(); + } + } + // determine if we should use the vertical position observation + if (_control_status.flags.ev_hgt) { + _fuse_height = true; } // determine if we should use the horizontal position observations @@ -224,11 +240,24 @@ void Ekf::controlExternalVisionFusion() _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); + + // if GPS data is being used, then use an incremental position fusion method for EV data + if (_control_status.flags.gps) { + _hpos_odometry = true; + } + } + + // Fuse available NED position data into the main filter + if (_fuse_height || _fuse_pos) { + fuseVelPosHeight(); + _fuse_pos = _fuse_height = false; + } // determine if we should use the yaw observation if (_control_status.flags.ev_yaw) { fuseHeading(); + } } @@ -245,7 +274,6 @@ void Ekf::controlExternalVisionFusion() } } - } void Ekf::controlOpticalFlowFusion() diff --git a/EKF/ekf.h b/EKF/ekf.h index 44d25ef691..91c7e191e0 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -238,6 +238,12 @@ private: bool _fuse_hor_vel{false}; ///< true when gps horizontal velocity measurement should be fused bool _fuse_vert_vel{false}; ///< true when gps vertical velocity measurement should be fused + // variables used when position data is being fused using a relative position odometry model + bool _hpos_odometry{false}; ///< true when the NE position data is being fused using an odometry assumption + Vector2f _hpos_meas_prev; ///< previous value of NE 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 + // 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 bool _mag_data_ready{false}; ///< true when new magnetometer 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 ca40c0e3e9..6c7d3459c3 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -98,6 +98,9 @@ bool Ekf::resetPosition() posNE_before_reset(0) = _state.pos(0); posNE_before_reset(1) = _state.pos(1); + // let the next odometry update know that the previous value of states cannot be used to calculate the change in position + _hpos_prev_available = false; + if (_control_status.flags.gps) { // this reset is only called if we have new gps data at the fusion time horizon _state.pos(0) = _gps_sample_delayed.pos(0); diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index 93981fc023..b4ee09e446 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -99,10 +99,34 @@ void Ekf::fuseVelPosHeight() } else if (_control_status.flags.ev_pos) { - // we are using external vision measurements + // calculate innovations + if(_hpos_odometry) { + if(!_hpos_prev_available) { + // no previous observation available to calculate position change + fuse_map[3] = fuse_map[4] = false; + _hpos_prev_available = true; + + } else { + // use the change in position since the last measurement + _vel_pos_innov[3] = _state.pos(0) - _hpos_pred_prev(0) - _ev_sample_delayed.posNED(0) + _hpos_meas_prev(0); + _vel_pos_innov[4] = _state.pos(1) - _hpos_pred_prev(1) - _ev_sample_delayed.posNED(1) + _hpos_meas_prev(1); + + } + + // record observation and estimate for use next time + _hpos_meas_prev(0) = _ev_sample_delayed.posNED(0); + _hpos_meas_prev(1) = _ev_sample_delayed.posNED(1); + _hpos_pred_prev(0) = _state.pos(0); + _hpos_pred_prev(1) = _state.pos(1); + + } 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); + } + + // observation 1-STD error R[3] = fmaxf(_ev_sample_delayed.posErr, 0.01f); - _vel_pos_innov[3] = _state.pos(0) - _ev_sample_delayed.posNED(0); - _vel_pos_innov[4] = _state.pos(1) - _ev_sample_delayed.posNED(1); // innovation gate size gate_size[3] = fmaxf(_params.ev_innov_gate, 1.0f);