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/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};