ekf run output predictor immediately with new IMU data (#471)

* EKF collect_imu take const imu sample and populate buffer

* EKF calculateOutputStates cleanup

* EKF add calculate_quaternion output predictor method

* EKF: update documentation

* EKF: remove unnecessary getter function

* EKF calculateOutputStates only apply dt correction to bias

* EKF pytest assert attitude validity, not update() return

* EKF: correct documentation

* EKF: Do not make attitude validity dependent on yaw alignment status

Yaw alignment could fail in flight due to temporary loss of data and yet the quaternions would still usable for stabilisation even though the absolute earth yaw angle wrt true north was uncertain.

EKF/common.h

@@ -45,6 +45,7 @@
 namespace estimator
 {
 
+using matrix::AxisAnglef;
 using matrix::Dcmf;
 using matrix::Eulerf;
 using matrix::Matrix3f;

EKF/ekf.cpp

@@ -99,6 +99,8 @@ bool Ekf::init(uint64_t timestamp)
 
 bool Ekf::update()
 {
+	bool updated = false;
+
 	if (!_filter_initialised) {
 		_filter_initialised = initialiseFilter();
 
@@ -120,18 +122,14 @@ bool Ekf::update()
 		// run a separate filter for terrain estimation
 		runTerrainEstimator();
 
+		updated = true;
 	}
 
 	// the output observer always runs
+	// Use full rate IMU data at the current time horizon
 	calculateOutputStates();
 
-	// check for NaN or inf on attitude states
-	if (!ISFINITE(_state.quat_nominal(0)) || !ISFINITE(_output_new.quat_nominal(0))) {
-		return false;
-	}
-
-	// We don't have valid data to output until tilt and yaw alignment is complete
-	return _control_status.flags.tilt_align && _control_status.flags.yaw_align;
+	return updated;
 }
 
 bool Ekf::initialiseFilter()
@@ -365,7 +363,7 @@ void Ekf::predictState()
 	_dt_ekf_avg = 0.99f * _dt_ekf_avg + 0.01f * input;
 }
 
-bool Ekf::collect_imu(imuSample &imu)
+bool Ekf::collect_imu(const imuSample &imu)
 {
 	// accumulate and downsample IMU data across a period FILTER_UPDATE_PERIOD_MS long
 
@@ -389,7 +387,7 @@ bool Ekf::collect_imu(imuSample &imu)
 
 	// accumulate the most recent delta velocity data at the updated rotation frame
 	// assume effective sample time is halfway between the previous and current rotation frame
-	_imu_down_sampled.delta_vel += (_imu_sample_new.delta_vel + delta_R * _imu_sample_new.delta_vel) * 0.5f;
+	_imu_down_sampled.delta_vel += (imu.delta_vel + delta_R * imu.delta_vel) * 0.5f;
 
 	// if the target time delta between filter prediction steps has been exceeded
 	// write the accumulated IMU data to the ring buffer
@@ -402,11 +400,23 @@ bool Ekf::collect_imu(imuSample &imu)
 		_imu_collection_time_adj += 0.01f * (_imu_down_sampled.delta_ang_dt - target_dt);
 		_imu_collection_time_adj = math::constrain(_imu_collection_time_adj, -0.5f * target_dt, 0.5f * target_dt);
 
-		imu.delta_ang     = _q_down_sampled.to_axis_angle();
-		imu.delta_vel     = _imu_down_sampled.delta_vel;
-		imu.delta_ang_dt  = _imu_down_sampled.delta_ang_dt;
-		imu.delta_vel_dt  = _imu_down_sampled.delta_vel_dt;
+		imuSample imu_sample_new;
+		imu_sample_new.delta_ang = _q_down_sampled.to_axis_angle();
+		imu_sample_new.delta_vel = _imu_down_sampled.delta_vel;
+		imu_sample_new.delta_ang_dt = _imu_down_sampled.delta_ang_dt;
+		imu_sample_new.delta_vel_dt = _imu_down_sampled.delta_vel_dt;
+		imu_sample_new.time_us = imu.time_us;
 
+		_imu_buffer.push(imu_sample_new);
+
+		// get the oldest data from the buffer
+		_imu_sample_delayed = _imu_buffer.get_oldest();
+
+		// 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_new.time_us - _imu_sample_delayed.time_us) / (_obs_buffer_length - 1);
+
+		// reset
 		_imu_down_sampled.delta_ang.setZero();
 		_imu_down_sampled.delta_vel.setZero();
 		_imu_down_sampled.delta_ang_dt = 0.0f;
@@ -414,10 +424,13 @@ bool Ekf::collect_imu(imuSample &imu)
 		_q_down_sampled(0) = 1.0f;
 		_q_down_sampled(1) = _q_down_sampled(2) = _q_down_sampled(3) = 0.0f;
 
-		return true;
+		_imu_updated = true;
+
+	} else {
+		_imu_updated = false;
 	}
 
-	return false;
+	return _imu_updated;
 }
 
 /*
@@ -433,37 +446,30 @@ bool Ekf::collect_imu(imuSample &imu)
 void Ekf::calculateOutputStates()
 {
 	// Use full rate IMU data at the current time horizon
-	const imuSample &imu_new = _imu_sample_new;
+	const imuSample &imu = _imu_sample_new;
 
 	// correct delta angles for bias offsets
-	Vector3f delta_angle;
-	float dt_scale_correction = _dt_imu_avg / _dt_ekf_avg;
-	delta_angle(0) = _imu_sample_new.delta_ang(0) - _state.gyro_bias(0) * dt_scale_correction;
-	delta_angle(1) = _imu_sample_new.delta_ang(1) - _state.gyro_bias(1) * dt_scale_correction;
-	delta_angle(2) = _imu_sample_new.delta_ang(2) - _state.gyro_bias(2) * dt_scale_correction;
-
-	// calculate a yaw change about the earth frame vertical
-	float spin_del_ang_D = _R_to_earth_now(2, 0) * delta_angle(0) +
-			       _R_to_earth_now(2, 1) * delta_angle(1) +
-			       _R_to_earth_now(2, 2) * delta_angle(2);
-	_yaw_delta_ef += spin_del_ang_D;
-
-	// Calculate filtered yaw rate to be used by the magnetomer 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_sample_new.delta_ang_dt;
-
-	// correct delta velocity for bias offsets
-	Vector3f delta_vel = _imu_sample_new.delta_vel - _state.accel_bias * dt_scale_correction;
+	const float dt_scale_correction = _dt_imu_avg / _dt_ekf_avg;
 
 	// Apply corrections to the delta angle required to track the quaternion states at the EKF fusion time horizon
-	delta_angle += _delta_angle_corr;
+	const Vector3f delta_angle{imu.delta_ang - _state.gyro_bias * dt_scale_correction + _delta_angle_corr};
+
+	// calculate a yaw change about the earth frame vertical
+	const float spin_del_ang_D = _R_to_earth_now(2, 0) * delta_angle(0) +
+				     _R_to_earth_now(2, 1) * delta_angle(1) +
+				     _R_to_earth_now(2, 2) * delta_angle(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;
 
 	// convert the delta angle to an equivalent delta quaternions
 	Quatf dq;
 	dq.from_axis_angle(delta_angle);
 
 	// rotate the previous INS quaternion by the delta quaternions
-	_output_new.time_us = imu_new.time_us;
+	_output_new.time_us = imu.time_us;
 	_output_new.quat_nominal = _output_new.quat_nominal * dq;
 
 	// the quaternions must always be normalised afer modification
@@ -472,19 +478,22 @@ void Ekf::calculateOutputStates()
 	// calculate the rotation matrix from body to earth frame
 	_R_to_earth_now = quat_to_invrotmat(_output_new.quat_nominal);
 
-	// rotate the delta velocity to earth frame
-	Vector3f delta_vel_NED = _R_to_earth_now * delta_vel;
+	// correct delta velocity for bias offsets
+	const Vector3f delta_vel{imu.delta_vel - _state.accel_bias * dt_scale_correction};
 
-	// corrrect for measured accceleration due to gravity
-	delta_vel_NED(2) += CONSTANTS_ONE_G * imu_new.delta_vel_dt;
+	// rotate the delta velocity to earth frame
+	Vector3f delta_vel_NED{_R_to_earth_now * delta_vel};
+
+	// correct for measured acceleration due to gravity
+	delta_vel_NED(2) += CONSTANTS_ONE_G * imu.delta_vel_dt;
 
 	// calculate the earth frame velocity derivatives
-	if (imu_new.delta_vel_dt > 1e-4f) {
-		_vel_deriv_ned = delta_vel_NED * (1.0f / imu_new.delta_vel_dt);
+	if (imu.delta_vel_dt > 1e-4f) {
+		_vel_deriv_ned = delta_vel_NED * (1.0f / imu.delta_vel_dt);
 	}
 
 	// save the previous velocity so we can use trapezoidal integration
-	Vector3f vel_last = _output_new.vel;
+	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
@@ -493,31 +502,29 @@ void Ekf::calculateOutputStates()
 
 	// use trapezoidal integration to calculate the INS position states
 	// do the same for vertical state used by alternative correction algorithm
-	Vector3f delta_pos_NED = (_output_new.vel + vel_last) * (imu_new.delta_vel_dt * 0.5f);
+	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.vel_d_integ += delta_pos_NED(2);
 
 	// accumulate the time for each update
-	_output_vert_new.dt += imu_new.delta_vel_dt;
+	_output_vert_new.dt += imu.delta_vel_dt;
 
 	// correct velocity for IMU offset
-	if (_imu_sample_new.delta_ang_dt > 1e-4f) {
+	if (imu.delta_ang_dt > 1e-4f) {
 		// calculate the average angular rate across the last IMU update
-		Vector3f ang_rate = _imu_sample_new.delta_ang * (1.0f / _imu_sample_new.delta_ang_dt);
+		const Vector3f ang_rate = imu.delta_ang * (1.0f / imu.delta_ang_dt);
 
 		// calculate the velocity of the IMU relative to the body origin
-		Vector3f vel_imu_rel_body = cross_product(ang_rate, _params.imu_pos_body);
+		const Vector3f vel_imu_rel_body = cross_product(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);
-		_imu_updated = false;
 
 		// get the oldest INS state data from the ring buffer
 		// this data will be at the EKF fusion time horizon
@@ -530,7 +537,6 @@ void Ekf::calculateOutputStates()
 		q_error.normalize();
 
 		// convert the quaternion delta to a delta angle
-		Vector3f delta_ang_error;
 		float scalar;
 
 		if (q_error(0) >= 0.0f) {
@@ -540,23 +546,20 @@ void Ekf::calculateOutputStates()
 			scalar = 2.0f;
 		}
 
-		delta_ang_error(0) = scalar * q_error(1);
-		delta_ang_error(1) = scalar * q_error(2);
-		delta_ang_error(2) = scalar * q_error(3);
+		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
-		float time_delay = 1e-6f * (float)(_imu_sample_new.time_us - _imu_sample_delayed.time_us);
-		time_delay = fmaxf(time_delay, _dt_imu_avg);
-		float att_gain = 0.5f * _dt_imu_avg / time_delay;
+		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;
 
 		// calculate velocity and position tracking errors
-		Vector3f vel_err = (_state.vel - _output_sample_delayed.vel);
-		Vector3f pos_err = (_state.pos - _output_sample_delayed.pos);
+		const Vector3f vel_err{_state.vel - _output_sample_delayed.vel};
+		const Vector3f pos_err{_state.pos - _output_sample_delayed.pos};
 
 		// collect magnitude tracking error for diagnostics
 		_output_tracking_error[0] = delta_ang_error.norm();
@@ -571,8 +574,8 @@ void Ekf::calculateOutputStates()
 		 */
 
 		// Complementary filter gains
