From 877a82328ca8cb4d1d2a493c0d6215977693cfed Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 26 Oct 2023 12:49:41 -0400 Subject: [PATCH] WIP --- src/modules/ekf2/EKF/ekf.cpp | 21 ++ src/modules/ekf2/EKF/ekf_helper.cpp | 13 -- src/modules/ekf2/EKF/output_predictor.cpp | 259 ++++++++++++++++------ src/modules/ekf2/EKF/output_predictor.h | 22 +- src/modules/ekf2/EKF2.cpp | 23 +- 5 files changed, 242 insertions(+), 96 deletions(-) diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index 5f1228d244..2ea6534d8b 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -217,6 +217,27 @@ bool Ekf::update() runTerrainEstimator(imu_sample_delayed); #endif // CONFIG_EKF2_TERRAIN + + if (_state_reset_status.reset_count.quat != _state_reset_count_prev.quat) { + _output_predictor.resetQuaternion(); + } + + if (_state_reset_status.reset_count.velNE != _state_reset_count_prev.velNE) { + _output_predictor.resetHorizontalVelocity(); + } + + if (_state_reset_status.reset_count.velD != _state_reset_count_prev.velD) { + _output_predictor.resetVerticalVelocity(); + } + + if (_state_reset_status.reset_count.posNE != _state_reset_count_prev.posNE) { + _output_predictor.resetHorizontalPosition(); + } + + if (_state_reset_status.reset_count.posD != _state_reset_count_prev.posD) { + _output_predictor.resetVerticalPosition(); + } + updated = true; } diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index b2f976510c..6d9fab952d 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -74,8 +74,6 @@ void Ekf::resetHorizontalVelocityTo(const Vector2f &new_horz_vel, const Vector2f P.uncorrelateCovarianceSetVariance<1>(State::vel.idx + 1, math::max(sq(0.01f), new_horz_vel_var(1))); } - _output_predictor.resetHorizontalVelocityTo(_time_delayed_us, _state.vel.xy()); - // record the state change if (_state_reset_status.reset_count.velNE == _state_reset_count_prev.velNE) { _state_reset_status.velNE_change = delta_horz_vel; @@ -100,8 +98,6 @@ void Ekf::resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var) P.uncorrelateCovarianceSetVariance<1>(State::vel.idx + 2, math::max(sq(0.01f), new_vert_vel_var)); } - _output_predictor.resetVerticalVelocityTo(_time_delayed_us, _state.vel(2)); - // record the state change if (_state_reset_status.reset_count.velD == _state_reset_count_prev.velD) { _state_reset_status.velD_change = delta_vert_vel; @@ -140,8 +136,6 @@ void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f P.uncorrelateCovarianceSetVariance<1>(State::pos.idx + 1, math::max(sq(0.01f), new_horz_pos_var(1))); } - _output_predictor.resetHorizontalPositionTo(_time_delayed_us, _state.pos.xy()); - // record the state change if (_state_reset_status.reset_count.posNE == _state_reset_count_prev.posNE) { _state_reset_status.posNE_change = delta_horz_pos; @@ -185,10 +179,6 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v const float delta_z = new_vert_pos - old_vert_pos; - // apply the change in height / height rate to our newest height / height rate estimate - // which have already been taken out from the output buffer - _output_predictor.resetVerticalPositionTo(_time_delayed_us, _state.pos(2)); - // record the state change if (_state_reset_status.reset_count.posD == _state_reset_count_prev.posD) { _state_reset_status.posD_change = delta_z; @@ -922,9 +912,6 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance) // restore covariance resetQuatCov(rot_var_ned_before_reset); - // add the reset amount to the output observer buffered data - _output_predictor.resetQuaternion(_time_delayed_us, _state.quat_nominal); - #if defined(CONFIG_EKF2_EXTERNAL_VISION) // update EV attitude error filter if (_ev_q_error_initialized) { diff --git a/src/modules/ekf2/EKF/output_predictor.cpp b/src/modules/ekf2/EKF/output_predictor.cpp index bdeba39ec8..a2e3f0c837 100644 --- a/src/modules/ekf2/EKF/output_predictor.cpp +++ b/src/modules/ekf2/EKF/output_predictor.cpp @@ -39,6 +39,8 @@ using matrix::Quatf; using matrix::Vector2f; using matrix::Vector3f; +//#define OUTPUT_PREDICTOR_DEBUG_VEROSE + void OutputPredictor::print_status() { printf("output predictor: IMU dt: %.4f, EKF dt: %.4f\n", (double)_dt_update_states_avg, (double)_dt_correct_states_avg); @@ -57,7 +59,6 @@ void OutputPredictor::reset() _accel_bias.zero(); _gyro_bias.zero(); - _time_last_update_states_us = 0; _time_last_correct_states_us = 0; _output_new = {}; @@ -72,14 +73,25 @@ void OutputPredictor::reset() _output_tracking_error.zero(); + _reset_quaternion = true; + _reset_velocity_xy = true; + _reset_velocity_z = true; + _reset_position_xy = true; + _reset_position_z = true; + _output_filter_aligned = false; } -void OutputPredictor::resetQuaternion(const uint64_t &time_delayed_us, const Quatf &new_quat) +void OutputPredictor::resetQuaternionTo(const uint64_t &time_delayed_us, const Quatf &new_quat) { // find the output observer state corresponding to the reset time const outputSample &delayed_sample = findOutputSample(time_delayed_us); +#if defined(OUTPUT_PREDICTOR_DEBUG_VEROSE) + const outputSample delayed_orig = delayed_sample; + const outputSample newest_orig = _output_new; +#endif // OUTPUT_PREDICTOR_DEBUG_VEROSE + const Quatf quat_change = (new_quat * delayed_sample.quat_nominal.inversed()).normalized(); // add the reset amount to the output observer buffered data @@ -90,6 +102,30 @@ void OutputPredictor::resetQuaternion(const uint64_t &time_delayed_us, const Qua // apply change to latest _output_new.quat_nominal = (quat_change * _output_new.quat_nominal).normalized(); _R_to_earth_now = Dcmf(_output_new.quat_nominal); + + + + _delta_angle_corr.zero(); + _output_tracking_error(0) = 0.f; + + _reset_quaternion = false; + + +#if defined(OUTPUT_PREDICTOR_DEBUG_VEROSE) + printf("output predictor: reset q delayed (%.1f,%.1f,%.1f)->(%.1f,%.1f,%.1f)\n", + (double)matrix::Eulerf(delayed_orig.quat_nominal).phi(), (double)matrix::Eulerf(delayed_orig.quat_nominal).theta(), + (double)matrix::Eulerf(delayed_orig.quat_nominal).psi(), + (double)matrix::Eulerf(delayed_sample.quat_nominal).phi(), (double)matrix::Eulerf(delayed_sample.quat_nominal).theta(), + (double)matrix::Eulerf(delayed_sample.quat_nominal).psi() + ); + + printf("output predictor: reset q newest (%.1f,%.1f,%.1f)->(%.1f,%.1f,%.1f)\n", + (double)matrix::Eulerf(newest_orig.quat_nominal).phi(), (double)matrix::Eulerf(newest_orig.quat_nominal).theta(), + (double)matrix::Eulerf(newest_orig.quat_nominal).psi(), + (double)matrix::Eulerf(_output_new.quat_nominal).phi(), (double)matrix::Eulerf(_output_new.quat_nominal).theta(), + (double)matrix::Eulerf(_output_new.quat_nominal).psi() + ); +#endif // OUTPUT_PREDICTOR_DEBUG_VEROSE } void OutputPredictor::resetHorizontalVelocityTo(const uint64_t &time_delayed_us, const Vector2f &new_horz_vel) @@ -97,6 +133,11 @@ void OutputPredictor::resetHorizontalVelocityTo(const uint64_t &time_delayed_us, // find the output observer state corresponding to the reset time const outputSample &delayed_sample = findOutputSample(time_delayed_us); +#if defined(OUTPUT_PREDICTOR_DEBUG_VEROSE) + const outputSample delayed_orig = delayed_sample; + const outputSample newest_orig = _output_new; +#endif // OUTPUT_PREDICTOR_DEBUG_VEROSE + const Vector2f delta_vxy = new_horz_vel - delayed_sample.vel.xy(); // horizontal velocity // add the reset amount to the output observer buffered data @@ -106,6 +147,24 @@ void OutputPredictor::resetHorizontalVelocityTo(const uint64_t &time_delayed_us, // apply change to latest velocity _output_new.vel.xy() += delta_vxy; + + + + _vel_err_integ(0) = 0.f; + _vel_err_integ(1) = 0.f; + _reset_velocity_xy = false; + +#if defined(OUTPUT_PREDICTOR_DEBUG_VEROSE) + printf("output predictor: reset v_xy delayed (%.1f,%.1f)->(%.1f,%.1f)\n", + (double)delayed_orig.vel(0), (double)delayed_orig.vel(1), + (double)delayed_sample.vel(0), (double)delayed_sample.vel(1) + ); + + printf("output predictor: reset v_xy newest (%.1f,%.1f)->(%.1f,%.1f)\n", + (double)newest_orig.vel(0), (double)newest_orig.vel(1), + (double)_output_new.vel(0), (double)_output_new.vel(1) + ); +#endif // OUTPUT_PREDICTOR_DEBUG_VEROSE } void OutputPredictor::resetVerticalVelocityTo(const uint64_t &time_delayed_us, const float new_vert_vel) @@ -113,6 +172,11 @@ void OutputPredictor::resetVerticalVelocityTo(const uint64_t &time_delayed_us, c // find the output observer state corresponding to the reset time const outputSample &delayed_sample = findOutputSample(time_delayed_us); +#if defined(OUTPUT_PREDICTOR_DEBUG_VEROSE) + const outputSample delayed_orig = delayed_sample; + const outputSample newest_orig = _output_new; +#endif // OUTPUT_PREDICTOR_DEBUG_VEROSE + const float delta_vz = new_vert_vel - delayed_sample.vel(2); // vertical velocity const float delta_vz_alt = new_vert_vel - delayed_sample.vert_vel_alt; // vertical velocity (alternative) @@ -125,6 +189,16 @@ void OutputPredictor::resetVerticalVelocityTo(const uint64_t &time_delayed_us, c // apply change to latest _output_new.vel(2) += delta_vz; _output_new.vert_vel_alt += delta_vz_alt; + + + _vel_err_integ(2) = 0.f; + _reset_velocity_z = false; + +#if defined(OUTPUT_PREDICTOR_DEBUG_VEROSE) + printf("output predictor: reset v_z delayed %.1f->%.1f\n", (double)delayed_orig.vel(2), (double)delayed_sample.vel(2)); + + printf("output predictor: reset v_z newest %.1f->%.1f\n", (double)newest_orig.vel(2), (double)_output_new.vel(2)); +#endif // OUTPUT_PREDICTOR_DEBUG_VEROSE } void OutputPredictor::resetHorizontalPositionTo(const uint64_t &time_delayed_us, const Vector2f &new_horz_pos) @@ -132,6 +206,11 @@ void OutputPredictor::resetHorizontalPositionTo(const uint64_t &time_delayed_us, // find the output observer state corresponding to the reset time const outputSample &delayed_sample = findOutputSample(time_delayed_us); +#if defined(OUTPUT_PREDICTOR_DEBUG_VEROSE) + const outputSample delayed_orig = delayed_sample; + const outputSample newest_orig = _output_new; +#endif // OUTPUT_PREDICTOR_DEBUG_VEROSE + const Vector2f delta_xy = new_horz_pos - delayed_sample.pos.xy(); // horizontal position // add the reset amount to the output observer buffered data @@ -141,6 +220,23 @@ void OutputPredictor::resetHorizontalPositionTo(const uint64_t &time_delayed_us, // apply change to latest _output_new.pos.xy() += delta_xy; + + + _pos_err_integ(0) = 0.f; + _pos_err_integ(1) = 0.f; + _reset_position_xy = false; + +#if defined(OUTPUT_PREDICTOR_DEBUG_VEROSE) + printf("output predictor: reset xy delayed (%.1f,%.1f)->(%.1f,%.1f)\n", + (double)delayed_orig.pos(0), (double)delayed_orig.pos(1), + (double)delayed_sample.pos(0), (double)delayed_sample.pos(1) + ); + + printf("output predictor: reset xy newest (%.1f,%.1f)->(%.1f,%.1f)\n", + (double)newest_orig.pos(0), (double)newest_orig.pos(1), + (double)_output_new.pos(0), (double)_output_new.pos(1) + ); +#endif // OUTPUT_PREDICTOR_DEBUG_VEROSE } void OutputPredictor::resetVerticalPositionTo(const uint64_t &time_delayed_us, const float new_vert_pos) @@ -148,6 +244,16 @@ void OutputPredictor::resetVerticalPositionTo(const uint64_t &time_delayed_us, c // find the output observer state corresponding to the reset time const outputSample &delayed_sample = findOutputSample(time_delayed_us); +#if defined(OUTPUT_PREDICTOR_DEBUG_VEROSE) + const outputSample delayed_orig = delayed_sample; + const outputSample newest_orig = _output_new; + + if (time_delayed_us != delayed_sample.time_us) { + printf("output predictor: BAD RESET TIME!\n"); + } + +#endif // OUTPUT_PREDICTOR_DEBUG_VEROSE + const float delta_z = new_vert_pos - delayed_sample.pos(2); // vertical position const float delta_z_alt = new_vert_pos - delayed_sample.vert_vel_integ; // vertical position (alternative) @@ -160,23 +266,36 @@ void OutputPredictor::resetVerticalPositionTo(const uint64_t &time_delayed_us, c // apply change to latest _output_new.pos(2) += delta_z; _output_new.vert_vel_integ += delta_z_alt; + + + _pos_err_integ(2) = 0.f; + _reset_position_z = false; + +#if defined(OUTPUT_PREDICTOR_DEBUG_VEROSE) + printf("output predictor: reset z, delayed (%llu):%.1f->%.1f, newest (%llu):%.1f->%.1f\n", + time_delayed_us, (double)delayed_orig.pos(2), (double)delayed_sample.pos(2), + _output_new.time_us, (double)newest_orig.pos(2), (double)_output_new.pos(2) + ); + + //printf("output predictor: %llu reset z newest %.1f->%.1f\n", _output_new.time_us, (double)newest_orig.pos(2), (double)_output_new.pos(2)); +#endif // OUTPUT_PREDICTOR_DEBUG_VEROSE } bool OutputPredictor::calculateOutputStates(const uint64_t time_us, const Vector3f &delta_angle, const float delta_angle_dt, const Vector3f &delta_velocity, const float delta_velocity_dt) { - if (time_us <= _time_last_update_states_us) { + if (time_us <= _output_new.time_us) { reset(); return false; } // Use full rate IMU data at the current time horizon - if (_time_last_update_states_us != 0) { - const float dt = math::constrain((time_us - _time_last_update_states_us) * 1e-6f, 0.0001f, 0.03f); + if (_output_new.time_us != 0) { + const float dt = math::constrain((time_us - _output_new.time_us) * 1e-6f, 0.0001f, 0.03f); _dt_update_states_avg = 0.8f * _dt_update_states_avg + 0.2f * dt; } - _time_last_update_states_us = time_us; + _output_new.time_us = time_us; // correct delta angle and delta velocity for bias offsets // Apply corrections to the delta angle required to track the quaternion states at the EKF fusion time horizon @@ -186,8 +305,6 @@ bool OutputPredictor::calculateOutputStates(const uint64_t time_us, const Vector const Vector3f delta_vel_bias_scaled = _accel_bias * delta_velocity_dt; const Vector3f delta_velocity_corrected(delta_velocity - delta_vel_bias_scaled); - _output_new.time_us = time_us; - const Quatf dq(AxisAnglef{delta_angle_corrected}); // rotate the previous INS quaternion by the delta quaternions @@ -276,52 +393,94 @@ bool OutputPredictor::correctOutputStates(const uint64_t &time_delayed_us, if (!_output_filter_aligned) { if (delayed_index_found) { - resetQuaternion(time_delayed_us, quat_state); + resetQuaternionTo(time_delayed_us, quat_state); resetHorizontalVelocityTo(time_delayed_us, vel_state.xy()); resetVerticalVelocityTo(time_delayed_us, vel_state(2)); resetHorizontalPositionTo(time_delayed_us, pos_state.xy()); resetVerticalPositionTo(time_delayed_us, pos_state(2)); - _delta_angle_corr.zero(); - _vel_err_integ.zero(); - _pos_err_integ.zero(); - _output_tracking_error.zero(); - _output_filter_aligned = true; + +#if defined(OUTPUT_PREDICTOR_DEBUG_VEROSE) + printf("output predictor: ALIGNED, IMU dt: %.4f, EKF dt: %.4f\n", + (double)_dt_update_states_avg, (double)_dt_correct_states_avg); +#endif // OUTPUT_PREDICTOR_DEBUG_VEROSE } return false; } - // calculate the quaternion delta between the INS and EKF quaternions at the EKF fusion time horizon - const Quatf q_error((quat_state.inversed() * output_delayed.quat_nominal).normalized()); + if (_reset_quaternion) { + resetQuaternionTo(time_delayed_us, quat_state); - // convert the quaternion delta to a delta angle - const float scalar = (q_error(0) >= 0.0f) ? -2.f : 2.f; + _output_tracking_error(0) = 0.f; - const Vector3f delta_ang_error{scalar * q_error(1), scalar * q_error(2), scalar * q_error(3)}; + } else { + // calculate the quaternion delta between the INS and EKF quaternions at the EKF fusion time horizon + const Quatf q_error((quat_state.inversed() * output_delayed.quat_nominal).normalized()); - // calculate a gain that provides tight tracking of the estimator attitude states and - // adjust for changes in time delay to maintain consistent damping ratio of ~0.7 - const float time_delay = fmaxf((time_latest_us - time_delayed_us) * 1e-6f, _dt_update_states_avg); - const float att_gain = 0.5f * _dt_update_states_avg / time_delay; + // convert the quaternion delta to a delta angle + const float scalar = (q_error(0) >= 0.0f) ? -2.f : 2.f; - // calculate a corrrection to the delta angle - // that will cause the INS to track the EKF quaternions - _delta_angle_corr = delta_ang_error * att_gain; - _output_tracking_error(0) = delta_ang_error.norm(); + const Vector3f delta_ang_error{scalar * q_error(1), scalar * q_error(2), scalar * q_error(3)}; - /* - * 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 - * but because it eliminates the time delay in the 'correction loop' it allows higher tracking gains - * to be used and reduces tracking error relative to EKF states. - */ + // calculate a gain that provides tight tracking of the estimator attitude states and + // adjust for changes in time delay to maintain consistent damping ratio of ~0.7 + const float time_delay = fmaxf((time_latest_us - time_delayed_us) * 1e-6f, _dt_update_states_avg); + const float att_gain = 0.5f * _dt_update_states_avg / time_delay; - // Complementary filter gains - const float vel_gain = _dt_correct_states_avg / math::constrain(_vel_tau, _dt_correct_states_avg, 10.f); - const float pos_gain = _dt_correct_states_avg / math::constrain(_pos_tau, _dt_correct_states_avg, 10.f); + // calculate a corrrection to the delta angle + // that will cause the INS to track the EKF quaternions + _delta_angle_corr = delta_ang_error * att_gain; + _output_tracking_error(0) = delta_ang_error.norm(); + } + + // velocity + if (_reset_velocity_xy) { + resetHorizontalVelocityTo(time_delayed_us, vel_state.xy()); + } + + if (_reset_velocity_z) { + resetVerticalVelocityTo(time_delayed_us, vel_state(2)); + } + + // calculate a velocity correction and apply it to the output state history + const Vector3f vel_err(vel_state - output_delayed.vel); + _vel_err_integ += vel_err; + const float vel_gain = _dt_correct_states_avg / math::constrain(_vel_tau, _dt_correct_states_avg, 10.f); // complementary filter gains + const Vector3f vel_correction = vel_err * vel_gain + _vel_err_integ * sq(vel_gain) * 0.1f; + + for (uint8_t i = 0; i < _output_buffer.get_length(); i++) { + _output_buffer[i].vel += vel_correction; + } + + _output_tracking_error(1) = vel_err.norm(); // logging + + + // position + if (_reset_position_xy) { + resetHorizontalPositionTo(time_delayed_us, pos_state.xy()); + } + + if (_reset_position_z) { + resetVerticalPositionTo(time_delayed_us, pos_state(2)); + } + + // calculate a position correction and apply it to the output state history + const Vector3f pos_err(pos_state - output_delayed.pos); + _pos_err_integ += pos_err; + const float pos_gain = _dt_correct_states_avg / math::constrain(_pos_tau, _dt_correct_states_avg, 10.f); // complementary filter gains + const Vector3f pos_correction = pos_err * pos_gain + _pos_err_integ * sq(pos_gain) * 0.1f; + + for (uint8_t i = 0; i < _output_buffer.get_length(); i++) { + _output_buffer[i].pos += pos_correction; + } + + _output_tracking_error(2) = pos_err.norm(); // logging + + + // vertical position alternate // calculate down velocity and position tracking errors const float vert_vel_err = (vel_state(2) - output_delayed.vert_vel_alt); const float vert_vel_integ_err = (pos_state(2) - output_delayed.vert_vel_integ); @@ -332,22 +491,6 @@ bool OutputPredictor::correctOutputStates(const uint64_t &time_delayed_us, applyCorrectionToVerticalOutputBuffer(vert_vel_correction); - // calculate velocity and position tracking errors - const Vector3f vel_err(vel_state - output_delayed.vel); - const Vector3f pos_err(pos_state - output_delayed.pos); - - _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 - _vel_err_integ += vel_err; - const 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 - _pos_err_integ += pos_err; - const Vector3f pos_correction = pos_err * pos_gain + _pos_err_integ * sq(pos_gain) * 0.1f; - - applyCorrectionToOutputBuffer(vel_correction, pos_correction); // update output state to corrected values and @@ -386,15 +529,3 @@ void OutputPredictor::applyCorrectionToVerticalOutputBuffer(float vert_vel_corre index = (index + 1) % size; } } - -void OutputPredictor::applyCorrectionToOutputBuffer(const Vector3f &vel_correction, const Vector3f &pos_correction) -{ - // loop through the output filter state history and apply the corrections to the velocity and position states - for (uint8_t index = 0; index < _output_buffer.get_length(); index++) { - // a constant velocity correction is applied - _output_buffer[index].vel += vel_correction; - - // a constant position correction is applied - _output_buffer[index].pos += pos_correction; - } -} diff --git a/src/modules/ekf2/EKF/output_predictor.h b/src/modules/ekf2/EKF/output_predictor.h index 7ea662b964..c144c69712 100644 --- a/src/modules/ekf2/EKF/output_predictor.h +++ b/src/modules/ekf2/EKF/output_predictor.h @@ -69,7 +69,7 @@ public: const matrix::Vector3f &vel_state, const matrix::Vector3f &pos_state, const matrix::Vector3f &gyro_bias, const matrix::Vector3f &accel_bias); - void resetQuaternion(const uint64_t &time_delayed_us, const matrix::Quatf &new_quat); + void resetQuaternionTo(const uint64_t &time_delayed_us, const matrix::Quatf &new_quat); void resetHorizontalVelocityTo(const uint64_t &time_delayed_us, const matrix::Vector2f &new_horz_vel); void resetVerticalVelocityTo(const uint64_t &time_delayed_us, const float new_vert_vel); @@ -77,6 +77,12 @@ public: void resetHorizontalPositionTo(const uint64_t &time_delayed_us, const matrix::Vector2f &new_horz_pos); void resetVerticalPositionTo(const uint64_t &time_delayed_us, const float new_vert_pos); + void resetQuaternion() { _reset_quaternion = true; } + void resetHorizontalVelocity() { _reset_velocity_xy = true; } + void resetVerticalVelocity() { _reset_velocity_z = true; } + void resetHorizontalPosition() { _reset_position_xy = true; } + void resetVerticalPosition() { _reset_position_z = true; } + void print_status(); bool allocate(uint8_t size) @@ -136,13 +142,6 @@ private: */ void applyCorrectionToVerticalOutputBuffer(float vert_vel_correction); - /* - * Calculate corrections to be applied to vel and pos output state history. - * The vel and pos state history are corrected individually so they track the EKF states at - * the fusion time horizon. This option provides the most accurate tracking of EKF states. - */ - void applyCorrectionToOutputBuffer(const matrix::Vector3f &vel_correction, const matrix::Vector3f &pos_correction); - // return the square of two floating point numbers - used in auto coded sections static constexpr float sq(float var) { return var * var; } @@ -183,7 +182,6 @@ private: float _dt_update_states_avg{0.005f}; // average imu update period in s float _dt_correct_states_avg{0.010f}; // average update rate of the ekf in s - uint64_t _time_last_update_states_us{0}; ///< last time the output states were updated (uSec) uint64_t _time_last_correct_states_us{0}; ///< last time the output states were updated (uSec) // Output Predictor @@ -197,6 +195,12 @@ private: matrix::Vector3f _vel_err_integ{}; ///< integral of velocity tracking error (m) matrix::Vector3f _pos_err_integ{}; ///< integral of position tracking error (m.s) + bool _reset_quaternion{true}; + bool _reset_velocity_xy{true}; + bool _reset_velocity_z{true}; + bool _reset_position_xy{true}; + bool _reset_position_z{true}; + matrix::Vector3f _output_tracking_error{}; ///< contains the magnitude of the angle, velocity and position track errors (rad, m/s, m) bool _output_filter_aligned{false}; diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 4eb0009a85..3521572074 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -758,12 +758,19 @@ void EKF2::Run() perf_set_elapsed(_ecl_ekf_update_full_perf, hrt_elapsed_time(&ekf_update_start)); if (_ekf.output_predictor().aligned()) { - // publish output predictor output + // publish output predictor PublishLocalPosition(now); PublishOdometry(now, imu_sample_new); PublishGlobalPosition(now); } + // update sensor calibration (in-run bias) before publishing sensor bias + UpdateAccelCalibration(now); + UpdateGyroCalibration(now); +#if defined(CONFIG_EKF2_MAGNETOMETER) + UpdateMagCalibration(now); +#endif // CONFIG_EKF2_MAGNETOMETER + // publish other state output used by the system not dependent on output predictor PublishSensorBias(now); @@ -773,13 +780,15 @@ void EKF2::Run() // publish status/logging messages PublishEventFlags(now); + PublishStatus(now); + PublishStatusFlags(now); + + // verbose logging + PublishAidSourceStatus(now); PublishInnovations(now); PublishInnovationTestRatios(now); PublishInnovationVariances(now); PublishStates(now); - PublishStatus(now); - PublishStatusFlags(now); - PublishAidSourceStatus(now); #if defined(CONFIG_EKF2_BAROMETER) PublishBaroBias(now); @@ -803,12 +812,6 @@ void EKF2::Run() PublishOpticalFlowVel(now); #endif // CONFIG_EKF2_OPTICAL_FLOW - UpdateAccelCalibration(now); - UpdateGyroCalibration(now); -#if defined(CONFIG_EKF2_MAGNETOMETER) - UpdateMagCalibration(now); -#endif // CONFIG_EKF2_MAGNETOMETER - } else { // ekf no update perf_set_elapsed(_ecl_ekf_update_perf, hrt_elapsed_time(&ekf_update_start));