WIP: ekf2 allow initialising without baro depending on configuration

src/lib/mathlib/math/filter/MedianFilter.hpp

@@ -48,7 +48,7 @@
 namespace math
 {
 
-template<typename T, int WINDOW = 3>
+template<typename T, size_t WINDOW = 3>
 class MedianFilter
 {
 public:
@@ -78,6 +78,8 @@ public:
 		return median();
 	}
 
+	size_t window_size() const { return WINDOW; }
+
 private:
 
 	static int cmp(const void *a, const void *b)

src/modules/ekf2/EKF/RingBuffer.h

@@ -153,6 +153,27 @@ public:
 		return false;
 	}
 
+	bool peek_first_older_than(const uint64_t &timestamp, data_type *sample) const
+	{
+		// start looking from newest observation data
+		for (uint8_t i = 0; i < _size; i++) {
+			int index = (_head - i);
+			index = index < 0 ? _size + index : index;
+
+			if (timestamp >= _buffer[index].time_us && timestamp < _buffer[index].time_us + (uint64_t)1e5) {
+				*sample = _buffer[index];
+				return true;
+			}
+
+			if (index == _tail) {
+				// we have reached the tail and haven't got a match
+				return false;
+			}
+		}
+
+		return false;
+	}
+
 	int get_total_size() const { return sizeof(*this) + sizeof(data_type) * _size; }
 
 private:

src/modules/ekf2/EKF/control.cpp

@@ -94,11 +94,18 @@ void Ekf::controlFusionModes()
 
 	// check for arrival of new sensor data at the fusion time horizon
 	_time_prev_gps_us = _gps_sample_delayed.time_us;
+
+	const uint64_t delta_time_prev_gps_us = _gps_sample_delayed.time_us;
 	_gps_data_ready = _gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed);
+
+	// if we have a new sample save the delta time between this sample and the last sample which is used for height offset calculations
+	if (_gps_data_ready && (delta_time_prev_gps_us != 0)) {
+		_delta_time_gps_us = _gps_sample_delayed.time_us - delta_time_prev_gps_us;
+	}
+
 	_mag_data_ready = _mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed);
 
 	if (_mag_data_ready) {
-		_mag_lpf.update(_mag_sample_delayed.mag);
 
 		// if enabled, use knowledge of theoretical magnetic field vector to calculate a synthetic magnetomter Z component value.
 		// this is useful if there is a lot of interference on the sensor measurement.
@@ -114,18 +121,25 @@ void Ekf::controlFusionModes()
 		}
 	}
 
-	_delta_time_baro_us = _baro_sample_delayed.time_us;
+	const uint64_t delta_time_prev_baro_us = _baro_sample_delayed.time_us;
 	_baro_data_ready = _baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed);
 
 	// if we have a new baro sample save the delta time between this sample and the last sample which is
 	// used below for baro offset calculations
-	if (_baro_data_ready) {
-		_delta_time_baro_us = _baro_sample_delayed.time_us - _delta_time_baro_us;
+	if (_baro_data_ready && (delta_time_prev_baro_us != 0)) {
+		_delta_time_baro_us = _baro_sample_delayed.time_us - delta_time_prev_baro_us;
 	}
 
 	{
 		// Get range data from buffer and check validity
+		const uint64_t delta_time_prev_rng_us = _range_sensor.sample().time_us;
 		const bool is_rng_data_ready = _range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, _range_sensor.getSampleAddress());
+
+		// if we have a new sample save the delta time between this sample and the last sample which is used for height offset calculations
+		if (is_rng_data_ready && (delta_time_prev_rng_us != 0)) {
+			_delta_time_rng_us = _range_sensor.sample().time_us - delta_time_prev_rng_us;
+		}
+
 		_range_sensor.setDataReadiness(is_rng_data_ready);
 
 		// update range sensor angle parameters in case they have changed
@@ -160,7 +174,14 @@ void Ekf::controlFusionModes()
 		_flow_for_terrain_data_ready &= (!_control_status.flags.opt_flow && _control_status.flags.gps);
 	}
 
+	const uint64_t delta_time_prev_ev_us = _ev_sample_delayed.time_us;
 	_ev_data_ready = _ext_vision_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_ev_sample_delayed);
+
+	// if we have a new EV sample save the delta time between this sample and the last sample which is used for height offset calculations
+	if (_ev_data_ready && (delta_time_prev_ev_us != 0)) {
+		_delta_time_ev_us = _ev_sample_delayed.time_us - delta_time_prev_ev_us;
+	}
+
 	_tas_data_ready = _airspeed_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_airspeed_sample_delayed);
 
 	// check for height sensor timeouts and reset and change sensor if necessary
@@ -891,7 +912,7 @@ void Ekf::controlHeightSensorTimeouts()
 
 			// check the baro height source for consistency and freshness
 			const baroSample &baro_init = _baro_buffer.get_newest();
-			const float baro_innov = _state.pos(2) - (_hgt_sensor_offset - baro_init.hgt + _baro_hgt_offset);
+			const float baro_innov = _state.pos(2) - (_gps_hgt_offset - baro_init.hgt + _baro_hgt_offset);
 			const bool baro_data_consistent = fabsf(baro_innov) < (sq(_params.baro_noise) + P(9, 9)) * sq(_params.baro_innov_gate);
 
 			// if baro data is acceptable and GPS data is inaccurate, reset height to baro
