From 47624a0f021e2c187cc4763ad829afe2aa4fb11a Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Tue, 7 Apr 2020 18:51:51 -0400
Subject: [PATCH] EKF increase delta velocity process noise per axis if
 clipping (#663)

---
 EKF/common.h                  |  1 +
 EKF/covariance.cpp            | 14 ++++++++++++++
 EKF/estimator_interface.cpp   |  3 +++
 EKF/imu_down_sampler.cpp      |  6 ++++++
 test/sensor_simulator/imu.cpp |  2 +-
 5 files changed, 25 insertions(+), 1 deletion(-)

diff --git a/EKF/common.h b/EKF/common.h
index ce1743d38f..c870d0e00d 100644
--- a/EKF/common.h
+++ b/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 {
diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp
index 414d4d3013..8d1bfd06cd 100644
--- a/EKF/covariance.cpp
+++ b/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
diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp
index d0306ad978..1af98abd81 100644
--- a/EKF/estimator_interface.cpp
+++ b/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;
 
diff --git a/EKF/imu_down_sampler.cpp b/EKF/imu_down_sampler.cpp
index e09eec51e6..f0414c4d5e 100644
--- a/EKF/imu_down_sampler.cpp
+++ b/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;
 }
diff --git a/test/sensor_simulator/imu.cpp b/test/sensor_simulator/imu.cpp
index a966176ef6..e7f1105f14 100644
--- a/test/sensor_simulator/imu.cpp
+++ b/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;