ekf2: refactor output predictor to class

- refactor all EKF backend output predictor pieces into new OutputPredictor class
 - output states are now calculated immediately with new high rate IMU rather than after EKF update
 - IMU delayed sample is passed as around as control data to avoid storing an extra copy and make the requirement clear

src/modules/ekf2/CMakeLists.txt

@@ -108,6 +108,7 @@ px4_add_module(
 		EKF/mag_fusion.cpp
 		EKF/optical_flow_control.cpp
 		EKF/optflow_fusion.cpp
+		EKF/output_predictor.cpp
 		EKF/range_finder_consistency_check.cpp
 		EKF/range_height_control.cpp
 		EKF/sensor_range_finder.cpp

src/modules/ekf2/EKF/CMakeLists.txt

@@ -59,6 +59,7 @@ add_library(ecl_EKF
 	mag_fusion.cpp
 	optical_flow_control.cpp
 	optflow_fusion.cpp
+	output_predictor.cpp
 	range_finder_consistency_check.cpp
 	range_height_control.cpp
 	sensor_range_finder.cpp

src/modules/ekf2/EKF/RingBuffer.h

@@ -37,6 +37,9 @@
  * Template RingBuffer.
  */
 
+#ifndef EKF_RINGBUFFER_H
+#define EKF_RINGBUFFER_H
+
 #include <inttypes.h>
 #include <cstdio>
 #include <cstring>
@@ -177,3 +180,5 @@ private:
 
 	bool _first_write{true};
 };
+
+#endif // !EKF_RINGBUFFER_H

src/modules/ekf2/EKF/airspeed_fusion.cpp

@@ -127,7 +127,7 @@ void Ekf::fuseAirspeed(estimator_aid_source1d_s &airspeed)
 	_fault_status.flags.bad_airspeed = !is_fused;
 
 	if (is_fused) {
-		airspeed.time_last_fuse = _imu_sample_delayed.time_us;
+		airspeed.time_last_fuse = _time_delayed_us;
 	}
 }
 
@@ -159,7 +159,7 @@ void Ekf::resetWindUsingAirspeed()
 
 	resetWindCovarianceUsingAirspeed();
 
-	_aid_src_airspeed.time_last_fuse = _imu_sample_delayed.time_us;
+	_aid_src_airspeed.time_last_fuse = _time_delayed_us;
 }
 
 void Ekf::resetWindToZero()

src/modules/ekf2/EKF/baro_height_control.cpp

@@ -49,7 +49,7 @@ void Ekf::controlBaroHeightFusion()
 
 	baroSample baro_sample;
 
-	if (_baro_buffer && _baro_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &baro_sample)) {
+	if (_baro_buffer && _baro_buffer->pop_first_older_than(_time_delayed_us, &baro_sample)) {
 
 		const float measurement = compensateBaroForDynamicPressure(baro_sample.hgt);
 		const float measurement_var = sq(_params.baro_noise);
@@ -135,7 +135,7 @@ void Ekf::controlBaroHeightFusion()
 					// reset vertical velocity
 					resetVerticalVelocityToZero();
 
-					aid_src.time_last_fuse = _imu_sample_delayed.time_us;
+					aid_src.time_last_fuse = _time_delayed_us;
 
 				} else if (is_fusion_failing) {
 					// Some other height source is still working
@@ -164,7 +164,7 @@ void Ekf::controlBaroHeightFusion()
 					bias_est.setBias(_state.pos(2) + _baro_lpf.getState());
 				}
 
-				aid_src.time_last_fuse = _imu_sample_delayed.time_us;
+				aid_src.time_last_fuse = _time_delayed_us;
 				bias_est.setFusionActive();
 				_control_status.flags.baro_hgt = true;
 			}

src/modules/ekf2/EKF/common.h

@@ -178,20 +178,6 @@ struct gpsMessage {
 	float       pdop{};             ///< position dilution of precision
 };
 
-struct outputSample {
-	uint64_t    time_us{};          ///< timestamp of the measurement (uSec)
-	Quatf       quat_nominal{};     ///< nominal quaternion describing vehicle attitude
-	Vector3f    vel{};              ///< NED velocity estimate in earth frame (m/sec)
-	Vector3f    pos{};              ///< NED position estimate in earth frame (m/sec)
-};
-
-struct outputVert {
-	uint64_t    time_us{};          ///< timestamp of the measurement (uSec)
-	float       vert_vel{};         ///< Vertical velocity calculated using alternative algorithm (m/sec)
-	float       vert_vel_integ{};   ///< Integral of vertical velocity (m)
-	float       dt{};               ///< delta time (sec)
-};
-
 struct imuSample {
 	uint64_t    time_us{};                ///< timestamp of the measurement (uSec)
 	Vector3f    delta_ang{};              ///< delta angle in body frame (integrated gyro measurements) (rad)
@@ -424,10 +410,6 @@ struct parameters {
 	Vector3f flow_pos_body{};               ///< xyz position of range sensor focal point in body frame (m)
 	Vector3f ev_pos_body{};                 ///< xyz position of VI-sensor focal point in body frame (m)
 
-	// output complementary filter tuning
-	float vel_Tau{0.25f};                   ///< velocity state correction time constant (1/sec)
-	float pos_Tau{0.25f};                   ///< position state correction time constant (1/sec)
-
 	// accel bias learning control
 	float acc_bias_lim{0.4f};               ///< maximum accel bias magnitude (m/sec**2)
 	float acc_bias_learn_acc_lim{25.0f};    ///< learning is disabled if the magnitude of the IMU acceleration vector is greater than this (m/sec**2)

src/modules/ekf2/EKF/control.cpp

@@ -43,7 +43,7 @@
 #include "ekf.h"
 #include <mathlib/mathlib.h>
 
-void Ekf::controlFusionModes()
+void Ekf::controlFusionModes(const imuSample &imu_delayed)
 {
 	// Store the status to enable change detection
 	_control_status_prev.value = _control_status.value;
@@ -52,7 +52,7 @@ void Ekf::controlFusionModes()
 	if (_system_flag_buffer) {
 		systemFlagUpdate system_flags_delayed;
 
-		if (_system_flag_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &system_flags_delayed)) {
+		if (_system_flag_buffer->pop_first_older_than(imu_delayed.time_us, &system_flags_delayed)) {
 
 			set_vehicle_at_rest(system_flags_delayed.at_rest);
 			set_in_air_status(system_flags_delayed.in_air);
@@ -97,7 +97,7 @@ void Ekf::controlFusionModes()
 
 			if (height_source) {
 				ECL_INFO("%llu: EKF aligned, (%s hgt, IMU buf: %i, OBS buf: %i)",
-					 (unsigned long long)_imu_sample_delayed.time_us, height_source, (int)_imu_buffer_length, (int)_obs_buffer_length);
+					 (unsigned long long)imu_delayed.time_us, height_source, (int)_imu_buffer_length, (int)_obs_buffer_length);
 			}
 		}
 	}
@@ -107,7 +107,7 @@ void Ekf::controlFusionModes()
 
 		// check for arrival of new sensor data at the fusion time horizon
 		_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);
+		_gps_data_ready = _gps_buffer->pop_first_older_than(imu_delayed.time_us, &_gps_sample_delayed);
 
 		if (_gps_data_ready) {
 			// correct velocity for offset relative to IMU
@@ -132,7 +132,7 @@ void Ekf::controlFusionModes()
 
 	if (_range_buffer) {
 		// Get range data from buffer and check validity
-		_rng_data_ready = _range_buffer->pop_first_older_than(_imu_sample_delayed.time_us, _range_sensor.getSampleAddress());
+		_rng_data_ready = _range_buffer->pop_first_older_than(imu_delayed.time_us, _range_sensor.getSampleAddress());
 		_range_sensor.setDataReadiness(_rng_data_ready);
 
 		// update range sensor angle parameters in case they have changed
@@ -140,7 +140,7 @@ void Ekf::controlFusionModes()
 		_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
 		_range_sensor.setQualityHysteresis(_params.range_valid_quality_s);
 
-		_range_sensor.runChecks(_imu_sample_delayed.time_us, _R_to_earth);
+		_range_sensor.runChecks(imu_delayed.time_us, _R_to_earth);
 
 		if (_range_sensor.isDataHealthy()) {
 			// correct the range data for position offset relative to the IMU
@@ -156,7 +156,7 @@ void Ekf::controlFusionModes()
 				const float var = sq(_params.range_noise) + dist_dependant_var;
 
 				_rng_consistency_check.setGate(_params.range_kin_consistency_gate);
-				_rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2), P(6, 6), _imu_sample_delayed.time_us);
+				_rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2), P(6, 6), imu_delayed.time_us);
 			}
 
 		} else {
@@ -179,25 +179,25 @@ void Ekf::controlFusionModes()
 		// This means we stop looking for new data until the old data has been fused, unless we are not fusing optical flow,
 		// in this case we need to empty the buffer
 		if (!_flow_data_ready || (!_control_status.flags.opt_flow && !_hagl_sensor_status.flags.flow)) {
-			_flow_data_ready = _flow_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_flow_sample_delayed);
+			_flow_data_ready = _flow_buffer->pop_first_older_than(imu_delayed.time_us, &_flow_sample_delayed);
 		}
 	}
 
 	if (_airspeed_buffer) {
-		_tas_data_ready = _airspeed_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_airspeed_sample_delayed);
+		_tas_data_ready = _airspeed_buffer->pop_first_older_than(imu_delayed.time_us, &_airspeed_sample_delayed);
 	}
 
-	// run EKF-GSF yaw estimator once per _imu_sample_delayed update after all main EKF data samples available
-	runYawEKFGSF();
+	// run EKF-GSF yaw estimator once per imu_delayed update after all main EKF data samples available
+	runYawEKFGSF(imu_delayed);
 
 	// control use of observations for aiding
 	controlMagFusion();
-	controlOpticalFlowFusion();
+	controlOpticalFlowFusion(imu_delayed);
 	controlGpsFusion();
 	controlAirDataFusion();
-	controlBetaFusion();
+	controlBetaFusion(imu_delayed);
 	controlDragFusion();
-	controlHeightFusion();
+	controlHeightFusion(imu_delayed);
 
 	// Additional data odometry data from an external estimator can be fused.
 	controlExternalVisionFusion();
@@ -358,7 +358,7 @@ void Ekf::controlAirDataFusion()
 	}
 }
 
-void Ekf::controlBetaFusion()
+void Ekf::controlBetaFusion(const imuSample &imu_delayed)
 {
 	_control_status.flags.fuse_beta = _params.beta_fusion_enabled && _control_status.flags.fixed_wing
 		&& _control_status.flags.in_air && !_control_status.flags.fake_pos;
@@ -378,7 +378,7 @@ void Ekf::controlBetaFusion()
 				// activate the wind states
 				_control_status.flags.wind = true;
 				// reset the timeout timers to prevent repeated resets
-				_aid_src_sideslip.time_last_fuse = _imu_sample_delayed.time_us;
+				_aid_src_sideslip.time_last_fuse = imu_delayed.time_us;
 				resetWind();
 			}
 
@@ -404,7 +404,7 @@ void Ekf::controlDragFusion()
 
 		dragSample drag_sample;
 
-		if (_drag_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &drag_sample)) {
+		if (_drag_buffer->pop_first_older_than(_time_delayed_us, &drag_sample)) {
 			fuseDrag(drag_sample);
 		}
 	}