@@ -1030,40 +1051,28 @@ void Ekf::checkVerticalAccelerationHealth()
 
 void Ekf::controlHeightFusion()
 {
+	bool do_range_aid = false;
+
+	if (_params.range_aid == 1) {
 		checkRangeAidSuitability();
-	const bool do_range_aid = (_params.range_aid == 1) && isRangeAidSuitable();
 
-	bool fuse_height = false;
+		if (isRangeAidSuitable()) {
+			do_range_aid = true;
+		}
+	}
 
+	if (do_range_aid && !_control_status_prev.flags.rng_hgt && _range_sensor.isDataHealthy()) {
+		setControlRangeHeight();
+
+	} else {
 		switch (_params.vdist_sensor_type) {
 		default:
 			ECL_ERR("Invalid hgt mode: %d", _params.vdist_sensor_type);
 
 		// FALLTHROUGH
 		case VDIST_SENSOR_BARO:
-		if (do_range_aid && _range_sensor.isDataHealthy()) {
-			setControlRangeHeight();
-			fuse_height = true;
-
-			// we have just switched to using range finder, calculate height sensor offset such that current
-			// measurement matches our current height estimate
-			if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) {
-				_hgt_sensor_offset = _terrain_vpos;
-			}
-
-		} else if (!do_range_aid && _baro_data_ready && !_baro_hgt_faulty) {
+			if (!do_range_aid && _baro_data_ready && !_baro_hgt_faulty) {
 				startBaroHgtFusion();
-			fuse_height = true;
-
-		} else if (_control_status.flags.gps_hgt && _gps_data_ready && !_gps_hgt_intermittent) {
-			// switch to gps if there was a reset to gps
-			fuse_height = true;
-
-			// we have just switched to using gps height, calculate height sensor offset such that current
-			// measurement 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);
-			}
 			}
 
 			break;
@@ -1071,44 +1080,23 @@ void Ekf::controlHeightFusion()
 		case VDIST_SENSOR_RANGE:
 			if (_range_sensor.isDataHealthy()) {
 				setControlRangeHeight();
-			fuse_height = true;
 
-			if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) {
-				// we have just switched to using range finder, calculate height sensor offset such that current
-				// measurement matches our current height estimate
-				// use the parameter rng_gnd_clearance if on ground to avoid a noisy offset initialization (e.g. sonar)
-				if (_control_status.flags.in_air && isTerrainEstimateValid()) {
-					_hgt_sensor_offset = _terrain_vpos;
-
-				} else if (_control_status.flags.in_air) {
-					_hgt_sensor_offset = _range_sensor.getDistBottom() + _state.pos(2);
-
-				} else {
-					_hgt_sensor_offset = _params.rng_gnd_clearance;
-				}
-			}
-
-		} else if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) {
-			// fuse baro data if there was a reset to baro
-			fuse_height = true;
+			} else if (isTimedOut(_time_last_hgt_fuse, 2 * RNG_MAX_INTERVAL) && _range_sensor.isRegularlySendingData() && !_control_status.flags.in_air) {
+				// 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
+				_range_sensor.setRange(_params.rng_gnd_clearance);
+				_range_sensor.setDataReadiness(true);
+				_range_sensor.setValidity(true); // bypass the checks
 			}
 
 			break;
 
 		case VDIST_SENSOR_GPS:
-
 			// NOTE: emergency fallback due to extended loss of currently selected sensor data or failure
-		// to pass innovation cinsistency checks is handled elsewhere in Ekf::controlHeightSensorTimeouts.
+			// to pass innovation inconsistency checks is handled elsewhere in Ekf::controlHeightSensorTimeouts.
 			// Do switching between GPS and rangefinder if using range finder as a height source when close
 			// to ground and moving slowly. Also handle switch back from emergency Baro sensor when GPS recovers.
-		if (!_control_status_prev.flags.rng_hgt && do_range_aid && _range_sensor.isDataHealthy()) {
-			setControlRangeHeight();
-
-			// we have just switched to using range finder, calculate height sensor offset such that current
-			// measurement matches our current height estimate
-			_hgt_sensor_offset = _terrain_vpos;
-
-		} else if (_control_status_prev.flags.rng_hgt && !do_range_aid) {
+			if (_control_status_prev.flags.rng_hgt) {
 				// must stop using range finder so find another sensor now
 				if (!_gps_hgt_intermittent && _gps_checks_passed) {
 					// GPS quality OK
@@ -1119,76 +1107,35 @@ void Ekf::controlHeightFusion()
 					startBaroHgtFusion();
 				}
 
-		} else if (_control_status.flags.baro_hgt && !do_range_aid && !_gps_hgt_intermittent && _gps_checks_passed) {
+			} else if (_control_status.flags.baro_hgt && !_gps_hgt_intermittent && _gps_checks_passed) {
 				// In baro fallback mode and GPS has recovered so start using it
 				startGpsHgtFusion();
 			}
 
-		if (_control_status.flags.gps_hgt && _gps_data_ready) {
-			fuse_height = true;
-
-		} else if (_control_status.flags.rng_hgt && _range_sensor.isDataHealthy()) {
-			fuse_height = true;
-
-		} else if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) {
-			fuse_height = true;
-		}
-
 			break;
 
 		case VDIST_SENSOR_EV:
