Merge pull request #150 from PX4/pr-ekf2StatusReporting

EKF: Publish full reset and innovation consistency check status data

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) {
-			_airspeed_healthy = false;
+			_innov_check_fail_status.flags.reject_airspeed = true;
 			return;
 		}
 		else {
-			_airspeed_healthy = true;
+			_innov_check_fail_status.flags.reject_airspeed = false;
 		}
 
 		// Airspeed measurement sample has passed check so record it

EKF/common.h

@@ -388,6 +388,25 @@ union 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
+		bool reject_pos_D: 1;		// 2 - true if true if vertical position observations have been rejected
+		bool reject_mag_x: 1;		// 3 - true if the X magnetometer observation has been rejected
+		bool reject_mag_y: 1;		// 4 - true if the Y magnetometer observation has been rejected
+		bool reject_mag_z: 1;		// 5 - true if the Z magnetometer observation has been rejected
+		bool reject_yaw: 1;		// 6 - true if the yaw observation has been rejected
+		bool reject_airspeed: 1;	// 7 - true if the airspeed observation has been rejected
+		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
+	} flags;
+	uint16_t value;
+
+};
+
 // publish the status of various GPS quality checks
 union gps_check_fail_status_u {
 	struct {

EKF/control.cpp

@@ -107,9 +107,27 @@ void Ekf::controlExternalVisionAiding()
 			matrix::Euler<float> euler_obs(q_obs);
 			euler_init(2) = euler_obs(2);
 
+			// save a copy of the quaternion state for later use in calculating the amount of reset change
+			Quaternion quat_before_reset = _state.quat_nominal;
+
 			// calculate initial quaternion states for the ekf
 			_state.quat_nominal = Quaternion(euler_init);
 
+			// calculate the amount that the quaternion has changed by
+			_state_reset_status.quat_change = _state.quat_nominal * quat_before_reset.inversed();
+
+			// add the reset amount to the output observer buffered data
+			outputSample output_states;
+			unsigned output_length = _output_buffer.get_length();
+			for (unsigned i=0; i < output_length; i++) {
+				output_states = _output_buffer.get_from_index(i);
+				output_states.quat_nominal *= _state_reset_status.quat_change;
+				_output_buffer.push_to_index(i,output_states);
+			}
+
+			// capture the reset event
+			_state_reset_status.quat_counter++;
+
 			// flag the yaw as aligned
 			_control_status.flags.yaw_align = true;
 

EKF/ekf.cpp

@@ -107,10 +107,6 @@ Ekf::Ekf():
 	_gps_hgt_faulty(false),
 	_rng_hgt_faulty(false),
 	_baro_hgt_offset(0.0f),
-	_vert_pos_reset_delta(0.0f),
-	_time_vert_pos_reset(0),
-	_vert_vel_reset_delta(0.0f),
-	_time_vert_vel_reset(0),
 	_time_bad_vert_accel(0)
 {
 	_state = {};
@@ -132,6 +128,7 @@ Ekf::Ekf():
 	_flow_gyro_bias = {};
 	_imu_del_ang_of = {};
 	_gps_check_fail_status.value = 0;
+	_state_reset_status = {};
 }
 
 Ekf::~Ekf()
@@ -170,7 +167,6 @@ bool Ekf::init(uint64_t timestamp)
 	_imu_updated = false;
 	_NED_origin_initialised = false;
 	_gps_speed_valid = false;
-	_mag_healthy = false;
 
 	_filter_initialised = false;
 	_terrain_initialised = false;
@@ -180,6 +176,9 @@ bool Ekf::init(uint64_t timestamp)
 
 	_dt_ekf_avg = 0.001f * (float)(FILTER_UPDATE_PERIOD_MS);
 
+	_fault_status.value = 0;
+	_innov_check_fail_status.value = 0;
+
 	return ret;
 }
 
@@ -568,6 +567,9 @@ bool Ekf::initialiseFilter(void)
 		_time_last_hagl_fuse = _time_last_imu;
 		_time_last_of_fuse = _time_last_imu;
 
+		// reset the output predictor state history to match the EKF initial values
+		alignOutputFilter();
+
 		return true;
 	}
 }