@@ -415,7 +415,7 @@ void Ekf::controlAuxVelFusion()
 	if (_auxvel_buffer) {
 		auxVelSample auxvel_sample_delayed;
 
-		if (_auxvel_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &auxvel_sample_delayed)) {
+		if (_auxvel_buffer->pop_first_older_than(_time_delayed_us, &auxvel_sample_delayed)) {
 
 			resetEstimatorAidStatus(_aid_src_aux_vel);
 

src/modules/ekf2/EKF/covariance.cpp

@@ -98,7 +98,7 @@ void Ekf::initialiseCovariance()
 
 }
 
-void Ekf::predictCovariance()
+void Ekf::predictCovariance(const imuSample &imu_delayed)
 {
 	// Use average update interval to reduce accumulated covariance prediction errors due to small single frame dt values
 	const float dt = _dt_ekf_avg;
@@ -114,9 +114,9 @@ void Ekf::predictCovariance()
 	// xy accel bias learning is also disabled on ground as those states are poorly observable when perpendicular to the gravity vector
 	const float alpha = math::constrain((dt / _params.acc_bias_learn_tc), 0.0f, 1.0f);
 	const float beta = 1.0f - alpha;
-	_ang_rate_magnitude_filt = fmaxf(dt_inv * _imu_sample_delayed.delta_ang.norm(), beta * _ang_rate_magnitude_filt);
-	_accel_magnitude_filt = fmaxf(dt_inv * _imu_sample_delayed.delta_vel.norm(), beta * _accel_magnitude_filt);
-	_accel_vec_filt = alpha * dt_inv * _imu_sample_delayed.delta_vel + beta * _accel_vec_filt;
+	_ang_rate_magnitude_filt = fmaxf(dt_inv * imu_delayed.delta_ang.norm(), beta * _ang_rate_magnitude_filt);
+	_accel_magnitude_filt = fmaxf(dt_inv * imu_delayed.delta_vel.norm(), beta * _accel_magnitude_filt);
+	_accel_vec_filt = alpha * dt_inv * imu_delayed.delta_vel + beta * _accel_vec_filt;
 
 	const bool is_manoeuvre_level_high = _ang_rate_magnitude_filt > _params.acc_bias_learn_gyr_lim
 					     || _accel_magnitude_filt > _params.acc_bias_learn_acc_lim;
@@ -141,7 +141,7 @@ void Ekf::predictCovariance()
 			is_bias_observable = (fabsf(_R_to_earth(2, index)) > 0.966f); // cos 15 degrees ~= 0.966
 		}
 
-		const bool do_inhibit_axis = do_inhibit_all_axes || _imu_sample_delayed.delta_vel_clipping[index] || !is_bias_observable;
+		const bool do_inhibit_axis = do_inhibit_all_axes || imu_delayed.delta_vel_clipping[index] || !is_bias_observable;
 
 		if (do_inhibit_axis) {
 			// store the bias state variances to be reinstated later
@@ -220,7 +220,7 @@ void Ekf::predictCovariance()
 	Vector3f d_vel_var;
 
 	for (int i = 0; i <= 2; i++) {
-		if (_fault_status.flags.bad_acc_vertical || _imu_sample_delayed.delta_vel_clipping[i]) {
+		if (_fault_status.flags.bad_acc_vertical || imu_delayed.delta_vel_clipping[i]) {
 			// Increase accelerometer process noise if bad accel data is detected
 			d_vel_var(i) = sq(dt * BADACC_BIAS_PNOISE);
 
@@ -233,7 +233,7 @@ void Ekf::predictCovariance()
 	SquareMatrix24f nextP;
 
 	// calculate variances and upper diagonal covariances for quaternion, velocity, position and gyro bias states
-	sym::PredictCovariance(getStateAtFusionHorizonAsVector(), P, _imu_sample_delayed.delta_vel, d_vel_var, _imu_sample_delayed.delta_ang, d_ang_var, dt, &nextP);
+	sym::PredictCovariance(getStateAtFusionHorizonAsVector(), P, imu_delayed.delta_vel, d_vel_var, imu_delayed.delta_ang, d_ang_var, dt, &nextP);
 
 	// process noise contribution for delta angle states can be very small compared to
 	// the variances, therefore use algorithm to minimise numerical error
@@ -423,7 +423,7 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
 		// record the pass/fail
 		if (!bad_acc_bias) {
 			_fault_status.flags.bad_acc_bias = false;
-			_time_acc_bias_check = _imu_sample_delayed.time_us;
+			_time_acc_bias_check = _time_delayed_us;
 
 		} else {
 			_fault_status.flags.bad_acc_bias = true;
@@ -435,7 +435,7 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
 
 			P.uncorrelateCovariance<3>(13);
 
-			_time_acc_bias_check = _imu_sample_delayed.time_us;
+			_time_acc_bias_check = _time_delayed_us;
 			_fault_status.flags.bad_acc_bias = false;
 			_warning_events.flags.invalid_accel_bias_cov_reset = true;
 			ECL_WARN("invalid accel bias - covariance reset");

src/modules/ekf2/EKF/ekf.cpp

@@ -61,13 +61,6 @@ void Ekf::reset()
 	_state.wind_vel.setZero();
 	_state.quat_nominal.setIdentity();
 
-	// TODO: who resets the output buffer content?
-	_output_new.vel.setZero();
-	_output_new.pos.setZero();
-	_output_new.quat_nominal.setIdentity();
-
-	_delta_angle_corr.setZero();
-
 	_range_sensor.setPitchOffset(_params.rng_sens_pitch);
 	_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
 	_range_sensor.setQualityHysteresis(_params.range_valid_quality_s);
@@ -86,12 +79,12 @@ void Ekf::reset()
 	_prev_dvel_bias_var.zero();
 
 	resetGpsDriftCheckFilters();
+
+	_output_predictor.reset();
 }
 
 bool Ekf::update()
 {
-	bool updated = false;
-
 	if (!_filter_initialised) {
 		_filter_initialised = initialiseFilter();
 
@@ -102,24 +95,29 @@ bool Ekf::update()
 
 	// Only run the filter if IMU data in the buffer has been updated
 	if (_imu_updated) {
+		_imu_updated = false;
+
+		// get the oldest IMU data from the buffer
+		// TODO: explicitly pop at desired time horizon
+		const imuSample imu_sample_delayed = _imu_buffer.get_oldest();
+
 		// perform state and covariance prediction for the main filter
-		predictCovariance();
-		predictState();
+		predictCovariance(imu_sample_delayed);
+		predictState(imu_sample_delayed);
 
 		// control fusion of observation data
-		controlFusionModes();
+		controlFusionModes(imu_sample_delayed);
 
 		// run a separate filter for terrain estimation
-		runTerrainEstimator();
+		runTerrainEstimator(imu_sample_delayed);
 
-		updated = true;
+		_output_predictor.correctOutputStates(imu_sample_delayed.time_us, getGyroBias(), getAccelBias(),
+							_state.quat_nominal, _state.vel, _state.pos);
+
+		return true;
 	}
 
-	// the output observer always runs
-	// Use full rate IMU data at the current time horizon
-	calculateOutputStates(_newest_high_rate_imu_sample);
-
-	return updated;
+	return false;
 }
 
 bool Ekf::initialiseFilter()
@@ -146,7 +144,7 @@ bool Ekf::initialiseFilter()
 	if (_mag_buffer) {
 		magSample mag_sample;
 
-		if (_mag_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &mag_sample)) {
+		if (_mag_buffer->pop_first_older_than(_time_delayed_us, &mag_sample)) {
 			if (mag_sample.time_us != 0) {
 				if (_mag_counter == 0) {
 					_mag_lpf.reset(mag_sample.mag);
@@ -201,12 +199,12 @@ bool Ekf::initialiseFilter()
 	initHagl();
 
 	// reset the essential fusion timeout counters
-	_time_last_hgt_fuse = _imu_sample_delayed.time_us;
-	_time_last_hor_pos_fuse = _imu_sample_delayed.time_us;
-	_time_last_hor_vel_fuse = _imu_sample_delayed.time_us;
+	_time_last_hgt_fuse = _time_delayed_us;
+	_time_last_hor_pos_fuse = _time_delayed_us;
+	_time_last_hor_vel_fuse = _time_delayed_us;
 
 	// reset the output predictor state history to match the EKF initial values
-	alignOutputFilter();
+	_output_predictor.alignOutputFilter(_state.quat_nominal, _state.vel, _state.pos);
 
 	return true;
 }
@@ -233,14 +231,14 @@ bool Ekf::initialiseTilt()
 	return true;
 }
 
-void Ekf::predictState()
+void Ekf::predictState(const imuSample &imu_delayed)
 {
 	// apply imu bias corrections
-	const Vector3f delta_ang_bias_scaled = getGyroBias() * _imu_sample_delayed.delta_ang_dt;
-	Vector3f corrected_delta_ang = _imu_sample_delayed.delta_ang - delta_ang_bias_scaled;
+	const Vector3f delta_ang_bias_scaled = getGyroBias() * imu_delayed.delta_ang_dt;
+	Vector3f corrected_delta_ang = imu_delayed.delta_ang - delta_ang_bias_scaled;
 
 	// subtract component of angular rate due to earth rotation
-	corrected_delta_ang -= _R_to_earth.transpose() * _earth_rate_NED * _imu_sample_delayed.delta_ang_dt;
+	corrected_delta_ang -= _R_to_earth.transpose() * _earth_rate_NED * imu_delayed.delta_ang_dt;
 
 	const Quatf dq(AxisAnglef{corrected_delta_ang});
 
@@ -249,15 +247,10 @@ void Ekf::predictState()
 	_R_to_earth = Dcmf(_state.quat_nominal);
 
 	// Calculate an earth frame delta velocity
-	const Vector3f delta_vel_bias_scaled = getAccelBias() * _imu_sample_delayed.delta_vel_dt;
-	const Vector3f corrected_delta_vel = _imu_sample_delayed.delta_vel - delta_vel_bias_scaled;
+	const Vector3f delta_vel_bias_scaled = getAccelBias() * imu_delayed.delta_vel_dt;
+	const Vector3f corrected_delta_vel = imu_delayed.delta_vel - delta_vel_bias_scaled;
 	const Vector3f corrected_delta_vel_ef = _R_to_earth * corrected_delta_vel;
 
-	// calculate a filtered horizontal acceleration with a 1 sec time constant
-	// this are used for manoeuvre detection elsewhere
-	const float alpha = 1.0f - _imu_sample_delayed.delta_vel_dt;
-	_accel_lpf_NE = _accel_lpf_NE * alpha + corrected_delta_vel_ef.xy();
-
 	// save the previous value of velocity so we can use trapzoidal integration
 	const Vector3f vel_last = _state.vel;
 
@@ -265,15 +258,15 @@ void Ekf::predictState()
 	_state.vel += corrected_delta_vel_ef;
 
 	// compensate for acceleration due to gravity
-	_state.vel(2) += CONSTANTS_ONE_G * _imu_sample_delayed.delta_vel_dt;
+	_state.vel(2) += CONSTANTS_ONE_G * imu_delayed.delta_vel_dt;
 
 	// predict position states via trapezoidal integration of velocity
-	_state.pos += (vel_last + _state.vel) * _imu_sample_delayed.delta_vel_dt * 0.5f;
+	_state.pos += (vel_last + _state.vel) * imu_delayed.delta_vel_dt * 0.5f;
 
 	constrainStates();
 
 	// calculate an average filter update time
-	float input = 0.5f * (_imu_sample_delayed.delta_vel_dt + _imu_sample_delayed.delta_ang_dt);
+	float input = 0.5f * (imu_delayed.delta_vel_dt + imu_delayed.delta_ang_dt);
 
 	// filter and limit input between -50% and +100% of nominal value
 	const float filter_update_s = 1e-6f * _params.filter_update_interval_us;
@@ -283,242 +276,21 @@ void Ekf::predictState()
 	// some calculations elsewhere in code require a raw angular rate vector so calculate here to avoid duplication
 	// protect against possible small timesteps resulting from timing slip on previous frame that can drive spikes into the rate
 	// due to insufficient averaging
-	if (_imu_sample_delayed.delta_ang_dt > 0.25f * _dt_ekf_avg) {
-		_ang_rate_delayed_raw = _imu_sample_delayed.delta_ang / _imu_sample_delayed.delta_ang_dt;
+	if (imu_delayed.delta_ang_dt > 0.25f * _dt_ekf_avg) {
+		_ang_rate_delayed_raw = imu_delayed.delta_ang / imu_delayed.delta_ang_dt;
 	}
-}
 
-/*
- * Implement a strapdown INS algorithm using the latest IMU data at the current time horizon.
- * Buffer the INS states and calculate the difference with the EKF states at the delayed fusion time horizon.
- * Calculate delta angle, delta velocity and velocity corrections from the differences and apply them at the
- * current time horizon so that the INS states track the EKF states at the delayed fusion time horizon.
- * The inspiration for using a complementary filter to correct for time delays in the EKF
- * is based on the work by A Khosravian:
- * “Recursive Attitude Estimation in the Presence of Multi-rate and Multi-delay Vector Measurements”
- * A Khosravian, J Trumpf, R Mahony, T Hamel, Australian National University
-*/
-void Ekf::calculateOutputStates(const imuSample &imu)
-{
-	// Use full rate IMU data at the current time horizon
 
-	// correct delta angles for bias offsets
-	const Vector3f delta_ang_bias_scaled = getGyroBias() * imu.delta_ang_dt;
-
-	// Apply corrections to the delta angle required to track the quaternion states at the EKF fusion time horizon
-	const Vector3f delta_angle(imu.delta_ang - delta_ang_bias_scaled + _delta_angle_corr);
+	// calculate a filtered horizontal acceleration with a 1 sec time constant
+	// this are used for manoeuvre detection elsewhere
+	const float alpha = 1.0f - imu_delayed.delta_vel_dt;
+	_accel_lpf_NE = _accel_lpf_NE * alpha + corrected_delta_vel_ef.xy();
 
 	// calculate a yaw change about the earth frame vertical
-	const float spin_del_ang_D = delta_angle.dot(Vector3f(_R_to_earth_now.row(2)));
+	const float spin_del_ang_D = corrected_delta_ang.dot(Vector3f(_R_to_earth.row(2)));
 	_yaw_delta_ef += spin_del_ang_D;
 
 	// Calculate filtered yaw rate to be used by the magnetometer fusion type selection logic
 	// Note fixed coefficients are used to save operations. The exact time constant is not important.
-	_yaw_rate_lpf_ef = 0.95f * _yaw_rate_lpf_ef + 0.05f * spin_del_ang_D / imu.delta_ang_dt;
-
-
-	_output_new.time_us = imu.time_us;
-	_output_vert_new.time_us = imu.time_us;
-
-	const Quatf dq(AxisAnglef{delta_angle});
-
-	// rotate the previous INS quaternion by the delta quaternions
-	_output_new.quat_nominal = _output_new.quat_nominal * dq;
-
-	// the quaternions must always be normalised after modification
-	_output_new.quat_nominal.normalize();
-
-	// calculate the rotation matrix from body to earth frame
-	_R_to_earth_now = Dcmf(_output_new.quat_nominal);
-
-	// correct delta velocity for bias offsets
-	const Vector3f delta_vel_bias_scaled = (_state.delta_vel_bias / _dt_ekf_avg) * _imu_sample_delayed.delta_vel_dt;
-	const Vector3f delta_vel_body{imu.delta_vel - delta_vel_bias_scaled};
-
-	// rotate the delta velocity to earth frame
-	Vector3f delta_vel_earth{_R_to_earth_now * delta_vel_body};
-
-	// correct for measured acceleration due to gravity
-	delta_vel_earth(2) += CONSTANTS_ONE_G * imu.delta_vel_dt;
-
-	// calculate the earth frame velocity derivatives
-	if (imu.delta_vel_dt > 1e-4f) {
-		_vel_deriv = delta_vel_earth * (1.0f / imu.delta_vel_dt);
-	}
-
-	// save the previous velocity so we can use trapezoidal integration
-	const Vector3f vel_last(_output_new.vel);
-
-	// increment the INS velocity states by the measurement plus corrections
-	// do the same for vertical state used by alternative correction algorithm
-	_output_new.vel += delta_vel_earth;
-	_output_vert_new.vert_vel += delta_vel_earth(2);
-
-	// use trapezoidal integration to calculate the INS position states
-	// do the same for vertical state used by alternative correction algorithm
-	const Vector3f delta_pos_NED = (_output_new.vel + vel_last) * (imu.delta_vel_dt * 0.5f);
-	_output_new.pos += delta_pos_NED;
-	_output_vert_new.vert_vel_integ += delta_pos_NED(2);
-
-	// accumulate the time for each update
-	_output_vert_new.dt += imu.delta_vel_dt;
-
-	// correct velocity for IMU offset
-	if (imu.delta_ang_dt > 1e-4f) {
-		// calculate the average angular rate across the last IMU update
-		const Vector3f ang_rate = imu.delta_ang * (1.0f / imu.delta_ang_dt);
-
-		// calculate the velocity of the IMU relative to the body origin
-		const Vector3f vel_imu_rel_body = ang_rate % _params.imu_pos_body;
-
-		// rotate the relative velocity into earth frame
-		_vel_imu_rel_body_ned = _R_to_earth_now * vel_imu_rel_body;
-	}
-
-	// store the INS states in a ring buffer with the same length and time coordinates as the IMU data buffer
-	if (_imu_updated) {
-		_output_buffer.push(_output_new);
-		_output_vert_buffer.push(_output_vert_new);
-
-		// get the oldest INS state data from the ring buffer
-		// this data will be at the EKF fusion time horizon
-		// TODO: there is no guarantee that data is at delayed fusion horizon
-		//       Shouldnt we use pop_first_older_than?
-		const outputSample &output_delayed = _output_buffer.get_oldest();
-		const outputVert &output_vert_delayed = _output_vert_buffer.get_oldest();
-
-		// calculate the quaternion delta between the INS and EKF quaternions at the EKF fusion time horizon
-		const Quatf q_error((_state.quat_nominal.inversed() * output_delayed.quat_nominal).normalized());
-
-		// convert the quaternion delta to a delta angle
-		const float scalar = (q_error(0) >= 0.0f) ? -2.f : 2.f;
-
-		const Vector3f delta_ang_error{scalar * q_error(1), scalar * q_error(2), scalar * q_error(3)};
-
-		// calculate a gain that provides tight tracking of the estimator attitude states and
-		// adjust for changes in time delay to maintain consistent damping ratio of ~0.7
-		const float time_delay = fmaxf((imu.time_us - _imu_sample_delayed.time_us) * 1e-6f, _dt_imu_avg);
-		const float att_gain = 0.5f * _dt_imu_avg / time_delay;
-
-		// calculate a corrrection to the delta angle
-		// that will cause the INS to track the EKF quaternions
-		_delta_angle_corr = delta_ang_error * att_gain;
-		_output_tracking_error(0) = delta_ang_error.norm();
-
-		/*
-		 * Loop through the output filter state history and apply the corrections to the velocity and position states.
-		 * This method is too expensive to use for the attitude states due to the quaternion operations required
-		 * but because it eliminates the time delay in the 'correction loop' it allows higher tracking gains
-		 * to be used and reduces tracking error relative to EKF states.
-		 */
-
-		// Complementary filter gains
-		const float vel_gain = _dt_ekf_avg / math::constrain(_params.vel_Tau, _dt_ekf_avg, 10.0f);
-		const float pos_gain = _dt_ekf_avg / math::constrain(_params.pos_Tau, _dt_ekf_avg, 10.0f);
-
-		// calculate down velocity and position tracking errors
-		const float vert_vel_err = (_state.vel(2) - output_vert_delayed.vert_vel);
-		const float vert_vel_integ_err = (_state.pos(2) - output_vert_delayed.vert_vel_integ);
-
-		// calculate a velocity correction that will be applied to the output state history
-		// using a PD feedback tuned to a 5% overshoot
-		const float vert_vel_correction = vert_vel_integ_err * pos_gain + vert_vel_err * vel_gain * 1.1f;
-
-		applyCorrectionToVerticalOutputBuffer(vert_vel_correction);
-
-		// calculate velocity and position tracking errors
-		const Vector3f vel_err(_state.vel - output_delayed.vel);
-		const Vector3f pos_err(_state.pos - output_delayed.pos);
-
-		_output_tracking_error(1) = vel_err.norm();
-		_output_tracking_error(2) = pos_err.norm();
-
-		// calculate a velocity correction that will be applied to the output state history
-		_vel_err_integ += vel_err;
-		const Vector3f vel_correction = vel_err * vel_gain + _vel_err_integ * sq(vel_gain) * 0.1f;
-
-		// calculate a position correction that will be applied to the output state history
-		_pos_err_integ += pos_err;
-		const Vector3f pos_correction = pos_err * pos_gain + _pos_err_integ * sq(pos_gain) * 0.1f;
-
-		applyCorrectionToOutputBuffer(vel_correction, pos_correction);
-	}
-}
-
-/*
-* Calculate a correction to be applied to vert_vel that casues vert_vel_integ to track the EKF
-* down position state at the fusion time horizon using an alternative algorithm to what
-* is used for the vel and pos state tracking. The algorithm applies a correction to the vert_vel
-* state history and propagates vert_vel_integ forward in time using the corrected vert_vel history.
-* This provides an alternative vertical velocity output that is closer to the first derivative
-* of the position but does degrade tracking relative to the EKF state.
-*/
-void Ekf::applyCorrectionToVerticalOutputBuffer(float vert_vel_correction)
-{
-	// loop through the vertical output filter state history starting at the oldest and apply the corrections to the
-	// vert_vel states and propagate vert_vel_integ forward using the corrected vert_vel
-	uint8_t index = _output_vert_buffer.get_oldest_index();
-
-	const uint8_t size = _output_vert_buffer.get_length();
-
-	for (uint8_t counter = 0; counter < (size - 1); counter++) {
-		const uint8_t index_next = (index + 1) % size;
-		outputVert &current_state = _output_vert_buffer[index];
-		outputVert &next_state = _output_vert_buffer[index_next];
-
-		// correct the velocity
-		if (counter == 0) {
-			current_state.vert_vel += vert_vel_correction;
-		}
-
-		next_state.vert_vel += vert_vel_correction;
-
-		// position is propagated forward using the corrected velocity and a trapezoidal integrator
-		next_state.vert_vel_integ = current_state.vert_vel_integ + (current_state.vert_vel + next_state.vert_vel) * 0.5f * next_state.dt;
-
-		// advance the index
-		index = (index + 1) % size;
-	}
-
-	// update output state to corrected values
-	_output_vert_new = _output_vert_buffer.get_newest();
-
-	// reset time delta to zero for the next accumulation of full rate IMU data
-	_output_vert_new.dt = 0.0f;
-}
-
-/*
-* Calculate corrections to be applied to vel and pos output state history.
-* The vel and pos state history are corrected individually so they track the EKF states at
-* the fusion time horizon. This option provides the most accurate tracking of EKF states.
-*/
-void Ekf::applyCorrectionToOutputBuffer(const Vector3f &vel_correction, const Vector3f &pos_correction)
-{
-	// loop through the output filter state history and apply the corrections to the velocity and position states
-	for (uint8_t index = 0; index < _output_buffer.get_length(); index++) {
-		// a constant velocity correction is applied
-		_output_buffer[index].vel += vel_correction;
-
-		// a constant position correction is applied
-		_output_buffer[index].pos += pos_correction;
-	}
-
-	// update output state to corrected values
-	_output_new = _output_buffer.get_newest();
-}
-
-/*
- * Predict the previous quaternion output state forward using the latest IMU delta angle data.
-*/
-Quatf Ekf::calculate_quaternion() const
-{
-	// Correct delta angle data for bias errors using bias state estimates from the EKF and also apply
-	// corrections required to track the EKF quaternion states
-	const Vector3f delta_ang_bias_scaled = getGyroBias() * _newest_high_rate_imu_sample.delta_ang_dt;
-
-	const Vector3f delta_angle{_newest_high_rate_imu_sample.delta_ang - delta_ang_bias_scaled + _delta_angle_corr};
-
-	// increment the quaternions using the corrected delta angle vector
-	// the quaternions must always be normalised after modification
-	return Quatf{_output_new.quat_nominal * AxisAnglef{delta_angle}}.unit();
+	_yaw_rate_lpf_ef = 0.95f * _yaw_rate_lpf_ef + 0.05f * spin_del_ang_D / imu_delayed.delta_ang_dt;
 }

src/modules/ekf2/EKF/ekf.h

@@ -115,7 +115,8 @@ public:
 	const Vector3f getFlowGyro() const { return _flow_sample_delayed.gyro_xyz * (1.f / _flow_sample_delayed.dt); }
 	const Vector3f &getFlowGyroIntegral() const { return _flow_sample_delayed.gyro_xyz; }
 
-	void getHeadingInnov(float &heading_innov) const {
+	void getHeadingInnov(float &heading_innov) const
+	{
 		if (_control_status.flags.mag_hdg) {
 			heading_innov = _aid_src_mag_heading.innovation;
 
@@ -130,7 +131,8 @@ public:
 		}
 	}
 
-	void getHeadingInnovVar(float &heading_innov_var) const {
+	void getHeadingInnovVar(float &heading_innov_var) const
+	{
 		if (_control_status.flags.mag_hdg) {
 			heading_innov_var = _aid_src_mag_heading.innovation_variance;
 
@@ -145,7 +147,8 @@ public:
 		}
 	}
 
-	void getHeadingInnovRatio(float &heading_innov_ratio) const {
+	void getHeadingInnovRatio(float &heading_innov_ratio) const
+	{
 		if (_control_status.flags.mag_hdg) {
 			heading_innov_ratio = _aid_src_mag_heading.test_ratio;
 
@@ -244,10 +247,6 @@ public:
 
 	Vector3f getPositionVariance() const { return P.slice<3, 3>(7, 7).diag(); }
 
-	// return an array containing the output predictor angular, velocity and position tracking
-	// error magnitudes (rad), (m/sec), (m)
-	const Vector3f &getOutputTrackingError() const { return _output_tracking_error; }
-
 	// First argument returns GPS drift  metrics in the following array locations
 	// 0 : Horizontal position drift rate (m/s)
 	// 1 : Vertical position drift rate (m/s)
@@ -287,7 +286,7 @@ public:
 		const bool is_using_mag = (_control_status.flags.mag_3D || _control_status.flags.mag_hdg);
 		const bool is_mag_alignment_in_flight_complete = is_using_mag
 				&& _control_status.flags.mag_aligned_in_flight
-				&& ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) > (uint64_t)1e6);
+				&& ((_time_delayed_us - _flt_mag_align_start_time) > (uint64_t)1e6);
 		return _control_status.flags.yaw_align
 		       && (is_mag_alignment_in_flight_complete || !is_using_mag);
 	}
@@ -380,9 +379,6 @@ public:
 	// return a bitmask integer that describes which state estimates can be used for flight control
 	void get_ekf_soln_status(uint16_t *status) const;
 
-	// use the latest IMU data at the current time horizon.
-	Quatf calculate_quaternion() const;
-
 	// rotate quaternion covariances into variances for an equivalent rotation vector
 	Vector3f calcRotVecVariances() const;
 
@@ -448,8 +444,7 @@ private:
 	void updateHorizontalDeadReckoningstatus();
 	void updateVerticalDeadReckoningStatus();
 
-	struct StateResetCounts
-	{
+	struct StateResetCounts {
 		uint8_t velNE{0};	///< number of horizontal position reset events (allow to wrap if count exceeds 255)
 		uint8_t velD{0};	///< number of vertical velocity reset events (allow to wrap if count exceeds 255)
 		uint8_t posNE{0};	///< number of horizontal position reset events (allow to wrap if count exceeds 255)
@@ -576,12 +571,6 @@ private:
 
 	estimator_aid_source2d_s _aid_src_optical_flow{};
 
-	// output predictor states
-	Vector3f _delta_angle_corr{};	///< delta angle correction vector (rad)
-	Vector3f _vel_err_integ{};	///< integral of velocity tracking error (m)
-	Vector3f _pos_err_integ{};	///< integral of position tracking error (m.s)
-	Vector3f _output_tracking_error{}; ///< contains the magnitude of the angle, velocity and position track errors (rad, m/s, m)
-
 	// variables used for the GPS quality checks
 	Vector3f _gps_pos_deriv_filt{};	///< GPS NED position derivative (m/sec)
 	Vector2f _gps_velNE_filt{};	///< filtered GPS North and East velocity (m/sec)
@@ -642,12 +631,6 @@ private:
 
 	float _height_rate_lpf{0.0f};
 
-	// update the real time complementary filter states. This includes the prediction
-	// and the correction step
-	void calculateOutputStates(const imuSample &imu);
-	void applyCorrectionToVerticalOutputBuffer(float vert_vel_correction);
-	void applyCorrectionToOutputBuffer(const Vector3f &vel_correction, const Vector3f &pos_correction);
-
 	// initialise filter states of both the delayed ekf and the real time complementary filter
 	bool initialiseFilter(void);
 
@@ -655,10 +638,10 @@ private:
 	void initialiseCovariance();
 
 	// predict ekf state
-	void predictState();
+	void predictState(const imuSample &imu_delayed);
 
 	// predict ekf covariance
-	void predictCovariance();
+	void predictCovariance(const imuSample &imu_delayed);
 
 	// ekf sequential fusion of magnetometer measurements
 	bool fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bool update_all_states = true);
@@ -745,8 +728,8 @@ private:
 	// initialise the terrain vertical position estimator
 	void initHagl();
 
-	void runTerrainEstimator();
-	void predictHagl();
+	void runTerrainEstimator(const imuSample &imu_delayed);
+	void predictHagl(const imuSample &imu_delayed);
 
 	// update the terrain vertical position estimate using a height above ground measurement from the range finder
 	void controlHaglRngFusion();
@@ -773,9 +756,6 @@ private:
 	// Return the magnetic declination in radians to be used by the alignment and fusion processing
 	float getMagDeclination();
 
-	// modify output filter to match the the EKF state at the fusion time horizon
-	void alignOutputFilter();
-
 	bool measurementUpdate(Vector24f &K, float innovation_variance, float innovation)
 	{
 		for (unsigned i = 0; i < 3; i++) {
@@ -834,23 +814,23 @@ private:
 	bool gps_is_good(const gpsMessage &gps);
 
 	// Control the filter fusion modes
-	void controlFusionModes();
+	void controlFusionModes(const imuSample &imu_delayed);
 
 	// control fusion of external vision observations
 	void controlExternalVisionFusion();
 
-	void controlEvHeightFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s& aid_src);
+	void controlEvHeightFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src);
 
-	void controlEvPosFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source2d_s& aid_src);
+	void controlEvPosFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source2d_s &aid_src);
 	void startEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, estimator_aid_source2d_s &aid_src);
 	void updateEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, bool quality_sufficient, bool reset, estimator_aid_source2d_s &aid_src);
 	void stopEvPosFusion();
 
-	void controlEvVelFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source3d_s& aid_src);
-	void controlEvYawFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s& aid_src);
+	void controlEvVelFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source3d_s &aid_src);
+	void controlEvYawFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src);
 
 	// control fusion of optical flow observations
-	void controlOpticalFlowFusion();
+	void controlOpticalFlowFusion(const imuSample &imu_delayed);
 	void updateOnGroundMotionForOpticalFlowChecks();
 	void resetOnGroundMotionForOpticalFlowChecks();
 
@@ -887,7 +867,7 @@ private:
 	void controlAirDataFusion();
 
 	// control fusion of synthetic sideslip observations
-	void controlBetaFusion();
+	void controlBetaFusion(const imuSample &imu_delayed);
 
 	// control fusion of multi-rotor drag specific force observations
 	void controlDragFusion();
@@ -907,11 +887,11 @@ private:
 	// control fusion of auxiliary velocity observations
 	void controlAuxVelFusion();
 
-	void checkVerticalAccelerationHealth();
+	void checkVerticalAccelerationHealth(const imuSample &imu_delayed);
 	Likelihood estimateInertialNavFallingLikelihood() const;
 
 	// control for combined height fusion mode (implemented for switching between baro and range height)
-	void controlHeightFusion();
+	void controlHeightFusion(const imuSample &imu_delayed);
 	void checkHeightSensorRefFallback();
 	void controlBaroHeightFusion();
 	void controlGnssHeightFusion(const gpsSample &gps_sample);
@@ -978,17 +958,17 @@ private:
 
 	bool isTimedOut(uint64_t last_sensor_timestamp, uint64_t timeout_period) const
 	{
-		return last_sensor_timestamp + timeout_period < _imu_sample_delayed.time_us;
+		return last_sensor_timestamp + timeout_period < _time_delayed_us;
 	}
 
 	bool isRecent(uint64_t sensor_timestamp, uint64_t acceptance_interval) const
 	{
-		return sensor_timestamp + acceptance_interval > _imu_sample_delayed.time_us;
+		return sensor_timestamp + acceptance_interval > _time_delayed_us;
 	}
 
 	bool isNewestSampleRecent(uint64_t sensor_timestamp, uint64_t acceptance_interval) const
 	{
-		return sensor_timestamp + acceptance_interval > _newest_high_rate_imu_sample.time_us;
+		return sensor_timestamp + acceptance_interval > _time_latest_us;
 	}
 
 	void startAirspeedFusion();
@@ -1032,7 +1012,7 @@ private:
 	HeightBiasEstimator _ev_hgt_b_est{HeightSensor::EV, _height_sensor_ref};
 	PositionBiasEstimator _ev_pos_b_est{static_cast<uint8_t>(PositionSensor::EV), _position_sensor_ref};
 
-	void runYawEKFGSF();
+	void runYawEKFGSF(const imuSample &imu_delayed);
 
 	// Resets the main Nav EKf yaw to the estimator from the EKF-GSF yaw estimator
 	// Resets the horizontal velocity and position to the default navigation sensor
@@ -1088,7 +1068,10 @@ private:
 
 	void setEstimatorAidStatusTestRatio(estimator_aid_source1d_s &status, float innovation_gate) const
 	{
-		if (PX4_ISFINITE(status.innovation) && PX4_ISFINITE(status.innovation_variance) && (status.innovation_variance > 0.f)) {
+		if (PX4_ISFINITE(status.innovation)
+		    && PX4_ISFINITE(status.innovation_variance)
+		    && (status.innovation_variance > 0.f)
+		   ) {
 			status.test_ratio = sq(status.innovation) / (sq(innovation_gate) * status.innovation_variance);
 			status.innovation_rejected = (status.test_ratio > 1.f);
 
@@ -1104,7 +1087,10 @@ private:
 		bool innovation_rejected = false;
 
 		for (size_t i = 0; i < (sizeof(status.test_ratio) / sizeof(status.test_ratio[0])); i++) {
-			if (PX4_ISFINITE(status.innovation[i]) && PX4_ISFINITE(status.innovation_variance[i]) && (status.innovation_variance[i] > 0.f)) {
+			if (PX4_ISFINITE(status.innovation[i])
+			    && PX4_ISFINITE(status.innovation_variance[i])
+			    && (status.innovation_variance[i] > 0.f)
+			   ) {
 				status.test_ratio[i] = sq(status.innovation[i]) / (sq(innovation_gate) * status.innovation_variance[i]);
 
 				if (status.test_ratio[i] > 1.f) {

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -71,11 +71,7 @@ void Ekf::resetHorizontalVelocityTo(const Vector2f &new_horz_vel, const Vector2f
 		P.uncorrelateCovarianceSetVariance<1>(5, math::max(sq(0.01f), new_horz_vel_var(1)));
 	}
 
-	for (uint8_t index = 0; index < _output_buffer.get_length(); index++) {
-		_output_buffer[index].vel.xy() += delta_horz_vel;
-	}
-
-	_output_new.vel.xy() += delta_horz_vel;
+	_output_predictor.resetHorizontalVelocityTo(delta_horz_vel);
 
 	// record the state change
 	if (_state_reset_status.reset_count.velNE == _state_reset_count_prev.velNE) {
@@ -89,7 +85,7 @@ void Ekf::resetHorizontalVelocityTo(const Vector2f &new_horz_vel, const Vector2f
 	_state_reset_status.reset_count.velNE++;
 
 	// Reset the timout timer
-	_time_last_hor_vel_fuse = _imu_sample_delayed.time_us;
+	_time_last_hor_vel_fuse = _time_delayed_us;
 }
 
 void Ekf::resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var)
@@ -101,13 +97,7 @@ void Ekf::resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var)
 		P.uncorrelateCovarianceSetVariance<1>(6, math::max(sq(0.01f), new_vert_vel_var));
 	}
 
-	for (uint8_t index = 0; index < _output_buffer.get_length(); index++) {
-		_output_buffer[index].vel(2) += delta_vert_vel;
-		_output_vert_buffer[index].vert_vel += delta_vert_vel;
-	}
-
-	_output_new.vel(2) += delta_vert_vel;
-	_output_vert_new.vert_vel += delta_vert_vel;
+	_output_predictor.resetVerticalVelocityTo(delta_vert_vel);
 
 	// record the state change
 	if (_state_reset_status.reset_count.velD == _state_reset_count_prev.velD) {
@@ -121,7 +111,7 @@ void Ekf::resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var)
 	_state_reset_status.reset_count.velD++;
 
 	// Reset the timout timer
-	_time_last_ver_vel_fuse = _imu_sample_delayed.time_us;
+	_time_last_ver_vel_fuse = _time_delayed_us;
 }
 
 void Ekf::resetHorizontalPositionToLastKnown()
@@ -145,11 +135,7 @@ void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f
 		P.uncorrelateCovarianceSetVariance<1>(8, math::max(sq(0.01f), new_horz_pos_var(1)));
 	}
 
-	for (uint8_t index = 0; index < _output_buffer.get_length(); index++) {
-		_output_buffer[index].pos.xy() += delta_horz_pos;
-	}
-
-	_output_new.pos.xy() += delta_horz_pos;
+	_output_predictor.resetHorizontalPositionTo(delta_horz_pos);
 
 	// record the state change
 	if (_state_reset_status.reset_count.posNE == _state_reset_count_prev.posNE) {
@@ -166,7 +152,7 @@ void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f
 	//_gps_pos_b_est.setBias(_gps_pos_b_est.getBias() + _state_reset_status.posNE_change);
 
 	// Reset the timout timer
-	_time_last_hor_pos_fuse = _imu_sample_delayed.time_us;
+	_time_last_hor_pos_fuse = _time_delayed_us;
 }
 
 bool Ekf::isHeightResetRequired() const
@@ -194,16 +180,7 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v
 
 	// apply the change in height / height rate to our newest height / height rate estimate
 	// which have already been taken out from the output buffer
-	_output_new.pos(2) += delta_z;
-
-	// add the reset amount to the output observer buffered data
-	for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
-		_output_buffer[i].pos(2) += delta_z;
-		_output_vert_buffer[i].vert_vel_integ += delta_z;
-	}
-
-	// add the reset amount to the output observer vertical position state
-	_output_vert_new.vert_vel_integ = _state.pos(2);
+	_output_predictor.resetVerticalPositionTo(new_vert_pos, delta_z);
 
 	// record the state change
 	if (_state_reset_status.reset_count.posD == _state_reset_count_prev.posD) {
@@ -222,7 +199,7 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v
 	_rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + delta_z);
 
 	// Reset the timout timer
-	_time_last_hgt_fuse = _imu_sample_delayed.time_us;
+	_time_last_hgt_fuse = _time_delayed_us;
 }
 
 void Ekf::resetVerticalVelocityToZero()
@@ -233,35 +210,11 @@ void Ekf::resetVerticalVelocityToZero()
 	resetVerticalVelocityTo(0.0f, 10.f);
 }
 
-// align output filter states to match EKF states at the fusion time horizon
-void Ekf::alignOutputFilter()
-{
-	const outputSample &output_delayed = _output_buffer.get_oldest();
-
-	// calculate the quaternion rotation delta from the EKF to output observer states at the EKF fusion time horizon
-	Quatf q_delta{_state.quat_nominal * output_delayed.quat_nominal.inversed()};
-	q_delta.normalize();
-
-	// calculate the velocity and position deltas between the output and EKF at the EKF fusion time horizon
-	const Vector3f vel_delta = _state.vel - output_delayed.vel;
-	const Vector3f pos_delta = _state.pos - output_delayed.pos;
-
-	// loop through the output filter state history and add the deltas
-	for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
-		_output_buffer[i].quat_nominal = q_delta * _output_buffer[i].quat_nominal;
-		_output_buffer[i].quat_nominal.normalize();
-		_output_buffer[i].vel += vel_delta;
-		_output_buffer[i].pos += pos_delta;
-	}
-
-	_output_new = _output_buffer.get_newest();
-}
-
 // Reset heading and magnetic field states
 bool Ekf::resetMagHeading()
 {
 	// prevent a reset being performed more than once on the same frame
-	if ((_flt_mag_align_start_time == _imu_sample_delayed.time_us) || (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align)) {
+	if ((_flt_mag_align_start_time == _time_delayed_us) || (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align)) {
 		return false;
 	}
 
@@ -298,7 +251,7 @@ bool Ekf::resetMagHeading()
 		resetMagCov();
 
 		// record the time for the magnetic field alignment event
-		_flt_mag_align_start_time = _imu_sample_delayed.time_us;
+		_flt_mag_align_start_time = _time_delayed_us;
 
 		return true;
 	}
@@ -518,7 +471,7 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons
 		const float gps_alt_ref_prev = getEkfGlobalOriginAltitude();
 
 		// reinitialize map projection to latitude, longitude, altitude, and reset position
-		_pos_ref.initReference(latitude, longitude, _imu_sample_delayed.time_us);
+		_pos_ref.initReference(latitude, longitude, _time_delayed_us);
 		_gps_alt_ref = altitude;
 
 		// minimum change in position or height that triggers a reset
@@ -883,8 +836,8 @@ void Ekf::updateHorizontalDeadReckoningstatus()
 	_control_status.flags.inertial_dead_reckoning = !velPosAiding && !optFlowAiding && !airDataAiding;
 
 	if (!_control_status.flags.inertial_dead_reckoning) {
-		if (_imu_sample_delayed.time_us > _params.no_aid_timeout_max) {
-			_time_last_horizontal_aiding = _imu_sample_delayed.time_us - _params.no_aid_timeout_max;
+		if (_time_delayed_us > _params.no_aid_timeout_max) {
+			_time_last_horizontal_aiding = _time_delayed_us - _params.no_aid_timeout_max;
 		}
 	}
 
@@ -1328,13 +1281,7 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
 	}
 
 	// add the reset amount to the output observer buffered data
-	for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
-		_output_buffer[i].quat_nominal = q_error * _output_buffer[i].quat_nominal;
-	}
-
-	// apply the change in attitude quaternion to our newest quaternion estimate
-	// which was already taken out from the output buffer
-	_output_new.quat_nominal = q_error * _output_new.quat_nominal;
+	_output_predictor.resetQuaternion(q_error);
 
 	// record the state change
 	if (_state_reset_status.reset_count.quat == _state_reset_count_prev.quat) {
@@ -1363,7 +1310,7 @@ bool Ekf::resetYawToEKFGSF()
 	resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar());
 
 	// record a magnetic field alignment event to prevent possibility of the EKF trying to reset the yaw to the mag later in flight
-	_flt_mag_align_start_time = _imu_sample_delayed.time_us;
+	_flt_mag_align_start_time = _time_delayed_us;
 	_control_status.flags.yaw_align = true;
 
 	if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
@@ -1400,7 +1347,7 @@ bool Ekf::getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_M
 	return _yawEstimator.getLogData(yaw_composite, yaw_variance, yaw, innov_VN, innov_VE, weight);
 }
 
-void Ekf::runYawEKFGSF()
+void Ekf::runYawEKFGSF(const imuSample &imu_delayed)
 {
 	float TAS = 0.f;
 
@@ -1414,7 +1361,7 @@ void Ekf::runYawEKFGSF()
 	}
 
 	const Vector3f imu_gyro_bias = getGyroBias();
-	_yawEstimator.update(_imu_sample_delayed, _control_status.flags.in_air, TAS, imu_gyro_bias);
+	_yawEstimator.update(imu_delayed, _control_status.flags.in_air, TAS, imu_gyro_bias);
 }
 
 void Ekf::resetGpsDriftCheckFilters()

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -65,27 +65,24 @@ void EstimatorInterface::setIMUData(const imuSample &imu_sample)
 		_initialised = init(imu_sample.time_us);
 	}
 