-
 			// don't start using EV data unless data is arriving frequently, do not reset if pref mode was height
 			if (!_control_status.flags.ev_hgt && isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL)) {
-			fuse_height = true;
 				setControlEVHeight();
-
-			if (!_control_status_prev.flags.rng_hgt) {
-				resetHeight();
-			}
-		}
-
-		if (_control_status.flags.ev_hgt && _ev_data_ready) {
-			fuse_height = true;
-
-		} else if (_control_status.flags.rng_hgt && _range_sensor.isDataHealthy()) {
-			fuse_height = true;
-
-		} else if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) {
-			fuse_height = true;
 			}
 
 			break;
 		}
-
-	updateBaroHgtOffset();
-
-	if (_control_status.flags.rng_hgt
-	    && isTimedOut(_time_last_hgt_fuse, 2 * RNG_MAX_INTERVAL)
-	    && !_range_sensor.isDataHealthy()
-	    && _range_sensor.isRegularlySendingData()
-	    && !_control_status.flags.in_air) {
-
-		// 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
-		_range_sensor.setRange(_params.rng_gnd_clearance);
-		_range_sensor.setDataReadiness(true);
-		_range_sensor.setValidity(true); // bypass the checks
-
-		fuse_height = true;
 	}
 
-	if (fuse_height) {
-		if (_control_status.flags.baro_hgt) {
-			Vector2f baro_hgt_innov_gate;
-			Vector3f baro_hgt_obs_var;
+
+	if (_baro_data_ready && !_baro_hgt_faulty) {
+		if (!PX4_ISFINITE(_baro_hgt_offset)) {
+			_baro_hgt_offset = _state.pos(2) + _baro_sample_delayed.hgt;
+		}
+
+		if (!_control_status_prev.flags.baro_hgt && _control_status.flags.baro_hgt) {
+			ECL_INFO("baro height offset %.3f", (double)_baro_hgt_offset);
+		}
 
 		// vertical position innovation - baro measurement has opposite sign to earth z axis
 		_baro_hgt_innov(2) = _state.pos(2) + _baro_sample_delayed.hgt - _baro_hgt_offset;
-			// observation variance - user parameter defined
-			baro_hgt_obs_var(2) = sq(fmaxf(_params.baro_noise, 0.01f));
-			// innovation gate size
-			baro_hgt_innov_gate(1) = fmaxf(_params.baro_innov_gate, 1.0f);
 
 		// Compensate for positive static pressure transients (negative vertical position innovations)
 		// caused by rotor wash ground interaction by applying a temporary deadzone to baro innovations.
@@ -1206,49 +1153,125 @@ void Ekf::controlHeightFusion()
 			}
 		}
 
-			// fuse height information
-			fuseVerticalPosition(_baro_hgt_innov, baro_hgt_innov_gate,
-					     baro_hgt_obs_var, _baro_hgt_innov_var, _baro_hgt_test_ratio);
-
-		} else if (_control_status.flags.gps_hgt) {
-			Vector2f gps_hgt_innov_gate;
-			Vector3f gps_hgt_obs_var;
-			// vertical position innovation - gps measurement has opposite sign to earth z axis
-			_gps_pos_innov(2) = _state.pos(2) + _gps_sample_delayed.hgt - _gps_alt_ref - _hgt_sensor_offset;
-			gps_hgt_obs_var(2) = getGpsHeightVariance();
+		if (_control_status.flags.baro_hgt) {
 			// innovation gate size
-			gps_hgt_innov_gate(1) = fmaxf(_params.baro_innov_gate, 1.0f);
-			// fuse height information
-			fuseVerticalPosition(_gps_pos_innov, gps_hgt_innov_gate,
-					     gps_hgt_obs_var, _gps_pos_innov_var, _gps_pos_test_ratio);
+			Vector2f baro_hgt_innov_gate{0, fmaxf(_params.baro_innov_gate, 1.f)};
 
-		} else if (_control_status.flags.rng_hgt) {
-			Vector2f rng_hgt_innov_gate;
-			Vector3f rng_hgt_obs_var;
-			// use range finder with tilt correction
-			_rng_hgt_innov(2) = _state.pos(2) - (-math::max(_range_sensor.getDistBottom(),
-							     _params.rng_gnd_clearance)) - _hgt_sensor_offset;
 			// observation variance - user parameter defined
-			rng_hgt_obs_var(2) = fmaxf(sq(_params.range_noise)
-						   + sq(_params.range_noise_scaler * _range_sensor.getDistBottom()), 0.01f);
-			// innovation gate size
-			rng_hgt_innov_gate(1) = fmaxf(_params.range_innov_gate, 1.0f);
-			// fuse height information
-			fuseVerticalPosition(_rng_hgt_innov, rng_hgt_innov_gate,
-					     rng_hgt_obs_var, _rng_hgt_innov_var, _rng_hgt_test_ratio);
+			Vector3f baro_hgt_obs_var{0, 0, sq(fmaxf(_params.baro_noise, 0.01f))};
 
-		} else if (_control_status.flags.ev_hgt) {
-			Vector2f ev_hgt_innov_gate;
-			Vector3f ev_hgt_obs_var;
-			// calculate the innovation assuming the external vision observation is in local NED frame
-			_ev_pos_innov(2) = _state.pos(2) - _ev_sample_delayed.pos(2);
-			// observation variance - defined externally
-			ev_hgt_obs_var(2) = fmaxf(_ev_sample_delayed.posVar(2), sq(0.01f));
-			// innovation gate size
-			ev_hgt_innov_gate(1) = fmaxf(_params.ev_pos_innov_gate, 1.0f);
 			// fuse height information
-			fuseVerticalPosition(_ev_pos_innov, ev_hgt_innov_gate,
-					     ev_hgt_obs_var, _ev_pos_innov_var, _ev_pos_test_ratio);
+			fuseVerticalPosition(_baro_hgt_innov, baro_hgt_innov_gate, baro_hgt_obs_var, _baro_hgt_innov_var, _baro_hgt_test_ratio);
+
+		} else if (!_control_status.flags.baro_hgt && (_delta_time_baro_us != 0)) {
+			// calculate a filtered offset between the baro origin and local NED origin if we are not using the baro as a height reference
+			const float local_time_step = math::constrain(1e-6f * _delta_time_baro_us, 0.f, 1.f);
+
+			// apply a 10 second first order low pass filter to height offset
+			_baro_hgt_offset += local_time_step * math::constrain(0.1f * _baro_hgt_innov(2), -0.1f, 0.1f);
+		}
+	}
+
+	if (_gps_data_ready && !_gps_hgt_intermittent) {
+		if (!PX4_ISFINITE(_gps_hgt_offset)) {
+			_gps_hgt_offset = _state.pos(2) + _gps_sample_delayed.hgt - _gps_alt_ref;
+		}
+
+		if (!_control_status_prev.flags.gps_hgt && _control_status.flags.gps_hgt) {
+			ECL_INFO("GPS height offset %.3f, Altitude ref: %.3f", (double)_gps_hgt_offset, (double)_gps_alt_ref);
+		}
+
+		// vertical position innovation - gps measurement has opposite sign to earth z axis
+		_gps_pos_innov(2) = _state.pos(2) + _gps_sample_delayed.hgt - _gps_alt_ref - _gps_hgt_offset;
+
+		if (_control_status.flags.gps_hgt) {
+			// innovation gate size
+			Vector2f gps_hgt_innov_gate{0, fmaxf(_params.baro_innov_gate, 1.f)};
+
+			// observation variance
+			Vector3f gps_hgt_obs_var{0, 0, getGpsHeightVariance()};
+
+			// fuse height information
+			fuseVerticalPosition(_gps_pos_innov, gps_hgt_innov_gate, gps_hgt_obs_var, _gps_pos_innov_var, _gps_pos_test_ratio);
+
+		} else if (!_control_status.flags.gps_hgt && (_delta_time_gps_us != 0)) {
+			// calculate a filtered offset between the GPS origin and local NED origin if we are not using the GPS as a height reference
+			const float local_time_step = math::constrain(1e-6f * _delta_time_gps_us, 0.f, 1.f);
+
+			// apply a 10 second first order low pass filter to height offset
+			_gps_hgt_offset += local_time_step * math::constrain(0.1f * _gps_pos_innov(2), -0.1f, 0.1f);
+		}
+	}
+
+	if (_range_sensor.isDataHealthy()) {
+		if (!PX4_ISFINITE(_rng_hgt_offset)) {
+			// calculate height sensor offset such that current measurement matches our current height estimate
+			// use the parameter rng_gnd_clearance if on ground to avoid a noisy offset initialization (e.g. sonar)
+			if (_control_status.flags.in_air && isTerrainEstimateValid()) {
+				_rng_hgt_offset = _terrain_vpos;
+
+			} else if (_control_status.flags.in_air) {
+				_rng_hgt_offset = _range_sensor.getDistBottom() + _state.pos(2);
+
+			} else {
+				_rng_hgt_offset = _params.rng_gnd_clearance;
+			}
+		}
+
+		if (!_control_status_prev.flags.rng_hgt && _control_status.flags.rng_hgt) {
+			ECL_INFO("RNG height offset %.3f", (double)_rng_hgt_offset);
+		}
+
+		// use range finder with tilt correction
+		_rng_hgt_innov(2) = _state.pos(2) - (-math::max(_range_sensor.getDistBottom(), _params.rng_gnd_clearance)) - _rng_hgt_offset;
+
+		if (_control_status.flags.rng_hgt) {
+			// innovation gate size
+			Vector2f rng_hgt_innov_gate{0, fmaxf(_params.range_innov_gate, 1.f)};
+
+			// observation variance - user parameter defined
+			Vector3f rng_hgt_obs_var{0, 0, fmaxf(sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sensor.getDistBottom()), 0.01f)};
+
+			// fuse height information
+			fuseVerticalPosition(_rng_hgt_innov, rng_hgt_innov_gate, rng_hgt_obs_var, _rng_hgt_innov_var, _rng_hgt_test_ratio);
+
+		} else if (!_control_status.flags.rng_hgt && (_delta_time_rng_us != 0)) {
+			// calculate a filtered offset between the range origin and local NED origin if we are not using the range as a height reference
+			const float local_time_step = math::constrain(1e-6f * _delta_time_rng_us, 0.f, 1.f);
+
+			// apply a 10 second first order low pass filter to height offset
+			_rng_hgt_offset += local_time_step * math::constrain(0.1f * _rng_hgt_innov(2), -0.1f, 0.1f);
+		}
+	}
+
+	if (_ev_data_ready) {
+		if (!PX4_ISFINITE(_ev_hgt_offset)) {
+			_ev_hgt_offset = _state.pos(2) - _ev_sample_delayed.pos(2);
+		}
+
+		if (!_control_status_prev.flags.ev_hgt && _control_status.flags.ev_hgt) {
+			ECL_INFO("EV height offset %.3f", (double)_ev_hgt_offset);
+		}
+
+		// calculate the innovation assuming the external vision observation is in local NED frame
+		_ev_pos_innov(2) = _state.pos(2) - _ev_sample_delayed.pos(2) - _ev_hgt_offset;
+
+		if (_control_status.flags.ev_hgt && _ev_data_ready) {
+			// innovation gate size
+			Vector2f ev_hgt_innov_gate{0, fmaxf(_params.ev_pos_innov_gate, 1.f)};
+
+			// observation variance - defined externally
+			Vector3f ev_hgt_obs_var{0, 0, fmaxf(_ev_sample_delayed.posVar(2), sq(0.01f))};
+
+			// fuse height information
+			fuseVerticalPosition(_ev_pos_innov, ev_hgt_innov_gate, ev_hgt_obs_var, _ev_pos_innov_var, _ev_pos_test_ratio);
+
+		} else if (!_control_status.flags.ev_hgt && (_delta_time_ev_us != 0)) {
+			// calculate a filtered offset between the EV height origin and local NED origin if we are not using EV as a height reference
+			const float local_time_step = math::constrain(1e-6f * _delta_time_ev_us, 0.f, 1.f);
+
+			// apply a 10 second first order low pass filter to height offset
+			_ev_hgt_offset += local_time_step * math::constrain(0.1f * _ev_pos_innov(2), -0.1f, 0.1f);
 		}
 	}
 }