EKF/ekf.h

@@ -127,8 +127,24 @@ public:
 	// get GPS check status
 	void get_gps_check_status(uint16_t *_gps_check_fail_status);
 
-	// return the amount the local vertical position changed in the last height reset and the time of the reset
-	void get_vert_pos_reset(float *delta, uint64_t *time_us) {*delta = _vert_pos_reset_delta; *time_us = _time_vert_pos_reset;}
+	// return the amount the local vertical position changed in the last reset and the number of reset events
+	void get_posD_reset(float *delta, uint8_t *counter) {*delta = _state_reset_status.posD_change; *counter = _state_reset_status.posD_counter;}
+
+	// return the amount the local vertical velocity changed in the last reset and the number of reset events
+	void get_velD_reset(float *delta, uint8_t *counter) {*delta = _state_reset_status.velD_change; *counter = _state_reset_status.velD_counter;}
+
+	// return the amount the local horizontal position changed in the last reset and the number of reset events
+	void get_posNE_reset(Vector2f *delta, uint8_t *counter) {*delta = _state_reset_status.posNE_change; *counter = _state_reset_status.posNE_counter;}
+
+	// return the amount the local horizontal velocity changed in the last reset and the number of reset events
+	void get_velNE_reset(Vector2f *delta, uint8_t *counter) {*delta = _state_reset_status.velNE_change; *counter = _state_reset_status.velNE_counter;}
+
+	// return the amount the quaternion has changed in the last reset and the number of reset events
+	void get_quat_reset(Quaternion *delta, uint8_t *counter)
+	{
+		*delta = _state_reset_status.quat_change;
+		*counter = _state_reset_status.quat_counter;
+	}
 
 private:
 
@@ -136,6 +152,21 @@ private:
 	static const float _k_earth_rate;
 	static const float _gravity_mss;
 
+	// reset event monitoring
+	// structure containing velocity, position, height and yaw reset information
+	struct {
+		uint8_t velNE_counter;	// number of horizontal position reset events (allow to wrap if count exceeds 255)
+		uint8_t velD_counter;	// number of vertical velocity reset events (allow to wrap if count exceeds 255)
+		uint8_t posNE_counter;	// number of horizontal position reset events (allow to wrap if count exceeds 255)
+		uint8_t posD_counter;	// number of vertical position reset events (allow to wrap if count exceeds 255)
+		uint8_t quat_counter;	// number of quaternion reset events (allow to wrap if count exceeds 255)
+		Vector2f velNE_change;  // North East velocity change due to last reset (m)
+		float velD_change;	// Down velocity change due to last reset (m/s)
+		Vector2f posNE_change;	// North, East position change due to last reset (m)
+		float posD_change;	// Down position change due to last reset (m)
+		Quaternion quat_change;	// quaternion delta due to last reset - multiply pre-reset quaternion by this to get post-reset quaternion
+	} _state_reset_status;
+
 	float _dt_ekf_avg;		// average update rate of the ekf
 
 	stateSample _state;		// state struct of the ekf running at the delayed time horizon
@@ -235,10 +266,6 @@ private:
 	int _primary_hgt_source;	// priary source of height data set at initialisation
 
 	float _baro_hgt_offset;		// baro height reading at the local NED origin (m)
-	float _vert_pos_reset_delta;	// increase in vertical position state at the last reset(m)
-	uint64_t _time_vert_pos_reset;	// last system time in usec that the vertical position state was reset
-	float _vert_vel_reset_delta;	// increase in vertical position velocity at the last reset(m)
-	uint64_t _time_vert_vel_reset;	// last system time in usec that the vertical velocity state was reset
 
 	// imu fault status
 	uint64_t _time_bad_vert_accel;	// last time a bad vertical accel was detected (usec)

EKF/ekf_helper.cpp

