From b4d2b8c57d820beb26ea9c0aa1888471c0bf98ba Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Sat, 19 May 2018 14:16:08 -0400 Subject: [PATCH] EKF : introduce new architechture for navigation limits --- EKF/common.h | 1 - EKF/control.cpp | 2 +- EKF/ekf.h | 8 ++---- EKF/ekf_helper.cpp | 57 ++++++++++++++++++++++++------------- EKF/estimator_interface.cpp | 2 +- EKF/estimator_interface.h | 30 +++++++++++++++---- EKF/optflow_fusion.cpp | 2 +- 7 files changed, 67 insertions(+), 35 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index 5cfcfeff62..4b76296457 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -291,7 +291,6 @@ struct parameters { float flow_noise_qual_min{0.5f}; ///< observation noise for optical flow LOS rate measurements when flow sensor quality is at the minimum useable (rad/sec) int32_t flow_qual_min{1}; ///< minimum acceptable quality integer from the flow sensor float flow_innov_gate{3.0f}; ///< optical flow fusion innovation consistency gate size (STD) - float flow_rate_max{2.5f}; ///< maximum valid optical flow rate (rad/sec) // these parameters control the strictness of GPS quality checks used to determine if the GPS is // good enough to set a local origin and commence aiding diff --git a/EKF/control.cpp b/EKF/control.cpp index 447ef9b49e..207ae9a1d5 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -343,7 +343,7 @@ void Ekf::controlOpticalFlowFusion() float accel_norm = _accel_vec_filt.norm(); bool motion_is_excessive = ((accel_norm > 14.7f) // accel greater than 1.5g || (accel_norm < 4.9f) // accel less than 0.5g - || (_ang_rate_mag_filt > _params.flow_rate_max) // angular rate exceeds flow sensor limit + || (_ang_rate_mag_filt > _flow_max_rate) // angular rate exceeds flow sensor limit || (_R_to_earth(2,2) < 0.866f)); // tilted more than 30 degrees if (motion_is_excessive) { _time_bad_motion_us = _imu_sample_delayed.time_us; diff --git a/EKF/ekf.h b/EKF/ekf.h index 4dfe9976d5..8fcdb99806 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -140,12 +140,8 @@ public: // get the 1-sigma horizontal and vertical velocity uncertainty void get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv); - /* - Returns the following vehicle control limits required by the estimator. - vxy_max : Maximum ground relative horizontal speed (metres/sec). NaN when no limiting required. - tilt_rate_max : maximum allowed tilt rate against the direction of travel (rad/sec). NaN when no limiting required. - */ - void get_ekf_ctrl_limits(float *vxy_max, bool *limit_hagl); + // get the vehicle control limits required by the estimator to keep within sensor limitations + void get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max); /* Reset all IMU bias states and covariances to initial alignment values. diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index aea0ad8fbf..42465569e8 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -1051,33 +1051,52 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) } /* -Returns the following vehicle control limits required by the estimator. -vxy_max : Maximum ground relative horizontal speed (metres/sec). NaN when no limiting required. -limit_hagl : Boolean true when height above ground needs to be controlled to remain between optical flow focus and rang efinder max range limits. +Returns the following vehicle control limits required by the estimator to keep within sensor limitations. +vxy_max : Maximum ground relative horizontal speed (meters/sec). NaN when limiting is not needed. +vz_max : Maximum ground relative vertical speed (meters/sec). NaN when limiting is not needed. +hagl_min : Minimum height above ground (meters). NaN when limiting is not needed. +hagl_max : Maximum height above ground (meters). NaN when limiting is not needed. */ -void Ekf::get_ekf_ctrl_limits(float *vxy_max, bool *limit_hagl) +void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max) { - float flow_gnd_spd_max; - bool flow_limit_hagl; + // Calculate range finder limits + float rangefinder_hagl_min = _rng_min_distance; + // Allow use of 75% of rangefinder maximum range to allow for angular motion + float rangefinder_hagl_max = 0.75f * _rng_max_distance; + + // Calculate optical flow limits + // Allow ground relative velocity to use 50% of available flow sensor range to allow for angular motion + float flow_vxy_max = fmaxf(0.5f * _flow_max_rate * (_terrain_vpos - _state.pos(2)), 0.0f); + float flow_hagl_min = _flow_min_distance; + float flow_hagl_max = _flow_max_distance; + + // TODO : calculate visual odometry limits + + bool relying_on_rangefinder = _control_status.flags.rng_hgt; - // If relying on optical flow for navigation we need to keep within flow and range sensor limits bool relying_on_optical_flow = _control_status.flags.opt_flow && !(_control_status.flags.gps || _control_status.flags.ev_pos); - if (relying_on_optical_flow) { - // Allow ground relative velocity to use 50% of available flow sensor range to allow for angular motion - flow_gnd_spd_max = 0.5f * _params.flow_rate_max * (_terrain_vpos - _state.pos(2)); - flow_gnd_spd_max = fmaxf(flow_gnd_spd_max, 0.0f); - - flow_limit_hagl = true; - - } else { - flow_gnd_spd_max = NAN; - flow_limit_hagl = false; + // Do not require limiting by default + *vxy_max = NAN; + *vz_max = NAN; + *hagl_min = NAN; + *hagl_max = NAN; + // Keep within range sensor limit when using rangefinder as primary height source + if (relying_on_rangefinder) { + *vxy_max = NAN; + *vz_max = NAN; + *hagl_min = rangefinder_hagl_min; + *hagl_max = rangefinder_hagl_max; } - memcpy(vxy_max, &flow_gnd_spd_max, sizeof(float)); - memcpy(limit_hagl, &flow_limit_hagl, sizeof(bool)); + // Keep within flow AND range sensor limits when exclusively using optical flow + if (relying_on_optical_flow) { + *vxy_max = flow_vxy_max; + *vz_max = NAN; + *hagl_min = fmaxf(rangefinder_hagl_min, flow_hagl_min); + *hagl_max = fminf(rangefinder_hagl_max, flow_hagl_max); + } } diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 4fd543ad4b..585bf4c6eb 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -382,7 +382,7 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl bool flow_magnitude_good = true; if (delta_time_good) { flow_rate_magnitude = flow->flowdata.norm() / delta_time; - flow_magnitude_good = (flow_rate_magnitude <= _params.flow_rate_max); + flow_magnitude_good = (flow_rate_magnitude <= _flow_max_rate); } bool relying_on_flow = _control_status.flags.opt_flow diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index bae75c5046..8b3e23f317 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -152,12 +152,8 @@ public: // get the 1-sigma horizontal and vertical velocity uncertainty virtual void get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) = 0; - /* - Returns the following vehicle control limits required by the estimator. - vxy_max : Maximum ground relative horizontal speed (metres/sec). NaN when no limiting required. - limit_hagl : Boolean true when height above ground needs to be controlled to remain between optical flow focus and rang efinder max range limits. - */ - virtual void get_ekf_ctrl_limits(float *vxy_max, bool *limit_hagl) = 0; + // get the vehicle control limits required by the estimator to keep within sensor limitations + virtual void get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max) = 0; // ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined virtual bool collect_gps(uint64_t time_usec, struct gps_message *gps) { return true; } @@ -233,6 +229,21 @@ public: // set air density used by the multi-rotor specific drag force fusion void set_air_density(float air_density) {_air_density = air_density;} + // set sensor limitations reported by the rangefinder + void set_rangefinder_limits(float min_distance, float max_distance) + { + _rng_min_distance = min_distance; + _rng_max_distance = max_distance; + } + + // set sensor limitations reported by the optical flow sensor + void set_optical_flow_limits(float max_flow_rate, float min_distance, float max_distance) + { + _flow_max_rate = max_flow_rate; + _flow_min_distance = min_distance; + _flow_max_distance = max_distance; + } + // return true if the global position estimate is valid virtual bool global_position_is_valid() = 0; @@ -420,6 +431,13 @@ protected: float _drag_sample_time_dt{0.0f}; // time integral across all samples used to form _drag_down_sampled (sec) float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C}; // air density (kg/m**3) + // Sensor limitations + float _rng_min_distance{0.0f}; ///< minimum distance that the rangefinder can measure (m) + float _rng_max_distance{0.0f}; ///< maximum distance that the rangefinder can measure (m) + float _flow_max_rate{0.0f}; ///< maximum angular flow rate that the optical flow sensor can measure (rad/s) + float _flow_min_distance{0.0f}; ///< minimum distance that the optical flow sensor can operate at (m) + float _flow_max_distance{0.0f}; ///< maximum distance that the optical flow sensor can operate at (m) + // Output Predictor outputSample _output_sample_delayed{}; // filter output on the delayed time horizon outputSample _output_new{}; // filter output on the non-delayed time horizon diff --git a/EKF/optflow_fusion.cpp b/EKF/optflow_fusion.cpp index c79416ee26..a79a5b0a65 100644 --- a/EKF/optflow_fusion.cpp +++ b/EKF/optflow_fusion.cpp @@ -95,7 +95,7 @@ void Ekf::fuseOptFlow() opt_flow_rate(0) = _flow_sample_delayed.flowRadXYcomp(0) / _flow_sample_delayed.dt + _flow_gyro_bias(0); opt_flow_rate(1) = _flow_sample_delayed.flowRadXYcomp(1) / _flow_sample_delayed.dt + _flow_gyro_bias(1); - if (opt_flow_rate.norm() < _params.flow_rate_max) { + if (opt_flow_rate.norm() < _flow_max_rate) { _flow_innov[0] = vel_body(1) / range - opt_flow_rate(0); // flow around the X axis _flow_innov[1] = -vel_body(0) / range - opt_flow_rate(1); // flow around the Y axis