src/modules/ekf2/EKF/ekf.cpp

@@ -89,6 +89,9 @@ bool Ekf::update()
 {
 	bool updated = false;
 
+	// Only run the filter if IMU data in the buffer has been updated
+	if (_imu_updated) {
+
 		if (!_filter_initialised) {
 			_filter_initialised = initialiseFilter();
 
@@ -97,8 +100,6 @@ bool Ekf::update()
 			}
 		}
 
-	// Only run the filter if IMU data in the buffer has been updated
-	if (_imu_updated) {
 		// perform state and covariance prediction for the main filter
 		predictState();
 		predictCovariance();
@@ -115,59 +116,30 @@ bool Ekf::update()
 		runYawEKFGSF();
 	}
 
+	if (_filter_initialised) {
 		// the output observer always runs
 		// Use full rate IMU data at the current time horizon
 		calculateOutputStates(_newest_high_rate_imu_sample);
+	}
 
 	return updated;
 }
 
 bool Ekf::initialiseFilter()
 {
-	// Filter accel for tilt initialization
-	const imuSample &imu_init = _imu_buffer.get_newest();
-
 	// protect against zero data
-	if (imu_init.delta_vel_dt < 1e-4f || imu_init.delta_ang_dt < 1e-4f) {
+	if (_imu_sample_delayed.delta_vel_dt < 1e-4f || _imu_sample_delayed.delta_ang_dt < 1e-4f) {
 		return false;
 	}
 
 	if (_is_first_imu_sample) {
-		_accel_lpf.reset(imu_init.delta_vel / imu_init.delta_vel_dt);
-		_gyro_lpf.reset(imu_init.delta_ang / imu_init.delta_ang_dt);
+		_accel_lpf.reset(_imu_sample_delayed.delta_vel / _imu_sample_delayed.delta_vel_dt);
+		_gyro_lpf.reset(_imu_sample_delayed.delta_ang / _imu_sample_delayed.delta_ang_dt);
 		_is_first_imu_sample = false;
 
 	} else {
-		_accel_lpf.update(imu_init.delta_vel / imu_init.delta_vel_dt);
-		_gyro_lpf.update(imu_init.delta_ang / imu_init.delta_ang_dt);
-	}
-
-	// Sum the magnetometer measurements
-	if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) {
-		if (_mag_sample_delayed.time_us != 0) {
-			if (_mag_counter == 0) {
-				_mag_lpf.reset(_mag_sample_delayed.mag);
-
-			} else {
-				_mag_lpf.update(_mag_sample_delayed.mag);
-			}
-
-			_mag_counter++;
-		}
-	}
-
-	// accumulate enough height measurements to be confident in the quality of the data
-	if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) {
-		if (_baro_sample_delayed.time_us != 0) {
-			if (_baro_counter == 0) {
-				_baro_hgt_offset = _baro_sample_delayed.hgt;
-
-			} else {
-				_baro_hgt_offset = 0.9f * _baro_hgt_offset + 0.1f * _baro_sample_delayed.hgt;
-			}
-
-			_baro_counter++;
-		}
+		_accel_lpf.update(_imu_sample_delayed.delta_vel / _imu_sample_delayed.delta_vel_dt);
+		_gyro_lpf.update(_imu_sample_delayed.delta_ang / _imu_sample_delayed.delta_ang_dt);
 	}
 
 	if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
