From c81f9f1dba1cd4a49a1da9dd6d921f5f8cf6aaf1 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 24 Jul 2016 08:22:52 +1000 Subject: [PATCH 1/3] EKF: Reduce output predictor tracking errors Implements a PI tracking loop --- EKF/ekf.cpp | 24 +++++++++++------------- EKF/ekf.h | 4 +++- 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 14a1e26fa0..5df0c20eb1 100644 --- a/EKF/ekf.cpp +++ b/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); diff --git a/EKF/ekf.h b/EKF/ekf.h index be8c65595b..19d567f878 100644 --- a/EKF/ekf.h +++ b/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) From 65762e5998e5d49a50cb0d5e16bb1d82c6840424 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 14 Aug 2016 09:08:44 +1000 Subject: [PATCH 2/3] EKF: publish output predictor tracking errors --- EKF/ekf.cpp | 19 +++++++++++++------ EKF/ekf.h | 5 +++++ EKF/ekf_helper.cpp | 7 +++++++ EKF/estimator_interface.h | 8 +++++++- 4 files changed, 32 insertions(+), 7 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 5df0c20eb1..6c76c527aa 100644 --- a/EKF/ekf.cpp +++ b/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 diff --git a/EKF/ekf.h b/EKF/ekf.h index 19d567f878..53182f0baa 100644 --- a/EKF/ekf.h +++ b/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) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index c89c02f006..39626f6eb6 100644 --- a/EKF/ekf_helper.cpp +++ b/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) { diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index dfe192da83..ec0b5fa6fe 100644 --- a/EKF/estimator_interface.h +++ b/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; From b5ba4f280fbe595e2abcfaece211552ca78df692 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 15 Aug 2016 10:43:47 +1000 Subject: [PATCH 3/3] EKF: reduce output predictor velocity time constant --- EKF/common.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/EKF/common.h b/EKF/common.h index 85345457b4..4845cda8c4 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -348,7 +348,7 @@ struct parameters { ev_pos_body = {}; // output complementary filter tuning time constants - vel_Tau = 0.5f; + vel_Tau = 0.25f; pos_Tau = 0.25f; }