@@ -51,6 +51,10 @@
 // gps measurement then use for velocity initialisation
 bool Ekf::resetVelocity()
 {
+	// used to calculate the velocity change due to the reset
+	Vector3f vel_before_reset = _state.vel;
+
+	// reset EKF states
 	if (_control_status.flags.gps) {
 		// if we have a valid GPS measurement use it to initialise velocity states
 		gpsSample gps_newest = _gps_buffer.get_newest();
@@ -69,12 +73,35 @@ bool Ekf::resetVelocity()
 	} else {
 		return false;
 	}
+
+	// calculate the change in velocity and apply to the output predictor state history
+	Vector3f velocity_change = _state.vel - vel_before_reset;
+	outputSample output_states;
+	unsigned max_index = _output_buffer.get_length() - 1;
+	for (unsigned index=0; index <= max_index; index++) {
+		output_states = _output_buffer.get_from_index(index);
+		output_states.vel += velocity_change;
+		_output_buffer.push_to_index(index,output_states);
+	}
+
+	// capture the reset event
+	_state_reset_status.velNE_change(0) = velocity_change(0);
+	_state_reset_status.velNE_change(1) = velocity_change(1);
+	_state_reset_status.velD_change = velocity_change(2);
+	_state_reset_status.velNE_counter++;
+	_state_reset_status.velD_counter++;
+
 }
 
 // Reset position states. If we have a recent and valid
 // gps measurement then use for position initialisation
 bool Ekf::resetPosition()
 {
+	// used to calculate the position change due to the reset
+	Vector2f posNE_before_reset;
+	posNE_before_reset(0) = _state.pos(0);
+	posNE_before_reset(1) = _state.pos(1);
+
 	if (_control_status.flags.gps) {
 		// if we have a fresh GPS measurement, use it to initialise position states and correct the position for the measurement delay
 		gpsSample gps_newest = _gps_buffer.get_newest();
@@ -119,6 +146,24 @@ bool Ekf::resetPosition()
 	} else {
 		return false;
 	}
+
+	// calculate the change in position and apply to the output predictor state history
+	Vector2f posNE_change;
+	posNE_change(0) = _state.pos(0) - posNE_before_reset(0);
+	posNE_change(1) = _state.pos(1) - posNE_before_reset(1);
+	outputSample output_states;
+	unsigned max_index = _output_buffer.get_length() - 1;
+	for (unsigned index=0; index <= max_index; index++) {
+		output_states = _output_buffer.get_from_index(index);
+		output_states.pos(0) += posNE_change(0);
+		output_states.pos(1) += posNE_change(1);
+		_output_buffer.push_to_index(index,output_states);
+	}
+
+	// capture the reset event
+	_state_reset_status.posNE_change = posNE_change;
+	_state_reset_status.posNE_counter++;
+
 }
 
 // Reset height state using the last height measurement
@@ -244,18 +289,18 @@ void Ekf::resetHeight()
 
 	// store the reset amount and time to be published
 	if (vert_pos_reset) {
-		_vert_pos_reset_delta = _state.pos(2) - old_vert_pos;
-		_time_vert_pos_reset = _time_last_imu;
+		_state_reset_status.posD_change = _state.pos(2) - old_vert_pos;
+		_state_reset_status.posD_counter++;
 	}
 
 	if (vert_vel_reset) {
-		_vert_vel_reset_delta = _state.vel(2) - old_vert_vel;
-		_time_vert_vel_reset = _time_last_imu;
+		_state_reset_status.velD_change = _state.vel(2) - old_vert_vel;
+		_state_reset_status.velD_counter++;
 	}
 
 	// add the reset amount to the output observer states
-	_output_new.pos(2) += _vert_pos_reset_delta;
-	_output_new.vel(2) += _vert_vel_reset_delta;
+	_output_new.pos(2) += _state_reset_status.posD_change;
+	_output_new.vel(2) += _state_reset_status.velD_change;
 
 	// add the reset amount to the output observer buffered data
 	outputSample output_states;
@@ -264,11 +309,11 @@ void Ekf::resetHeight()
 		output_states = _output_buffer.get_from_index(i);
 
 		if (vert_pos_reset) {
-			output_states.pos(2) += _vert_pos_reset_delta;
+			output_states.pos(2) += _state_reset_status.posD_change;
 		}
 
 		if (vert_vel_reset) {
-			output_states.vel(2) += _vert_vel_reset_delta;
+			output_states.vel(2) += _state_reset_status.velD_change;
 		}
 
 		_output_buffer.push_to_index(i,output_states);
