Merge pull request #152 from PX4/pr-ekf2FixObsDataHandling

EKF: Fix vulnerability to loss of observations due to high input data rates

EKF/estimator_interface.cpp

@@ -48,6 +48,7 @@
 
 
 EstimatorInterface::EstimatorInterface():
+	_min_obs_interval_us(0),
 	_dt_imu_avg(0.0f),
 	_imu_ticks(0),
 	_imu_updated(false),
@@ -103,30 +104,35 @@ void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, u
 	memcpy(&imu_sample_new.delta_ang._data[0], delta_ang, sizeof(imu_sample_new.delta_ang._data));
 	memcpy(&imu_sample_new.delta_vel._data[0], delta_vel, sizeof(imu_sample_new.delta_vel._data));
 
-	//convert time from us to secs
+	// 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.time_us = time_usec;
 	_imu_ticks++;
 
-
+	// 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);
+
 	} else {
 		_imu_updated = false;
-	}
 
-
-	_imu_sample_delayed = _imu_buffer.get_oldest();
+	}
 }
 
 void EstimatorInterface::setMagData(uint64_t time_usec, float *data)
 {
-
-	if (time_usec - _time_last_mag > 70000) {
+	// limit data rate to prevent data being lost
+	if (time_usec - _time_last_mag > _min_obs_interval_us) {
 
 		magSample mag_sample_new = {};
 		mag_sample_new.time_us = time_usec  - _params.mag_delay_ms * 1000;
@@ -143,9 +149,13 @@ void EstimatorInterface::setMagData(uint64_t time_usec, float *data)
 
 void EstimatorInterface::setGpsData(uint64_t time_usec, struct gps_message *gps)
 {
-	// Limit the GPS data rate to a maximum of 14Hz
+	if (!_initialised) {
+		return;
+	}
+
+	// limit data rate to prevent data being lost
 	bool need_gps = (_params.fusion_mode & MASK_USE_GPS) || (_params.vdist_sensor_type == VDIST_SENSOR_GPS);
-	if (time_usec - _time_last_gps > 65000 && need_gps) {
+	if (((time_usec - _time_last_gps) > _min_obs_interval_us) && need_gps) {
 		gpsSample gps_sample_new = {};
 		gps_sample_new.time_us = gps->time_usec - _params.gps_delay_ms * 1000;
 
@@ -182,11 +192,12 @@ void EstimatorInterface::setGpsData(uint64_t time_usec, struct gps_message *gps)
 
 void EstimatorInterface::setBaroData(uint64_t time_usec, float *data)
 {
-	if (!collect_baro(time_usec, data) || !_initialised) {
+	if (!_initialised) {
 		return;
 	}
 
-	if (time_usec - _time_last_baro > 68000) {
+	// limit data rate to prevent data being lost
+	if (time_usec - _time_last_baro > _min_obs_interval_us) {
 
 		baroSample baro_sample_new;
 		baro_sample_new.hgt = *data;
@@ -203,11 +214,12 @@ void EstimatorInterface::setBaroData(uint64_t time_usec, float *data)
 
 void EstimatorInterface::setAirspeedData(uint64_t time_usec, float *true_airspeed, float *eas2tas)
 {
-	if (!collect_airspeed(time_usec, true_airspeed, eas2tas) || !_initialised) {
+	if (!_initialised) {
 		return;
 	}
 
-	if (time_usec - _time_last_airspeed > 80000) {
+	// limit data rate to prevent data being lost
+	if (time_usec - _time_last_airspeed > _min_obs_interval_us) {
 		airspeedSample airspeed_sample_new;
 		airspeed_sample_new.true_airspeed = *true_airspeed;
 		airspeed_sample_new.eas2tas = *eas2tas;
@@ -222,11 +234,12 @@ static float rng;
 // set range data
 void EstimatorInterface::setRangeData(uint64_t time_usec, float *data)
 {
-	if (!collect_range(time_usec, data) || !_initialised) {
+	if (!_initialised) {
 		return;
 	}
 
-	if (time_usec - _time_last_range > 45000) {
+	// limit data rate to prevent data being lost
+	if (time_usec - _time_last_range > _min_obs_interval_us) {
 		rangeSample range_sample_new = {};
 		range_sample_new.rng = *data;
 		rng = *data;
@@ -242,12 +255,12 @@ void EstimatorInterface::setRangeData(uint64_t time_usec, float *data)
 // set optical flow data
 void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *flow)
 {
-	if (!collect_opticalflow(time_usec, flow) || !_initialised) {
+	if (!_initialised) {
 		return;
 	}
 
-	// if data passes checks, push to buffer
-	if (time_usec - _time_last_optflow > 40000) {
+	// limit data rate to prevent data being lost
+	if (time_usec - _time_last_optflow > _min_obs_interval_us) {
 		// check if enough integration time
 		float delta_time = 1e-6f * (float)flow->dt;
 		bool delta_time_good = (delta_time >= 0.05f);
@@ -293,8 +306,8 @@ void EstimatorInterface::setExtVisionData(uint64_t time_usec, ext_vision_message
 		return;
 	}
 
-	// if data passes checks, push to buffer
-	if (time_usec > _time_last_ext_vision) {
+	// limit data rate to prevent data being lost
+	if (time_usec - _time_last_ext_vision > _min_obs_interval_us) {
 		extVisionSample ev_sample_new;
 		// calculate the system time-stamp for the mid point of the integration period
 		ev_sample_new.time_us = time_usec - _params.ev_delay_ms * 1000;

EKF/estimator_interface.h

@@ -111,18 +111,9 @@ public:
 	// ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined
 	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_mag(uint64_t time_usec, float *data) { return true; }
-
-	virtual bool collect_baro(uint64_t time_usec, float *data) { return true; }
-
-	virtual bool collect_airspeed(uint64_t time_usec, float *true_airspeed, float *eas2tas) { return true; }
-
-	virtual bool collect_range(uint64_t time_usec, float *data) { return true; }
-
-	virtual bool collect_opticalflow(uint64_t time_usec, flow_message *flow) { return true; }
-
 	// set delta angle imu data
 	void setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64_t delta_vel_dt, float *delta_ang, float *delta_vel);
 
@@ -237,6 +228,8 @@ protected:
 	static const uint8_t IMU_BUFFER_LENGTH = 30;	// defines how many imu samples we can buffer
 	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)
+
 	float _dt_imu_avg;	// average imu update period in s
 
 	imuSample _imu_sample_delayed;	// captures the imu sample on the delayed time horizon