@@ -177,13 +149,62 @@ bool Ekf::initialiseFilter()
 		}
 	}
 
-	if (_baro_counter < _obs_buffer_length) {
-		// not enough baro samples accumulated
+	if (_baro_hgt_counter > 0) {
+		_baro_hgt_offset = _baro_hgt_lpf.getState();
+	}
+
+	if (_gps_hgt_counter > 0) {
+		_gps_hgt_offset = _gps_hgt_lpf.getState();
+	}
+
+	if (_rng_hgt_counter >= _rng_hgt_filter.window_size()) {
+		_rng_hgt_offset = _rng_hgt_filter.median();
+	}
+
+	if (_ev_hgt_counter > 0) {
+		_ev_hgt_offset = _ev_hgt_lpf.getState();
+	}
+
+	switch (_params.vdist_sensor_type) {
+	default:
+
+	// FALLTHROUGH
+	case VDIST_SENSOR_BARO:
+		if (_baro_hgt_counter < _obs_buffer_length) {
+			// not enough samples accumulated
 			return false;
 		}
 
-	// we use baro height initially and switch to GPS/range/EV finder later when it passes checks.
 		setControlBaroHeight();
+		break;
+
+	case VDIST_SENSOR_GPS:
+		if (_gps_hgt_counter < _obs_buffer_length) {
+			// not enough samples accumulated
+			return false;
+		}
+
+		setControlGPSHeight();
+		break;
+
+	case VDIST_SENSOR_RANGE:
+		if (_rng_hgt_counter < _obs_buffer_length) {
+			// not enough samples accumulated
+			return false;
+		}
+
+		setControlRangeHeight();
+		break;
+
+	case VDIST_SENSOR_EV:
+		if (_ev_hgt_counter < _obs_buffer_length) {
+			// not enough samples accumulated
+			return false;
+		}
+
+		setControlEVHeight();
+		break;
+	}
 
 	if (!initialiseTilt()) {
 		return false;

src/modules/ekf2/EKF/ekf.h

@@ -378,7 +378,11 @@ private:
 	float _imu_collection_time_adj{0.0f};	///< the amount of time the IMU collection needs to be advanced to meet the target set by FILTER_UPDATE_PERIOD_MS (sec)
 
 	uint64_t _time_acc_bias_check{0};	///< last time the  accel bias check passed (uSec)
+
 	uint64_t _delta_time_baro_us{0};	///< delta time between two consecutive delayed baro samples from the buffer (uSec)
+	uint64_t _delta_time_gps_us{0};
+	uint64_t _delta_time_rng_us{0};
+	uint64_t _delta_time_ev_us{0};
 
 	Vector3f _earth_rate_NED{};	///< earth rotation vector (NED) in rad/s
 
@@ -491,15 +495,15 @@ private:
 
 	// Variables used by the initial filter alignment
 	bool _is_first_imu_sample{true};
-	uint32_t _baro_counter{0};		///< number of baro samples read during initialisation
-	uint32_t _mag_counter{0};		///< number of magnetometer samples read during initialisation
 	AlphaFilter<Vector3f> _accel_lpf{0.1f};	///< filtered accelerometer measurement used to align tilt (m/s/s)
 	AlphaFilter<Vector3f> _gyro_lpf{0.1f};	///< filtered gyro measurement used for alignment excessive movement check (rad/sec)
 
 	// Variables used to perform in flight resets and switch between height sources
 	AlphaFilter<Vector3f> _mag_lpf{0.1f};	///< filtered magnetometer measurement for instant reset (Gauss)
-	float _hgt_sensor_offset{0.0f};		///< set as necessary if desired to maintain the same height after a height reset (m)
-	float _baro_hgt_offset{0.0f};		///< baro height reading at the local NED origin (m)
+	float _baro_hgt_offset{NAN};		///< baro height reading at the local NED origin (m)
+	float _gps_hgt_offset{NAN};
+	float _rng_hgt_offset{NAN};
+	float _ev_hgt_offset{NAN};
 
 	// Variables used to control activation of post takeoff functionality
 	float _last_on_ground_posD{0.0f};	///< last vertical position when the in_air status was false (m)
@@ -889,8 +893,6 @@ private:
 	void startBaroHgtFusion();
 	void startGpsHgtFusion();
 
-	void updateBaroHgtOffset();
-
 	// return an estimation of the GPS altitude variance
 	float getGpsHeightVariance();
 

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -255,9 +255,6 @@ void Ekf::resetVerticalPositionTo(const float &new_vert_pos)
 // Reset height state using the last height measurement
 void Ekf::resetHeight()
 {
-	// Get the most recent GPS data
-	const gpsSample &gps_newest = _gps_buffer.get_newest();
-
 	// reset the vertical position
 	if (_control_status.flags.rng_hgt) {
 
@@ -265,33 +262,28 @@ void Ekf::resetHeight()
 		if (!_control_status_prev.flags.rng_hgt) {
 
 			if (_control_status.flags.in_air && isTerrainEstimateValid()) {
-				_hgt_sensor_offset = _terrain_vpos;
+				_rng_hgt_offset = _terrain_vpos;
 
 			} else if (_control_status.flags.in_air) {
-				_hgt_sensor_offset = _range_sensor.getDistBottom() + _state.pos(2);
+				_rng_hgt_offset = _range_sensor.getDistBottom() + _state.pos(2);
 
 			} else {
-				_hgt_sensor_offset = _params.rng_gnd_clearance;
+				_rng_hgt_offset = _params.rng_gnd_clearance;
 			}
 
 		}
 
 		// update the state and associated variance
-		resetVerticalPositionTo(_hgt_sensor_offset - _range_sensor.getDistBottom());
+		resetVerticalPositionTo(_rng_hgt_offset - _range_sensor.getDistBottom());
 
 		// the state variance is the same as the observation
 		P.uncorrelateCovarianceSetVariance<1>(9, sq(_params.range_noise));
 
-		// reset the baro offset which is subtracted from the baro reading if we need to use it as a backup
-		const baroSample &baro_newest = _baro_buffer.get_newest();
-		_baro_hgt_offset = baro_newest.hgt + _state.pos(2);
-
 	} else if (_control_status.flags.baro_hgt) {
-		// initialize vertical position with newest baro measurement
-		const baroSample &baro_newest = _baro_buffer.get_newest();
 
 		if (!_baro_hgt_faulty) {
-			resetVerticalPositionTo(-baro_newest.hgt + _baro_hgt_offset);
+			// initialize vertical position with newest baro measurement
+			resetVerticalPositionTo(-_baro_hgt_lpf.getState() + _baro_hgt_offset);
 
 			// the state variance is the same as the observation
 			P.uncorrelateCovarianceSetVariance<1>(9, sq(_params.baro_noise));
@@ -303,39 +295,65 @@ void Ekf::resetHeight()
 	} else if (_control_status.flags.gps_hgt) {
 		// initialize vertical position and velocity with newest gps measurement
 		if (!_gps_hgt_intermittent) {
-			resetVerticalPositionTo(_hgt_sensor_offset - gps_newest.hgt + _gps_alt_ref);
+			gpsSample gps_sample;
+
+			if (!_gps_buffer.peek_first_older_than(_imu_sample_delayed.time_us, &gps_sample)) {
+				gps_sample = _gps_buffer.get_newest();
+			}
+
+			resetVerticalPositionTo(_gps_hgt_offset - _gps_hgt_lpf.getState() + _gps_alt_ref);
 
 			// the state variance is the same as the observation
-			P.uncorrelateCovarianceSetVariance<1>(9, sq(gps_newest.vacc));
-
-			// reset the baro offset which is subtracted from the baro reading if we need to use it as a backup
-			const baroSample &baro_newest = _baro_buffer.get_newest();
-			_baro_hgt_offset = baro_newest.hgt + _state.pos(2);
+			P.uncorrelateCovarianceSetVariance<1>(9, sq(gps_sample.vacc));
 
 		} else {
 			// TODO: reset to last known gps based estimate
 		}
 
 	} else if (_control_status.flags.ev_hgt) {
-		// initialize vertical position with newest measurement
-		const extVisionSample &ev_newest = _ext_vision_buffer.get_newest();
+		extVisionSample ext_vision_sample;
 
-		// use the most recent data if it's time offset from the fusion time horizon is smaller
-		if (ev_newest.time_us >= _ev_sample_delayed.time_us) {
-			resetVerticalPositionTo(ev_newest.pos(2));
+		if (!_ext_vision_buffer.peek_first_older_than(_imu_sample_delayed.time_us, &ext_vision_sample)) {
+			ext_vision_sample = _ext_vision_buffer.get_newest();
+		}
 
-		} else {
-			resetVerticalPositionTo(_ev_sample_delayed.pos(2));
-		}
+		resetVerticalPositionTo(_ev_hgt_lpf.getState() + _ev_hgt_offset);
+
+		P.uncorrelateCovarianceSetVariance<1>(9, sq(ext_vision_sample.posVar(2)));
 	}
 
+	// reset the height sensor offsets which is subtracted from the readings if we need to use it as a backup
+	_baro_hgt_offset = _state.pos(2) + _baro_hgt_lpf.getState();
+	_gps_hgt_offset  = _state.pos(2) + _gps_hgt_lpf.getState() - _gps_alt_ref;
+	_rng_hgt_offset  = _state.pos(2) + _range_sensor.getDistBottom();
+	_ev_hgt_offset   = _state.pos(2) + _ev_hgt_lpf.getState();
+
+
 	// reset the vertical velocity state
 	if (_control_status.flags.gps && !_gps_hgt_intermittent) {
+		gpsSample gps_sample;
+
+		if (!_gps_buffer.peek_first_older_than(_imu_sample_delayed.time_us, &gps_sample)) {
+			gps_sample = _gps_buffer.get_newest();
+		}
+
 		// If we are using GPS, then use it to reset the vertical velocity
-		resetVerticalVelocityTo(gps_newest.vel(2));
+		resetVerticalVelocityTo(gps_sample.vel(2));
 
 		// the state variance is the same as the observation
-		P.uncorrelateCovarianceSetVariance<1>(6, sq(1.5f * gps_newest.sacc));
+		P.uncorrelateCovarianceSetVariance<1>(6, sq(1.5f * gps_sample.sacc));
+
+	} else if (_control_status.flags.ev_vel) {
+		extVisionSample ext_vision_sample;
+
+		if (!_ext_vision_buffer.peek_first_older_than(_imu_sample_delayed.time_us, &ext_vision_sample)) {
+			ext_vision_sample = _ext_vision_buffer.get_newest();
+		}
+
+		// use the most recent data if it's time offset from the fusion time horizon is smaller
+		resetVerticalVelocityTo(ext_vision_sample.vel(2));
+
+		P.uncorrelateCovarianceSetVariance<1>(6, math::max(getVisionVelocityVarianceInEkfFrame()(2), sq(0.05f)));
 
 	} else {
 		// we don't know what the vertical velocity is, so set it to zero
@@ -1287,10 +1305,6 @@ void Ekf::startBaroHgtFusion()
 {
 	setControlBaroHeight();
 
-	// We don't need to set a height sensor offset
-	// since we track a separate _baro_hgt_offset
-	_hgt_sensor_offset = 0.0f;
-
 	// Turn off ground effect compensation if it times out
 	if (_control_status.flags.gnd_effect) {
 		if (isTimedOut(_time_last_gnd_effect_on, GNDEFFECT_TIMEOUT)) {
@@ -1303,25 +1317,6 @@ void Ekf::startBaroHgtFusion()
 void Ekf::startGpsHgtFusion()
 {
 	setControlGPSHeight();
-
-	// we have just switched to using gps height, calculate height sensor offset such that current
-	// measurement 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);
-	}
-}
-
-void Ekf::updateBaroHgtOffset()
-{
-	// 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) {
-		const float local_time_step = math::constrain(1e-6f * _delta_time_baro_us, 0.0f, 1.0f);
-
-		// apply a 10 second first order low pass filter to baro offset
-		const float offset_rate_correction = 0.1f * (_baro_sample_delayed.hgt + _state.pos(2) - _baro_hgt_offset);
-		_baro_hgt_offset += local_time_step * math::constrain(offset_rate_correction, -0.1f, 0.1f);
-	}
 }
 
 float Ekf::getGpsHeightVariance()

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -163,6 +163,16 @@ void EstimatorInterface::setMagData(const magSample &mag_sample)
 		_mag_data_sum.setZero();
 		_mag_timestamp_sum = 0;
 	}
+
+
+	if (_mag_counter == 0) {
+		_mag_lpf.reset(mag_sample.mag);
+
+	} else {
+		_mag_lpf.update(mag_sample.mag);
+	}
+
+	_mag_counter++;
 }
 
 void EstimatorInterface::setGpsData(const gps_message &gps)
@@ -227,6 +237,20 @@ void EstimatorInterface::setGpsData(const gps_message &gps)
 
 		_gps_buffer.push(gps_sample_new);
 	}
+
+	if (gps.fix_type > 2) {
+		// accumulate enough height measurements to be confident in the quality of the data
+		const float hgt = gps.alt * 1e-3f;
+
+		if (_gps_hgt_counter == 0) {
+			_gps_hgt_lpf.reset(hgt);
+
+		} else {
+			_gps_hgt_lpf.update(hgt);
+		}
+
+		_gps_hgt_counter++;
+	}
 }
 
 void EstimatorInterface::setBaroData(const baroSample &baro_sample)
@@ -272,6 +296,17 @@ void EstimatorInterface::setBaroData(const baroSample &baro_sample)
 		_baro_alt_sum = 0.0f;
 		_baro_timestamp_sum = 0;
 	}
+
+
+	// accumulate enough height measurements to be confident in the quality of the data
+	if (_baro_hgt_counter == 0) {
+		_baro_hgt_lpf.reset(baro_sample.hgt);
+
+	} else {
+		_baro_hgt_lpf.update(baro_sample.hgt);
+	}
+
+	_baro_hgt_counter++;
 }
 
 void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample)
