EKF: capture innovation checks and reset events in separate variables

rename the innovation check status class variable and remove the reset flags from it.

EKF/airspeed_fusion.cpp

@@ -138,11 +138,11 @@ void Ekf::fuseAirspeed()
 
 		// If the innovation consistency check fails then don't fuse the sample and indicate bad airspeed health
 		if (_tas_test_ratio > 1.0f) {
-			_sensor_fault_status.flags.reject_airspeed = true;
+			_innov_check_fail_status.flags.reject_airspeed = true;
 			return;
 		}
 		else {
-			_sensor_fault_status.flags.reject_airspeed = false;
+			_innov_check_fail_status.flags.reject_airspeed = false;
 		}
 
 		// Airspeed measurement sample has passed check so record it

EKF/common.h

@@ -388,8 +388,8 @@ union fault_status_u {
 
 };
 
-// define structure used to communicate innovation test failures and state resets
-union sensor_fault_status_u {
+// define structure used to communicate innovation test failures
+union innovation_fault_status_u {
 	struct {
 		bool reject_vel_NED: 1;		// 0 - true if velocity observations have been rejected
 		bool reject_pos_NE: 1;		// 1 - true if horizontal position observations have been rejected
@@ -402,13 +402,8 @@ union sensor_fault_status_u {
 		bool reject_hagl: 1;		// 8 - true if the height above ground observation has been rejected
 		bool reject_optflow_X: 1;	// 9 - true if the X optical flow observation has been rejected
 		bool reject_optflow_Y: 1;	// 10 - true if the Y optical flow observation has been rejected
-		bool reset_pos_NE: 1;		// 11 - true if the horizontal positoin has been reset
-		bool reset_pos_D: 1;		// 12 - true if the vertical position  has been reset
-		bool reset_vel_NE: 1;		// 13 - true if the horizontal velocity has been reset
-		bool reset_vel_D: 1;		// 14 - true if the vertical velocity has been reset
-		bool reset_yaw: 1;		// 15 - true if teh yaw angle has been reset
 	} flags;
-	uint32_t value;
+	uint16_t value;
 
 };
 

EKF/ekf.cpp

@@ -180,7 +180,7 @@ bool Ekf::init(uint64_t timestamp)
 	_dt_ekf_avg = 0.001f * (float)(FILTER_UPDATE_PERIOD_MS);
 
 	_fault_status.value = 0;
-	_sensor_fault_status.value = 0;
+	_innov_check_fail_status.value = 0;
 
 	return ret;
 }

EKF/estimator_interface.h

@@ -259,10 +259,12 @@ protected:
 	float _gps_origin_epv; // vertical position uncertainty of the GPS origin
 	struct map_projection_reference_s _pos_ref;    // Contains WGS-84 position latitude and longitude (radians)
 
+	// innovation consistency check monitoring ratios
 	float _yaw_test_ratio;          // yaw innovation consistency check ratio
 	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
+	innovation_fault_status_u _innov_check_fail_status;
 
 	// data buffer instances
 	RingBuffer<imuSample> _imu_buffer;
@@ -285,7 +287,6 @@ protected:
 	uint64_t _time_last_optflow;
 
 	fault_status_u _fault_status;
-	sensor_fault_status_u _sensor_fault_status;
 
 	// allocate data buffers and intialise interface variables
 	bool initialise_interface(uint64_t timestamp);

EKF/mag_fusion.cpp

@@ -182,9 +182,9 @@ void Ekf::fuseMag()
 
 		if (_mag_test_ratio[index] > 1.0f) {
 			mag_fail = false;
-			_sensor_fault_status.value |= (1 << (index + 3));
+			_innov_check_fail_status.value |= (1 << (index + 3));
 		} else {
-			_sensor_fault_status.value &= !(1 << (index + 3));
+			_innov_check_fail_status.value &= !(1 << (index + 3));
 		}
 	}
 
@@ -605,7 +605,7 @@ void Ekf::fuseHeading()
 
 	// set the magnetometer unhealthy if the test fails
 	if (_yaw_test_ratio > 1.0f) {
-		_sensor_fault_status.flags.reject_yaw = true;
+		_innov_check_fail_status.flags.reject_yaw = true;
 
 		// if we are in air we don't want to fuse the measurement
 		// we allow to use it when on the ground because the large innovation could be caused
@@ -620,7 +620,7 @@ void Ekf::fuseHeading()
 		}
 
 	} else {
-		_sensor_fault_status.flags.reject_yaw = false;
+		_innov_check_fail_status.flags.reject_yaw = false;
 	}
 
 	// apply covariance correction via P_new = (I -K*H)*P

EKF/optflow_fusion.cpp

@@ -407,10 +407,10 @@ void Ekf::fuseOptFlow()
 	for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) {
 		if (optflow_test_ratio[obs_index] > 1.0f) {
 			flow_fail = true;
-			_sensor_fault_status.value |= (1 << (obs_index + 9));
+			_innov_check_fail_status.value |= (1 << (obs_index + 9));
 
 		} else {
-			_sensor_fault_status.value &= ~(1 << (obs_index + 9));
+			_innov_check_fail_status.value &= ~(1 << (obs_index + 9));
 
 		}
 	}

EKF/terrain_estimator.cpp

@@ -117,10 +117,10 @@ void Ekf::fuseHagl()
 			_terrain_var = fmaxf(_terrain_var * (1.0f - gain), 0.0f);
 			// record last successful fusion event
 			_time_last_hagl_fuse = _time_last_imu;
-			_sensor_fault_status.flags.reject_hagl = false;
+			_innov_check_fail_status.flags.reject_hagl = false;
 
 		} else {
-			_sensor_fault_status.flags.reject_hagl = true;
+			_innov_check_fail_status.flags.reject_hagl = true;
 
 		}
 

EKF/vel_pos_fusion.cpp

@@ -204,25 +204,25 @@ void Ekf::fuseVelPosHeight()
 	// record the successful velocity fusion event
 	if (vel_check_pass && _fuse_hor_vel) {
 		_time_last_vel_fuse = _time_last_imu;
-		_sensor_fault_status.flags.reject_vel_NED = false;
+		_innov_check_fail_status.flags.reject_vel_NED = false;
 	} else if (!vel_check_pass) {
-		_sensor_fault_status.flags.reject_vel_NED = true;
+		_innov_check_fail_status.flags.reject_vel_NED = true;
 	}
 
 	// record the successful position fusion event
 	if (pos_check_pass && _fuse_pos) {
 		_time_last_pos_fuse = _time_last_imu;
-		_sensor_fault_status.flags.reject_pos_NE = false;
+		_innov_check_fail_status.flags.reject_pos_NE = false;
 	} else if (!pos_check_pass) {
-		_sensor_fault_status.flags.reject_pos_NE = true;
+		_innov_check_fail_status.flags.reject_pos_NE = true;
 	}
 
 	// record the successful height fusion event
 	if (innov_check_pass_map[5] && _fuse_height) {
 		_time_last_hgt_fuse = _time_last_imu;
-		_sensor_fault_status.flags.reject_pos_D = false;
+		_innov_check_fail_status.flags.reject_pos_D = false;
 	} else if (!innov_check_pass_map[5]) {
-		_sensor_fault_status.flags.reject_pos_D = true;
+		_innov_check_fail_status.flags.reject_pos_D = true;
 	}
 
 	for (unsigned obs_index = 0; obs_index < 6; obs_index++) {