@@ -305,6 +350,8 @@ void Ekf::alignOutputFilter()
 // Reset heading and magnetic field states
 bool Ekf::resetMagHeading(Vector3f &mag_init)
 {
+	// save a copy of the quaternion state for later use in calculating the amount of reset change
+	Quaternion quat_before_reset = _state.quat_nominal;
 
 	// calculate the variance for the rotation estimate expressed as a rotation vector
 	// this will be used later to reset the quaternion state covariances
@@ -437,6 +484,21 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
 		P[index][index] = sq(_params.mag_noise);
 	}
 
+	// calculate the amount that the quaternion has changed by
+	_state_reset_status.quat_change = _state.quat_nominal * quat_before_reset.inversed();
+
+	// add the reset amount to the output observer buffered data
+	outputSample output_states;
+	unsigned output_length = _output_buffer.get_length();
+	for (unsigned i=0; i < output_length; i++) {
+		output_states = _output_buffer.get_from_index(i);
+		output_states.quat_nominal *= _state_reset_status.quat_change;
+		_output_buffer.push_to_index(i,output_states);
+	}
+
+	// capture the reset event
+	_state_reset_status.quat_counter++;
+
 	return true;
 }
 

EKF/estimator_interface.cpp

@@ -57,8 +57,6 @@ EstimatorInterface::EstimatorInterface():
 	_gps_speed_valid(false),
 	_gps_origin_eph(0.0f),
 	_gps_origin_epv(0.0f),
-	_mag_healthy(false),
-	_airspeed_healthy(false),
 	_yaw_test_ratio(0.0f),
 	_time_last_imu(0),
 	_time_last_gps(0),

EKF/estimator_interface.h

@@ -217,8 +217,26 @@ public:
 	// get GPS check status
 	virtual void get_gps_check_status(uint16_t *val) = 0;
 
-	// return the amount the local vertical position changed in the last height reset and the time of the reset
-	virtual void get_vert_pos_reset(float *delta, uint64_t *time_us) = 0;
+	// return the amount the local vertical position changed in the last reset and the number of reset events
+	virtual void get_posD_reset(float *delta, uint8_t *counter) = 0;
+
+	// return the amount the local vertical velocity changed in the last reset and the number of reset events
+	virtual void get_velD_reset(float *delta, uint8_t *counter) = 0;
+
+	// return the amount the local horizontal position changed in the last reset and the number of reset events
+	virtual void get_posNE_reset(Vector2f *delta, uint8_t *counter) = 0;
+
+	// return the amount the local horizontal velocity changed in the last reset and the number of reset events
+	virtual void get_velNE_reset(Vector2f *delta, uint8_t *counter) = 0;
+
+	// return the amount the quaternion has changed in the last reset and the number of reset events
+	virtual void get_quat_reset(Quaternion *delta, uint8_t *counter) = 0;
+
+	// get EKF innovation consistency check status
+	virtual void get_innovation_test_status(uint16_t *val)
+	{
+		*val = _innov_check_fail_status.value;
+	}
 
 protected:
 
@@ -259,12 +277,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)
 
-	bool _mag_healthy;              // computed by mag innovation test
-	bool _airspeed_healthy;			// computed by airspeed innovation test
+	// 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
+	float _tas_test_ratio;		// tas innovation consistency check ratio
+	innovation_fault_status_u _innov_check_fail_status;
 
 	// data buffer instances
 	RingBuffer<imuSample> _imu_buffer;

EKF/mag_fusion.cpp

@@ -176,17 +176,19 @@ void Ekf::fuseMag()
 
 	// Perform an innovation consistency check on each measurement and if one axis fails
 	// do not fuse any data from the sensor because the most common errors affect multiple axes.