@@ -331,6 +366,8 @@ void EstimatorInterface::setRangeData(const rangeSample &range_sample)
 
 		_range_buffer.push(range_sample_new);
 	}
+
+	_rng_hgt_filter.insert(range_sample.rng);
 }
 
 void EstimatorInterface::setOpticalFlowData(const flowSample &flow)
@@ -392,6 +429,17 @@ void EstimatorInterface::setExtVisionData(const extVisionSample &evdata)
 
 		_ext_vision_buffer.push(ev_sample_new);
 	}
+
+
+	// accumulate enough height measurements to be confident in the quality of the data
+	if (_ev_hgt_counter == 0) {
+		_ev_hgt_lpf.reset(evdata.pos(2));
+
+	} else {
+		_ev_hgt_lpf.update(evdata.pos(2));
+	}
+
+	_ev_hgt_counter++;
 }
 
 void EstimatorInterface::setAuxVelData(const auxVelSample &auxvel_sample)

src/modules/ekf2/EKF/estimator_interface.h

@@ -63,6 +63,7 @@
 #include <matrix/math.hpp>
 #include <mathlib/mathlib.h>
 #include <mathlib/math/filter/AlphaFilter.hpp>
+#include <mathlib/math/filter/MedianFilter.hpp>
 
 using namespace estimator;
 
