From 3bd941549846fc0ee3deded133584d94dee8041d Mon Sep 17 00:00:00 2001 From: kamilritz Date: Sun, 26 Jul 2020 11:44:00 +0200 Subject: [PATCH] Make delayed output stayed not a member variable --- EKF/ekf.cpp | 17 ++++++++++------- EKF/ekf_helper.cpp | 13 +++++-------- EKF/estimator_interface.h | 2 -- 3 files changed, 15 insertions(+), 17 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 178ba12e3f..3e90cb4e8d 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -65,6 +65,7 @@ void Ekf::reset() _state.wind_vel.setZero(); _state.quat_nominal.setIdentity(); + // TODO: who resets the output buffer content? _output_new.vel.setZero(); _output_new.pos.setZero(); _output_new.quat_nominal.setIdentity(); @@ -394,11 +395,13 @@ void Ekf::calculateOutputStates() // get the oldest INS state data from the ring buffer // this data will be at the EKF fusion time horizon - _output_sample_delayed = _output_buffer.get_oldest(); - _output_vert_delayed = _output_vert_buffer.get_oldest(); + // TODO: there is no guarantee that data is at delayed fusion horizon + // Shouldnt we use pop_first_older_than? + const outputSample output_delayed = _output_buffer.get_oldest(); + const outputVert output_vert_delayed = _output_vert_buffer.get_oldest(); // calculate the quaternion delta between the INS and EKF quaternions at the EKF fusion time horizon - const Quatf q_error( (_state.quat_nominal.inversed() * _output_sample_delayed.quat_nominal).normalized() ); + const Quatf q_error( (_state.quat_nominal.inversed() * output_delayed.quat_nominal).normalized() ); // convert the quaternion delta to a delta angle const float scalar = (q_error(0) >= 0.0f) ? -2.f : 2.f; @@ -427,8 +430,8 @@ void Ekf::calculateOutputStates() const float pos_gain = _dt_ekf_avg / math::constrain(_params.pos_Tau, _dt_ekf_avg, 10.0f); // calculate down velocity and position tracking errors - const float vert_vel_err = (_state.vel(2) - _output_vert_delayed.vert_vel); - const float vert_vel_integ_err = (_state.pos(2) - _output_vert_delayed.vert_vel_integ); + const float vert_vel_err = (_state.vel(2) - output_vert_delayed.vert_vel); + const float vert_vel_integ_err = (_state.pos(2) - output_vert_delayed.vert_vel_integ); // calculate a velocity correction that will be applied to the output state history // using a PD feedback tuned to a 5% overshoot @@ -437,8 +440,8 @@ void Ekf::calculateOutputStates() applyCorrectionToVerticalOutputBuffer(vert_vel_correction); // calculate velocity and position tracking errors - const Vector3f vel_err(_state.vel - _output_sample_delayed.vel); - const Vector3f pos_err(_state.pos - _output_sample_delayed.pos); + const Vector3f vel_err(_state.vel - output_delayed.vel); + const Vector3f pos_err(_state.pos - output_delayed.pos); _output_tracking_error(1) = vel_err.norm(); _output_tracking_error(2) = pos_err.norm(); diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 8999280d2f..f3f6cdcf60 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -141,7 +141,6 @@ void Ekf::resetVerticalVelocityTo(float new_vert_vel) { _output_vert_buffer[index].vert_vel += delta_vert_vel; } _output_new.vel(2) += delta_vert_vel; - _output_vert_delayed.vert_vel = new_vert_vel; _output_vert_new.vert_vel += delta_vert_vel; _state_reset_status.velD_change = delta_vert_vel; @@ -330,7 +329,6 @@ void Ekf::resetHeight() // add the reset amount to the output observer vertical position state if (vert_pos_reset) { - _output_vert_delayed.vert_vel_integ = _state.pos(2); _output_vert_new.vert_vel_integ = _state.pos(2); } } @@ -338,13 +336,14 @@ void Ekf::resetHeight() // align output filter states to match EKF states at the fusion time horizon void Ekf::alignOutputFilter() { + const outputSample output_delayed = _output_buffer.get_oldest(); // calculate the quaternion rotation delta from the EKF to output observer states at the EKF fusion time horizon - Quatf q_delta = _state.quat_nominal * _output_sample_delayed.quat_nominal.inversed(); + Quatf q_delta = _state.quat_nominal * output_delayed.quat_nominal.inversed(); q_delta.normalize(); // calculate the velocity and position deltas between the output and EKF at the EKF fusion time horizon - const Vector3f vel_delta = _state.vel - _output_sample_delayed.vel; - const Vector3f pos_delta = _state.pos - _output_sample_delayed.pos; + const Vector3f vel_delta = _state.vel - output_delayed.vel; + const Vector3f pos_delta = _state.pos - output_delayed.pos; // loop through the output filter state history and add the deltas for (uint8_t i = 0; i < _output_buffer.get_length(); i++) { @@ -356,9 +355,7 @@ void Ekf::alignOutputFilter() _output_new.quat_nominal = q_delta * _output_new.quat_nominal; _output_new.quat_nominal.normalize(); - - _output_sample_delayed.quat_nominal = q_delta * _output_sample_delayed.quat_nominal; - _output_sample_delayed.quat_nominal.normalize(); + // TODO: what about vel and pos of _output_new, shouldn't they be aligned too? } // Do a forced re-alignment of the yaw angle to align with the horizontal velocity vector from the GPS. diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 4668cab586..7adbd66541 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -456,9 +456,7 @@ protected: float _flow_max_distance{0.0f}; ///< maximum distance that the optical flow sensor can operate at (m) // Output Predictor - outputSample _output_sample_delayed{}; // filter output on the delayed time horizon outputSample _output_new{}; // filter output on the non-delayed time horizon - outputVert _output_vert_delayed{}; // vertical filter output on the delayed time horizon outputVert _output_vert_new{}; // vertical filter output on the non-delayed time horizon imuSample _newest_high_rate_imu_sample{}; // imu sample capturing the newest imu data Matrix3f _R_to_earth_now; // rotation matrix from body to earth frame at current time