-	_mag_healthy = true;
-
+	bool mag_fail = false;
 	for (uint8_t index = 0; index <= 2; index++) {
 		_mag_test_ratio[index] = sq(_mag_innov[index]) / (sq(math::max(_params.mag_innov_gate, 1.0f)) * _mag_innov_var[index]);
 
 		if (_mag_test_ratio[index] > 1.0f) {
-			_mag_healthy = false;
+			mag_fail = false;
+			_innov_check_fail_status.value |= (1 << (index + 3));
+		} else {
+			_innov_check_fail_status.value &= !(1 << (index + 3));
 		}
 	}
 
-	if (!_mag_healthy) {
+	if (mag_fail) {
 		return;
 	}
 
@@ -603,7 +605,7 @@ void Ekf::fuseHeading()
 
 	// set the magnetometer unhealthy if the test fails
 	if (_yaw_test_ratio > 1.0f) {
-		_mag_healthy = false;
+		_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
@@ -618,7 +620,7 @@ void Ekf::fuseHeading()
 		}
 
 	} else {
-		_mag_healthy = true;
+		_innov_check_fail_status.flags.reject_yaw = false;
 	}
 
 	// apply covariance correction via P_new = (I -K*H)*P

EKF/optflow_fusion.cpp

@@ -402,9 +402,23 @@ void Ekf::fuseOptFlow()
 		}
 	}
 
-	// if either axis fails, we fail the sensor
-	if (optflow_test_ratio[0] > 1.0f || optflow_test_ratio[1] > 1.0f) {
+	// record the innovation test pass/fail
+	bool flow_fail = false;
+	for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) {
+		if (optflow_test_ratio[obs_index] > 1.0f) {
+			flow_fail = true;
+			_innov_check_fail_status.value |= (1 << (obs_index + 9));
+
+		} else {
+			_innov_check_fail_status.value &= ~(1 << (obs_index + 9));
+
+		}
+	}
+
+	// if either axis fails we abort the fusion
+	if (flow_fail) {
 		return;
+
 	}
 
 	for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) {

EKF/terrain_estimator.cpp

@@ -115,10 +115,14 @@ void Ekf::fuseHagl()
 			_terrain_vpos -= gain * _hagl_innov;
 			// correct the variance
 			_terrain_var = fmaxf(_terrain_var * (1.0f - gain), 0.0f);
-			// record last successful fusion time
+			// record last successful fusion event
 			_time_last_hagl_fuse = _time_last_imu;
-		}
+			_innov_check_fail_status.flags.reject_hagl = false;
 
+		} else {
+			_innov_check_fail_status.flags.reject_hagl = true;
+
+		}
 
 	} else {
 		return;

EKF/vel_pos_fusion.cpp

@@ -201,19 +201,28 @@ void Ekf::fuseVelPosHeight()
 	innov_check_pass_map[4] = innov_check_pass_map[3] = pos_check_pass;
 	innov_check_pass_map[5] = (_vel_pos_test_ratio[5] <= 1.0f) || !_control_status.flags.tilt_align;
 
-	// record the successful velocity fusion time
+	// record the successful velocity fusion event
 	if (vel_check_pass && _fuse_hor_vel) {
 		_time_last_vel_fuse = _time_last_imu;
+		_innov_check_fail_status.flags.reject_vel_NED = false;
+	} else if (!vel_check_pass) {
+		_innov_check_fail_status.flags.reject_vel_NED = true;
 	}
 
-	// record the successful position fusion time
+	// record the successful position fusion event
 	if (pos_check_pass && _fuse_pos) {
 		_time_last_pos_fuse = _time_last_imu;
+		_innov_check_fail_status.flags.reject_pos_NE = false;
+	} else if (!pos_check_pass) {
+		_innov_check_fail_status.flags.reject_pos_NE = true;
 	}
 
-	// record the successful height fusion time
+	// record the successful height fusion event
 	if (innov_check_pass_map[5] && _fuse_height) {
 		_time_last_hgt_fuse = _time_last_imu;
+		_innov_check_fail_status.flags.reject_pos_D = false;
+	} else if (!innov_check_pass_map[5]) {
+		_innov_check_fail_status.flags.reject_pos_D = true;
 	}
 
 	for (unsigned obs_index = 0; obs_index < 6; obs_index++) {