EKF: Add IMU vibration calculation and reporting

Add calculation and reporting of IMU delta angle and velocity coning and high frequency vibration

EKF/ekf.h

@@ -121,6 +121,14 @@ public:
 	// error magnitudes (rad), (m/s), (m)
 	void get_output_tracking_error(float error[3]);
 
+	/*
+	Returns  following IMU vibration metrics in the following array locations
+	0 : Gyro delta angle coning metric = filtered length of (delta_angle x prev_delta_angle)
+	1 : Gyro high frequency vibe = filtered length of (delta_angle - prev_delta_angle)
+	2 : Accel high frequency vibe = filtered length of (delta_velocity - prev_delta_velocity)
+	*/
+	void get_imu_vibe_metrics(float vibe[3]);
+
 	// return true if the global position estimate is valid
 	bool global_position_is_valid();
 

EKF/ekf_helper.cpp

@@ -696,6 +696,17 @@ void Ekf::get_output_tracking_error(float error[3])
 	memcpy(error, _output_tracking_error, 3 * sizeof(float));
 }
 
+/*
+Returns  following IMU vibration metrics in the following array locations
+0 : Gyro delta angle coning metric = filtered length of (delta_angle x prev_delta_angle)
+1 : Gyro high frequency vibe = filtered length of (delta_angle - prev_delta_angle)
+2 : Accel high frequency vibe = filtered length of (delta_velocity - prev_delta_velocity)
+*/
+void Ekf::get_imu_vibe_metrics(float vibe[3])
+{
+	memcpy(vibe, _vibe_metrics, 3 * sizeof(float));
+}
+
 // get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position
 void Ekf::get_ekf_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_reckoning)
 {

EKF/estimator_interface.cpp

@@ -73,6 +73,9 @@ EstimatorInterface::EstimatorInterface():
 	_pos_ref = {};
 	memset(_mag_test_ratio, 0, sizeof(_mag_test_ratio));
 	memset(_vel_pos_test_ratio, 0, sizeof(_vel_pos_test_ratio));
+	_delta_ang_prev.setZero();
+	_delta_vel_prev.setZero();
+	memset(_vibe_metrics, 0, sizeof(_vibe_metrics));
 }
 
 EstimatorInterface::~EstimatorInterface()
@@ -110,6 +113,20 @@ void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, u
 	imu_sample_new.time_us = time_usec;
 	_imu_ticks++;
 
+	// calculate a metric which indicates the amount of coning vibration
+	Vector3f temp = cross_product(imu_sample_new.delta_ang , _delta_ang_prev);
+	_vibe_metrics[0] = 0.99f * _vibe_metrics[0] + 0.01f * temp.length();
+
+	// calculate a metric which indiates the amount of high frequency gyro vibration
+	temp = imu_sample_new.delta_ang - _delta_ang_prev;
+	_delta_ang_prev = imu_sample_new.delta_ang;
+	_vibe_metrics[1] = 0.99f * _vibe_metrics[1] + 0.01f * temp.length();
+
+	// calculate a metric which indicates the amount of high fequency accelerometer vibration
+	temp = imu_sample_new.delta_vel - _delta_vel_prev;
+	_delta_vel_prev = imu_sample_new.delta_vel;
+	_vibe_metrics[2] = 0.99f * _vibe_metrics[2] + 0.01f * temp.length();
+
 	// 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);

EKF/estimator_interface.h

@@ -111,6 +111,14 @@ public:
 	// error magnitudes (rad), (m/s), (m)
 	virtual void get_output_tracking_error(float error[3]) = 0;
 
+	/*
+	Returns  following IMU vibration metrics in the following array locations
+	0 : Gyro delta angle coning metric = filtered length of (delta_angle x prev_delta_angle)
+	1 : Gyro high frequency vibe = filtered length of (delta_angle - prev_delta_angle)
+	2 : Accel high frequency vibe = filtered length of (delta_velocity - prev_delta_velocity)
+	*/
+	virtual void get_imu_vibe_metrics(float vibe[3]) = 0;
+
 	// get the ekf WGS-84 origin positoin and height and the system time it was last set
 	virtual void get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt) = 0;
 
@@ -299,6 +307,14 @@ protected:
 	float _terr_test_ratio;		// height above terrain measurement innovation consistency check ratio
 	innovation_fault_status_u _innov_check_fail_status;
 
+	// IMU vibration monitoring
+	Vector3f _delta_ang_prev;	// delta angle from the previous IMU measurement
+	Vector3f _delta_vel_prev;	// delta velocity from the previous IMU measurement
+	float _vibe_metrics[3];		// IMU vibration metrics
+					// [0] Level of coning vibration in the IMU delta angles (rad^2)
+					// [1] high frequency vibraton level in the IMU delta angle data (rad)
+					// [2] high frequency vibration level in the IMU delta velocity data (m/s)
+
 	// data buffer instances
 	RingBuffer<imuSample> _imu_buffer;
 	RingBuffer<gpsSample> _gps_buffer;