EKF increase delta velocity process noise per axis if clipping (#663)

EKF/common.h

@@ -93,6 +93,7 @@ struct imuSample {
 	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 {

EKF/covariance.cpp

@@ -246,6 +246,20 @@ void Ekf::predictCovariance()
 
 	dvxVar = dvyVar = dvzVar = sq(dt * accel_noise);
 
+	// Accelerometer Clipping
+	// delta velocity X: increase process noise if sample contained any X axis clipping
+	if (_imu_sample_delayed.delta_vel_clipping[0]) {
+		dvxVar = sq(dt * BADACC_BIAS_PNOISE);
+	}
+	// delta velocity Y: increase process noise if sample contained any Y axis clipping
+	if (_imu_sample_delayed.delta_vel_clipping[1]) {
+		dvyVar = sq(dt * BADACC_BIAS_PNOISE);
+	}
+	// delta velocity Z: increase process noise if sample contained any Z axis clipping
+	if (_imu_sample_delayed.delta_vel_clipping[2]) {
+		dvzVar = sq(dt * BADACC_BIAS_PNOISE);
+	}
+
 	// predict the covariance
 
 	// intermediate calculations

EKF/estimator_interface.cpp

@@ -540,6 +540,9 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 	}
 
 	_imu_sample_delayed.time_us = timestamp;
+	_imu_sample_delayed.delta_vel_clipping[0] = false;
+	_imu_sample_delayed.delta_vel_clipping[1] = false;
+	_imu_sample_delayed.delta_vel_clipping[2] = false;
 
 	_fault_status.value = 0;
 

EKF/imu_down_sampler.cpp

@@ -14,6 +14,9 @@ bool ImuDownSampler::update(const imuSample &imu_sample_new) {
 	_imu_down_sampled.delta_ang_dt += imu_sample_new.delta_ang_dt;
 	_imu_down_sampled.delta_vel_dt += imu_sample_new.delta_vel_dt;
 	_imu_down_sampled.time_us = imu_sample_new.time_us;
+	_imu_down_sampled.delta_vel_clipping[0] += imu_sample_new.delta_vel_clipping[0];
+	_imu_down_sampled.delta_vel_clipping[1] += imu_sample_new.delta_vel_clipping[1];
+	_imu_down_sampled.delta_vel_clipping[2] += imu_sample_new.delta_vel_clipping[2];
 
 	// use a quaternion to accumulate delta angle data
 	// this quaternion represents the rotation from the start to end of the accumulation period
@@ -51,6 +54,9 @@ void ImuDownSampler::reset() {
 	_imu_down_sampled.delta_vel.setZero();
 	_imu_down_sampled.delta_ang_dt = 0.0f;
 	_imu_down_sampled.delta_vel_dt = 0.0f;
+	_imu_down_sampled.delta_vel_clipping[0] = false;
+	_imu_down_sampled.delta_vel_clipping[1] = false;
+	_imu_down_sampled.delta_vel_clipping[2] = false;
 	_delta_angle_accumulated.setIdentity();
 	_do_reset = false;
 }

test/sensor_simulator/imu.cpp

@@ -15,7 +15,7 @@ Imu::~Imu()
 
 void Imu::send(uint64_t time)
 {
-	imuSample imu_sample;
+	imuSample imu_sample{};
 	imu_sample.time_us = time;
 	imu_sample.delta_ang_dt = (time - _time_last_data_sent) * 1.e-6f;
 	imu_sample.delta_ang = _gyro_data * imu_sample.delta_ang_dt;