From 303e1e5adb68389e027f67bc03a0dc83798cc1c8 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 19 Sep 2023 13:18:32 -0400 Subject: [PATCH] ekf2: output predictor updates - merge OutputSample and outputVert - always check timestamps for any resets or corrections - output predictor no longer needs to be explicitly aligned - ekf2 only publish latest estimates if output predictor aligned - output predictor init immediately with EKF filter initialisation --- .../python_src/px4_it/mavros/mission_test.py | 2 +- src/modules/ekf2/EKF/ekf.cpp | 35 +- src/modules/ekf2/EKF/ekf_helper.cpp | 10 +- src/modules/ekf2/EKF/output_predictor.cpp | 300 ++++++++++-------- src/modules/ekf2/EKF/output_predictor.h | 60 ++-- src/modules/ekf2/EKF2.cpp | 15 +- 6 files changed, 233 insertions(+), 189 deletions(-) diff --git a/integrationtests/python_src/px4_it/mavros/mission_test.py b/integrationtests/python_src/px4_it/mavros/mission_test.py index 8d7f6e6478..a9dccf7284 100755 --- a/integrationtests/python_src/px4_it/mavros/mission_test.py +++ b/integrationtests/python_src/px4_it/mavros/mission_test.py @@ -308,7 +308,7 @@ class MavrosMissionTest(MavrosTestCommon): self.assertTrue(res['pitch_error_std'] < 5.0, str(res)) # TODO: fix by excluding initial heading init and reset preflight - self.assertTrue(res['yaw_error_std'] < 10.0, str(res)) + self.assertTrue(res['yaw_error_std'] < 13.0, str(res)) if __name__ == '__main__': diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index f1f5204e98..a714481b0e 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -179,11 +179,11 @@ void Ekf::reset() bool Ekf::update() { - if (!_filter_initialised) { - _filter_initialised = initialiseFilter(); + bool updated = false; - if (!_filter_initialised) { - return false; + if (!_filter_initialised) { + if (initialiseFilter()) { + _filter_initialised = true; } } @@ -203,24 +203,28 @@ bool Ekf::update() updateIMUBiasInhibit(imu_sample_delayed); - // perform state and covariance prediction for the main filter - predictCovariance(imu_sample_delayed); - predictState(imu_sample_delayed); + if (_filter_initialised) { + // perform state and covariance prediction for the main filter + predictCovariance(imu_sample_delayed); + predictState(imu_sample_delayed); - // control fusion of observation data - controlFusionModes(imu_sample_delayed); + // control fusion of observation data + controlFusionModes(imu_sample_delayed); #if defined(CONFIG_EKF2_TERRAIN) - // run a separate filter for terrain estimation - runTerrainEstimator(imu_sample_delayed); + // run a separate filter for terrain estimation + runTerrainEstimator(imu_sample_delayed); #endif // CONFIG_EKF2_TERRAIN - _output_predictor.correctOutputStates(imu_sample_delayed.time_us, _state.quat_nominal, _state.vel, _state.pos, _state.gyro_bias, _state.accel_bias); + updated = true; + } - return true; + // output predictor corrections: always call so that once the filter is initialized the output predictor + // will already have timing information and prepopulated output buffer ready for immediate initial alignment + _output_predictor.correctOutputStates(imu_sample_delayed.time_us, _state.quat_nominal, _state.vel, _state.pos, _state.gyro_bias, _state.accel_bias); } - return false; + return updated; } bool Ekf::initialiseFilter() @@ -255,9 +259,6 @@ bool Ekf::initialiseFilter() initHagl(); #endif // CONFIG_EKF2_TERRAIN - // reset the output predictor state history to match the EKF initial values - _output_predictor.alignOutputFilter(_state.quat_nominal, _state.vel, _state.pos); - return true; } diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index e75d9c01cb..b2f976510c 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -74,7 +74,7 @@ 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(delta_horz_vel); + _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) { @@ -100,7 +100,7 @@ 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(delta_vert_vel); + _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) { @@ -140,7 +140,7 @@ 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(delta_horz_pos); + _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) { @@ -187,7 +187,7 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v // 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(new_vert_pos, delta_z); + _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) { @@ -923,7 +923,7 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance) resetQuatCov(rot_var_ned_before_reset); // add the reset amount to the output observer buffered data - _output_predictor.resetQuaternion(q_error); + _output_predictor.resetQuaternion(_time_delayed_us, _state.quat_nominal); #if defined(CONFIG_EKF2_EXTERNAL_VISION) // update EV attitude error filter diff --git a/src/modules/ekf2/EKF/output_predictor.cpp b/src/modules/ekf2/EKF/output_predictor.cpp index 32bf4fe8ad..bdeba39ec8 100644 --- a/src/modules/ekf2/EKF/output_predictor.cpp +++ b/src/modules/ekf2/EKF/output_predictor.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2022 PX4. All rights reserved. + * Copyright (c) 2022-2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -48,126 +48,128 @@ void OutputPredictor::print_status() printf("output buffer: %d/%d (%d Bytes)\n", _output_buffer.entries(), _output_buffer.get_length(), _output_buffer.get_total_size()); - - printf("output vert buffer: %d/%d (%d Bytes)\n", _output_vert_buffer.entries(), _output_vert_buffer.get_length(), - _output_vert_buffer.get_total_size()); -} - -void OutputPredictor::alignOutputFilter(const Quatf &quat_state, const Vector3f &vel_state, const Vector3f &pos_state) -{ - 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{quat_state * 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 = vel_state - output_delayed.vel; - const Vector3f pos_delta = pos_state - 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++) { - _output_buffer[i].quat_nominal = q_delta * _output_buffer[i].quat_nominal; - _output_buffer[i].quat_nominal.normalize(); - _output_buffer[i].vel += vel_delta; - _output_buffer[i].pos += pos_delta; - } - - _output_new = _output_buffer.get_newest(); } void OutputPredictor::reset() { - // TODO: who resets the output buffer content? - _output_new = {}; - _output_vert_new = {}; + _output_buffer.reset(); - _accel_bias.setZero(); - _gyro_bias.setZero(); + _accel_bias.zero(); + _gyro_bias.zero(); _time_last_update_states_us = 0; _time_last_correct_states_us = 0; + _output_new = {}; + _R_to_earth_now.setIdentity(); - _vel_imu_rel_body_ned.setZero(); - _vel_deriv.setZero(); + _vel_imu_rel_body_ned.zero(); + _vel_deriv.zero(); - _delta_angle_corr.setZero(); + _delta_angle_corr.zero(); + _vel_err_integ.zero(); + _pos_err_integ.zero(); - _vel_err_integ.setZero(); - _pos_err_integ.setZero(); + _output_tracking_error.zero(); - _output_tracking_error.setZero(); - - for (uint8_t index = 0; index < _output_buffer.get_length(); index++) { - _output_buffer[index] = {}; - } - - for (uint8_t index = 0; index < _output_vert_buffer.get_length(); index++) { - _output_vert_buffer[index] = {}; - } + _output_filter_aligned = false; } -void OutputPredictor::resetQuaternion(const Quatf &quat_change) +void OutputPredictor::resetQuaternion(const uint64_t &time_delayed_us, const Quatf &new_quat) { - // add the reset amount to the output observer buffered data - for (uint8_t i = 0; i < _output_buffer.get_length(); i++) { - _output_buffer[i].quat_nominal = quat_change * _output_buffer[i].quat_nominal; - } + // find the output observer state corresponding to the reset time + const outputSample &delayed_sample = findOutputSample(time_delayed_us); - // apply the change in attitude quaternion to our newest quaternion estimate - // which was already taken out from the output buffer - _output_new.quat_nominal = quat_change * _output_new.quat_nominal; -} - -void OutputPredictor::resetHorizontalVelocityTo(const Vector2f &delta_horz_vel) -{ - for (uint8_t index = 0; index < _output_buffer.get_length(); index++) { - _output_buffer[index].vel.xy() += delta_horz_vel; - } - - _output_new.vel.xy() += delta_horz_vel; -} - -void OutputPredictor::resetVerticalVelocityTo(float delta_vert_vel) -{ - for (uint8_t index = 0; index < _output_buffer.get_length(); index++) { - _output_buffer[index].vel(2) += delta_vert_vel; - _output_vert_buffer[index].vert_vel += delta_vert_vel; - } - - _output_new.vel(2) += delta_vert_vel; - _output_vert_new.vert_vel += delta_vert_vel; -} - -void OutputPredictor::resetHorizontalPositionTo(const Vector2f &delta_horz_pos) -{ - for (uint8_t index = 0; index < _output_buffer.get_length(); index++) { - _output_buffer[index].pos.xy() += delta_horz_pos; - } - - _output_new.pos.xy() += delta_horz_pos; -} - -void OutputPredictor::resetVerticalPositionTo(const float new_vert_pos, const float vert_pos_change) -{ - // 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_new.pos(2) += vert_pos_change; + const Quatf quat_change = (new_quat * delayed_sample.quat_nominal.inversed()).normalized(); // add the reset amount to the output observer buffered data for (uint8_t i = 0; i < _output_buffer.get_length(); i++) { - _output_buffer[i].pos(2) += vert_pos_change; - _output_vert_buffer[i].vert_vel_integ += vert_pos_change; + _output_buffer[i].quat_nominal = (quat_change * _output_buffer[i].quat_nominal).normalized(); } - // add the reset amount to the output observer vertical position state - _output_vert_new.vert_vel_integ = new_vert_pos; + // apply change to latest + _output_new.quat_nominal = (quat_change * _output_new.quat_nominal).normalized(); + _R_to_earth_now = Dcmf(_output_new.quat_nominal); } -void OutputPredictor::calculateOutputStates(const uint64_t time_us, const Vector3f &delta_angle, +void OutputPredictor::resetHorizontalVelocityTo(const uint64_t &time_delayed_us, const Vector2f &new_horz_vel) +{ + // find the output observer state corresponding to the reset time + const outputSample &delayed_sample = findOutputSample(time_delayed_us); + + const Vector2f delta_vxy = new_horz_vel - delayed_sample.vel.xy(); // horizontal velocity + + // add the reset amount to the output observer buffered data + for (uint8_t i = 0; i < _output_buffer.get_length(); i++) { + _output_buffer[i].vel.xy() += delta_vxy; + } + + // apply change to latest velocity + _output_new.vel.xy() += delta_vxy; +} + +void OutputPredictor::resetVerticalVelocityTo(const uint64_t &time_delayed_us, const float new_vert_vel) +{ + // find the output observer state corresponding to the reset time + const outputSample &delayed_sample = findOutputSample(time_delayed_us); + + 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) + + // add the reset amount to the output observer buffered data + for (uint8_t i = 0; i < _output_buffer.get_length(); i++) { + _output_buffer[i].vel(2) += delta_vz; + _output_buffer[i].vert_vel_alt += delta_vz_alt; + } + + // apply change to latest + _output_new.vel(2) += delta_vz; + _output_new.vert_vel_alt += delta_vz_alt; +} + +void OutputPredictor::resetHorizontalPositionTo(const uint64_t &time_delayed_us, const Vector2f &new_horz_pos) +{ + // find the output observer state corresponding to the reset time + const outputSample &delayed_sample = findOutputSample(time_delayed_us); + + const Vector2f delta_xy = new_horz_pos - delayed_sample.pos.xy(); // horizontal position + + // add the reset amount to the output observer buffered data + for (uint8_t i = 0; i < _output_buffer.get_length(); i++) { + _output_buffer[i].pos.xy() += delta_xy; + } + + // apply change to latest + _output_new.pos.xy() += delta_xy; +} + +void OutputPredictor::resetVerticalPositionTo(const uint64_t &time_delayed_us, const float new_vert_pos) +{ + // find the output observer state corresponding to the reset time + const outputSample &delayed_sample = findOutputSample(time_delayed_us); + + 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) + + // add the reset amount to the output observer buffered data + for (uint8_t i = 0; i < _output_buffer.get_length(); i++) { + _output_buffer[i].pos(2) += delta_z; + _output_buffer[i].vert_vel_integ += delta_z_alt; + } + + // apply change to latest + _output_new.pos(2) += delta_z; + _output_new.vert_vel_integ += delta_z_alt; +} + +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) { + 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); @@ -185,17 +187,11 @@ void OutputPredictor::calculateOutputStates(const uint64_t time_us, const Vector const Vector3f delta_velocity_corrected(delta_velocity - delta_vel_bias_scaled); _output_new.time_us = time_us; - _output_vert_new.time_us = time_us; const Quatf dq(AxisAnglef{delta_angle_corrected}); // rotate the previous INS quaternion by the delta quaternions - _output_new.quat_nominal = _output_new.quat_nominal * dq; - - // the quaternions must always be normalised after modification - _output_new.quat_nominal.normalize(); - - // calculate the rotation matrix from body to earth frame + _output_new.quat_nominal = (_output_new.quat_nominal * dq).normalized(); _R_to_earth_now = Dcmf(_output_new.quat_nominal); // rotate the delta velocity to earth frame @@ -215,16 +211,16 @@ void OutputPredictor::calculateOutputStates(const uint64_t time_us, const Vector // increment the INS velocity states by the measurement plus corrections // do the same for vertical state used by alternative correction algorithm _output_new.vel += delta_vel_earth; - _output_vert_new.vert_vel += delta_vel_earth(2); + _output_new.vert_vel_alt += delta_vel_earth(2); // use trapezoidal integration to calculate the INS position states // do the same for vertical state used by alternative correction algorithm const Vector3f delta_pos_NED = (_output_new.vel + vel_last) * (delta_velocity_dt * 0.5f); _output_new.pos += delta_pos_NED; - _output_vert_new.vert_vel_integ += delta_pos_NED(2); + _output_new.vert_vel_integ += delta_pos_NED(2); // accumulate the time for each update - _output_vert_new.dt += delta_velocity_dt; + _output_new.dt += delta_velocity_dt; // correct velocity for IMU offset if (delta_angle_dt > 0.001f) { @@ -242,33 +238,60 @@ void OutputPredictor::calculateOutputStates(const uint64_t time_us, const Vector const Vector3f unbiased_delta_angle = delta_angle - delta_angle_bias_scaled; const float spin_del_ang_D = unbiased_delta_angle.dot(Vector3f(_R_to_earth_now.row(2))); _unaided_yaw = matrix::wrap_pi(_unaided_yaw + spin_del_ang_D); + + return true; } -void OutputPredictor::correctOutputStates(const uint64_t time_delayed_us, - const Quatf &quat_state, const Vector3f &vel_state, const Vector3f &pos_state, const matrix::Vector3f &gyro_bias, const matrix::Vector3f &accel_bias) +bool OutputPredictor::correctOutputStates(const uint64_t &time_delayed_us, + const Quatf &quat_state, const Vector3f &vel_state, const Vector3f &pos_state, + const matrix::Vector3f &gyro_bias, const matrix::Vector3f &accel_bias) { + const uint64_t time_latest_us = _output_new.time_us; + + if (time_latest_us < time_delayed_us) { + return false; + } + + // store the INS states in a ring buffer with the same length and time coordinates as the IMU data buffer + if ((_output_new.time_us != 0) && (_output_new.dt > 0.f)) { + _output_buffer.push(_output_new); + _output_new.dt = 0.f; // reset time delta to zero for the next accumulation of full rate IMU data + } + + // store IMU bias for calculateOutputStates + _gyro_bias = gyro_bias; + _accel_bias = accel_bias; + // calculate an average filter update time - if (_time_last_correct_states_us != 0) { + if ((_time_last_correct_states_us != 0) && (time_delayed_us > _time_last_correct_states_us)) { const float dt = math::constrain((time_delayed_us - _time_last_correct_states_us) * 1e-6f, 0.0001f, 0.03f); _dt_correct_states_avg = 0.8f * _dt_correct_states_avg + 0.2f * dt; } _time_last_correct_states_us = time_delayed_us; - // store IMU bias for calculateOutputStates - _gyro_bias = gyro_bias; - _accel_bias = accel_bias; + // get the output buffer state data at the EKF fusion time horizon + const outputSample &output_delayed = findOutputSample(time_delayed_us); + const bool delayed_index_found = (output_delayed.time_us == time_delayed_us); - // store the INS states in a ring buffer with the same length and time coordinates as the IMU data buffer - _output_buffer.push(_output_new); - _output_vert_buffer.push(_output_vert_new); + if (!_output_filter_aligned) { + if (delayed_index_found) { + resetQuaternion(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)); - // get the oldest INS state data from the ring buffer - // this data will be at the EKF fusion time horizon - // 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(); + _delta_angle_corr.zero(); + _vel_err_integ.zero(); + _pos_err_integ.zero(); + _output_tracking_error.zero(); + + _output_filter_aligned = true; + } + + 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()); @@ -280,7 +303,6 @@ void OutputPredictor::correctOutputStates(const uint64_t time_delayed_us, // 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 uint64_t time_latest_us = _time_last_update_states_us; 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; @@ -301,8 +323,8 @@ void OutputPredictor::correctOutputStates(const uint64_t time_delayed_us, const float pos_gain = _dt_correct_states_avg / math::constrain(_pos_tau, _dt_correct_states_avg, 10.f); // calculate down velocity and position tracking errors - const float vert_vel_err = (vel_state(2) - output_vert_delayed.vert_vel); - const float vert_vel_integ_err = (pos_state(2) - output_vert_delayed.vert_vel_integ); + 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); // calculate a velocity correction that will be applied to the output state history // using a PD feedback tuned to a 5% overshoot @@ -326,40 +348,43 @@ void OutputPredictor::correctOutputStates(const uint64_t time_delayed_us, 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 + // reset time delta to zero for the next accumulation of full rate IMU data + _output_new = _output_buffer.get_newest(); + _output_new.dt = 0.0f; + + return true; } void OutputPredictor::applyCorrectionToVerticalOutputBuffer(float vert_vel_correction) { // loop through the vertical output filter state history starting at the oldest and apply the corrections to the // vert_vel states and propagate vert_vel_integ forward using the corrected vert_vel - uint8_t index = _output_vert_buffer.get_oldest_index(); + uint8_t index = _output_buffer.get_oldest_index(); - const uint8_t size = _output_vert_buffer.get_length(); + const uint8_t size = _output_buffer.get_length(); for (uint8_t counter = 0; counter < (size - 1); counter++) { const uint8_t index_next = (index + 1) % size; - outputVert ¤t_state = _output_vert_buffer[index]; - outputVert &next_state = _output_vert_buffer[index_next]; + outputSample ¤t_state = _output_buffer[index]; + outputSample &next_state = _output_buffer[index_next]; // correct the velocity if (counter == 0) { - current_state.vert_vel += vert_vel_correction; + current_state.vert_vel_alt += vert_vel_correction; } - next_state.vert_vel += vert_vel_correction; + next_state.vert_vel_alt += vert_vel_correction; // position is propagated forward using the corrected velocity and a trapezoidal integrator - next_state.vert_vel_integ = current_state.vert_vel_integ + (current_state.vert_vel + next_state.vert_vel) * 0.5f * next_state.dt; + next_state.vert_vel_integ = current_state.vert_vel_integ + + (current_state.vert_vel_alt + next_state.vert_vel_alt) * 0.5f * next_state.dt; // advance the index index = (index + 1) % size; } - - // update output state to corrected values - _output_vert_new = _output_vert_buffer.get_newest(); - - // reset time delta to zero for the next accumulation of full rate IMU data - _output_vert_new.dt = 0.0f; } void OutputPredictor::applyCorrectionToOutputBuffer(const Vector3f &vel_correction, const Vector3f &pos_correction) @@ -372,7 +397,4 @@ void OutputPredictor::applyCorrectionToOutputBuffer(const Vector3f &vel_correcti // a constant position correction is applied _output_buffer[index].pos += pos_correction; } - - // update output state to corrected values - _output_new = _output_buffer.get_newest(); } diff --git a/src/modules/ekf2/EKF/output_predictor.h b/src/modules/ekf2/EKF/output_predictor.h index ee5cc0f710..7ea662b964 100644 --- a/src/modules/ekf2/EKF/output_predictor.h +++ b/src/modules/ekf2/EKF/output_predictor.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2022 PX4. All rights reserved. + * Copyright (c) 2022-2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -51,9 +51,6 @@ public: ~OutputPredictor() = default; - // modify output filter to match the the EKF state at the fusion time horizon - void alignOutputFilter(const matrix::Quatf &quat_state, const matrix::Vector3f &vel_state, - const matrix::Vector3f &pos_state); /* * Implement a strapdown INS algorithm using the latest IMU data at the current time horizon. * Buffer the INS states and calculate the difference with the EKF states at the delayed fusion time horizon. @@ -64,25 +61,27 @@ public: * “Recursive Attitude Estimation in the Presence of Multi-rate and Multi-delay Vector Measurements” * A Khosravian, J Trumpf, R Mahony, T Hamel, Australian National University */ - void calculateOutputStates(const uint64_t time_us, const matrix::Vector3f &delta_angle, const float delta_angle_dt, + bool calculateOutputStates(const uint64_t time_us, + const matrix::Vector3f &delta_angle, const float delta_angle_dt, const matrix::Vector3f &delta_velocity, const float delta_velocity_dt); - void correctOutputStates(const uint64_t time_delayed_us, - const matrix::Quatf &quat_state, const matrix::Vector3f &vel_state, const matrix::Vector3f &pos_state, const matrix::Vector3f &gyro_bias, const matrix::Vector3f &accel_bias); + bool correctOutputStates(const uint64_t &time_delayed_us, const matrix::Quatf &quat_state, + const matrix::Vector3f &vel_state, const matrix::Vector3f &pos_state, + const matrix::Vector3f &gyro_bias, const matrix::Vector3f &accel_bias); - void resetQuaternion(const matrix::Quatf &quat_change); + void resetQuaternion(const uint64_t &time_delayed_us, const matrix::Quatf &new_quat); - void resetHorizontalVelocityTo(const matrix::Vector2f &delta_horz_vel); - void resetVerticalVelocityTo(float delta_vert_vel); + 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); - void resetHorizontalPositionTo(const matrix::Vector2f &delta_horz_pos); - void resetVerticalPositionTo(const float new_vert_pos, const float vert_pos_change); + 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 print_status(); bool allocate(uint8_t size) { - if (_output_buffer.allocate(size) && _output_vert_buffer.allocate(size)) { + if (_output_buffer.allocate(size)) { reset(); return true; } @@ -104,7 +103,7 @@ public: const matrix::Vector3f &getVelocityDerivative() const { return _vel_deriv; } // get the derivative of the vertical position of the body frame origin in local NED earth frame - float getVerticalPositionDerivative() const { return _output_vert_new.vert_vel - _vel_imu_rel_body_ned(2); } + float getVerticalPositionDerivative() const { return _output_new.vert_vel_alt - _vel_imu_rel_body_ned(2); } // get the position of the body frame origin in local earth frame matrix::Vector3f getPosition() const @@ -119,6 +118,8 @@ public: // error magnitudes (rad), (m/sec), (m) const matrix::Vector3f &getOutputTrackingError() const { return _output_tracking_error; } + bool aligned() const { return _output_filter_aligned; } + void set_imu_offset(const matrix::Vector3f &offset) { _imu_pos_body = offset; } void set_pos_correction_tc(const float tau) { _pos_tau = tau; } void set_vel_correction_tc(const float tau) { _vel_tau = tau; } @@ -150,17 +151,31 @@ private: matrix::Quatf quat_nominal{1.f, 0.f, 0.f, 0.f}; ///< nominal quaternion describing vehicle attitude matrix::Vector3f vel{0.f, 0.f, 0.f}; ///< NED velocity estimate in earth frame (m/sec) matrix::Vector3f pos{0.f, 0.f, 0.f}; ///< NED position estimate in earth frame (m/sec) - }; - struct outputVert { - uint64_t time_us{0}; ///< timestamp of the measurement (uSec) - float vert_vel{0.f}; ///< Vertical velocity calculated using alternative algorithm (m/sec) - float vert_vel_integ{0.f}; ///< Integral of vertical velocity (m) - float dt{0.f}; ///< delta time (sec) + float vert_vel_alt{0.f}; ///< Vertical velocity calculated using alternative algorithm (m/sec) + float vert_vel_integ{0.f}; ///< Integral of vertical velocity (m) + float dt{0.f}; ///< delta time (sec) }; RingBuffer _output_buffer{12}; - RingBuffer _output_vert_buffer{12}; + + // try to find the matching output sample, return oldest if not found + const outputSample &findOutputSample(const uint64_t &time_us) + { + uint8_t index = _output_buffer.get_oldest_index(); + const uint8_t size = _output_buffer.get_length(); + + for (uint8_t counter = 0; counter < (size - 1); counter++) { + if (_output_buffer[index].time_us == time_us) { + return _output_buffer[index]; + } + + // advance the index + index = (index + 1) % size; + } + + return _output_buffer.get_oldest(); + } matrix::Vector3f _accel_bias{}; matrix::Vector3f _gyro_bias{}; @@ -173,7 +188,6 @@ private: // Output Predictor outputSample _output_new{}; // filter output on the non-delayed time horizon - outputVert _output_vert_new{}; // vertical filter output on the non-delayed time horizon matrix::Matrix3f _R_to_earth_now{}; // rotation matrix from body to earth frame at current time matrix::Vector3f _vel_imu_rel_body_ned{}; // velocity of IMU relative to body origin in NED earth frame matrix::Vector3f _vel_deriv{}; // velocity derivative at the IMU in NED earth frame (m/s/s) @@ -185,6 +199,8 @@ private: 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}; + matrix::Vector3f _imu_pos_body{}; ///< xyz position of IMU in body frame (m) float _unaided_yaw{}; diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 72d8244148..f9e577dc06 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -757,9 +757,14 @@ void EKF2::Run() if (_ekf.update()) { perf_set_elapsed(_ecl_ekf_update_full_perf, hrt_elapsed_time(&ekf_update_start)); - PublishLocalPosition(now); - PublishOdometry(now, imu_sample_new); - PublishGlobalPosition(now); + if (_ekf.output_predictor().aligned()) { + // publish output predictor output + PublishLocalPosition(now); + PublishOdometry(now, imu_sample_new); + PublishGlobalPosition(now); + } + + // publish other state output used by the system not dependent on output predictor PublishSensorBias(now); #if defined(CONFIG_EKF2_WIND) @@ -1120,7 +1125,7 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime ×tamp) void EKF2::PublishAttitude(const hrt_abstime ×tamp) { - if (_ekf.attitude_valid()) { + if (_ekf.output_predictor().aligned()) { // generate vehicle attitude quaternion data vehicle_attitude_s att; att.timestamp_sample = timestamp; @@ -1130,7 +1135,7 @@ void EKF2::PublishAttitude(const hrt_abstime ×tamp) att.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); _attitude_pub.publish(att); - } else if (_replay_mode) { + } else if (_replay_mode) { // in replay mode we have to tell the replay module not to wait for an update // we do this by publishing an attitude with zero timestamp vehicle_attitude_s att{};