EKF: Reduce output predictor tracking errors

Implements a PI tracking loop

EKF/ekf.cpp

@@ -130,6 +130,8 @@ Ekf::Ekf():
 	_last_known_posNE.setZero();
 	_imu_down_sampled = {};
 	_q_down_sampled.setZero();
+	_vel_err_integ.setZero();
+	_pos_err_integ.setZero();
 	_mag_filt_state = {};
 	_delVel_sum = {};
 	_flow_gyro_bias = {};
@@ -631,19 +633,15 @@ void Ekf::calculateOutputStates()
 
 		// 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);
-		Vector3f pos_delta = (_state.pos - _output_sample_delayed.pos) * pos_gain;
+		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;
 
 		// calculate a velocity correction that will be applied to the output state history
-		Vector3f vel_delta;
-		if (_params.pos_Tau <= 0.0f) {
-			// this method will cause the velocity to be kinematically consistent with
-			// the position corretions rather than tracking the EKF states
-			vel_delta = pos_delta * (1.0f / time_delay);
-		} else {
-			// this method makes the velocity track the EKF states with the specified time constant
-			float vel_gain = _dt_ekf_avg / math::constrain(_params.vel_Tau, _dt_ekf_avg, 10.0f);
-			vel_delta = (_state.vel - _output_sample_delayed.vel) * vel_gain;
-		}
+		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;
 
 		// 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
@@ -655,10 +653,10 @@ void Ekf::calculateOutputStates()
 			output_states = _output_buffer.get_from_index(index);
 
 			// a constant  velocity correction is applied
-			output_states.vel += vel_delta;
+			output_states.vel += vel_correction;
 
 			// a constant position correction is applied
-			output_states.pos += pos_delta;
+			output_states.pos += pos_correction;
 
 			// push the updated data to the buffer
 			_output_buffer.push_to_index(index,output_states);

EKF/ekf.h

@@ -240,10 +240,12 @@ private:
 
 	float _mag_declination;		// magnetic declination used by reset and fusion functions (rad)
 
-	// complementary filter states
+	// output predictor states
 	Vector3f _delta_angle_corr;	// delta angle correction vector
 	imuSample _imu_down_sampled;	// down sampled imu data (sensor rate -> filter update rate)
 	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
 
 	// variables used for the GPS quality checks
 	float _gpsDriftVelN;		// GPS north position derivative (m/s)