-	const float dt = math::constrain((imu_sample.time_us - _newest_high_rate_imu_sample.time_us) / 1e6f, 0.0001f, 0.02f);
+	_time_latest_us = imu_sample.time_us;
 
-	if (_newest_high_rate_imu_sample.time_us > 0) {
-		_dt_imu_avg = 0.8f * _dt_imu_avg + 0.2f * dt;
-	}
-
-	_newest_high_rate_imu_sample = imu_sample;
-
-	_imu_updated = _imu_down_sampler.update(imu_sample);
+	// the output observer always runs
+	_output_predictor.calculateOutputStates(imu_sample.time_us, imu_sample.delta_ang, imu_sample.delta_ang_dt, imu_sample.delta_vel, imu_sample.delta_vel_dt);
 
 	// accumulate and down-sample imu data and push to the buffer when new downsampled data becomes available
-	if (_imu_updated) {
+	if (_imu_down_sampler.update(imu_sample)) {
+
+		_imu_updated = true;
 
 		_imu_buffer.push(_imu_down_sampler.getDownSampledImuAndTriggerReset());
 
 		// get the oldest data from the buffer
-		_imu_sample_delayed = _imu_buffer.get_oldest();
+		_time_delayed_us = _imu_buffer.get_oldest().time_us;
 
 		// calculate the minimum interval between observations required to guarantee no loss of data
 		// this will occur if data is overwritten before its time stamp falls behind the fusion time horizon
-		_min_obs_interval_us = (imu_sample.time_us - _imu_sample_delayed.time_us) / (_obs_buffer_length - 1);
+		_min_obs_interval_us = (imu_sample.time_us - _time_delayed_us) / (_obs_buffer_length - 1);
 	}
 
 	setDragData(imu_sample);
@@ -120,7 +117,7 @@ void EstimatorInterface::setMagData(const magSample &mag_sample)
 		mag_sample_new.time_us = time_us;
 
 		_mag_buffer->push(mag_sample_new);
-		_time_last_mag_buffer_push = _newest_high_rate_imu_sample.time_us;
+		_time_last_mag_buffer_push = _time_latest_us;
 
 	} else {
 		ECL_WARN("mag data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _mag_buffer->get_newest().time_us, _min_obs_interval_us);
@@ -193,10 +190,10 @@ void EstimatorInterface::setGpsData(const gpsMessage &gps)
 		}
 
 		_gps_buffer->push(gps_sample_new);
-		_time_last_gps_buffer_push = _newest_high_rate_imu_sample.time_us;
+		_time_last_gps_buffer_push = _time_latest_us;
 
 		if (PX4_ISFINITE(gps.yaw)) {
-			_time_last_gps_yaw_buffer_push = _newest_high_rate_imu_sample.time_us;
+			_time_last_gps_yaw_buffer_push = _time_latest_us;
 		}
 
 	} else {
@@ -233,7 +230,7 @@ void EstimatorInterface::setBaroData(const baroSample &baro_sample)
 		baro_sample_new.time_us = time_us;
 
 		_baro_buffer->push(baro_sample_new);
-		_time_last_baro_buffer_push = _newest_high_rate_imu_sample.time_us;
+		_time_last_baro_buffer_push = _time_latest_us;
 
 	} else {
 		ECL_WARN("baro data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _baro_buffer->get_newest().time_us, _min_obs_interval_us);
@@ -304,7 +301,7 @@ void EstimatorInterface::setRangeData(const rangeSample &range_sample)
 		range_sample_new.time_us = time_us;
 
 		_range_buffer->push(range_sample_new);
-		_time_last_range_buffer_push = _newest_high_rate_imu_sample.time_us;
+		_time_last_range_buffer_push = _time_latest_us;
 
 	} else {
 		ECL_WARN("range data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _range_buffer->get_newest().time_us, _min_obs_interval_us);
@@ -377,7 +374,7 @@ void EstimatorInterface::setExtVisionData(const extVisionSample &evdata)
 		ev_sample_new.time_us = time_us;
 
 		_ext_vision_buffer->push(ev_sample_new);
-		_time_last_ext_vision_buffer_push = _newest_high_rate_imu_sample.time_us;
+		_time_last_ext_vision_buffer_push = _time_latest_us;
 
 	} else {
 		ECL_WARN("EV data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _ext_vision_buffer->get_newest().time_us, _min_obs_interval_us);
@@ -561,17 +558,14 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 
 	ECL_DEBUG("EKF max time delay %.1f ms, OBS length %d\n", (double)ekf_delay_ms, _obs_buffer_length);
 
-	if (!_imu_buffer.allocate(_imu_buffer_length) || !_output_buffer.allocate(_imu_buffer_length)
-	    || !_output_vert_buffer.allocate(_imu_buffer_length)) {
+	if (!_imu_buffer.allocate(_imu_buffer_length) || !_output_predictor.allocate(_imu_buffer_length)) {
 
 		printBufferAllocationFailed("IMU and output");
 		return false;
 	}
 
-	_imu_sample_delayed.time_us = timestamp;
-	_imu_sample_delayed.delta_vel_clipping[0] = false;
-	_imu_sample_delayed.delta_vel_clipping[1] = false;
-	_imu_sample_delayed.delta_vel_clipping[2] = false;
+	_time_delayed_us = timestamp;
+	_time_latest_us = timestamp;
 
 	_fault_status.value = 0;
 
@@ -654,7 +648,6 @@ void EstimatorInterface::printBufferAllocationFailed(const char *buffer_name)
 
 void EstimatorInterface::print_status()
 {
-	printf("IMU average dt: %.6f seconds\n", (double)_dt_imu_avg);
 	printf("EKF average dt: %.6f seconds\n", (double)_dt_ekf_avg);
 
 	printf("IMU buffer: %d (%d Bytes)\n", _imu_buffer.get_length(), _imu_buffer.get_total_size());
@@ -693,6 +686,5 @@ void EstimatorInterface::print_status()
 		printf("drag buffer: %d/%d (%d Bytes)\n", _drag_buffer->entries(), _drag_buffer->get_length(), _drag_buffer->get_total_size());
 	}
 
-	printf("output buffer: %d/%d (%d Bytes)\n", _output_buffer.entries(), _output_buffer.get_length(), _output_buffer.get_total_size());
-	printf("output vert buffer: %d/%d (%d Bytes)\n", _output_vert_buffer.entries(), _output_vert_buffer.get_length(), _output_vert_buffer.get_total_size());
+	_output_predictor.print_status();
 }

src/modules/ekf2/EKF/estimator_interface.h

@@ -67,6 +67,7 @@
 #include "range_finder_consistency_check.hpp"
 #include "sensor_range_finder.hpp"
 #include "utils.hpp"
+#include "output_predictor.h"
 
 #include <lib/geo/geo.h>
 #include <matrix/math.hpp>
@@ -111,10 +112,10 @@ public:
 	void set_in_air_status(bool in_air)
 	{
 		if (!in_air) {
-			_time_last_on_ground_us = _imu_sample_delayed.time_us;
+			_time_last_on_ground_us = _time_delayed_us;
 
 		} else {
-			_time_last_in_air = _imu_sample_delayed.time_us;
+			_time_last_in_air = _time_delayed_us;
 		}
 
 		_control_status.flags.in_air = in_air;
@@ -123,7 +124,7 @@ public:
 	void set_vehicle_at_rest(bool at_rest) { _control_status.flags.vehicle_at_rest = at_rest; }
 
 	// return true if the attitude is usable
-	bool attitude_valid() const { return _output_new.quat_nominal.isAllFinite() && _control_status.flags.tilt_align; }
+	bool attitude_valid() const { return _control_status.flags.tilt_align; }
 
 	// get vehicle landed status data
 	bool get_in_air_status() const { return _control_status.flags.in_air; }
@@ -140,7 +141,7 @@ public:
 	void set_gnd_effect()
 	{
 		_control_status.flags.gnd_effect = true;
-		_time_last_gnd_effect_on = _imu_sample_delayed.time_us;
+		_time_last_gnd_effect_on = _time_delayed_us;
 	}
 
 	// set air density used by the multi-rotor specific drag force fusion
@@ -190,25 +191,12 @@ public:
 	bool isVerticalVelocityAidingActive() const;
 	int getNumberOfActiveVerticalVelocityAidingSources() const;
 
-	const matrix::Quatf &getQuaternion() const { return _output_new.quat_nominal; }
-
-	// get the velocity of the body frame origin in local NED earth frame
-	Vector3f getVelocity() const { return _output_new.vel - _vel_imu_rel_body_ned; }
-
-	// get the velocity derivative in earth frame
-	const Vector3f &getVelocityDerivative() const { return _vel_deriv; }
-
-	// get the derivative of the vertical position of the body frame origin in local NED earth frame
-	float getVerticalPositionDerivative() const { return _output_vert_new.vert_vel - _vel_imu_rel_body_ned(2); }
-
-	// get the position of the body frame origin in local earth frame
-	Vector3f getPosition() const
-	{
-		// rotate the position of the IMU relative to the boy origin into earth frame
-		const Vector3f pos_offset_earth = _R_to_earth_now * _params.imu_pos_body;
-		// subtract from the EKF position (which is at the IMU) to get position at the body origin
-		return _output_new.pos - pos_offset_earth;
-	}
+	const matrix::Quatf &getQuaternion() const { return _output_predictor.getQuaternion(); }
+	Vector3f getVelocity() const { return _output_predictor.getVelocity(); }
+	const Vector3f &getVelocityDerivative() const { return _output_predictor.getVelocityDerivative(); }
+	float getVerticalPositionDerivative() const { return _output_predictor.getVerticalPositionDerivative(); }
+	Vector3f getPosition() const { return _output_predictor.getPosition(); }
+	const Vector3f &getOutputTrackingError() const { return _output_predictor.getOutputTrackingError(); }
 
 	// Get the value of magnetic declination in degrees to be saved for use at the next startup
 	// Returns true when the declination can be saved
@@ -246,15 +234,12 @@ public:
 	const decltype(information_event_status_u::flags) &information_event_flags() const { return _information_events.flags; }
 	void clear_information_events() { _information_events.value = 0; }
 
-	// Getter for the average imu update period in s
-	float get_dt_imu_avg() const { return _dt_imu_avg; }
-
 	// Getter for the average EKF update period in s
 	float get_dt_ekf_avg() const { return _dt_ekf_avg; }
 
 	// Getters for samples on the delayed time horizon
-	const imuSample &get_imu_sample_delayed() const { return _imu_sample_delayed; }
-	const imuSample &get_imu_sample_newest() const { return _newest_high_rate_imu_sample; }
+	const imuSample &get_imu_sample_delayed() const { return _imu_buffer.get_oldest(); }
+	const uint64_t &time_delayed_us() const { return _time_delayed_us; }
 
 	const gpsSample &get_gps_sample_delayed() const { return _gps_sample_delayed; }
 	const rangeSample &get_rng_sample_delayed() { return *(_range_sensor.getSampleAddress()); }
@@ -268,6 +253,8 @@ public:
 	float gps_vertical_position_drift_rate_m_s() const { return _gps_vertical_position_drift_rate_m_s; }
 	float gps_filtered_horizontal_velocity_m_s() const { return _gps_filtered_horizontal_velocity_m_s; }
 
+	OutputPredictor &output_predictor() { return _output_predictor; };
+
 protected:
 
 	EstimatorInterface() = default;
@@ -294,10 +281,12 @@ protected:
 	*/
 	uint8_t _imu_buffer_length{0};
 
-	float _dt_imu_avg{0.005f};	// average imu update period in s
 	float _dt_ekf_avg{0.010f}; ///< average update rate of the ekf in s
 
-	imuSample _imu_sample_delayed{};	// captures the imu sample on the delayed time horizon
+	uint64_t _time_delayed_us{0}; // captures the imu sample on the delayed time horizon
+	uint64_t _time_latest_us{0}; // imu sample capturing the newest imu data
+
+	OutputPredictor _output_predictor{};
 
 	// measurement samples capturing measurements on the delayed time horizon
 	gpsSample _gps_sample_delayed{};
@@ -316,14 +305,6 @@ protected:
 	float _flow_min_distance{0.0f};	///< minimum distance that the optical flow sensor can operate at (m)
 	float _flow_max_distance{10.f};	///< maximum distance that the optical flow sensor can operate at (m)
 
-	// Output Predictor
-	outputSample _output_new{};		// filter output on the non-delayed time horizon
-	outputVert _output_vert_new{};		// vertical filter output on the non-delayed time horizon
-	imuSample _newest_high_rate_imu_sample{};		// imu sample capturing the newest imu data
-	Matrix3f _R_to_earth_now{};		// rotation matrix from body to earth frame at current time
-	Vector3f _vel_imu_rel_body_ned{};		// velocity of IMU relative to body origin in NED earth frame
-	Vector3f _vel_deriv{};		// velocity derivative at the IMU in NED earth frame (m/s/s)
-
 	bool _imu_updated{false};      // true if the ekf should update (completed downsampling process)
 	bool _initialised{false};      // true if the ekf interface instance (data buffering) is initialized
 
@@ -356,8 +337,6 @@ protected:
 	// data buffer instances
 	static constexpr uint8_t kBufferLengthDefault = 12;
 	RingBuffer<imuSample> _imu_buffer{kBufferLengthDefault};
-	RingBuffer<outputSample> _output_buffer{kBufferLengthDefault};
-	RingBuffer<outputVert> _output_vert_buffer{kBufferLengthDefault};
 
 	RingBuffer<gpsSample> *_gps_buffer{nullptr};
 	RingBuffer<magSample> *_mag_buffer{nullptr};

src/modules/ekf2/EKF/ev_control.cpp

@@ -45,7 +45,7 @@ void Ekf::controlExternalVisionFusion()
 	// Check for new external vision data
 	extVisionSample ev_sample;
 
-	if (_ext_vision_buffer && _ext_vision_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &ev_sample)) {
+	if (_ext_vision_buffer && _ext_vision_buffer->pop_first_older_than(_time_delayed_us, &ev_sample)) {
 
 		bool ev_reset = (ev_sample.reset_counter != _ev_sample_prev.reset_counter);
 

src/modules/ekf2/EKF/ev_height_control.cpp

@@ -123,7 +123,7 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com
 						bias_est.setBias(-_state.pos(2) + measurement);
 					}
 
-					aid_src.time_last_fuse = _imu_sample_delayed.time_us;
+					aid_src.time_last_fuse = _time_delayed_us;
 
 				} else {
 					// EV has reset, but quality isn't sufficient
@@ -175,7 +175,7 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com
 					resetVerticalVelocityToZero();
 				}
 
-				aid_src.time_last_fuse = _imu_sample_delayed.time_us;
+				aid_src.time_last_fuse = _time_delayed_us;
 
 			} else if (is_fusion_failing) {
 				// A reset did not fix the issue but all the starting checks are not passing
@@ -206,7 +206,7 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com
 				bias_est.setBias(-_state.pos(2) + measurement);
 			}
 
-			aid_src.time_last_fuse = _imu_sample_delayed.time_us;
+			aid_src.time_last_fuse = _time_delayed_us;
 			bias_est.setFusionActive();
 			_control_status.flags.ev_hgt = true;
 		}

src/modules/ekf2/EKF/ev_pos_control.cpp

@@ -214,7 +214,7 @@ void Ekf::startEvPosFusion(const Vector2f &measurement, const Vector2f &measurem
 		_ev_pos_b_est.reset();
 	}
 
-	aid_src.time_last_fuse = _imu_sample_delayed.time_us;
+	aid_src.time_last_fuse = _time_delayed_us;
 
 	_nb_ev_pos_reset_available = 5;
 	_information_events.flags.starting_vision_pos_fusion = true;
@@ -237,7 +237,7 @@ void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measure
 				_ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement);
 			}
 
