ekf: enable range finder to be used for estimating height even if it's not

the primary height source - moved height control into single function in order to decide which sensor should be used for estimating height - under certain conditions allow to use the range finder to estimate height even if it's not the primary height source - fixed a bug where the delta time for the baro offset calculation was always zero - use methods to set height control flags to reduce code duplication and to prevent bugs Signed-off-by: Roman <bapstroman@gmail.com>
2026-10-16 06:53:33 +08:00 · 2017-06-15 09:10:43 +02:00
parent 36bffd2571
commit 3778f0921a
3 changed files with 123 additions and 92 deletions
@@ -256,6 +256,10 @@ struct parameters {
 	float rng_gnd_clearance{0.1f};		// minimum valid value for range when on ground (m)
 	float rng_sens_pitch{0.0f};		// Pitch offset of the range sensor (rad). Sensor points out along Z axis when offset is zero. Positive rotation is RH about Y axis.
 	float range_noise_scaler{0.0f};		// scaling from range measurement to noise (m/m)
+	float vehicle_variance_scaler{0.0f};	// gain applied to vehicle height variance used in calculation of height above ground observation variance
+	float max_hagl_for_range_aid{5.0f};	// maximum height above ground for which we allow to use the range finder as height source (if range_aid == 1)
+	float max_vel_for_range_aid{1.0f};	// maximum ground velocity for which we allow to use the range finder as height source (if range_aid == 1)
+	int range_aid{0};			// allow switching primary height source to range finder if certian conditions are met

 	// vision position fusion
 	float ev_innov_gate{5.0f};		// vision estimator fusion innovation consistency gate size (STD)
@@ -110,11 +110,10 @@ void Ekf::controlFusionModes()
 	controlExternalVisionFusion();
 	controlOpticalFlowFusion();
 	controlGpsFusion();
-	controlBaroFusion();
-	controlRangeFinderFusion();
 	controlAirDataFusion();
 	controlBetaFusion();
 	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)
@@ -137,12 +136,8 @@ void Ekf::controlExternalVisionFusion()
 			// 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
-				_control_status.flags.ev_pos = true;
+				setControlEVHeight();
 				ECL_INFO("EKF commencing external vision position fusion");
-				// turn off other forms of height aiding
-				_control_status.flags.baro_hgt = false;
-				_control_status.flags.gps_hgt = false;
-				_control_status.flags.rng_hgt = false;
 				// reset the position, height and velocity
 				resetPosition();
 				resetVelocity();
@@ -206,10 +201,7 @@ void Ekf::controlExternalVisionFusion()

 		// determine if we should use the height observation
 		if (_params.vdist_sensor_type == VDIST_SENSOR_EV) {
-			_control_status.flags.baro_hgt = false;
-			_control_status.flags.gps_hgt = false;
-			_control_status.flags.rng_hgt = false;
-			_control_status.flags.ev_hgt = true;
+			setControlEVHeight();
 			_fuse_height = true;

 		}
@@ -432,15 +424,6 @@ void Ekf::controlGpsFusion()

 		}