-		float vel_gain = _dt_ekf_avg / math::constrain(_params.vel_Tau, _dt_ekf_avg, 10.0f);
-		float pos_gain = _dt_ekf_avg / math::constrain(_params.pos_Tau, _dt_ekf_avg, 10.0f);
+		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 a correction to be applied to vel_d that casues vel_d_integ to track the EKF
@@ -584,12 +587,12 @@ void Ekf::calculateOutputStates()
 			 */
 
 			// calculate down velocity and position tracking errors
-			float vel_d_err = (_state.vel(2) - _output_vert_delayed.vel_d);
-			float pos_d_err = (_state.pos(2) - _output_vert_delayed.vel_d_integ);
+			const float vel_d_err = (_state.vel(2) - _output_vert_delayed.vel_d);
+			const float pos_d_err = (_state.pos(2) - _output_vert_delayed.vel_d_integ);
 
 			// calculate a velocity correction that will be applied to the output state history
 			// using a PD feedback tuned to a 5% overshoot
-			float vel_d_correction = pos_d_err * pos_gain + vel_d_err * pos_gain * 1.1f;
+			const float vel_d_correction = pos_d_err * pos_gain + vel_d_err * pos_gain * 1.1f;
 
 			/*
 			 * Calculate corrections to be applied to vel and pos output state history.
@@ -638,11 +641,11 @@ void Ekf::calculateOutputStates()
 
 			// calculate a velocity correction that will be applied to the output state history
 			_vel_err_integ += vel_err;
-			Vector3f vel_correction = vel_err * vel_gain + _vel_err_integ * sq(vel_gain) * 0.1f;
+			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;
-			Vector3f pos_correction = pos_err * pos_gain + _pos_err_integ * sq(pos_gain) * 0.1f;
+			const Vector3f pos_correction = pos_err * pos_gain + _pos_err_integ * sq(pos_gain) * 0.1f;
 
 			// 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++) {
@@ -658,3 +661,17 @@ void Ekf::calculateOutputStates()
 		}
 	}
 }
+
+/*
+ * 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_angle{_imu_sample_new.delta_ang - _state.gyro_bias * (_dt_imu_avg / _dt_ekf_avg) + _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();
+}

EKF/ekf.h

@@ -125,7 +125,8 @@ public:
 
 	// ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined
 	bool collect_gps(uint64_t time_usec, struct gps_message *gps);
-	bool collect_imu(imuSample &imu);
+
+	bool collect_imu(const imuSample &imu);
 
 	// get the ekf WGS-84 origin position and height and the system time it was last set
 	// return true if the origin is valid
@@ -230,6 +231,9 @@ public:
 	// return the quaternion defining the rotation from the EKF to the External Vision reference frame
 	void get_ekf2ev_quaternion(float *quat);
 
+	// use the latest IMU data at the current time horizon.
+	Quatf calculate_quaternion() const;
+
 private:
 
 	static constexpr uint8_t _k_num_states{24};		///< number of EKF states

EKF/estimator_interface.cpp

@@ -62,16 +62,12 @@ void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, u
 		_dt_imu_avg = 0.8f * _dt_imu_avg + 0.2f * dt;
 	}
 
-	// copy data
 	imuSample imu_sample_new;
 	imu_sample_new.delta_ang = Vector3f(delta_ang);
 	imu_sample_new.delta_vel = Vector3f(delta_vel);
-
-	// convert time from us to secs
-	imu_sample_new.delta_ang_dt = delta_ang_dt / 1e6f;
-	imu_sample_new.delta_vel_dt = delta_vel_dt / 1e6f;
+	imu_sample_new.delta_ang_dt = delta_ang_dt * 1e-6f;
+	imu_sample_new.delta_vel_dt = delta_vel_dt * 1e-6f;
 	imu_sample_new.time_us = time_usec;
-	_imu_ticks++;
 
 	// calculate a metric which indicates the amount of coning vibration
 	Vector3f temp = cross_product(imu_sample_new.delta_ang, _delta_ang_prev);
@@ -92,26 +88,19 @@ void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, u
 		if ((_vibe_metrics[1] * 4.0E4f > _params.is_moving_scaler)
 				|| (_vibe_metrics[2] * 2.1E2f > _params.is_moving_scaler)
 				|| ((imu_sample_new.delta_ang.norm() / dt) > 0.05f * _params.is_moving_scaler)) {
-			_time_last_move_detect_us = _imu_sample_new.time_us;
+
+			_time_last_move_detect_us = imu_sample_new.time_us;
 		}
-		_vehicle_at_rest = ((_imu_sample_new.time_us - _time_last_move_detect_us) > (uint64_t)1E6);
+
+		_vehicle_at_rest = ((imu_sample_new.time_us - _time_last_move_detect_us) > (uint64_t)1E6);
+
 	} else {
-		_time_last_move_detect_us = _imu_sample_new.time_us;
+		_time_last_move_detect_us = imu_sample_new.time_us;
 		_vehicle_at_rest = false;
 	}
 
 	// accumulate and down-sample imu data and push to the buffer when new downsampled data becomes available
 	if (collect_imu(imu_sample_new)) {
-		_imu_buffer.push(imu_sample_new);
-		_imu_ticks = 0;
-		_imu_updated = true;
-
-		// get the oldest data from the buffer
-		_imu_sample_delayed = _imu_buffer.get_oldest();
-
-		// 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_new.time_us - _imu_sample_delayed.time_us) / (_obs_buffer_length - 1);
 
 		// down-sample the drag specific force data by accumulating and calculating the mean when
 		// sufficient samples have been collected
@@ -157,13 +146,8 @@ void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, u
 				_drag_down_sampled.accelXY.zero();
 				_drag_down_sampled.time_us = 0;
 				_drag_sample_time_dt = 0.0f;
-
 			}
 		}
-
-	} else {
-		_imu_updated = false;
-
 	}
 }
 
@@ -528,8 +512,6 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 	_imu_sample_delayed.delta_vel_dt = 0.0f;
 	_imu_sample_delayed.time_us = timestamp;
 
-	_imu_ticks = 0;
-
 	_initialised = false;
 
 	_time_last_imu = 0;

EKF/estimator_interface.h

@@ -42,6 +42,7 @@
 #include "common.h"
 #include "RingBuffer.h"
 
+#include <ecl.h>
 #include <geo/geo.h>
 #include <matrix/math.hpp>
 #include <mathlib/mathlib.h>
@@ -159,7 +160,7 @@ public:
 	virtual bool collect_gps(uint64_t time_usec, struct gps_message *gps) { return true; }
 
 	// accumulate and downsample IMU data to the EKF prediction rate
-	virtual bool collect_imu(imuSample &imu) { return true; }
+	virtual bool collect_imu(const imuSample &imu) = 0;
 
 	// set delta angle imu data
 	void setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64_t delta_vel_dt, float (&delta_ang)[3], float (&delta_vel)[3]);
@@ -203,6 +204,9 @@ public:
 	*/
 	virtual bool reset_imu_bias() = 0;
 