-			aid_src.time_last_fuse = _imu_sample_delayed.time_us;
+			aid_src.time_last_fuse = _time_delayed_us;
 
 		} else {
 			// EV has reset, but quality isn't sufficient
@@ -279,7 +279,7 @@ void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measure
 				}
 			}
 
-			aid_src.time_last_fuse = _imu_sample_delayed.time_us;
+			aid_src.time_last_fuse = _time_delayed_us;
 
 			if (_control_status.flags.in_air) {
 				_nb_ev_pos_reset_available--;

src/modules/ekf2/EKF/ev_vel_control.cpp

@@ -149,7 +149,7 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
 					ECL_INFO("reset to %s", AID_SRC_NAME);
 					_information_events.flags.reset_vel_to_vision = true;
 					resetVelocityTo(measurement, measurement_var);
-					aid_src.time_last_fuse = _imu_sample_delayed.time_us;
+					aid_src.time_last_fuse = _time_delayed_us;
 
 				} else {
 					// EV has reset, but quality isn't sufficient
@@ -174,7 +174,7 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
 					_information_events.flags.reset_vel_to_vision = true;
 					ECL_WARN("%s fusion failing, resetting", AID_SRC_NAME);
 					resetVelocityTo(measurement, measurement_var);
-					aid_src.time_last_fuse = _imu_sample_delayed.time_us;
+					aid_src.time_last_fuse = _time_delayed_us;
 
 					if (_control_status.flags.in_air) {
 						_nb_ev_vel_reset_available--;
@@ -213,7 +213,7 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
 				ECL_INFO("starting %s fusion", AID_SRC_NAME);
 			}
 
-			aid_src.time_last_fuse = _imu_sample_delayed.time_us;
+			aid_src.time_last_fuse = _time_delayed_us;
 
 			_nb_ev_vel_reset_available = 5;
 			_information_events.flags.starting_vision_vel_fusion = true;

src/modules/ekf2/EKF/ev_yaw_control.cpp

@@ -82,7 +82,7 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common
 					ECL_INFO("reset to %s", AID_SRC_NAME);
 					//_information_events.flags.reset_yaw_to_vision = true; // TODO
 					resetQuatStateYaw(aid_src.observation, aid_src.observation_variance);
-					aid_src.time_last_fuse = _imu_sample_delayed.time_us;
+					aid_src.time_last_fuse = _time_delayed_us;
 
 				} else {
 					// EV has reset, but quality isn't sufficient
@@ -106,7 +106,7 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common
 					//_information_events.flags.reset_yaw_to_vision = true; // TODO
 					ECL_WARN("%s fusion failing, resetting", AID_SRC_NAME);
 					resetQuatStateYaw(aid_src.innovation, aid_src.observation_variance);
-					aid_src.time_last_fuse = _imu_sample_delayed.time_us;
+					aid_src.time_last_fuse = _time_delayed_us;
 
 					if (_control_status.flags.in_air) {
 						_nb_ev_yaw_reset_available--;
@@ -148,7 +148,7 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common
 					_control_status.flags.yaw_align = true;
 				}
 
-				aid_src.time_last_fuse = _imu_sample_delayed.time_us;
+				aid_src.time_last_fuse = _time_delayed_us;
 				_information_events.flags.starting_vision_yaw_fusion = true;
 				_control_status.flags.ev_yaw = true;
 
@@ -162,7 +162,7 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common
 
 				// reset yaw to EV
 				resetQuatStateYaw(aid_src.observation, aid_src.observation_variance);
-				aid_src.time_last_fuse = _imu_sample_delayed.time_us;
+				aid_src.time_last_fuse = _time_delayed_us;
 
 				_information_events.flags.starting_vision_yaw_fusion = true;
 				_control_status.flags.yaw_align = false;

src/modules/ekf2/EKF/fake_height_control.cpp

@@ -51,7 +51,7 @@ void Ekf::controlFakeHgtFusion()
 		const float obs_var = sq(_params.pos_noaid_noise);
 		const float innov_gate = 3.f;
 
-		updateVerticalPositionAidSrcStatus(_imu_sample_delayed.time_us, _last_known_pos(2), obs_var, innov_gate, aid_src);
+		updateVerticalPositionAidSrcStatus(_time_delayed_us, _last_known_pos(2), obs_var, innov_gate, aid_src);
 
 
 		const bool continuing_conditions_passing = !isVerticalAidingActive();
@@ -98,7 +98,7 @@ void Ekf::resetFakeHgtFusion()
 	resetVerticalVelocityToZero();
 	resetHeightToLastKnown();
 
-	_aid_src_fake_hgt.time_last_fuse = _imu_sample_delayed.time_us;
+	_aid_src_fake_hgt.time_last_fuse = _time_delayed_us;
 }
 
 void Ekf::resetHeightToLastKnown()