@@ -406,6 +407,21 @@ protected:
 	warning_event_status_u _warning_events{};
 	information_event_status_u _information_events{};
 
+	uint32_t _baro_hgt_counter{0};
+	AlphaFilter<float> _baro_hgt_lpf{0.1f};
+
+	uint32_t _gps_hgt_counter{0};
+	AlphaFilter<float> _gps_hgt_lpf{0.1f};
+
+	uint32_t _rng_hgt_counter{0};
+	math::MedianFilter<float, 5> _rng_hgt_filter{};
+
+	uint32_t _ev_hgt_counter{0};
+	AlphaFilter<float> _ev_hgt_lpf{0.1f};
+
+	uint32_t _mag_counter{0};
+	AlphaFilter<Vector3f> _mag_lpf{0.1f}; ///< filtered magnetometer measurement for instant reset (Gauss)
+
 private:
 
 	inline void setDragData(const imuSample &imu);

src/modules/ekf2/EKF/sensor_range_finder.hpp

@@ -105,6 +105,8 @@ public:
 	float getValidMinVal() const { return _rng_valid_min_val; }
 	float getValidMaxVal() const { return _rng_valid_max_val; }
 
+	const rangeSample &sample() const { return _sample; }
+
 private:
 	void updateSensorToEarthRotation(const matrix::Dcmf &R_to_earth);