-		// Determine if GPS should be used as the height source
-		if (((_params.vdist_sensor_type == VDIST_SENSOR_GPS)) && !_gps_hgt_faulty) {
-			_control_status.flags.baro_hgt = false;
-			_control_status.flags.gps_hgt = true;
-			_control_status.flags.rng_hgt = false;
-			_control_status.flags.ev_hgt = false;
-			_fuse_height = true;
-
-		}
 	} else {
 		// handle the case where we do not have GPS and have not been using it for an extended period, but are still relying on it
 		if ((_time_last_imu - _time_last_gps > 10e6) && (_time_last_imu - _time_last_airspeed > 1e6) && (_time_last_imu - _time_last_optflow > 1e6) && _control_status.flags.gps) {
@@ -531,10 +514,7 @@ void Ekf::controlHeightSensorTimeouts()
 				_gps_hgt_faulty = false;

 				// reset the height mode
-				_control_status.flags.baro_hgt = false;
-				_control_status.flags.gps_hgt = true;
-				_control_status.flags.rng_hgt = false;
-				_control_status.flags.ev_hgt = false;
+				setControlGPSHeight();

 				// request a reset
 				reset_height = true;
@@ -545,10 +525,7 @@ void Ekf::controlHeightSensorTimeouts()
 				_baro_hgt_faulty = false;

 				// reset the height mode
-				_control_status.flags.baro_hgt = true;
-				_control_status.flags.gps_hgt = false;
-				_control_status.flags.rng_hgt = false;
-				_control_status.flags.ev_hgt = false;
+				setControlBaroHeight();

 				// request a reset
 				reset_height = true;
@@ -590,10 +567,7 @@ void Ekf::controlHeightSensorTimeouts()
 				_baro_hgt_faulty = false;

 				// reset the height mode
-				_control_status.flags.baro_hgt = true;
-				_control_status.flags.gps_hgt = false;
-				_control_status.flags.rng_hgt = false;
-				_control_status.flags.ev_hgt = false;
+				setControlBaroHeight();

 				// request a reset
 				reset_height = true;
@@ -604,10 +578,7 @@ void Ekf::controlHeightSensorTimeouts()
 				_gps_hgt_faulty = false;

 				// reset the height mode
-				_control_status.flags.baro_hgt = false;
-				_control_status.flags.gps_hgt = true;
-				_control_status.flags.rng_hgt = false;
-				_control_status.flags.ev_hgt = false;
+				setControlGPSHeight();

 				// request a reset
 				reset_height = true;
@@ -642,10 +613,7 @@ void Ekf::controlHeightSensorTimeouts()
 				_baro_hgt_faulty = false;

 				// reset the height mode
-				_control_status.flags.baro_hgt = true;
-				_control_status.flags.gps_hgt = false;
-				_control_status.flags.rng_hgt = false;
-				_control_status.flags.ev_hgt = false;
+				setControlBaroHeight();

 				// request a reset
 				reset_height = true;
@@ -656,10 +624,7 @@ void Ekf::controlHeightSensorTimeouts()
 				_rng_hgt_faulty = false;

 				// reset the height mode
-				_control_status.flags.baro_hgt = false;
-				_control_status.flags.gps_hgt = false;
-				_control_status.flags.rng_hgt = true;
-				_control_status.flags.ev_hgt = false;
+				setControlRangeHeight();

 				// request a reset
 				reset_height = true;
@@ -694,10 +659,7 @@ void Ekf::controlHeightSensorTimeouts()
 				_baro_hgt_faulty = false;

 				// reset the height mode
-				_control_status.flags.baro_hgt = true;
-				_control_status.flags.gps_hgt = false;
-				_control_status.flags.rng_hgt = false;
-				_control_status.flags.ev_hgt = false;
+				setControlBaroHeight();

 				// request a reset
 				reset_height = true;
@@ -705,10 +667,7 @@ void Ekf::controlHeightSensorTimeouts()

 			} else if (reset_to_ev) {
 				// reset the height mode
-				_control_status.flags.baro_hgt = false;
-				_control_status.flags.gps_hgt = false;
-				_control_status.flags.rng_hgt = false;
-				_control_status.flags.ev_hgt = true;
+				setControlEVHeight();

 				// request a reset
 				reset_height = true;
@@ -732,57 +691,87 @@ void Ekf::controlHeightSensorTimeouts()
 	}
 }