+	// return true if the attitude is usable
+	bool attitude_valid() { return ISFINITE(_output_new.quat_nominal(0)) && _control_status.flags.tilt_align; }
+
 	// get vehicle landed status data
 	bool get_in_air_status() {return _control_status.flags.in_air;}
 
@@ -361,10 +365,7 @@ public:
 	virtual void get_ekf_soln_status(uint16_t *status) = 0;
 
 	// Getter for the average imu update period in s
-	float get_dt_imu_avg()
-	{
-		return _dt_imu_avg;
-	}
+	float get_dt_imu_avg() const { return _dt_imu_avg; }
 
 	// Getter for the imu sample on the delayed time horizon
 	imuSample get_imu_sample_delayed()
@@ -378,12 +379,6 @@ public:
 		return _baro_sample_delayed;
 	}
 
-	// Getter for a flag indicating if the ekf should update (completed downsampling process)
-	bool get_imu_updated()
-	{
-		return _imu_updated;
-	}
-
 	void print_status();
 
 	static const unsigned FILTER_UPDATE_PERIOD_MS = 8;	// ekf prediction period in milliseconds - this should ideally be an integer multiple of the IMU time delta
@@ -449,8 +444,6 @@ protected:
 	Vector3f _vel_imu_rel_body_ned;		// velocity of IMU relative to body origin in NED earth frame
 	Vector3f _vel_deriv_ned;		// velocity derivative at the IMU in NED earth frame (m/s/s)
 
-	uint64_t _imu_ticks{0};	// counter for imu updates
-
 	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
 

EKF/tests/pytest/test_basics.py

@@ -54,7 +54,7 @@ def test_filter_initialized(initialized_ekf):
     """Make sure the EKF updates after a few IMU, Mag and Baro updates
     """
     ekf, _ = initialized_ekf
-    assert ekf.update()
+    assert ekf.attitude_valid()
 
 
 @pytest.mark.benchmark