src/modules/ekf2/EKF/fake_pos_control.cpp

@@ -65,7 +65,7 @@ void Ekf::controlFakePosFusion()
 
 		const float innov_gate = 3.f;
 
-		updateHorizontalPositionAidSrcStatus(_imu_sample_delayed.time_us, Vector2f(_last_known_pos), obs_var, innov_gate, aid_src);
+		updateHorizontalPositionAidSrcStatus(_time_delayed_us, Vector2f(_last_known_pos), obs_var, innov_gate, aid_src);
 
 
 		const bool continuing_conditions_passing = !isHorizontalAidingActive();
@@ -118,7 +118,7 @@ void Ekf::resetFakePosFusion()
 	resetHorizontalPositionToLastKnown();
 	resetHorizontalVelocityToZero();
 
-	_aid_src_fake_pos.time_last_fuse = _imu_sample_delayed.time_us;
+	_aid_src_fake_pos.time_last_fuse = _time_delayed_us;
 }
 
 void Ekf::stopFakePosFusion()

src/modules/ekf2/EKF/gnss_height_control.cpp

@@ -123,7 +123,7 @@ void Ekf::controlGnssHeightFusion(const gpsSample &gps_sample)
 						resetVerticalVelocityToZero();
 					}
 
-					aid_src.time_last_fuse = _imu_sample_delayed.time_us;
+					aid_src.time_last_fuse = _time_delayed_us;
 
 				} else if (is_fusion_failing) {
 					// Some other height source is still working
@@ -151,7 +151,7 @@ void Ekf::controlGnssHeightFusion(const gpsSample &gps_sample)
 					bias_est.setBias(_state.pos(2) + measurement);
 				}
 
-				aid_src.time_last_fuse = _imu_sample_delayed.time_us;
+				aid_src.time_last_fuse = _time_delayed_us;
 				bias_est.setFusionActive();
 				_control_status.flags.gps_hgt = true;
 			}

src/modules/ekf2/EKF/gps_checks.cpp

@@ -239,7 +239,7 @@ bool Ekf::gps_is_good(const gpsMessage &gps)
 
 	// assume failed first time through
 	if (_last_gps_fail_us == 0) {
-		_last_gps_fail_us = _imu_sample_delayed.time_us;
+		_last_gps_fail_us = _time_delayed_us;
 	}
 
 	// if any user selected checks have failed, record the fail time
@@ -255,10 +255,10 @@ bool Ekf::gps_is_good(const gpsMessage &gps)
 		(_gps_check_fail_status.flags.hspeed  && (_params.gps_check_mask & MASK_GPS_HSPD)) ||
 		(_gps_check_fail_status.flags.vspeed  && (_params.gps_check_mask & MASK_GPS_VSPD))
 	) {
-		_last_gps_fail_us = _imu_sample_delayed.time_us;
+		_last_gps_fail_us = _time_delayed_us;
 
 	} else {
-		_last_gps_pass_us = _imu_sample_delayed.time_us;
+		_last_gps_pass_us = _time_delayed_us;
 	}
 
 	// continuous period without fail of x seconds required to return a healthy status

src/modules/ekf2/EKF/gps_control.cpp

@@ -185,13 +185,13 @@ void Ekf::controlGpsFusion()
 					// reset velocity
 					_information_events.flags.reset_vel_to_gps = true;
 					resetVelocityTo(velocity, vel_obs_var);
-					_aid_src_gnss_vel.time_last_fuse = _imu_sample_delayed.time_us;
+					_aid_src_gnss_vel.time_last_fuse = _time_delayed_us;
 				}
 
 				// reset position
 				_information_events.flags.reset_pos_to_gps = true;
 				resetHorizontalPositionTo(position, pos_obs_var);
-				_aid_src_gnss_pos.time_last_fuse = _imu_sample_delayed.time_us;
+				_aid_src_gnss_pos.time_last_fuse = _time_delayed_us;
 
 				_control_status.flags.gps = true;
 
@@ -204,12 +204,12 @@ void Ekf::controlGpsFusion()
 					// reset velocity
 					_information_events.flags.reset_vel_to_gps = true;
 					resetVelocityTo(velocity, vel_obs_var);
-					_aid_src_gnss_vel.time_last_fuse = _imu_sample_delayed.time_us;
+					_aid_src_gnss_vel.time_last_fuse = _time_delayed_us;
 
 					// reset position
 					_information_events.flags.reset_pos_to_gps = true;
 					resetHorizontalPositionTo(position, pos_obs_var);
-					_aid_src_gnss_pos.time_last_fuse = _imu_sample_delayed.time_us;
+					_aid_src_gnss_pos.time_last_fuse = _time_delayed_us;
 				}
 			}
 		}

src/modules/ekf2/EKF/gps_yaw_fusion.cpp

@@ -137,8 +137,8 @@ void Ekf::fuseGpsYaw()
 	gnss_yaw.fused = is_fused;
 
 	if (is_fused) {
-		_time_last_heading_fuse = _imu_sample_delayed.time_us;
-		gnss_yaw.time_last_fuse = _imu_sample_delayed.time_us;
+		_time_last_heading_fuse = _time_delayed_us;
+		gnss_yaw.time_last_fuse = _time_delayed_us;
 	}
 }
 
@@ -159,7 +159,7 @@ bool Ekf::resetYawToGps(const float gnss_yaw)
 	const float yaw_variance = sq(fmaxf(_params.gps_heading_noise, 1.e-2f));
 	resetQuatStateYaw(measured_yaw, yaw_variance);
 
-	_aid_src_gnss_yaw.time_last_fuse = _imu_sample_delayed.time_us;
+	_aid_src_gnss_yaw.time_last_fuse = _time_delayed_us;
 	_gnss_yaw_signed_test_ratio_lpf.reset(0.f);
 
 	return true;

src/modules/ekf2/EKF/height_control.cpp

@@ -37,9 +37,9 @@
 
 #include "ekf.h"
 
-void Ekf::controlHeightFusion()
+void Ekf::controlHeightFusion(const imuSample &imu_delayed)
 {
-	checkVerticalAccelerationHealth();
+	checkVerticalAccelerationHealth(imu_delayed);
 
 	updateGroundEffect();
 
@@ -61,6 +61,7 @@ void Ekf::checkHeightSensorRefFallback()
 
 	switch (_params.height_sensor_ref) {
 	default:
+
 	/* FALLTHROUGH */
 	case HeightSensor::UNKNOWN:
 		fallback_list[0] = HeightSensor::GNSS;
@@ -110,7 +111,7 @@ void Ekf::checkHeightSensorRefFallback()
 	}
 }
 
-void Ekf::checkVerticalAccelerationHealth()
+void Ekf::checkVerticalAccelerationHealth(const imuSample &imu_delayed)
 {
 	// Check for IMU accelerometer vibration induced clipping as evidenced by the vertical
 	// innovations being positive and not stale.
@@ -121,9 +122,9 @@ void Ekf::checkVerticalAccelerationHealth()
 
 	// Check for more than 50% clipping affected IMU samples within the past 1 second
 	const uint16_t clip_count_limit = 1.f / _dt_ekf_avg;
-	const bool is_clipping = _imu_sample_delayed.delta_vel_clipping[0] ||
-				 _imu_sample_delayed.delta_vel_clipping[1] ||
-				 _imu_sample_delayed.delta_vel_clipping[2];
+	const bool is_clipping = imu_delayed.delta_vel_clipping[0] ||
+				 imu_delayed.delta_vel_clipping[1] ||
+				 imu_delayed.delta_vel_clipping[2];
 
 	if (is_clipping && _clip_counter < clip_count_limit) {
 		_clip_counter++;
@@ -141,10 +142,10 @@ void Ekf::checkVerticalAccelerationHealth()
 				    || (inertial_nav_falling_likelihood == Likelihood::HIGH);
 
 	if (bad_vert_accel) {
-		_time_bad_vert_accel = _imu_sample_delayed.time_us;
+		_time_bad_vert_accel = imu_delayed.time_us;
 
 	} else {
-		_time_good_vert_accel = _imu_sample_delayed.time_us;
+		_time_good_vert_accel = imu_delayed.time_us;
 	}
 
 	// declare a bad vertical acceleration measurement and make the declaration persist

src/modules/ekf2/EKF/mag_control.cpp

@@ -46,7 +46,7 @@ void Ekf::controlMagFusion()
 	magSample mag_sample;
 
 	if (_mag_buffer) {
-		mag_data_ready = _mag_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &mag_sample);
+		mag_data_ready = _mag_buffer->pop_first_older_than(_time_delayed_us, &mag_sample);
 
 		if (mag_data_ready) {
 
@@ -129,7 +129,7 @@ void Ekf::controlMagFusion()
 	if (mag_data_ready && !_control_status.flags.ev_yaw && !_control_status.flags.gps_yaw) {
 
 		if (shouldInhibitMag()) {
-			if (uint32_t(_imu_sample_delayed.time_us - _mag_use_not_inhibit_us) > (uint32_t)5e6) {
+			if (uint32_t(_time_delayed_us - _mag_use_not_inhibit_us) > (uint32_t)5e6) {
 				// If magnetometer use has been inhibited continuously then stop the fusion
 				stopMagFusion();
 			}
@@ -137,7 +137,7 @@ void Ekf::controlMagFusion()
 			return;
 
 		} else {
-			_mag_use_not_inhibit_us = _imu_sample_delayed.time_us;
+			_mag_use_not_inhibit_us = _time_delayed_us;
 		}
 
 		const bool mag_enabled_previously = _control_status_prev.flags.mag_hdg || _control_status_prev.flags.mag_3D;
@@ -216,7 +216,7 @@ void Ekf::runOnGroundYawReset()
 void Ekf::runInAirYawReset()
 {
 	// prevent a reset being performed more than once on the same frame
-	if ((_flt_mag_align_start_time == _imu_sample_delayed.time_us)
+	if ((_flt_mag_align_start_time == _time_delayed_us)
 	    || (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align)) {
 		return;
 	}
@@ -269,7 +269,7 @@ void Ekf::runInAirYawReset()
 			_control_status.flags.mag_aligned_in_flight = true;
 
 			// record the time for the magnetic field alignment event
-			_flt_mag_align_start_time = _imu_sample_delayed.time_us;
+			_flt_mag_align_start_time = _time_delayed_us;
 		}
 	}
 }
@@ -286,7 +286,7 @@ void Ekf::check3DMagFusionSuitability()
 	checkMagBiasObservability();
 
 	if (_mag_bias_observable || _yaw_angle_observable) {
-		_time_last_mov_3d_mag_suitable = _imu_sample_delayed.time_us;
+		_time_last_mov_3d_mag_suitable = _time_delayed_us;
 	}
 }
 
@@ -316,13 +316,13 @@ void Ekf::checkMagBiasObservability()
 
 	} else if (_mag_bias_observable) {
 		// require sustained yaw motion of 50% the initial yaw rate threshold
-		const float yaw_dt = 1e-6f * (float)(_imu_sample_delayed.time_us - _time_yaw_started);
+		const float yaw_dt = 1e-6f * (float)(_time_delayed_us - _time_yaw_started);
 		const float min_yaw_change_req =  0.5f * _params.mag_yaw_rate_gate * yaw_dt;
 		_mag_bias_observable = fabsf(_yaw_delta_ef) > min_yaw_change_req;
 	}
 
 	_yaw_delta_ef = 0.0f;
-	_time_yaw_started = _imu_sample_delayed.time_us;
+	_time_yaw_started = _time_delayed_us;
 }
 
 bool Ekf::canUse3DMagFusion() const
@@ -330,7 +330,7 @@ bool Ekf::canUse3DMagFusion() const
 	// Use of 3D fusion requires an in-air heading alignment but it should not
 	// be used when the heading and mag biases are not observable for more than 2 seconds
 	return _control_status.flags.mag_aligned_in_flight
-	       && ((_imu_sample_delayed.time_us - _time_last_mov_3d_mag_suitable) < (uint64_t)2e6);
+	       && ((_time_delayed_us - _time_last_mov_3d_mag_suitable) < (uint64_t)2e6);
 }
 
 void Ekf::checkMagDeclRequired()
