EKF : introduce new architechture for navigation limits

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

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;

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.

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);
+	}
 
 }
 

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

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

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