From fbb51147b712fb62eb28c6c03a76c5cb54eb66f5 Mon Sep 17 00:00:00 2001 From: Roman Date: Sun, 14 Apr 2019 13:00:48 +0200 Subject: [PATCH] implemented use of optical flow for terrain estimation Signed-off-by: Roman --- EKF/control.cpp | 8 +++ EKF/ekf.h | 4 ++ EKF/terrain_estimator.cpp | 104 +++++++++++++++++++++++++++++++++++++- 3 files changed, 115 insertions(+), 1 deletion(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index c214653dfc..61018c4ae0 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -123,6 +123,14 @@ void Ekf::controlFusionModes() && (_R_to_earth(2, 2) > _params.range_cos_max_tilt); } + // check if we should fuse flow data for terrain estimation + if (!_flow_for_terrain_data_ready && _flow_data_ready) { + // only fuse flow for terrain if range data hasn't been fused for 5 seconds + _flow_for_terrain_data_ready = (_time_last_imu - _time_last_hagl_fuse) > 5 * 1000 * 1000; + // only fuse flow for terrain if the main filter is not fusing flow and we are using gps + _flow_for_terrain_data_ready &= (!_control_status.flags.opt_flow && _control_status.flags.gps); + } + _ev_data_ready = _ext_vision_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_ev_sample_delayed); _tas_data_ready = _airspeed_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_airspeed_sample_delayed); diff --git a/EKF/ekf.h b/EKF/ekf.h index 16e89563d2..88cce68191 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -315,6 +315,7 @@ private: bool _flow_data_ready{false}; ///< true when the leading edge of the optical flow integration period has fallen behind the fusion time horizon bool _ev_data_ready{false}; ///< true when new external vision system data has fallen behind the fusion time horizon and is available to be fused bool _tas_data_ready{false}; ///< true when new true airspeed data has fallen behind the fusion time horizon and is available to be fused + bool _flow_for_terrain_data_ready{false}; /// same flag as "_flow_data_ready" but used for separate terrain estimator uint64_t _time_last_fake_gps{0}; ///< last time we faked GPS position measurements to constrain tilt errors during operation without external aiding (uSec) uint64_t _time_ins_deadreckon_start{0}; ///< amount of time we have been doing inertial only deadreckoning (uSec) @@ -544,6 +545,9 @@ private: // update the terrain vertical position estimate using a height above ground measurement from the range finder void fuseHagl(); + // update the terrain vertical position estimate using an optical flow measurement + void fuseFlowForTerrain(); + // reset the heading and magnetic field states using the declination and magnetometer/external vision measurements // return true if successful bool resetMagHeading(Vector3f &mag_init, bool increase_yaw_var = true, bool update_buffer=true); diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index 353c7f4767..fc17d8c741 100644 --- a/EKF/terrain_estimator.cpp +++ b/EKF/terrain_estimator.cpp @@ -55,7 +55,10 @@ bool Ekf::initHagl() _terrain_var = sq(_params.range_noise); // success return true; - + } else if (_flow_for_terrain_data_ready) { + _terrain_vpos = _state.pos(2); + _terrain_var = 100.0f; + return true; } else if (!_control_status.flags.in_air) { // if on ground we assume a ground clearance _terrain_vpos = _state.pos(2) + _params.rng_gnd_clearance; @@ -104,6 +107,11 @@ void Ekf::runTerrainEstimator() } + if (_flow_for_terrain_data_ready) { + fuseFlowForTerrain(); + _flow_for_terrain_data_ready = false; + } + //constrain _terrain_vpos to be a minimum of _params.rng_gnd_clearance larger than _state.pos(2) if (_terrain_vpos - _state.pos(2) < _params.rng_gnd_clearance) { _terrain_vpos = _params.rng_gnd_clearance + _state.pos(2); @@ -162,6 +170,100 @@ void Ekf::fuseHagl() } } +void Ekf::fuseFlowForTerrain() +{ + // calculate optical LOS rates using optical flow rates that have had the body angular rate contribution removed + // correct for gyro bias errors in the data used to do the motion compensation + // Note the sign convention used: A positive LOS rate is a RH rotation of the scene about that axis. + Vector2f opt_flow_rate; + opt_flow_rate(0) = _flowRadXYcomp(0) / _flow_sample_delayed.dt + _flow_gyro_bias(0); + opt_flow_rate(1) = _flowRadXYcomp(1) / _flow_sample_delayed.dt + _flow_gyro_bias(1); + + // get latest estimated orientation + float q0 = _state.quat_nominal(0); + float q1 = _state.quat_nominal(1); + float q2 = _state.quat_nominal(2); + float q3 = _state.quat_nominal(3); + + // calculate the optical flow observation variance + float R_LOS = calcOptFlowMeasVar(); + + // get rotation matrix from earth to body + Dcmf earth_to_body(_state.quat_nominal); + earth_to_body = earth_to_body.transpose(); + + // calculate the sensor position relative to the IMU + Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body; + + // calculate the velocity of the sensor relative to the imu in body frame + // Note: _flow_sample_delayed.gyroXYZ is the negative of the body angular velocity, thus use minus sign + Vector3f vel_rel_imu_body = cross_product(-_flow_sample_delayed.gyroXYZ / _flow_sample_delayed.dt, pos_offset_body); + + // calculate the velocity of the sensor in the earth frame + Vector3f vel_rel_earth = _state.vel + _R_to_earth * vel_rel_imu_body; + + // rotate into body frame + Vector3f vel_body = earth_to_body * vel_rel_earth; + + float t0 = q0*q0 - q1*q1 - q2*q2 + q3*q3; + + // Calculate observation matrix for flow around the vehicle x axis + float Hx = vel_body(1) * t0 /((_terrain_vpos - _state.pos(2)) * (_terrain_vpos - _state.pos(2))); + + // Cacluate innovation variance + _flow_innov_var[0] = Hx * Hx * _terrain_var + R_LOS; + + // calculate the kalman gain for the flow x measurement + float Kx = _terrain_var * Hx / _flow_innov_var[0]; + + // calculate prediced optical flow about x axis + float pred_flow_x = vel_body(1) * earth_to_body(2,2) / (_terrain_vpos - _state.pos(2)); + + // calculate flow innovation (x axis) + _flow_innov[0] = pred_flow_x - opt_flow_rate(0); + + // calculate correction term for terrain variance + float KxHxP = Kx * Hx * _terrain_var; + + // innovation consistency check + float gate_size = fmaxf(_params.flow_innov_gate, 1.0f); + float flow_test_ratio = sq(_flow_innov[0]) / (sq(gate_size) * _flow_innov_var[0]); + + // do not perform measurement update if badly conditioned + if (flow_test_ratio <= 1.0f) { + _terrain_vpos += Kx * _flow_innov[0]; + // guard against negative variance + _terrain_var = fmaxf(_terrain_var - KxHxP, 0.0f); + } + + // Calculate observation matrix for flow around the vehicle y axis + float Hy = -vel_body(0) * t0 /((_terrain_vpos - _state.pos(2)) * (_terrain_vpos - _state.pos(2))); + + // Calculuate innovation variance + _flow_innov_var[1] = Hy * Hy * _terrain_var + R_LOS; + + // calculate the kalman gain for the flow y measurement + float Ky = _terrain_var * Hy / _flow_innov_var[1]; + + // calculate prediced optical flow about y axis + float pred_flow_y = -vel_body(0) * earth_to_body(2,2) / (_terrain_vpos - _state.pos(2)); + + // calculate flow innovation (y axis) + _flow_innov[1] = pred_flow_y - opt_flow_rate(1); + + // calculate correction term for terrain variance + float KyHyP = Ky * Hy * _terrain_var; + + // innovation consistency check + flow_test_ratio = sq(_flow_innov[1]) / (sq(gate_size) * _flow_innov_var[1]); + + if (flow_test_ratio <= 1.0f) { + _terrain_vpos += Ky * _flow_innov[1]; + // guard against negative variance + _terrain_var = fmaxf(_terrain_var - KyHyP, 0.0f); + } +} + // return true if the terrain height estimate is valid bool Ekf::get_terrain_valid() {