EKF: RingBuffer minor improvements

- allocate IMU and output buffers on construction according to defaults
 - determine buffer max time delay based on configuration parameters
 - reorder flowSample and extVisionSample to minimize padding
 - adjust parameter defaults to match PX4-Autopilot

EKF/RingBuffer.h

@@ -45,14 +45,8 @@ template <typename data_type>
 class RingBuffer
 {
 public:
-	RingBuffer()
-	{
-		if (allocate(1)) {
-			// initialize with one empty sample
-			data_type d = {};
-			push(d);
-		}
-	}
+	explicit RingBuffer(size_t size) { allocate(size); }
+	RingBuffer() { allocate(1); }
 	~RingBuffer() { delete[] _buffer; }
 
 	// no copy, assignment, move, move assignment
@@ -63,12 +57,20 @@ public:
 
 	bool allocate(uint8_t size)
 	{
+		if (valid() && (size == _size)) {
+			// no change
+			return true;
+		}
+
+		if (size == 0) {
+			return false;
+		}
 
 		if (_buffer != nullptr) {
 			delete[] _buffer;
 		}
 
-		_buffer = new data_type[size];
+		_buffer = new data_type[size]{};
 
 		if (_buffer == nullptr) {
 			return false;
@@ -79,26 +81,15 @@ public:
 		_head = 0;
 		_tail = 0;
 
-		// set the time elements to zero so that bad data is not
-		// retrieved from the buffers
-		for (uint8_t index = 0; index < _size; index++) {
-			_buffer[index] = {};
-		}
-
 		_first_write = true;
 
 		return true;
 	}
 
-	void unallocate()
-	{
-		delete[] _buffer;
-		_buffer = nullptr;
-	}
+	bool valid() const { return (_buffer != nullptr) && (_size > 0); }
 
 	void push(const data_type &sample)
 	{
-
 		uint8_t head_new = _head;
 
 		if (!_first_write) {
@@ -121,8 +112,8 @@ public:
 
 	data_type &operator[](const uint8_t index) { return _buffer[index]; }
 
-	const data_type &get_newest() { return _buffer[_head]; }
-	const data_type &get_oldest() { return _buffer[_tail]; }
+	const data_type &get_newest() const { return _buffer[_head]; }
+	const data_type &get_oldest() const { return _buffer[_tail]; }
 
 	uint8_t get_oldest_index() const { return _tail; }
 
@@ -162,7 +153,7 @@ public:
 		return false;
 	}
 
-	int get_total_size() { return sizeof(*this) + sizeof(data_type) * _size; }
+	int get_total_size() const { return sizeof(*this) + sizeof(data_type) * _size; }
 
 private:
 	data_type *_buffer{nullptr};

EKF/common.h

@@ -55,10 +55,13 @@ using matrix::Vector2f;
 using matrix::Vector3f;
 using matrix::wrap_pi;
 
-enum velocity_frame_t {LOCAL_FRAME_FRD, BODY_FRAME_FRD};
+enum class velocity_frame_t : uint8_t {
+	LOCAL_FRAME_FRD,
+	BODY_FRAME_FRD
+};
 
 struct gps_message {
-	uint64_t time_usec;
+	uint64_t time_usec{0};
 	int32_t lat;		///< Latitude in 1E-7 degrees
 	int32_t lon;		///< Longitude in 1E-7 degrees
 	int32_t alt;		///< Altitude in 1E-3 meters (millimeters) above MSL
@@ -76,29 +79,30 @@ struct gps_message {
 };
 
 struct outputSample {
+	uint64_t    time_us{0};	///< 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)
-	uint64_t    time_us;	///< timestamp of the measurement (uSec)
 };
 
 struct outputVert {
+	uint64_t    time_us{0};		///< 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)
-	uint64_t    time_us;		///< timestamp of the measurement (uSec)
 };
 
 struct imuSample {
+	uint64_t    time_us{0};		///< timestamp of the measurement (uSec)
 	Vector3f    delta_ang;		///< delta angle in body frame (integrated gyro measurements) (rad)
 	Vector3f    delta_vel;		///< delta velocity in body frame (integrated accelerometer measurements) (m/sec)
 	float       delta_ang_dt;	///< delta angle integration period (sec)
 	float       delta_vel_dt;	///< delta velocity integration period (sec)
-	uint64_t    time_us;		///< timestamp of the measurement (uSec)
 	bool        delta_vel_clipping[3]{}; ///< true (per axis) if this sample contained any accelerometer clipping
 };
 
 struct gpsSample {
+	uint64_t    time_us{0};	///< timestamp of the measurement (uSec)
 	Vector2f    pos;	///< NE earth frame gps horizontal position measurement (m)
 	float       hgt;	///< gps height measurement (m)
 	Vector3f    vel;	///< NED earth frame gps velocity measurement (m/sec)
@@ -106,59 +110,58 @@ struct gpsSample {
 	float	    hacc;	///< 1-std horizontal position error (m)
 	float	    vacc;	///< 1-std vertical position error (m)
 	float       sacc;	///< 1-std speed error (m/sec)
-	uint64_t    time_us;	///< timestamp of the measurement (uSec)
 };
 
 struct magSample {
+	uint64_t    time_us{0};	///< timestamp of the measurement (uSec)
 	Vector3f    mag;	///< NED magnetometer body frame measurements (Gauss)
-	uint64_t    time_us;	///< timestamp of the measurement (uSec)
 };
 
 struct baroSample {
+	uint64_t    time_us{0};	///< timestamp of the measurement (uSec)
 	float       hgt;	///< pressure altitude above sea level (m)
-	uint64_t    time_us;	///< timestamp of the measurement (uSec)
 };
 
 struct rangeSample {
+	uint64_t    time_us{0};	///< timestamp of the measurement (uSec)
 	float       rng;	    ///< range (distance to ground) measurement (m)
-	uint64_t    time_us;	///< timestamp of the measurement (uSec)
 	int8_t	    quality;    ///< Signal quality in percent (0...100%), where 0 = invalid signal, 100 = perfect signal, and -1 = unknown signal quality.
 };
 
 struct airspeedSample {
+	uint64_t    time_us{0};		///< timestamp of the measurement (uSec)
 	float       true_airspeed;	///< true airspeed measurement (m/sec)
 	float       eas2tas;		///< equivalent to true airspeed factor
-	uint64_t    time_us;		///< timestamp of the measurement (uSec)
 };
 
 struct flowSample {
-	uint8_t  quality;	///< quality indicator between 0 and 255
+	uint64_t time_us{0};	///< timestamp of the integration period leading edge (uSec)
 	Vector2f flow_xy_rad;	///< measured delta angle of the image about the X and Y body axes (rad), RH rotation is positive
 	Vector3f gyro_xyz;	///< measured delta angle of the inertial frame about the body axes obtained from rate gyro measurements (rad), RH rotation is positive
 	float    dt;		///< amount of integration time (sec)
-	uint64_t time_us;	///< timestamp of the integration period leading edge (uSec)
+	uint8_t  quality;	///< quality indicator between 0 and 255
 };
 
 struct extVisionSample {
+	uint64_t time_us{0};	///< timestamp of the measurement (uSec)
 	Vector3f pos;	///< XYZ position in external vision's local reference frame (m) - Z must be aligned with down axis
 	Vector3f vel;	///< FRD velocity in reference frame defined in vel_frame variable (m/sec) - Z must be aligned with down axis
 	Quatf quat;		///< quaternion defining rotation from body to earth frame
 	Vector3f posVar;	///< XYZ position variances (m**2)
 	Matrix3f velCov;	///< XYZ velocity covariances ((m/sec)**2)
 	float angVar;		///< angular heading variance (rad**2)
-	velocity_frame_t vel_frame = BODY_FRAME_FRD;
-	uint64_t time_us;	///< timestamp of the measurement (uSec)
+	velocity_frame_t vel_frame = velocity_frame_t::BODY_FRAME_FRD;
 };
 
 struct dragSample {
+	uint64_t time_us{0};	///< timestamp of the measurement (uSec)
 	Vector2f accelXY;	///< measured specific force along the X and Y body axes (m/sec**2)
-	uint64_t time_us;	///< timestamp of the measurement (uSec)
 };
 
 struct auxVelSample {
+	uint64_t time_us{0};	///< timestamp of the measurement (uSec)
 	Vector3f vel;		///< measured NE velocity relative to the local origin (m/sec)
 	Vector3f velVar;	///< estimated error variance of the NE velocity (m/sec)**2
-	uint64_t time_us;	///< timestamp of the measurement (uSec)
 };
 
 // Integer definitions for vdist_sensor_type
@@ -218,15 +221,14 @@ struct parameters {
 	int32_t sensor_interval_min_ms{20};		///< minimum time of arrival difference between non IMU sensor updates. Sets the size of the observation buffers. (mSec)
 
 	// measurement time delays
-	float min_delay_ms{0.0f};		///< Maximum time delay of any sensor used to increase buffer length to handle large timing jitter (mSec)
 	float mag_delay_ms{0.0f};		///< magnetometer measurement delay relative to the IMU (mSec)
 	float baro_delay_ms{0.0f};		///< barometer height measurement delay relative to the IMU (mSec)
 	float gps_delay_ms{110.0f};		///< GPS measurement delay relative to the IMU (mSec)
 	float airspeed_delay_ms{100.0f};	///< airspeed measurement delay relative to the IMU (mSec)
 	float flow_delay_ms{5.0f};		///< optical flow measurement delay relative to the IMU (mSec) - this is to the middle of the optical flow integration interval
 	float range_delay_ms{5.0f};		///< range finder measurement delay relative to the IMU (mSec)
-	float ev_delay_ms{100.0f};		///< off-board vision measurement delay relative to the IMU (mSec)
-	float auxvel_delay_ms{0.0f};		///< auxiliary velocity measurement delay relative to the IMU (mSec)
+	float ev_delay_ms{175.0f};		///< off-board vision measurement delay relative to the IMU (mSec)
+	float auxvel_delay_ms{5.0f};		///< auxiliary velocity measurement delay relative to the IMU (mSec)
 
 	// input noise
 	float gyro_noise{1.5e-2f};		///< IMU angular rate noise used for covariance prediction (rad/sec)

EKF/ekf_helper.cpp

@@ -1230,10 +1230,10 @@ Vector3f Ekf::getVisionVelocityInEkfFrame() const
 
 	// rotate measurement into correct earth frame if required
 	switch(_ev_sample_delayed.vel_frame) {
-		case BODY_FRAME_FRD:
+		case velocity_frame_t::BODY_FRAME_FRD:
 			vel = _R_to_earth * (_ev_sample_delayed.vel - vel_offset_body);
 			break;
-		case LOCAL_FRAME_FRD:
+		case velocity_frame_t::LOCAL_FRAME_FRD:
 			const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
 			if (_params.fusion_mode & MASK_ROTATE_EV)
 			{
@@ -1253,11 +1253,11 @@ Vector3f Ekf::getVisionVelocityVarianceInEkfFrame() const
 
 	// rotate measurement into correct earth frame if required
 	switch(_ev_sample_delayed.vel_frame) {
-		case BODY_FRAME_FRD:
+		case velocity_frame_t::BODY_FRAME_FRD:
 			ev_vel_cov = _R_to_earth * ev_vel_cov * _R_to_earth.transpose();
 			break;
 
-		case LOCAL_FRAME_FRD:
+		case velocity_frame_t::LOCAL_FRAME_FRD:
 			if(_params.fusion_mode & MASK_ROTATE_EV)
 			{
 				ev_vel_cov = _R_ev_to_ekf * ev_vel_cov * _R_ev_to_ekf.transpose();

EKF/estimator_interface.cpp

@@ -512,34 +512,46 @@ void EstimatorInterface::setDragData(const imuSample &imu)
 bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 {
 	// find the maximum time delay the buffers are required to handle
-	const uint16_t max_time_delay_ms = math::max(_params.mag_delay_ms,
-					     math::max(_params.range_delay_ms,
-					       math::max(_params.gps_delay_ms,
-						 math::max(_params.flow_delay_ms,
-						   math::max(_params.ev_delay_ms,
-						     math::max(_params.auxvel_delay_ms,
-						       math::max(_params.min_delay_ms,
-							 math::max(_params.airspeed_delay_ms,
-							 	     _params.baro_delay_ms))))))));
+	float max_time_delay_ms = math::max(_params.baro_delay_ms,
+					math::max(_params.auxvel_delay_ms,
+						_params.airspeed_delay_ms));
+
+	// mag mode
+	if (_params.mag_fusion_type != MAG_FUSE_TYPE_NONE) {
+		max_time_delay_ms = math::max(_params.mag_delay_ms, max_time_delay_ms);
+	}
+
+	// range aid or range height
+	if (_params.range_aid || (_params.vdist_sensor_type == VDIST_SENSOR_RANGE)) {
+		max_time_delay_ms = math::max(_params.range_delay_ms, max_time_delay_ms);
+	}
+
+	if (_params.fusion_mode & MASK_USE_GPS) {
+		max_time_delay_ms = math::max(_params.gps_delay_ms, max_time_delay_ms);
+	}
+
+	if (_params.fusion_mode & MASK_USE_OF) {
+		max_time_delay_ms = math::max(_params.flow_delay_ms, max_time_delay_ms);
+	}
+
+	if (_params.fusion_mode & (MASK_USE_EVPOS | MASK_USE_EVYAW | MASK_USE_EVVEL)) {
+		max_time_delay_ms = math::max(_params.ev_delay_ms, max_time_delay_ms);
+	}
 
 	// calculate the IMU buffer length required to accomodate the maximum delay with some allowance for jitter
-	_imu_buffer_length = (max_time_delay_ms / FILTER_UPDATE_PERIOD_MS) + 1;
+	_imu_buffer_length = ceilf(max_time_delay_ms / FILTER_UPDATE_PERIOD_MS) + 1;
 
 	// set the observation buffer length to handle the minimum time of arrival between observations in combination
 	// with the worst case delay from current time to ekf fusion time
 	// allow for worst case 50% extension of the ekf fusion time horizon delay due to timing jitter
-	const uint16_t ekf_delay_ms = max_time_delay_ms + (int)(ceilf((float)max_time_delay_ms * 0.5f));
-	_obs_buffer_length = (ekf_delay_ms / _params.sensor_interval_min_ms) + 1;
+	const float ekf_delay_ms = max_time_delay_ms * 1.5f;
+	_obs_buffer_length = ceilf(ekf_delay_ms / _params.sensor_interval_min_ms);
 
 	// limit to be no longer than the IMU buffer (we can't process data faster than the EKF prediction rate)
 	_obs_buffer_length = math::min(_obs_buffer_length, _imu_buffer_length);
 
-	if (!(_imu_buffer.allocate(_imu_buffer_length) &&
-	      _output_buffer.allocate(_imu_buffer_length) &&
-	      _output_vert_buffer.allocate(_imu_buffer_length))) {
-
-		printBufferAllocationFailed("");
-		unallocate_buffers();
+	if (!_imu_buffer.allocate(_imu_buffer_length) || !_output_buffer.allocate(_imu_buffer_length) || !_output_vert_buffer.allocate(_imu_buffer_length)) {
+		printBufferAllocationFailed("IMU and output");
 		return false;
 	}
 
@@ -553,22 +565,6 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 	return true;
 }
 
-void EstimatorInterface::unallocate_buffers()
-{
-	_imu_buffer.unallocate();
-	_gps_buffer.unallocate();
-	_mag_buffer.unallocate();
-	_baro_buffer.unallocate();
-	_range_buffer.unallocate();
-	_airspeed_buffer.unallocate();
-	_flow_buffer.unallocate();
-	_ext_vision_buffer.unallocate();
-	_output_buffer.unallocate();
-	_output_vert_buffer.unallocate();
-	_drag_buffer.unallocate();
-	_auxvel_buffer.unallocate();
-}
-
 bool EstimatorInterface::isOnlyActiveSourceOfHorizontalAiding(const bool aiding_flag) const
 {
 	return aiding_flag && !isOtherSourceOfHorizontalAidingThan(aiding_flag);

EKF/estimator_interface.h

@@ -323,7 +323,10 @@ protected:
 	bool _gps_drift_updated{false};	// true when _gps_drift_metrics has been updated and is ready for retrieval
 
 	// data buffer instances
-	RingBuffer<imuSample> _imu_buffer;
+	RingBuffer<imuSample> _imu_buffer{12};           // buffer length 12 with default parameters
+	RingBuffer<outputSample> _output_buffer{12};
+	RingBuffer<outputVert> _output_vert_buffer{12};
+
 	RingBuffer<gpsSample> _gps_buffer;
 	RingBuffer<magSample> _mag_buffer;
 	RingBuffer<baroSample> _baro_buffer;
@@ -331,8 +334,6 @@ protected:
 	RingBuffer<airspeedSample> _airspeed_buffer;
 	RingBuffer<flowSample> 	_flow_buffer;
 	RingBuffer<extVisionSample> _ext_vision_buffer;
-	RingBuffer<outputSample> _output_buffer;
-	RingBuffer<outputVert> _output_vert_buffer;
 	RingBuffer<dragSample> _drag_buffer;
 	RingBuffer<auxVelSample> _auxvel_buffer;
 
@@ -375,9 +376,6 @@ private:
 
 	void printBufferAllocationFailed(const char *buffer_name);
 
-	// free buffer memory
-	void unallocate_buffers();
-
 	ImuDownSampler _imu_down_sampler{FILTER_UPDATE_PERIOD_S};
 
 	unsigned _min_obs_interval_us{0}; // minimum time interval between observations that will guarantee data is not lost (usec)

test/sensor_simulator/baro.cpp

@@ -15,8 +15,7 @@ Baro::~Baro()
 
 void Baro::send(uint64_t time)
 {
-	const baroSample baro_sample {_baro_data, time};
-	_ekf->setBaroData(baro_sample);
+	_ekf->setBaroData(baroSample{time, _baro_data});
 }
 
 void Baro::setData(float baro)

test/sensor_simulator/ekf_wrapper.cpp

@@ -17,9 +17,7 @@ void EkfWrapper::setBaroHeight()
 
 bool EkfWrapper::isIntendingBaroHeightFusion() const
 {
-	filter_control_status_u control_status;
-	_ekf->get_control_mode(&control_status.value);
-	return control_status.flags.baro_hgt;
+	return _ekf->control_status_flags().baro_hgt;
 }
 
 void EkfWrapper::setGpsHeight()
@@ -29,9 +27,7 @@ void EkfWrapper::setGpsHeight()
 
 bool EkfWrapper::isIntendingGpsHeightFusion() const
 {
-	filter_control_status_u control_status;
-	_ekf->get_control_mode(&control_status.value);
-	return control_status.flags.gps_hgt;
+	return _ekf->control_status_flags().gps_hgt;
 }
 
 void EkfWrapper::setRangeHeight()
@@ -41,9 +37,7 @@ void EkfWrapper::setRangeHeight()
 
 bool EkfWrapper::isIntendingRangeHeightFusion() const
 {
-	filter_control_status_u control_status;
-	_ekf->get_control_mode(&control_status.value);
-	return control_status.flags.rng_hgt;
+	return _ekf->control_status_flags().rng_hgt;
 }
 
 void EkfWrapper::setVisionHeight()
@@ -53,9 +47,7 @@ void EkfWrapper::setVisionHeight()
 
 bool EkfWrapper::isIntendingVisionHeightFusion() const
 {
-	filter_control_status_u control_status;
-	_ekf->get_control_mode(&control_status.value);
-	return control_status.flags.ev_hgt;
+	return _ekf->control_status_flags().ev_hgt;
 }
 
 void EkfWrapper::enableGpsFusion()

test/sensor_simulator/mag.cpp

@@ -15,8 +15,7 @@ Mag::~Mag()
 
 void Mag::send(uint64_t time)
 {
-	const magSample mag_sample {_mag_data, time};
-	_ekf->setMagData(mag_sample);
+	_ekf->setMagData(magSample{time, _mag_data});
 }
 
 void Mag::setData(const Vector3f& mag)

test/sensor_simulator/vio.cpp

@@ -61,15 +61,14 @@ void Vio::setOrientation(const Quatf& quat)
 
 void Vio::setVelocityFrameToBody()
 {
-	_vio_data.vel_frame = BODY_FRAME_FRD;
+	_vio_data.vel_frame = velocity_frame_t::BODY_FRAME_FRD;
 }
 
 void Vio::setVelocityFrameToLocal()
 {
-	_vio_data.vel_frame = LOCAL_FRAME_FRD;
+	_vio_data.vel_frame = velocity_frame_t::LOCAL_FRAME_FRD;
 }
 
-
 extVisionSample Vio::dataAtRest()
 {
 	extVisionSample vio_data;
@@ -79,7 +78,7 @@ extVisionSample Vio::dataAtRest()
 	vio_data.posVar = Vector3f{0.1f, 0.1f, 0.1f};
 	vio_data.velCov = matrix::eye<float ,3>() * 0.1f;
 	vio_data.angVar = 0.05f;
-	vio_data.vel_frame = LOCAL_FRAME_FRD;
+	vio_data.vel_frame = velocity_frame_t::LOCAL_FRAME_FRD;
 	return vio_data;
 }
 

test/test_EKF_measurementSampling.cpp

@@ -66,8 +66,7 @@ TEST_F(EkfMeasurementSamplingTest, baroDownSampling)
 			imu_sample.time_us = time;
 			_ekf->setIMUData(imu_sample);
 		}
-		const baroSample baro_sample {baro_data, time};
-		_ekf->setBaroData(baro_sample);
+		_ekf->setBaroData(baroSample{time, baro_data});
 		baro_data *= -1.0f;
 		time += 1000000 / baro_rate_Hz;
 	}

test/test_EKF_ringbuffer.cpp

@@ -62,7 +62,6 @@ class EkfRingBufferTest : public ::testing::Test {
 
 	void TearDown() override
 	{
-		_buffer->unallocate();
 		delete _buffer;
 	}
 };

test/test_SensorRangeFinder.cpp

@@ -59,7 +59,7 @@ public:
 
 protected:
 	SensorRangeFinder _range_finder{};
-	const rangeSample _good_sample{1.f, (uint64_t)2e6, 100}; // {range, time_us, quality}
+	const rangeSample _good_sample{(uint64_t)2e6, 1.f, 100}; // {time_us, range, quality}
 	const float _min_range{0.5f};
 	const float _max_range{10.f};