-void Ekf::controlBaroFusion()
+void Ekf::controlHeightFusion()
 {
-	if (_baro_data_ready) {
-		// determine if we should use the baro as our height source
-		uint64_t last_baro_time_us = _baro_sample_delayed.time_us;
-		if (((_params.vdist_sensor_type == VDIST_SENSOR_BARO) || _control_status.flags.baro_hgt) && !_baro_hgt_faulty) {
-			_control_status.flags.baro_hgt = true;
-			_control_status.flags.gps_hgt = false;
-			_control_status.flags.rng_hgt = false;
-			_control_status.flags.ev_hgt = false;
-			_fuse_height = true;
+	// set control flags for the desired primary height source

-		}
-
-		// calculate a filtered offset between the baro origin and local NED origin if we are not using the baro as a height reference
-		if (!_control_status.flags.baro_hgt) {
-			float local_time_step = 1e-6f*(float)(_baro_sample_delayed.time_us - last_baro_time_us);
-			local_time_step = math::constrain(local_time_step,0.0f,1.0f);
-			last_baro_time_us = _baro_sample_delayed.time_us;
-			float offset_rate_correction =  0.1f * (_baro_sample_delayed.hgt - _hgt_sensor_offset) + _state.pos(2) - _baro_hgt_offset;
-			_baro_hgt_offset += local_time_step * math::constrain(offset_rate_correction, -0.1f, 0.1f);
-
-		}
-	}
-}
-
-void Ekf::controlRangeFinderFusion()
-{
-	// determine if we should use range finder data for height
 	if (_range_data_ready) {
-		// set the height data source to range if requested
-		if ((_params.vdist_sensor_type == VDIST_SENSOR_RANGE) && !_rng_hgt_faulty) {
-			_control_status.flags.baro_hgt = false;
-			_control_status.flags.gps_hgt = false;
-			_control_status.flags.rng_hgt = true;
-			_control_status.flags.ev_hgt = false;
-
-		}
-
 		// correct the range data for position offset relative to the IMU
 		Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body;
 		Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
 		_range_sample_delayed.rng += pos_offset_earth(2) / _R_rng_to_earth_2_2;
+	}

-		// only use range finder as a height observation in the main filter if specifically enabled
-		if (_control_status.flags.rng_hgt) {
+
+	if (_params.vdist_sensor_type == VDIST_SENSOR_BARO) {
+		bool can_use_range = rangeAidConditionsMet();
+
+		if (can_use_range && _range_data_ready && !_rng_hgt_faulty) {
+			setControlRangeHeight();
 			_fuse_height = true;

-		}
+			// we have just switched to using range finder, calculate height sensor offset such that current
+			// measurment matches our current height estimate
+			if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) {
+				_hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2);
+			}

-	} else if ((_time_last_imu - _time_last_hgt_fuse) > 2 * RNG_MAX_INTERVAL && _control_status.flags.rng_hgt) {
+		} else if (_baro_data_ready && !_baro_hgt_faulty && !can_use_range) {
+			setControlBaroHeight();
+			_fuse_height = true;
+
+			// we have just switched to using baro height, calculate height sensor offset such that current
+			// measurment matches our current height estimate
+			if (_control_status_prev.flags.baro_hgt != _control_status.flags.baro_hgt) {
+				_hgt_sensor_offset = _baro_sample_delayed.hgt - _baro_hgt_offset + _state.pos(2);
+			}
+		}
+	}
+
+	// set the height data source to range if requested
+	if ((_params.vdist_sensor_type == VDIST_SENSOR_RANGE) && !_rng_hgt_faulty) {
+		setControlRangeHeight();
+		_fuse_height = _range_data_ready;
+	}
+
+	// Determine if GPS should be used as the height source
+	if (_params.vdist_sensor_type == VDIST_SENSOR_GPS) {
+		bool can_use_range = rangeAidConditionsMet();
+
+		if (can_use_range && _range_data_ready && !_rng_hgt_faulty) {
+			setControlRangeHeight();
+			_fuse_height = true;
+
+			// we have just switched to using range finder, calculate height sensor offset such that current
+			// measurment matches our current height estimate
+			if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) {
+				_hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2);
+			}
+
+		} else if (_gps_data_ready && !_gps_hgt_faulty && !can_use_range) {
+			setControlGPSHeight();
+			_fuse_height = true;
+
+			// we have just switched to using gps height, calculate height sensor offset such that current
+			// measurment matches our current height estimate
+			if (_control_status_prev.flags.gps_hgt != _control_status.flags.gps_hgt) {
+				_hgt_sensor_offset = _gps_sample_delayed.hgt - _gps_alt_ref + _state.pos(2);
+			}
+		}
+	}
+
+	// calculate a filtered offset between the baro origin and local NED origin if we are not using the baro as a height reference
+	if (!_control_status.flags.baro_hgt && _baro_data_ready) {
+		float local_time_step = 1e-6f * _delta_time_baro_us;
+		local_time_step = math::constrain(local_time_step, 0.0f, 1.0f);
+
+		// apply a 10 second first order low pass filter to baro offset
+		float offset_rate_correction =  0.1f * (_baro_sample_delayed.hgt - _hgt_sensor_offset + _state.pos(
+				2) - _baro_hgt_offset);
+		_baro_hgt_offset += local_time_step * math::constrain(offset_rate_correction, -0.1f, 0.1f);
+	}
+
+	if ((_time_last_imu - _time_last_hgt_fuse) > 2 * RNG_MAX_INTERVAL && _control_status.flags.rng_hgt && !_range_data_ready) {
 		// If we are supposed to be using range finder data as the primary height sensor, have missed or rejected measurements
 		// and are on the ground, then synthesise a measurement at the expected on ground value
 		if (!_control_status.flags.in_air) {
@@ -792,7 +781,40 @@ void Ekf::controlRangeFinderFusion()
 		}

 		_fuse_height = true;
+	}

+
+}
+
+bool Ekf::rangeAidConditionsMet()
+{
+	// if the parameter for range aid is enabled we allow to switch from using the primary height source to using range finder as height source
+	// under the following conditions
+	// 1) we are not further than max_range_for_dual_fusion away from the ground
+	// 2) our ground speed is not higher than max_vel_for_dual_fusion
+	// 3) Our terrain estimate is stable (needs better checks)
+	if (_params.range_aid) {
+		// check if we should use range finder measurements to estimate height
+		bool use_range_finder = (_terrain_vpos - _state.pos(2) < _params.max_hagl_for_range_aid) && _terrain_initialised;
+
+		bool horz_vel_valid = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow)
+				      && (_fault_status.value == 0);
+
+		if (horz_vel_valid) {
+			float ground_vel = sqrtf(_state.vel(0) * _state.vel(0) + _state.vel(1) * _state.vel(1));
+
+			use_range_finder &= ground_vel < _params.max_vel_for_range_aid;
+
+		} else {
+			use_range_finder = false;
+		}
+
+		// XXX need a way to tell if terrain is stable
+		use_range_finder &= fabsf(_hagl_innov) < 0.3f;
+
+		return use_range_finder;
+	} else {
+		return false;
 	}
 }

@@ -502,6 +502,11 @@ private:
 	// control for height sensor timeouts, sensor changes and state resets
 	void controlHeightSensorTimeouts();

+	// control for combined height fusion mode (implemented for switching between baro and range height)
+	void controlHeightFusion();
+
+	bool rangeAidConditionsMet();
+
 	// return the square of two floating point numbers - used in auto coded sections
 	inline float sq(float var)
 	{