EKF: Publish innovation test ratios

EKF/ekf.h

@@ -162,6 +162,13 @@ public:
 		*counter = _state_reset_status.quat_counter;
 	}
 
+	// get EKF innovation consistency check status information comprising of:
+	// status - a bitmask integer containing the pass/fail status for each EKF measurement innovation consistency check
+	// Innovation Test Ratios - these are the ratio of the innovation to the acceptance threshold.
+	// A value > 1 indicates that the sensor measurement has exceeded the maximum acceptable level and has been rejected by the EKF
+	// Where a measurement type is a vector quantity, eg magnetoemter, GPS position, etc, the maximum value is returned.
+	void get_innovation_test_status(uint16_t *status, float *mag, float *vel, float *pos, float *hgt, float *tas, float *hagl);
+
 private:
 
 	static const uint8_t _k_num_states = 24;

EKF/ekf_helper.cpp

@@ -696,6 +696,29 @@ void Ekf::get_ekf_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_reckoning)
 	memcpy(dead_reckoning, &temp3, sizeof(bool));
 }
 
+// get EKF innovation consistency check status information comprising of:
+// status - a bitmask integer containing the pass/fail status for each EKF measurement innovation consistency check
+// Innovation Test Ratios - these are the ratio of the innovation to the acceptance threshold.
+// A value > 1 indicates that the sensor measurement has exceeded the maximum acceptable level and has been rejected by the EKF
+// Where a measurement type is a vector quantity, eg magnetoemter, GPS position, etc, the maximum value is returned.
+void Ekf::get_innovation_test_status(uint16_t *status, float *mag, float *vel, float *pos, float *hgt, float *tas, float *hagl)
+{
+	// return the integer bitmask containing the consistency check pass/fail satus
+	*status = _innov_check_fail_status.value;
+	// return the largest magnetometer innovation test ratio
+	*mag = sqrtf(math::max(_yaw_test_ratio,math::max(math::max(_mag_test_ratio[0],_mag_test_ratio[1]),_mag_test_ratio[2])));
+	// return the largest NED velocity innovation test ratio
+	*vel = sqrtf(math::max(math::max(_vel_pos_test_ratio[0],_vel_pos_test_ratio[1]),_vel_pos_test_ratio[2]));
+	// return the largest NE position innovation test ratio
+	*pos = sqrtf(math::max(_vel_pos_test_ratio[3],_vel_pos_test_ratio[4]));
+	// return the vertical position innovation test ratio
+	*hgt = sqrtf(_vel_pos_test_ratio[5]);
+	// return the airspeed fusion innovation test ratio
+	*tas = sqrtf(_tas_test_ratio);
+	// return the terrain height innovation test ratio
+	*hagl = sqrt(_terr_test_ratio);
+}
+
 // fuse measurement
 void Ekf::fuse(float *K, float innovation)
 {

EKF/estimator_interface.cpp

@@ -58,6 +58,8 @@ EstimatorInterface::EstimatorInterface():
 	_gps_origin_eph(0.0f),
 	_gps_origin_epv(0.0f),
 	_yaw_test_ratio(0.0f),
+	_tas_test_ratio(0.0f),
+	_terr_test_ratio(0.0f),
 	_time_last_imu(0),
 	_time_last_gps(0),
 	_time_last_mag(0),

EKF/estimator_interface.h

@@ -44,6 +44,7 @@
 #include "RingBuffer.h"
 #include "geo.h"
 #include "common.h"
+#include "mathlib.h"
 
 using namespace estimator;
 class EstimatorInterface
@@ -241,11 +242,13 @@ public:
 	// return the amount the quaternion has changed in the last reset and the number of reset events
 	virtual void get_quat_reset(float delta_quat[4], uint8_t *counter) = 0;
 
-	// get EKF innovation consistency check status
+	// get EKF innovation consistency check status information comprising of:
-	virtual void get_innovation_test_status(uint16_t *val)
+	// status - a bitmask integer containing the pass/fail status for each EKF measurement innovation consistency check
-	{
+	// Innovation Test Ratios - these are the ratio of the innovation to the acceptance threshold.
-		*val = _innov_check_fail_status.value;
+	// A value > 1 indicates that the sensor measurement has exceeded the maximum acceptable level and has been rejected by the EKF
-	}
+	// Where a measurement type is a vector quantity, eg magnetoemter, GPS position, etc, the maximum value is returned.
+	virtual void get_innovation_test_status(uint16_t *status, float *mag, float *vel, float *pos, float *hgt, float *tas, float *hagl) = 0;
+
 
 protected:
 
@@ -291,6 +294,7 @@ protected:
 	float _mag_test_ratio[3];       // magnetometer XYZ innovation consistency check ratios
 	float _vel_pos_test_ratio[6];   // velocity and position innovation consistency check ratios
 	float _tas_test_ratio;		// tas innovation consistency check ratio
+	float _terr_test_ratio;		// height above terrain measurement innovation consistency check ratio
 	innovation_fault_status_u _innov_check_fail_status;
 
 	// data buffer instances

EKF/terrain_estimator.cpp

@@ -106,9 +106,9 @@ void Ekf::fuseHagl()
 
 		// perform an innovation consistency check and only fuse data if it passes
 		float gate_size = fmaxf(_params.range_innov_gate, 1.0f);
-		float test_ratio = sq(_hagl_innov) / (sq(gate_size) * _hagl_innov_var);
+		_terr_test_ratio = sq(_hagl_innov) / (sq(gate_size) * _hagl_innov_var);
 
-		if (test_ratio <= 1.0f) {
+		if (_terr_test_ratio <= 1.0f) {
 			// calculate the Kalman gain
 			float gain = _terrain_var / _hagl_innov_var;
 			// correct the state