EKF: publish output predictor tracking errors

EKF/ekf.cpp

@@ -631,17 +631,24 @@ void Ekf::calculateOutputStates()
 		// that will cause the INS to track the EKF quaternions
 		_delta_angle_corr = delta_ang_error * att_gain;
 
-		// calculate a position correction that will be applied to the output state history
-		float pos_gain = _dt_ekf_avg / math::constrain(_params.pos_Tau, _dt_ekf_avg, 10.0f);
+		// calculate velocity and position tracking errors
+		Vector3f vel_err = (_state.vel - _output_sample_delayed.vel);
 		Vector3f pos_err = (_state.pos - _output_sample_delayed.pos);
-		_pos_err_integ += pos_err;
-		Vector3f pos_correction = pos_err * pos_gain + _pos_err_integ * sq(pos_gain) * 0.1f;
+
+		// collect magnitude tracking error for diagnostics
+		_output_tracking_error[0] = delta_ang_error.norm();
+		_output_tracking_error[1] = vel_err.norm();
+		_output_tracking_error[2] = pos_err.norm();
 
 		// calculate a velocity correction that will be applied to the output state history
 		float vel_gain = _dt_ekf_avg / math::constrain(_params.vel_Tau, _dt_ekf_avg, 10.0f);
-		Vector3f vel_err = (_state.vel - _output_sample_delayed.vel);
 		_vel_err_integ += vel_err;
-		Vector3f vel_correction = vel_err * vel_gain + _vel_err_integ * sq(pos_gain) * 0.1f;
+		Vector3f vel_correction = vel_err * vel_gain + _vel_err_integ * sq(vel_gain) * 0.1f;
+
+		// calculate a position correction that will be applied to the output state history
+		float pos_gain = _dt_ekf_avg / math::constrain(_params.pos_Tau, _dt_ekf_avg, 10.0f);
+		_pos_err_integ += pos_err;
+		Vector3f pos_correction = pos_err * pos_gain + _pos_err_integ * sq(pos_gain) * 0.1f;
 
 		// loop through the output filter state history and apply the corrections to the velocity and position states
 		// this method is too expensive to use for the attitude states due to the quaternion operations required

EKF/ekf.h

@@ -117,6 +117,10 @@ public:
 
 	void get_pos_var(Vector3f &pos_var);
 
+	// return an array containing the output predictor angular, velocity and position tracking
+	// error magnitudes (rad), (m/s), (m)
+	void get_output_tracking_error(float error[3]);
+
 	// return true if the global position estimate is valid
 	bool global_position_is_valid();
 
@@ -246,6 +250,7 @@ private:
 	Quaternion _q_down_sampled;	// down sampled quaternion (tracking delta angles between ekf update steps)
 	Vector3f _vel_err_integ;	// integral of velocity tracking error
 	Vector3f _pos_err_integ;	// integral of position tracking error
+	float _output_tracking_error[3];// contains the magnitude of the angle, velocity and position track errors (rad, m/s, m)
 
 	// variables used for the GPS quality checks
 	float _gpsDriftVelN;		// GPS north position derivative (m/s)

EKF/ekf_helper.cpp

@@ -674,6 +674,13 @@ void Ekf::get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *orig
 	memcpy(origin_alt, &_gps_alt_ref, sizeof(float));
 }
 
+// return an array containing the output predictor angular, velocity and position tracking
+// error magnitudes (rad), (m/s), (m)
+void Ekf::get_output_tracking_error(float error[3])
+{
+	memcpy(error, _output_tracking_error, 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.h

@@ -88,8 +88,10 @@ public:
 
 	virtual void get_covariances(float *covariances) = 0;
 
-	// get the ekf WGS-84 origin position and height and the system time it was last set
+	// gets the variances for the NED velocity states
 	virtual void get_vel_var(Vector3f &vel_var) = 0;
+
+	// gets the variances for the NED position states
 	virtual void get_pos_var(Vector3f &pos_var) = 0;
 
 	// gets the innovation variance of the flow measurement
@@ -104,6 +106,10 @@ public:
 	// gets the innovation of the HAGL measurement
 	virtual void get_hagl_innov(float *flow_innov_var) = 0;
 
+	// return an array containing the output predictor angular, velocity and position tracking
+	// error magnitudes (rad), (m/s), (m)
+	virtual void get_output_tracking_error(float error[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;