From e08da1c599dafcc4b7a102024cab64c8e432db3a Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 26 Jul 2017 17:10:52 +1000 Subject: [PATCH 1/2] 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); From 3ee68987100ff0d412c8eef20e91b986a085829d Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 29 Jul 2017 15:31:34 +1000 Subject: [PATCH 2/2] EKF: Enable origin to be maintained when starting aiding using EV only When starting aiding using EV only and commencing GPS aiding later, this change means that the GPS origin is set to the local position 0,0 point rather than the current vehicle position. This avoids large changes in local position when GPs aiding starts. --- EKF/gps_checks.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp index b03f72d0f7..6f7b8afaf7 100644 --- a/EKF/gps_checks.cpp +++ b/EKF/gps_checks.cpp @@ -68,7 +68,7 @@ bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps) map_projection_init_timestamped(&_pos_ref, lat, lon, _time_last_imu); // if we are already doing aiding, corect for the change in posiiton since the EKF started navigating - if (_control_status.flags.opt_flow || _control_status.flags.gps) { + if (_control_status.flags.opt_flow || _control_status.flags.gps || _control_status.flags.ev_pos) { double est_lat, est_lon; map_projection_reproject(&_pos_ref, -_state.pos(0), -_state.pos(1), &est_lat, &est_lon); map_projection_init_timestamped(&_pos_ref, est_lat, est_lon, _time_last_imu);