@@ -411,7 +411,7 @@ void Ekf::run3DMagAndDeclFusions(const Vector3f &mag)
 {
 	// For the first few seconds after in-flight alignment we allow the magnetic field state estimates to stabilise
 	// before they are used to constrain heading drift
-	const bool update_all_states = ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) > (uint64_t)5e6);
+	const bool update_all_states = ((_time_delayed_us - _flt_mag_align_start_time) > (uint64_t)5e6);
 
 	if (!_mag_decl_cov_reset) {
 		// After any magnetic field covariance reset event the earth field state

src/modules/ekf2/EKF/mag_fusion.cpp

@@ -215,7 +215,7 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
 
 	if (fused[0] && fused[1] && fused[2]) {
 		aid_src_mag.fused = true;
-		aid_src_mag.time_last_fuse = _imu_sample_delayed.time_us;
+		aid_src_mag.time_last_fuse = _time_delayed_us;
 		return true;
 	}
 
@@ -313,9 +313,9 @@ bool Ekf::fuseYaw(const float innovation, const float variance, estimator_aid_so
 
 	if (measurementUpdate(Kfusion, aid_src_status.innovation_variance, aid_src_status.innovation)) {
 
-		_time_last_heading_fuse = _imu_sample_delayed.time_us;
+		_time_last_heading_fuse = _time_delayed_us;
 
-		aid_src_status.time_last_fuse = _imu_sample_delayed.time_us;
+		aid_src_status.time_last_fuse = _time_delayed_us;
 		aid_src_status.fused = true;
 
 		_fault_status.flags.bad_hdg = false;

src/modules/ekf2/EKF/optflow_fusion.cpp

@@ -151,7 +151,7 @@ void Ekf::fuseOptFlow()
 	_fault_status.flags.bad_optflow_Y = !fused[1];
 
 	if (fused[0] && fused[1]) {
-		_aid_src_optical_flow.time_last_fuse = _imu_sample_delayed.time_us;
+		_aid_src_optical_flow.time_last_fuse = _time_delayed_us;
 		_aid_src_optical_flow.fused = true;
 	}
 }

src/modules/ekf2/EKF/optical_flow_control.cpp

@@ -38,7 +38,7 @@
 
 #include "ekf.h"
 
-void Ekf::controlOpticalFlowFusion()
+void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 {
 	// Check if on ground motion is un-suitable for use of optical flow
 	if (!_control_status.flags.in_air) {
@@ -49,15 +49,15 @@ void Ekf::controlOpticalFlowFusion()
 	}
 
 	// Accumulate autopilot gyro data across the same time interval as the flow sensor
-	const Vector3f delta_angle(_imu_sample_delayed.delta_ang - (getGyroBias() * _imu_sample_delayed.delta_ang_dt));
+	const Vector3f delta_angle(imu_delayed.delta_ang - (getGyroBias() * imu_delayed.delta_ang_dt));
 	if (_delta_time_of < 0.1f) {
 		_imu_del_ang_of += delta_angle;
-		_delta_time_of += _imu_sample_delayed.delta_ang_dt;
+		_delta_time_of += imu_delayed.delta_ang_dt;
 
 	} else {
 		// reset the accumulators if the time interval is too large
 		_imu_del_ang_of = delta_angle;
-		_delta_time_of = _imu_sample_delayed.delta_ang_dt;
+		_delta_time_of = imu_delayed.delta_ang_dt;
 	}
 
 	if (_flow_data_ready) {
@@ -71,10 +71,10 @@ void Ekf::controlOpticalFlowFusion()
 
 		if (!is_delta_time_good && (_flow_sample_delayed.dt > FLT_EPSILON)) {
 
-			if (fabsf(_imu_sample_delayed.delta_ang_dt - _flow_sample_delayed.dt) < 0.1f) {
+			if (fabsf(imu_delayed.delta_ang_dt - _flow_sample_delayed.dt) < 0.1f) {
 				// reset accumulators to current IMU
 				_imu_del_ang_of = delta_angle;
-				_delta_time_of = _imu_sample_delayed.delta_ang_dt;
+				_delta_time_of = imu_delayed.delta_ang_dt;
 
 				is_delta_time_good = true;
 			}
@@ -82,7 +82,7 @@ void Ekf::controlOpticalFlowFusion()
 			if (is_quality_good && !is_delta_time_good) {
 				ECL_DEBUG("Optical flow: bad delta time: OF dt %.6f s (min: %.3f, max: %.3f), IMU dt %.6f s",
 					  (double)_flow_sample_delayed.dt, (double)delta_time_min, (double)delta_time_max,
-					  (double)_imu_sample_delayed.delta_ang_dt);
+					  (double)imu_delayed.delta_ang_dt);
 			}
 		}
 
@@ -140,8 +140,8 @@ void Ekf::controlOpticalFlowFusion()
 
 		// inhibit use of optical flow if motion is unsuitable and we are not reliant on it for flight navigation
 		const bool preflight_motion_not_ok = !_control_status.flags.in_air
-						     && ((_imu_sample_delayed.time_us > (_time_good_motion_us + (uint64_t)1E5))
-								     || (_imu_sample_delayed.time_us < (_time_bad_motion_us + (uint64_t)5E6)));
+						     && ((_time_delayed_us > (_time_good_motion_us + (uint64_t)1E5))
+								     || (_time_delayed_us < (_time_bad_motion_us + (uint64_t)5E6)));
 		const bool flight_condition_not_ok = _control_status.flags.in_air && !isTerrainEstimateValid();
 
 		const bool inhibit_flow_use = ((preflight_motion_not_ok || flight_condition_not_ok) && !is_flow_required)
@@ -184,7 +184,7 @@ void Ekf::controlOpticalFlowFusion()
 				}
 			}
 
-			_aid_src_optical_flow.time_last_fuse = _imu_sample_delayed.time_us;
+			_aid_src_optical_flow.time_last_fuse = _time_delayed_us;
 			_control_status.flags.opt_flow = true;
 
 			return;
@@ -192,7 +192,7 @@ void Ekf::controlOpticalFlowFusion()
 
 		if (_control_status.flags.opt_flow) {
 			// Wait until the midpoint of the flow sample has fallen behind the fusion time horizon
-			if (_imu_sample_delayed.time_us > (_flow_sample_delayed.time_us - uint32_t(1e6f * _flow_sample_delayed.dt) / 2)) {
+			if (_time_delayed_us > (_flow_sample_delayed.time_us - uint32_t(1e6f * _flow_sample_delayed.dt) / 2)) {
 				// Fuse optical flow LOS rate observations into the main filter only if height above ground has been updated recently
 				// but use a relaxed time criteria to enable it to coast through bad range finder data
 				if (isRecent(_time_last_hagl_fuse, (uint64_t)10e6)) {
@@ -216,7 +216,7 @@ void Ekf::controlOpticalFlowFusion()
 				_information_events.flags.reset_pos_to_last_known = true;
 				resetHorizontalPositionTo(_last_known_pos.xy(), 0.f);
 
-				_aid_src_optical_flow.time_last_fuse = _imu_sample_delayed.time_us;
+				_aid_src_optical_flow.time_last_fuse = _time_delayed_us;
 			}
 		}
 
@@ -237,15 +237,15 @@ void Ekf::updateOnGroundMotionForOpticalFlowChecks()
 					  || (_R_to_earth(2, 2) < cosf(math::radians(30.0f)))); // tilted excessively
 
 	if (motion_is_excessive) {
-		_time_bad_motion_us = _imu_sample_delayed.time_us;
+		_time_bad_motion_us = _time_delayed_us;
 
 	} else {
-		_time_good_motion_us = _imu_sample_delayed.time_us;
+		_time_good_motion_us = _time_delayed_us;
 	}
 }
 
 void Ekf::resetOnGroundMotionForOpticalFlowChecks()
 {
 	_time_bad_motion_us = 0;
-	_time_good_motion_us = _imu_sample_delayed.time_us;
+	_time_good_motion_us = _time_delayed_us;
 }

src/modules/ekf2/EKF/output_predictor.cpp

@@ -0,0 +1,374 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "output_predictor.h"
+
+using matrix::AxisAnglef;
+using matrix::Dcmf;
+using matrix::Quatf;
+using matrix::Vector2f;
+using matrix::Vector3f;
+
+void OutputPredictor::print_status()
+{
+	printf("output predictor: IMU dt: %.4f, EKF dt: %.4f\n", (double)_dt_update_states_avg, (double)_dt_correct_states_avg);
+
+	printf("output predictor: tracking error, angular: %.6f rad, velocity: %.3f m/s, position: %.3f m\n",
+	       (double)_output_tracking_error(0), (double)_output_tracking_error(1), (double)_output_tracking_error(2));
+
+	printf("output buffer: %d/%d (%d Bytes)\n", _output_buffer.entries(), _output_buffer.get_length(),
+	       _output_buffer.get_total_size());
+
+	printf("output vert buffer: %d/%d (%d Bytes)\n", _output_vert_buffer.entries(), _output_vert_buffer.get_length(),
+	       _output_vert_buffer.get_total_size());
+}
+
+void OutputPredictor::alignOutputFilter(const Quatf &quat_state, const Vector3f &vel_state, const Vector3f &pos_state)
+{
+	const outputSample &output_delayed = _output_buffer.get_oldest();
+
+	// calculate the quaternion rotation delta from the EKF to output observer states at the EKF fusion time horizon
+	Quatf q_delta{quat_state * output_delayed.quat_nominal.inversed()};
+	q_delta.normalize();
+
+	// calculate the velocity and position deltas between the output and EKF at the EKF fusion time horizon
+	const Vector3f vel_delta = vel_state - output_delayed.vel;
+	const Vector3f pos_delta = pos_state - output_delayed.pos;
+
+	// loop through the output filter state history and add the deltas
+	for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
+		_output_buffer[i].quat_nominal = q_delta * _output_buffer[i].quat_nominal;
+		_output_buffer[i].quat_nominal.normalize();
+		_output_buffer[i].vel += vel_delta;
+		_output_buffer[i].pos += pos_delta;
+	}
+
+	_output_new = _output_buffer.get_newest();
+}
+
+void OutputPredictor::reset()
+{
+	// TODO: who resets the output buffer content?
+	_output_new = {};
+	_output_vert_new = {};
+
+	_accel_bias.setZero();
+	_gyro_bias.setZero();
+
+	_time_last_update_states_us = 0;
+	_time_last_correct_states_us = 0;
+
+	_R_to_earth_now.setIdentity();
+	_vel_imu_rel_body_ned.setZero();
+	_vel_deriv.setZero();
+
+	_delta_angle_corr.setZero();
+
+	_vel_err_integ.setZero();
+	_pos_err_integ.setZero();
+
+	_output_tracking_error.setZero();
+
+	for (uint8_t index = 0; index < _output_buffer.get_length(); index++) {
+		_output_buffer[index] = {};
+	}
+
+	for (uint8_t index = 0; index < _output_vert_buffer.get_length(); index++) {
+		_output_vert_buffer[index] = {};
+	}
+}
+
+void OutputPredictor::resetQuaternion(const Quatf &quat_change)
+{
+	// add the reset amount to the output observer buffered data
+	for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
+		_output_buffer[i].quat_nominal = quat_change * _output_buffer[i].quat_nominal;
+	}
+
+	// apply the change in attitude quaternion to our newest quaternion estimate
+	// which was already taken out from the output buffer
+	_output_new.quat_nominal = quat_change * _output_new.quat_nominal;
+}
+
+void OutputPredictor::resetHorizontalVelocityTo(const Vector2f &delta_horz_vel)
+{
+	for (uint8_t index = 0; index < _output_buffer.get_length(); index++) {
+		_output_buffer[index].vel.xy() += delta_horz_vel;
+	}
+
+	_output_new.vel.xy() += delta_horz_vel;
+}
+
+void OutputPredictor::resetVerticalVelocityTo(float delta_vert_vel)
+{
+	for (uint8_t index = 0; index < _output_buffer.get_length(); index++) {
+		_output_buffer[index].vel(2) += delta_vert_vel;
+		_output_vert_buffer[index].vert_vel += delta_vert_vel;
+	}
+
+	_output_new.vel(2) += delta_vert_vel;
+	_output_vert_new.vert_vel += delta_vert_vel;
+}
+
+void OutputPredictor::resetHorizontalPositionTo(const Vector2f &delta_horz_pos)
+{
+	for (uint8_t index = 0; index < _output_buffer.get_length(); index++) {
+		_output_buffer[index].pos.xy() += delta_horz_pos;
+	}
+
+	_output_new.pos.xy() += delta_horz_pos;
+}
+
+void OutputPredictor::resetVerticalPositionTo(const float new_vert_pos, const float vert_pos_change)
+{
+	// apply the change in height / height rate to our newest height / height rate estimate
+	// which have already been taken out from the output buffer
+	_output_new.pos(2) += vert_pos_change;
+
+	// add the reset amount to the output observer buffered data
+	for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
+		_output_buffer[i].pos(2) += vert_pos_change;
+		_output_vert_buffer[i].vert_vel_integ += vert_pos_change;
+	}
+
+	// add the reset amount to the output observer vertical position state
+	_output_vert_new.vert_vel_integ = new_vert_pos;
+}
+
+void OutputPredictor::calculateOutputStates(const uint64_t time_us, const Vector3f &delta_angle,
+		const float delta_angle_dt, const Vector3f &delta_velocity, const float delta_velocity_dt)
+{
+	// Use full rate IMU data at the current time horizon
+	if (_time_last_update_states_us != 0) {
+		const float dt = math::constrain((time_us - _time_last_update_states_us) * 1e-6f, 0.0001f, 0.03f);
+		_dt_update_states_avg = 0.8f * _dt_update_states_avg + 0.2f * dt;
+	}
+
+	_time_last_update_states_us = time_us;
+
+	// correct delta angle and delta velocity for bias offsets
+	// Apply corrections to the delta angle required to track the quaternion states at the EKF fusion time horizon
+	const Vector3f delta_angle_bias_scaled = _gyro_bias * delta_angle_dt;
+	const Vector3f delta_angle_corrected(delta_angle - delta_angle_bias_scaled + _delta_angle_corr);
+
+	const Vector3f delta_vel_bias_scaled = _accel_bias * delta_velocity_dt;
+	const Vector3f delta_velocity_corrected(delta_velocity - delta_vel_bias_scaled);
+
+	_output_new.time_us = time_us;
+	_output_vert_new.time_us = time_us;
+
+	const Quatf dq(AxisAnglef{delta_angle_corrected});
+
+	// rotate the previous INS quaternion by the delta quaternions
+	_output_new.quat_nominal = _output_new.quat_nominal * dq;
+
+	// the quaternions must always be normalised after modification
+	_output_new.quat_nominal.normalize();
+
+	// calculate the rotation matrix from body to earth frame
+	_R_to_earth_now = Dcmf(_output_new.quat_nominal);
+
+	// rotate the delta velocity to earth frame
+	Vector3f delta_vel_earth{_R_to_earth_now * delta_velocity_corrected};
+
+	// correct for measured acceleration due to gravity
+	delta_vel_earth(2) += CONSTANTS_ONE_G * delta_velocity_dt;
+
+	// calculate the earth frame velocity derivatives
+	if (delta_velocity_dt > 0.001f) {
+		_vel_deriv = delta_vel_earth / delta_velocity_dt;
+	}
+
+	// save the previous velocity so we can use trapezoidal integration
+	const Vector3f vel_last(_output_new.vel);
+
+	// increment the INS velocity states by the measurement plus corrections
+	// do the same for vertical state used by alternative correction algorithm
+	_output_new.vel += delta_vel_earth;
+	_output_vert_new.vert_vel += delta_vel_earth(2);
+
+	// use trapezoidal integration to calculate the INS position states
+	// do the same for vertical state used by alternative correction algorithm
+	const Vector3f delta_pos_NED = (_output_new.vel + vel_last) * (delta_velocity_dt * 0.5f);
+	_output_new.pos += delta_pos_NED;
+	_output_vert_new.vert_vel_integ += delta_pos_NED(2);
+
+	// accumulate the time for each update
+	_output_vert_new.dt += delta_velocity_dt;
+
+	// correct velocity for IMU offset
+	if (delta_angle_dt > 0.001f) {
+		// calculate the average angular rate across the last IMU update
+		const Vector3f ang_rate = delta_angle_corrected / delta_angle_dt;
+
+		// calculate the velocity of the IMU relative to the body origin
+		const Vector3f vel_imu_rel_body = ang_rate % _imu_pos_body;
+
+		// rotate the relative velocity into earth frame
+		_vel_imu_rel_body_ned = _R_to_earth_now * vel_imu_rel_body;
+	}
+}
+
+void OutputPredictor::correctOutputStates(const uint64_t time_delayed_us,
+		const matrix::Vector3f &gyro_bias, const matrix::Vector3f &accel_bias,
+		const Quatf &quat_state, const Vector3f &vel_state, const Vector3f &pos_state)
+{
+	// calculate an average filter update time
+	if (_time_last_correct_states_us != 0) {
+		const float dt = math::constrain((time_delayed_us - _time_last_correct_states_us) * 1e-6f, 0.0001f, 0.03f);
+		_dt_correct_states_avg = 0.8f * _dt_correct_states_avg + 0.2f * dt;
+	}
+
+	_time_last_correct_states_us = time_delayed_us;
+
+	// store IMU bias for calculateOutputStates
+	_gyro_bias = gyro_bias;
+	_accel_bias = accel_bias;
+
+	// store the INS states in a ring buffer with the same length and time coordinates as the IMU data buffer
+	_output_buffer.push(_output_new);
+	_output_vert_buffer.push(_output_vert_new);
+
+	// get the oldest INS state data from the ring buffer
+	// this data will be at the EKF fusion time horizon
+	// TODO: there is no guarantee that data is at delayed fusion horizon
+	//       Shouldnt we use pop_first_older_than?
+	const outputSample &output_delayed = _output_buffer.get_oldest();
+	const outputVert &output_vert_delayed = _output_vert_buffer.get_oldest();
+
+	// calculate the quaternion delta between the INS and EKF quaternions at the EKF fusion time horizon
+	const Quatf q_error((quat_state.inversed() * output_delayed.quat_nominal).normalized());
+
+	// convert the quaternion delta to a delta angle
+	const float scalar = (q_error(0) >= 0.0f) ? -2.f : 2.f;
+
+	const Vector3f delta_ang_error{scalar * q_error(1), scalar * q_error(2), scalar * q_error(3)};
+
+	// calculate a gain that provides tight tracking of the estimator attitude states and
+	// adjust for changes in time delay to maintain consistent damping ratio of ~0.7
+	const uint64_t time_latest_us = _time_last_update_states_us;
+	const float time_delay = fmaxf((time_latest_us - time_delayed_us) * 1e-6f, _dt_update_states_avg);
+	const float att_gain = 0.5f * _dt_update_states_avg / time_delay;
+
+	// calculate a corrrection to the delta angle
+	// that will cause the INS to track the EKF quaternions
+	_delta_angle_corr = delta_ang_error * att_gain;
+	_output_tracking_error(0) = delta_ang_error.norm();
+
+	/*
+	* Loop through the output filter state history and apply the corrections to the velocity and position states.
+	* This method is too expensive to use for the attitude states due to the quaternion operations required
+	* but because it eliminates the time delay in the 'correction loop' it allows higher tracking gains
+	* to be used and reduces tracking error relative to EKF states.
+	*/
+
+	// Complementary filter gains
+	const float vel_gain = _dt_correct_states_avg / math::constrain(_vel_tau, _dt_correct_states_avg, 10.f);
+	const float pos_gain = _dt_correct_states_avg / math::constrain(_pos_tau, _dt_correct_states_avg, 10.f);
+
+	// calculate down velocity and position tracking errors
+	const float vert_vel_err = (vel_state(2) - output_vert_delayed.vert_vel);
+	const float vert_vel_integ_err = (pos_state(2) - output_vert_delayed.vert_vel_integ);
+
+	// calculate a velocity correction that will be applied to the output state history
+	// using a PD feedback tuned to a 5% overshoot
+	const float vert_vel_correction = vert_vel_integ_err * pos_gain + vert_vel_err * vel_gain * 1.1f;
+
+	applyCorrectionToVerticalOutputBuffer(vert_vel_correction);
+
+	// calculate velocity and position tracking errors
+	const Vector3f vel_err(vel_state - output_delayed.vel);
+	const Vector3f pos_err(pos_state - output_delayed.pos);
+
+	_output_tracking_error(1) = vel_err.norm();
+	_output_tracking_error(2) = pos_err.norm();
+
+	// calculate a velocity correction that will be applied to the output state history
+	_vel_err_integ += vel_err;
+	const Vector3f vel_correction = vel_err * vel_gain + _vel_err_integ * sq(vel_gain) * 0.1f;
+
+	// calculate a position correction that will be applied to the output state history
+	_pos_err_integ += pos_err;
+	const Vector3f pos_correction = pos_err * pos_gain + _pos_err_integ * sq(pos_gain) * 0.1f;
+
+	applyCorrectionToOutputBuffer(vel_correction, pos_correction);
+}
+
+void OutputPredictor::applyCorrectionToVerticalOutputBuffer(float vert_vel_correction)
+{
+	// loop through the vertical output filter state history starting at the oldest and apply the corrections to the
+	// vert_vel states and propagate vert_vel_integ forward using the corrected vert_vel
+	uint8_t index = _output_vert_buffer.get_oldest_index();
+
+	const uint8_t size = _output_vert_buffer.get_length();
+
+	for (uint8_t counter = 0; counter < (size - 1); counter++) {
+		const uint8_t index_next = (index + 1) % size;
+		outputVert &current_state = _output_vert_buffer[index];
+		outputVert &next_state = _output_vert_buffer[index_next];
+
+		// correct the velocity
+		if (counter == 0) {
+			current_state.vert_vel += vert_vel_correction;
+		}
+
+		next_state.vert_vel += vert_vel_correction;
+
+		// position is propagated forward using the corrected velocity and a trapezoidal integrator
+		next_state.vert_vel_integ = current_state.vert_vel_integ + (current_state.vert_vel + next_state.vert_vel) * 0.5f * next_state.dt;
+
+		// advance the index
+		index = (index + 1) % size;
+	}
+
+	// update output state to corrected values
+	_output_vert_new = _output_vert_buffer.get_newest();
+
+	// reset time delta to zero for the next accumulation of full rate IMU data
+	_output_vert_new.dt = 0.0f;
+}
+
+void OutputPredictor::applyCorrectionToOutputBuffer(const Vector3f &vel_correction, const Vector3f &pos_correction)
+{
+	// loop through the output filter state history and apply the corrections to the velocity and position states
+	for (uint8_t index = 0; index < _output_buffer.get_length(); index++) {
+		// a constant velocity correction is applied
+		_output_buffer[index].vel += vel_correction;
+
+		// a constant position correction is applied
+		_output_buffer[index].pos += pos_correction;
+	}
+
+	// update output state to corrected values
+	_output_new = _output_buffer.get_newest();
+}

