EKF: Minimise data buffer lengths

Saves RAM and reduces output predictor errors by using the smallest data buffer length that meets time delay and update rate requirements.

EKF/common.h

@@ -173,6 +173,7 @@ struct parameters {
 	// measurement source control
 	int fusion_mode;		// bitmasked integer that selects which of the GPS and optical flow aiding sources will be used
 	int vdist_sensor_type;		// selects the primary source for height data
+	int sensor_interval_min_ms;	// minimum time of arrival difference between non IMU sensor updates. Sets the size of the observation buffers.
 
 	// measurement time delays
 	float mag_delay_ms;		// magnetometer measurement delay relative to the IMU (msec)
@@ -275,6 +276,7 @@ struct parameters {
 		// measurement source control
 		fusion_mode = MASK_USE_GPS;
 		vdist_sensor_type = VDIST_SENSOR_BARO;
+		sensor_interval_min_ms = 20;
 
 		// measurement time delays
 		mag_delay_ms = 0.0f;

EKF/estimator_interface.cpp

@@ -48,6 +48,8 @@
 
 
 EstimatorInterface::EstimatorInterface():
+	OBS_BUFFER_LENGTH(10),
+	IMU_BUFFER_LENGTH(30),
 	_min_obs_interval_us(0),
 	_dt_imu_avg(0.0f),
 	_imu_ticks(0),
@@ -340,6 +342,22 @@ void EstimatorInterface::setExtVisionData(uint64_t time_usec, ext_vision_message
 
 bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 {
+	// find the maximum time delay required to compensate for
+	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.airspeed_delay_ms, _params.baro_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;
+
+	// set the observaton buffer length to handle the minimum time of arrival between observations
+	OBS_BUFFER_LENGTH = (max_time_delay_ms / _params.sensor_interval_min_ms) + 1;
+
+	ECL_INFO("EKF IMU buffer length = %i",(int)IMU_BUFFER_LENGTH);
+	ECL_INFO("EKF observation buffer length = %i",(int)OBS_BUFFER_LENGTH);
 
 	if (!(_imu_buffer.allocate(IMU_BUFFER_LENGTH) &&
 	      _gps_buffer.allocate(OBS_BUFFER_LENGTH) &&

EKF/estimator_interface.h

@@ -273,8 +273,21 @@ protected:
 
 	parameters _params;		// filter parameters
 
-	static const uint8_t OBS_BUFFER_LENGTH = 10;	// defines how many measurement samples we can buffer
-	static const uint8_t IMU_BUFFER_LENGTH = 30;	// defines how many imu samples we can buffer
+	/*
+	 OBS_BUFFER_LENGTH defines how many observations (non-IMU measurements) we can buffer
+	 which sets the maximum frequency at which we can process non-IMU measurements. Measurements that
+	 arrive too soon after the previous measurement will not be processed.
+	 max freq (Hz) = (OBS_BUFFER_LENGTH - 1) / (IMU_BUFFER_LENGTH * FILTER_UPDATE_PERIOD_MS * 0.001)
+	 This can be adjusted to match the max sensor data rate plus some margin for jitter.
+	*/
+	uint8_t OBS_BUFFER_LENGTH;
+	/*
+	IMU_BUFFER_LENGTH defines how many IMU samples we buffer which sets the time delay from current time to the
+	EKF fusion time horizon and therefore the maximum sensor time offset relative to the IMU that we can compensate for.
+	max sensor time offet (msec) =  IMU_BUFFER_LENGTH * FILTER_UPDATE_PERIOD_MS
+	This can be adjusted to a value that is FILTER_UPDATE_PERIOD_MS longer than the maximum observation time delay.
+	*/
+	uint8_t IMU_BUFFER_LENGTH;
 	static const unsigned FILTER_UPDATE_PERIOD_MS = 10;	// ekf prediction period in milliseconds
 
 	unsigned _min_obs_interval_us; // minimum time interval between observations that will guarantee data is not lost (usec)