src/modules/ekf2/EKF/output_predictor.h

@@ -0,0 +1,193 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#ifndef EKF_OUTPUT_PREDICTOR_H
+#define EKF_OUTPUT_PREDICTOR_H
+
+#include <matrix/math.hpp>
+
+#include "common.h"
+#include "RingBuffer.h"
+
+#include <lib/geo/geo.h>
+
+class OutputPredictor
+{
+public:
+	OutputPredictor()
+	{
+		reset();
+	};
+
+	~OutputPredictor() = default;
+
+	// modify output filter to match the the EKF state at the fusion time horizon
+	void alignOutputFilter(const matrix::Quatf &quat_state, const matrix::Vector3f &vel_state,
+			       const matrix::Vector3f &pos_state);
+	/*
+	* Implement a strapdown INS algorithm using the latest IMU data at the current time horizon.
+	* Buffer the INS states and calculate the difference with the EKF states at the delayed fusion time horizon.
+	* Calculate delta angle, delta velocity and velocity corrections from the differences and apply them at the
+	* current time horizon so that the INS states track the EKF states at the delayed fusion time horizon.
+	* The inspiration for using a complementary filter to correct for time delays in the EKF
+	* is based on the work by A Khosravian:
+	* “Recursive Attitude Estimation in the Presence of Multi-rate and Multi-delay Vector Measurements”
+	* A Khosravian, J Trumpf, R Mahony, T Hamel, Australian National University
+	*/
+	void calculateOutputStates(const uint64_t time_us, const matrix::Vector3f &delta_angle, const float delta_angle_dt,
+				   const matrix::Vector3f &delta_velocity, const float delta_velocity_dt);
+
+	void correctOutputStates(const uint64_t time_delayed_us,
+				 const matrix::Vector3f &gyro_bias, const matrix::Vector3f &accel_bias,
+				 const matrix::Quatf &quat_state, const matrix::Vector3f &vel_state, const matrix::Vector3f &pos_state);
+
+	void resetQuaternion(const matrix::Quatf &quat_change);
+
+	void resetHorizontalVelocityTo(const matrix::Vector2f &delta_horz_vel);
+	void resetVerticalVelocityTo(float delta_vert_vel);
+
+	void resetHorizontalPositionTo(const matrix::Vector2f &delta_horz_pos);
+	void resetVerticalPositionTo(const float new_vert_pos, const float vert_pos_change);
+
+	void print_status();
+
+	bool allocate(uint8_t size)
+	{
+		if (_output_buffer.allocate(size) && _output_vert_buffer.allocate(size)) {
+			reset();
+			return true;
+		}
+
+		return false;
+	}
+
+	void reset();
+
+	const matrix::Quatf &getQuaternion() const { return _output_new.quat_nominal; }
+
+	// get the velocity of the body frame origin in local NED earth frame
+	matrix::Vector3f getVelocity() const { return _output_new.vel - _vel_imu_rel_body_ned; }
+
+	// get the velocity derivative in earth frame
+	const matrix::Vector3f &getVelocityDerivative() const { return _vel_deriv; }
+
+	// get the derivative of the vertical position of the body frame origin in local NED earth frame
+	float getVerticalPositionDerivative() const { return _output_vert_new.vert_vel - _vel_imu_rel_body_ned(2); }
+
+	// get the position of the body frame origin in local earth frame
+	matrix::Vector3f getPosition() const
+	{
+		// rotate the position of the IMU relative to the boy origin into earth frame
+		const matrix::Vector3f pos_offset_earth{_R_to_earth_now * _imu_pos_body};
+		// subtract from the EKF position (which is at the IMU) to get position at the body origin
+		return _output_new.pos - pos_offset_earth;
+	}
+
+	// return an array containing the output predictor angular, velocity and position tracking
+	// error magnitudes (rad), (m/sec), (m)
+	const matrix::Vector3f &getOutputTrackingError() const { return _output_tracking_error; }
+
+	void set_imu_offset(const matrix::Vector3f &offset) { _imu_pos_body = offset; }
+	void set_pos_correction_tc(const float tau) { _pos_tau = tau; }
+	void set_vel_correction_tc(const float tau) { _vel_tau = tau; }
+
+private:
+
+	/*
+	* Calculate a correction to be applied to vert_vel that casues vert_vel_integ to track the EKF
+	* down position state at the fusion time horizon using an alternative algorithm to what
+	* is used for the vel and pos state tracking. The algorithm applies a correction to the vert_vel
+	* state history and propagates vert_vel_integ forward in time using the corrected vert_vel history.
+	* This provides an alternative vertical velocity output that is closer to the first derivative
+	* of the position but does degrade tracking relative to the EKF state.
+	*/
+	void applyCorrectionToVerticalOutputBuffer(float vert_vel_correction);
+
+	/*
+	* Calculate corrections to be applied to vel and pos output state history.
+	* The vel and pos state history are corrected individually so they track the EKF states at
+	* the fusion time horizon. This option provides the most accurate tracking of EKF states.
+	*/
+	void applyCorrectionToOutputBuffer(const matrix::Vector3f &vel_correction, const matrix::Vector3f &pos_correction);
+
+	// return the square of two floating point numbers - used in auto coded sections
+	static constexpr float sq(float var) { return var * var; }
+
+	struct outputSample {
+		uint64_t         time_us{0};                       ///< timestamp of the measurement (uSec)
+		matrix::Quatf    quat_nominal{1.f, 0.f, 0.f, 0.f}; ///< nominal quaternion describing vehicle attitude
+		matrix::Vector3f vel{0.f, 0.f, 0.f};               ///< NED velocity estimate in earth frame (m/sec)
+		matrix::Vector3f pos{0.f, 0.f, 0.f};               ///< NED position estimate in earth frame (m/sec)
+	};
+
+	struct outputVert {
+		uint64_t time_us{0};          ///< timestamp of the measurement (uSec)
+		float    vert_vel{0.f};       ///< Vertical velocity calculated using alternative algorithm (m/sec)
+		float    vert_vel_integ{0.f}; ///< Integral of vertical velocity (m)
+		float    dt{0.f};             ///< delta time (sec)
+	};
+
+	RingBuffer<outputSample> _output_buffer{12};
+	RingBuffer<outputVert> _output_vert_buffer{12};
+
+	matrix::Vector3f _accel_bias{};
+	matrix::Vector3f _gyro_bias{};
+
+	float _dt_update_states_avg{0.005f};  // average imu update period in s
+	float _dt_correct_states_avg{0.010f}; // average update rate of the ekf in s
+
+	uint64_t _time_last_update_states_us{0}; ///< last time the output states were updated (uSec)
+	uint64_t _time_last_correct_states_us{0}; ///< last time the output states were updated (uSec)
+
+	// Output Predictor
+	outputSample _output_new{};		// filter output on the non-delayed time horizon
+	outputVert _output_vert_new{};		// vertical filter output on the non-delayed time horizon
+	matrix::Matrix3f _R_to_earth_now{};		// rotation matrix from body to earth frame at current time
+	matrix::Vector3f _vel_imu_rel_body_ned{};		// velocity of IMU relative to body origin in NED earth frame
+	matrix::Vector3f _vel_deriv{};		// velocity derivative at the IMU in NED earth frame (m/s/s)
+
+	// output predictor states
+	matrix::Vector3f _delta_angle_corr{};	///< delta angle correction vector (rad)
+	matrix::Vector3f _vel_err_integ{};	///< integral of velocity tracking error (m)
+	matrix::Vector3f _pos_err_integ{};	///< integral of position tracking error (m.s)
+
+	matrix::Vector3f _output_tracking_error{}; ///< contains the magnitude of the angle, velocity and position track errors (rad, m/s, m)
+
+	matrix::Vector3f _imu_pos_body{};                ///< xyz position of IMU in body frame (m)
+
+	// output complementary filter tuning
+	float _vel_tau{0.25f};                   ///< velocity state correction time constant (1/sec)
+	float _pos_tau{0.25f};                   ///< position state correction time constant (1/sec)
+};
+
+#endif // !EKF_OUTPUT_PREDICTOR_H

src/modules/ekf2/EKF/range_height_control.cpp

@@ -101,7 +101,7 @@ void Ekf::controlRangeHeightFusion()
 					// reset vertical velocity
 					resetVerticalVelocityToZero();
 
-					aid_src.time_last_fuse = _imu_sample_delayed.time_us;
+					aid_src.time_last_fuse = _time_delayed_us;
 
 				} else if (is_fusion_failing) {
 					// Some other height source is still working
@@ -139,7 +139,7 @@ void Ekf::controlRangeHeightFusion()
 					bias_est.setBias(_state.pos(2) + measurement);
 				}
 
-				aid_src.time_last_fuse = _imu_sample_delayed.time_us;
+				aid_src.time_last_fuse = _time_delayed_us;
 				bias_est.setFusionActive();
 				_control_status.flags.rng_hgt = true;
 			}

src/modules/ekf2/EKF/sideslip_fusion.cpp

@@ -65,7 +65,7 @@ void Ekf::updateSideslip(estimator_aid_source1d_s &sideslip) const
 
 	sideslip.fusion_enabled = _control_status.flags.fuse_aspd;
 
-	sideslip.timestamp_sample = _imu_sample_delayed.time_us;
+	sideslip.timestamp_sample = _time_delayed_us;
 
 	const float innov_gate = fmaxf(_params.beta_innov_gate, 1.f);
 	setEstimatorAidStatusTestRatio(sideslip, innov_gate);
@@ -121,6 +121,6 @@ void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip)
 	_fault_status.flags.bad_sideslip = !is_fused;
 
 	if (is_fused) {
-		sideslip.time_last_fuse = _imu_sample_delayed.time_us;
+		sideslip.time_last_fuse = _time_delayed_us;
 	}
 }

src/modules/ekf2/EKF/terrain_estimator.cpp

@@ -55,7 +55,7 @@ void Ekf::initHagl()
 	_terrain_var = sq(_params.rng_gnd_clearance);
 }
 
-void Ekf::runTerrainEstimator()
+void Ekf::runTerrainEstimator(const imuSample &imu_delayed)
 {
 	// If we are on ground, store the local position and time to use as a reference
 	if (!_control_status.flags.in_air) {
@@ -63,7 +63,7 @@ void Ekf::runTerrainEstimator()
 		_control_status.flags.rng_fault = false;
 	}
 
-	predictHagl();
+	predictHagl(imu_delayed);
 
 	controlHaglRngFusion();
 	controlHaglFlowFusion();
@@ -75,15 +75,15 @@ void Ekf::runTerrainEstimator()
 	}
 }
 
-void Ekf::predictHagl()
+void Ekf::predictHagl(const imuSample &imu_delayed)
 {
 	// predict the state variance growth where the state is the vertical position of the terrain underneath the vehicle
 
 	// process noise due to errors in vehicle height estimate
-	_terrain_var += sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_p_noise);
+	_terrain_var += sq(imu_delayed.delta_vel_dt * _params.terrain_p_noise);
 
 	// process noise due to terrain gradient
-	_terrain_var += sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_gradient)
+	_terrain_var += sq(imu_delayed.delta_vel_dt * _params.terrain_gradient)
 			* (sq(_state.vel(0)) + sq(_state.vel(1)));
 
 	// limit the variance to prevent it becoming badly conditioned
@@ -104,7 +104,7 @@ void Ekf::controlHaglRngFusion()
 		//const bool continuing_conditions_passing = _control_status.flags.in_air && !_control_status.flags.rng_hgt; // TODO: should not be fused when using range height
 		const bool starting_conditions_passing = continuing_conditions_passing && _range_sensor.isRegularlySendingData() && (_rng_consistency_check.getTestRatio() < 1.f);
 
-		_time_last_healthy_rng_data = _imu_sample_delayed.time_us;
+		_time_last_healthy_rng_data = _time_delayed_us;
 
 		if (_hagl_sensor_status.flags.range_finder) {
 			if (continuing_conditions_passing) {
@@ -193,7 +193,7 @@ void Ekf::resetHaglRng()
 	_terrain_vpos = _state.pos(2) + _range_sensor.getDistBottom();
 	_terrain_var = getRngVar();
 	_terrain_vpos_reset_counter++;
-	_time_last_hagl_fuse = _imu_sample_delayed.time_us;
+	_time_last_hagl_fuse = _time_delayed_us;
 }
 
 void Ekf::stopHaglRngFusion()
@@ -238,7 +238,7 @@ void Ekf::fuseHaglRng()
 		// correct the variance
 		_terrain_var = fmaxf(_terrain_var * (1.0f - gain), 0.0f);
 		// record last successful fusion event
-		_time_last_hagl_fuse = _imu_sample_delayed.time_us;
+		_time_last_hagl_fuse = _time_delayed_us;
 		_innov_check_fail_status.flags.reject_hagl = false;
 
 	} else {
@@ -286,7 +286,7 @@ void Ekf::controlHaglFlowFusion()
 		}
 
 	} else if (_hagl_sensor_status.flags.flow
-		   && (_imu_sample_delayed.time_us > _flow_sample_delayed.time_us + (uint64_t)5e6)) {
+		   && (_time_delayed_us > _flow_sample_delayed.time_us + (uint64_t)5e6)) {
 		// No data anymore. Stop until it comes back.
 		stopHaglFlowFusion();
 	}
@@ -405,8 +405,8 @@ void Ekf::fuseFlowForTerrain()
 	_terrain_var = fmaxf(_terrain_var - KyHyP, sq(0.01f));
 
 
-	_time_last_flow_terrain_fuse = _imu_sample_delayed.time_us;
-	//_aid_src_optical_flow.time_last_fuse = _imu_sample_delayed.time_us; // TODO: separate aid source status for OF terrain?
+	_time_last_flow_terrain_fuse = _time_delayed_us;
+	//_aid_src_optical_flow.time_last_fuse = _time_delayed_us; // TODO: separate aid source status for OF terrain?
 	_aid_src_optical_flow.fused = true;
 }
 

src/modules/ekf2/EKF/vel_pos_fusion.cpp

@@ -137,7 +137,7 @@ void Ekf::fuseVelocity(estimator_aid_source2d_s &aid_src)
 		    && fuseVelPosHeight(aid_src.innovation[1], aid_src.innovation_variance[1], 1)
 		   ) {
 			aid_src.fused = true;
-			aid_src.time_last_fuse = _imu_sample_delayed.time_us;
+			aid_src.time_last_fuse = _time_delayed_us;
 
 		} else {
 			aid_src.fused = false;
@@ -154,7 +154,7 @@ void Ekf::fuseVelocity(estimator_aid_source3d_s &aid_src)
 		    && fuseVelPosHeight(aid_src.innovation[2], aid_src.innovation_variance[2], 2)
 		   ) {
 			aid_src.fused = true;
-			aid_src.time_last_fuse = _imu_sample_delayed.time_us;
+			aid_src.time_last_fuse = _time_delayed_us;
 
 		} else {
 			aid_src.fused = false;
@@ -170,7 +170,7 @@ void Ekf::fuseHorizontalPosition(estimator_aid_source2d_s &aid_src)
 		    && fuseVelPosHeight(aid_src.innovation[1], aid_src.innovation_variance[1], 4)
 		   ) {
 			aid_src.fused = true;
-			aid_src.time_last_fuse = _imu_sample_delayed.time_us;
+			aid_src.time_last_fuse = _time_delayed_us;
 
 		} else {
 			aid_src.fused = false;
@@ -184,7 +184,7 @@ void Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src)
 	if (aid_src.fusion_enabled && !aid_src.innovation_rejected) {
 		if (fuseVelPosHeight(aid_src.innovation, aid_src.innovation_variance, 5)) {
 			aid_src.fused = true;
-			aid_src.time_last_fuse = _imu_sample_delayed.time_us;
+			aid_src.time_last_fuse = _time_delayed_us;
 		}
 	}
 }
@@ -239,7 +239,7 @@ void Ekf::setVelPosStatus(const int index, const bool healthy)
 	case 0:
 		if (healthy) {
 			_fault_status.flags.bad_vel_N = false;
-			_time_last_hor_vel_fuse = _imu_sample_delayed.time_us;
+			_time_last_hor_vel_fuse = _time_delayed_us;
 
 		} else {
 			_fault_status.flags.bad_vel_N = true;
@@ -250,7 +250,7 @@ void Ekf::setVelPosStatus(const int index, const bool healthy)
 	case 1:
 		if (healthy) {
 			_fault_status.flags.bad_vel_E = false;
-			_time_last_hor_vel_fuse = _imu_sample_delayed.time_us;
+			_time_last_hor_vel_fuse = _time_delayed_us;
 
 		} else {
 			_fault_status.flags.bad_vel_E = true;
@@ -261,7 +261,7 @@ void Ekf::setVelPosStatus(const int index, const bool healthy)
 	case 2:
 		if (healthy) {
 			_fault_status.flags.bad_vel_D = false;
-			_time_last_ver_vel_fuse = _imu_sample_delayed.time_us;
+			_time_last_ver_vel_fuse = _time_delayed_us;
 
 		} else {
 			_fault_status.flags.bad_vel_D = true;
@@ -272,7 +272,7 @@ void Ekf::setVelPosStatus(const int index, const bool healthy)
 	case 3:
 		if (healthy) {
 			_fault_status.flags.bad_pos_N = false;
-			_time_last_hor_pos_fuse = _imu_sample_delayed.time_us;
+			_time_last_hor_pos_fuse = _time_delayed_us;
 
 		} else {
 			_fault_status.flags.bad_pos_N = true;
@@ -283,7 +283,7 @@ void Ekf::setVelPosStatus(const int index, const bool healthy)
 	case 4:
 		if (healthy) {
 			_fault_status.flags.bad_pos_E = false;
-			_time_last_hor_pos_fuse = _imu_sample_delayed.time_us;
+			_time_last_hor_pos_fuse = _time_delayed_us;
 
 		} else {
 			_fault_status.flags.bad_pos_E = true;
@@ -294,7 +294,7 @@ void Ekf::setVelPosStatus(const int index, const bool healthy)
 	case 5:
 		if (healthy) {
 			_fault_status.flags.bad_pos_D = false;
-			_time_last_hgt_fuse = _imu_sample_delayed.time_us;
+			_time_last_hgt_fuse = _time_delayed_us;
 
 		} else {
 			_fault_status.flags.bad_pos_D = true;

src/modules/ekf2/EKF/zero_velocity_update.cpp

@@ -64,7 +64,7 @@ void Ekf::controlZeroVelocityUpdate()
 			fuseVelPosHeight(innovation(1), innov_var(1), 1);
 			fuseVelPosHeight(innovation(2), innov_var(2), 2);
 
-			_time_last_zero_velocity_fuse = _imu_sample_delayed.time_us;
+			_time_last_zero_velocity_fuse = _time_delayed_us;
 		}
 	}
 }

src/modules/ekf2/EKF2.cpp

@@ -151,8 +151,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_param_ekf2_ev_pos_z(_params->ev_pos_body(2)),
 	_param_ekf2_arsp_thr(_params->arsp_thr),
 	_param_ekf2_fuse_beta(_params->beta_fusion_enabled),
-	_param_ekf2_tau_vel(_params->vel_Tau),
-	_param_ekf2_tau_pos(_params->pos_Tau),
 	_param_ekf2_gbias_init(_params->switch_on_gyro_bias),
 	_param_ekf2_abias_init(_params->switch_on_accel_bias),
 	_param_ekf2_angerr_init(_params->initial_tilt_err),
@@ -298,8 +296,8 @@ bool EKF2::multi_init(int imu, int mag)
 
 int EKF2::print_status()
 {
-	PX4_INFO_RAW("ekf2:%d EKF dt: %.4fs, IMU dt: %.4fs, attitude: %d, local position: %d, global position: %d\n",
-		     _instance, (double)_ekf.get_dt_ekf_avg(), (double)_ekf.get_dt_imu_avg(), _ekf.attitude_valid(),
+	PX4_INFO_RAW("ekf2:%d EKF dt: %.4fs, attitude: %d, local position: %d, global position: %d\n",
+		     _instance, (double)_ekf.get_dt_ekf_avg(), _ekf.attitude_valid(),
 		     _ekf.local_position_is_valid(), _ekf.global_position_is_valid());
 
 	perf_print_counter(_ecl_ekf_update_perf);
@@ -343,6 +341,13 @@ void EKF2::Run()
 
 		_ekf.set_min_required_gps_health_time(_param_ekf2_req_gps_h.get() * 1_s);
 
+		const matrix::Vector3f imu_pos_body(_param_ekf2_imu_pos_x.get(),
+						    _param_ekf2_imu_pos_y.get(),
+						    _param_ekf2_imu_pos_z.get());
+		_ekf.output_predictor().set_imu_offset(imu_pos_body);
+		_ekf.output_predictor().set_pos_correction_tc(_param_ekf2_tau_pos.get());
+		_ekf.output_predictor().set_vel_correction_tc(_param_ekf2_tau_vel.get());
+
 		// The airspeed scale factor correcton is only available via parameter as used by the airspeed module
 		param_t param_aspd_scale = param_find("ASPD_SCALE_1");
 
@@ -604,7 +609,7 @@ void EKF2::Run()
 			perf_set_elapsed(_ecl_ekf_update_full_perf, hrt_elapsed_time(&ekf_update_start));
 
 			PublishLocalPosition(now);
-			PublishOdometry(now);
+			PublishOdometry(now, imu_sample_new);
 			PublishGlobalPosition(now);
 			PublishWindEstimate(now);
 
@@ -790,8 +795,7 @@ void EKF2::PublishAttitude(const hrt_abstime &timestamp)
 		// generate vehicle attitude quaternion data
 		vehicle_attitude_s att;
 		att.timestamp_sample = timestamp;
-		const Quatf q{_ekf.calculate_quaternion()};
-		q.copyTo(att.q);
+		_ekf.getQuaternion().copyTo(att.q);
 
 		_ekf.get_quat_reset(&att.delta_q_reset[0], &att.quat_reset_counter);
 		att.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
@@ -916,7 +920,7 @@ void EKF2::PublishEventFlags(const hrt_abstime &timestamp)
 
 	if (information_event_updated || warning_event_updated) {
 		estimator_event_flags_s event_flags{};
-		event_flags.timestamp_sample = _ekf.get_imu_sample_delayed().time_us;
+		event_flags.timestamp_sample = _ekf.time_delayed_us();
 
 		event_flags.information_event_changes           = _filter_information_event_changes;
 		event_flags.gps_checks_passed                   = _ekf.information_event_flags().gps_checks_passed;
@@ -1055,7 +1059,7 @@ void EKF2::PublishInnovations(const hrt_abstime &timestamp)
 {
 	// publish estimator innovation data
 	estimator_innovations_s innovations{};
-	innovations.timestamp_sample = _ekf.get_imu_sample_delayed().time_us;
+	innovations.timestamp_sample = _ekf.time_delayed_us();
 	_ekf.getGpsVelPosInnov(innovations.gps_hvel, innovations.gps_vvel, innovations.gps_hpos, innovations.gps_vpos);
 	_ekf.getEvVelPosInnov(innovations.ev_hvel, innovations.ev_vvel, innovations.ev_hpos, innovations.ev_vpos);
 	_ekf.getBaroHgtInnov(innovations.baro_vpos);
@@ -1095,7 +1099,7 @@ void EKF2::PublishInnovations(const hrt_abstime &timestamp)
 
 		_preflt_checker.setVehicleCanObserveHeadingInFlight(_ekf.control_status_flags().fixed_wing);
 
-		_preflt_checker.update(_ekf.get_imu_sample_delayed().delta_ang_dt, innovations);
+		_preflt_checker.update(_ekf.get_dt_ekf_avg(), innovations);
 	}
 }
 
@@ -1103,7 +1107,7 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime &timestamp)
 {
 	// publish estimator innovation test ratio data
 	estimator_innovations_s test_ratios{};
-	test_ratios.timestamp_sample = _ekf.get_imu_sample_delayed().time_us;
+	test_ratios.timestamp_sample = _ekf.time_delayed_us();
 	_ekf.getGpsVelPosInnovRatio(test_ratios.gps_hvel[0], test_ratios.gps_vvel, test_ratios.gps_hpos[0],
 				    test_ratios.gps_vpos);
 	_ekf.getEvVelPosInnovRatio(test_ratios.ev_hvel[0], test_ratios.ev_vvel, test_ratios.ev_hpos[0], test_ratios.ev_vpos);
@@ -1129,7 +1133,7 @@ void EKF2::PublishInnovationVariances(const hrt_abstime &timestamp)
 {
 	// publish estimator innovation variance data
 	estimator_innovations_s variances{};
-	variances.timestamp_sample = _ekf.get_imu_sample_delayed().time_us;
+	variances.timestamp_sample = _ekf.time_delayed_us();
 	_ekf.getGpsVelPosInnovVar(variances.gps_hvel, variances.gps_vvel, variances.gps_hpos, variances.gps_vpos);
 	_ekf.getEvVelPosInnovVar(variances.ev_hvel, variances.ev_vvel, variances.ev_hpos, variances.ev_vpos);
 	_ekf.getBaroHgtInnovVar(variances.baro_vpos);
@@ -1252,11 +1256,11 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
 	_local_position_pub.publish(lpos);
 }
 
-void EKF2::PublishOdometry(const hrt_abstime &timestamp)
+void EKF2::PublishOdometry(const hrt_abstime &timestamp, const imuSample &imu_sample)
 {
 	// generate vehicle odometry data
 	vehicle_odometry_s odom;
-	odom.timestamp_sample = timestamp;
+	odom.timestamp_sample = imu_sample.time_us;
 
 	// position
 	odom.pose_frame = vehicle_odometry_s::POSE_FRAME_NED;
@@ -1270,7 +1274,7 @@ void EKF2::PublishOdometry(const hrt_abstime &timestamp)
 	_ekf.getVelocity().copyTo(odom.velocity);
 
 	// angular_velocity
-	const Vector3f rates{_ekf.get_imu_sample_newest().delta_ang / _ekf.get_imu_sample_newest().delta_ang_dt};
+	const Vector3f rates{imu_sample.delta_ang / imu_sample.delta_ang_dt};
 	const Vector3f angular_velocity = rates - _ekf.getGyroBias();
 	angular_velocity.copyTo(odom.angular_velocity);
 
@@ -1308,7 +1312,7 @@ void EKF2::PublishSensorBias(const hrt_abstime &timestamp)
 	    || (timestamp >= _last_sensor_bias_published + 1_s)) {
 
 		estimator_sensor_bias_s bias{};
-		bias.timestamp_sample = _ekf.get_imu_sample_delayed().time_us;
+		bias.timestamp_sample = _ekf.time_delayed_us();
 
 		// take device ids from sensor_selection_s if not using specific vehicle_imu_s
 		if (_device_id_gyro != 0) {
@@ -1352,7 +1356,7 @@ void EKF2::PublishStates(const hrt_abstime &timestamp)
 {
 	// publish estimator states
 	estimator_states_s states;
-	states.timestamp_sample = _ekf.get_imu_sample_delayed().time_us;
+	states.timestamp_sample = _ekf.time_delayed_us();
 	states.n_states = Ekf::_k_num_states;
 	_ekf.getStateAtFusionHorizonAsVector().copyTo(states.states);
 	_ekf.covariances_diagonal().copyTo(states.covariances);
@@ -1363,7 +1367,7 @@ void EKF2::PublishStates(const hrt_abstime &timestamp)
 void EKF2::PublishStatus(const hrt_abstime &timestamp)
 {
 	estimator_status_s status{};
-	status.timestamp_sample = _ekf.get_imu_sample_delayed().time_us;
+	status.timestamp_sample = _ekf.time_delayed_us();
 
 	_ekf.getOutputTrackingError().copyTo(status.output_tracking_error);
 
@@ -1439,7 +1443,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
 
 	if (update) {
 		estimator_status_flags_s status_flags{};
-		status_flags.timestamp_sample = _ekf.get_imu_sample_delayed().time_us;
+		status_flags.timestamp_sample = _ekf.time_delayed_us();
 
 		status_flags.control_status_changes   = _filter_control_status_changes;
 		status_flags.cs_tilt_align            = _ekf.control_status_flags().tilt_align;
@@ -1529,7 +1533,7 @@ void EKF2::PublishYawEstimatorStatus(const hrt_abstime &timestamp)
 			       yaw_est_test_data.weight)) {
 
 		yaw_est_test_data.yaw_composite_valid = _ekf.isYawEmergencyEstimateAvailable();
-		yaw_est_test_data.timestamp_sample = _ekf.get_imu_sample_delayed().time_us;
+		yaw_est_test_data.timestamp_sample = _ekf.time_delayed_us();
 		yaw_est_test_data.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 
 		_yaw_est_pub.publish(yaw_est_test_data);
@@ -1541,7 +1545,7 @@ void EKF2::PublishWindEstimate(const hrt_abstime &timestamp)
 	if (_ekf.get_wind_status()) {
 		// Publish wind estimate only if ekf declares them valid
 		wind_s wind{};
-		wind.timestamp_sample = _ekf.get_imu_sample_delayed().time_us;
+		wind.timestamp_sample = _ekf.time_delayed_us();
 
 		const Vector2f wind_vel = _ekf.getWindVelocity();
 		const Vector2f wind_vel_var = _ekf.getWindVelocityVariance();

src/modules/ekf2/EKF2.hpp

@@ -153,7 +153,7 @@ private:
 	void PublishInnovationTestRatios(const hrt_abstime &timestamp);
 	void PublishInnovationVariances(const hrt_abstime &timestamp);
 	void PublishLocalPosition(const hrt_abstime &timestamp);
-	void PublishOdometry(const hrt_abstime &timestamp);
+	void PublishOdometry(const hrt_abstime &timestamp, const imuSample &imu_sample);
 	void PublishOdometryAligned(const hrt_abstime &timestamp, const vehicle_odometry_s &ev_odom);
 	void PublishOpticalFlowVel(const hrt_abstime &timestamp);
 	void PublishSensorBias(const hrt_abstime &timestamp);
@@ -576,9 +576,9 @@ private:
 		_param_ekf2_fuse_beta,		///< Controls synthetic sideslip fusion, 0 disables, 1 enables
 
 		// output predictor filter time constants
-		(ParamExtFloat<px4::params::EKF2_TAU_VEL>)
+		(ParamFloat<px4::params::EKF2_TAU_VEL>)
 		_param_ekf2_tau_vel,		///< time constant used by the output velocity complementary filter (sec)
-		(ParamExtFloat<px4::params::EKF2_TAU_POS>)
+		(ParamFloat<px4::params::EKF2_TAU_POS>)
 		_param_ekf2_tau_pos,		///< time constant used by the output position complementary filter (sec)
 
 		// IMU switch on bias parameters

src/modules/ekf2/test/sensor_simulator/ekf_logger.cpp

@@ -48,7 +48,7 @@ void EkfLogger::writeStateToFile()
 void EkfLogger::writeState()
 {
 	if (_state_logging_enabled) {
-		uint64_t time = _ekf->get_imu_sample_delayed().time_us;
+		uint64_t time = _ekf->time_delayed_us();
 		_file << time;
 
 		if (_state_logging_enabled) {