diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index e994c09c4e..ec66a5470a 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -47,6 +47,7 @@ void Ekf::controlFusionModes() { // Store the status to enable change detection _control_status_prev.value = _control_status.value; + _state_reset_count_prev = _state_reset_status.reset_count; if (_system_flag_buffer) { systemFlagUpdate system_flags_delayed; diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 7534b695cb..f192069d92 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -335,43 +335,43 @@ public: const auto &state_reset_status() const { return _state_reset_status; } // return the amount the local vertical position changed in the last reset and the number of reset events - uint8_t get_posD_reset_count() const { return _state_reset_status.posD_counter; } + uint8_t get_posD_reset_count() const { return _state_reset_status.reset_count.posD; } void get_posD_reset(float *delta, uint8_t *counter) const { *delta = _state_reset_status.posD_change; - *counter = _state_reset_status.posD_counter; + *counter = _state_reset_status.reset_count.posD; } // return the amount the local vertical velocity changed in the last reset and the number of reset events - uint8_t get_velD_reset_count() const { return _state_reset_status.velD_counter; } + uint8_t get_velD_reset_count() const { return _state_reset_status.reset_count.velD; } void get_velD_reset(float *delta, uint8_t *counter) const { *delta = _state_reset_status.velD_change; - *counter = _state_reset_status.velD_counter; + *counter = _state_reset_status.reset_count.velD; } // return the amount the local horizontal position changed in the last reset and the number of reset events - uint8_t get_posNE_reset_count() const { return _state_reset_status.posNE_counter; } + uint8_t get_posNE_reset_count() const { return _state_reset_status.reset_count.posNE; } void get_posNE_reset(float delta[2], uint8_t *counter) const { _state_reset_status.posNE_change.copyTo(delta); - *counter = _state_reset_status.posNE_counter; + *counter = _state_reset_status.reset_count.posNE; } // return the amount the local horizontal velocity changed in the last reset and the number of reset events - uint8_t get_velNE_reset_count() const { return _state_reset_status.velNE_counter; } + uint8_t get_velNE_reset_count() const { return _state_reset_status.reset_count.velNE; } void get_velNE_reset(float delta[2], uint8_t *counter) const { _state_reset_status.velNE_change.copyTo(delta); - *counter = _state_reset_status.velNE_counter; + *counter = _state_reset_status.reset_count.velNE; } // return the amount the quaternion has changed in the last reset and the number of reset events - uint8_t get_quat_reset_count() const { return _state_reset_status.quat_counter; } + uint8_t get_quat_reset_count() const { return _state_reset_status.reset_count.quat; } void get_quat_reset(float delta_quat[4], uint8_t *counter) const { _state_reset_status.quat_change.copyTo(delta_quat); - *counter = _state_reset_status.quat_counter; + *counter = _state_reset_status.reset_count.quat; } // get EKF innovation consistency check status information comprising of: @@ -451,18 +451,27 @@ private: void updateHorizontalDeadReckoningstatus(); void updateVerticalDeadReckoningStatus(); - struct { - uint8_t velNE_counter; ///< number of horizontal position reset events (allow to wrap if count exceeds 255) - uint8_t velD_counter; ///< number of vertical velocity reset events (allow to wrap if count exceeds 255) - uint8_t posNE_counter; ///< number of horizontal position reset events (allow to wrap if count exceeds 255) - uint8_t posD_counter; ///< number of vertical position reset events (allow to wrap if count exceeds 255) - uint8_t quat_counter; ///< number of quaternion reset events (allow to wrap if count exceeds 255) + struct StateResetCounts + { + uint8_t velNE{0}; ///< number of horizontal position reset events (allow to wrap if count exceeds 255) + uint8_t velD{0}; ///< number of vertical velocity reset events (allow to wrap if count exceeds 255) + uint8_t posNE{0}; ///< number of horizontal position reset events (allow to wrap if count exceeds 255) + uint8_t posD{0}; ///< number of vertical position reset events (allow to wrap if count exceeds 255) + uint8_t quat{0}; ///< number of quaternion reset events (allow to wrap if count exceeds 255) + }; + + struct StateResets { Vector2f velNE_change; ///< North East velocity change due to last reset (m) float velD_change; ///< Down velocity change due to last reset (m/sec) Vector2f posNE_change; ///< North, East position change due to last reset (m) float posD_change; ///< Down position change due to last reset (m) Quatf quat_change; ///< quaternion delta due to last reset - multiply pre-reset quaternion by this to get post-reset quaternion - } _state_reset_status{}; ///< reset event monitoring structure containing velocity, position, height and yaw reset information + + StateResetCounts reset_count{}; + }; + + StateResets _state_reset_status{}; ///< reset event monitoring structure containing velocity, position, height and yaw reset information + StateResetCounts _state_reset_count_prev{}; Vector3f _ang_rate_delayed_raw{}; ///< uncorrected angular rate vector at fusion time horizon (rad/sec) diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 80f28d9f0a..3dd48d438c 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -77,8 +77,16 @@ void Ekf::resetHorizontalVelocityTo(const Vector2f &new_horz_vel, const Vector2f _output_new.vel.xy() += delta_horz_vel; - _state_reset_status.velNE_change = delta_horz_vel; - _state_reset_status.velNE_counter++; + // record the state change + if (_state_reset_status.reset_count.velNE == _state_reset_count_prev.velNE) { + _state_reset_status.velNE_change = delta_horz_vel; + + } else { + // there's already a reset this update, accumulate total delta + _state_reset_status.velNE_change += delta_horz_vel; + } + + _state_reset_status.reset_count.velNE++; // Reset the timout timer _time_last_hor_vel_fuse = _imu_sample_delayed.time_us; @@ -101,8 +109,16 @@ void Ekf::resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var) _output_new.vel(2) += delta_vert_vel; _output_vert_new.vert_vel += delta_vert_vel; - _state_reset_status.velD_change = delta_vert_vel; - _state_reset_status.velD_counter++; + // record the state change + if (_state_reset_status.reset_count.velD == _state_reset_count_prev.velD) { + _state_reset_status.velD_change = delta_vert_vel; + + } else { + // there's already a reset this update, accumulate total delta + _state_reset_status.velD_change += delta_vert_vel; + } + + _state_reset_status.reset_count.velD++; // Reset the timout timer _time_last_ver_vel_fuse = _imu_sample_delayed.time_us; @@ -151,8 +167,16 @@ void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f _output_new.pos.xy() += delta_horz_pos; - _state_reset_status.posNE_change = delta_horz_pos; - _state_reset_status.posNE_counter++; + // record the state change + if (_state_reset_status.reset_count.posNE == _state_reset_count_prev.posNE) { + _state_reset_status.posNE_change = delta_horz_pos; + + } else { + // there's already a reset this update, accumulate total delta + _state_reset_status.posNE_change += delta_horz_pos; + } + + _state_reset_status.reset_count.posNE++; // Reset the timout timer _time_last_hor_pos_fuse = _imu_sample_delayed.time_us; @@ -180,27 +204,36 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v P.uncorrelateCovarianceSetVariance<1>(9, math::max(sq(0.01f), new_vert_pos_var)); } - // store the reset amount and time to be published - _state_reset_status.posD_change = new_vert_pos - old_vert_pos; - _state_reset_status.posD_counter++; + 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_new.pos(2) += _state_reset_status.posD_change; + _output_new.pos(2) += delta_z; // 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) += _state_reset_status.posD_change; - _output_vert_buffer[i].vert_vel_integ += _state_reset_status.posD_change; + _output_buffer[i].pos(2) += delta_z; + _output_vert_buffer[i].vert_vel_integ += delta_z; } // add the reset amount to the output observer vertical position state _output_vert_new.vert_vel_integ = _state.pos(2); - _baro_b_est.setBias(_baro_b_est.getBias() + _state_reset_status.posD_change); - _ev_hgt_b_est.setBias(_ev_hgt_b_est.getBias() - _state_reset_status.posD_change); - _gps_hgt_b_est.setBias(_gps_hgt_b_est.getBias() + _state_reset_status.posD_change); - _rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + _state_reset_status.posD_change); + // record the state change + if (_state_reset_status.reset_count.posD == _state_reset_count_prev.posD) { + _state_reset_status.posD_change = delta_z; + + } else { + // there's already a reset this update, accumulate total delta + _state_reset_status.posD_change += delta_z; + } + + _state_reset_status.reset_count.posD++; + + _baro_b_est.setBias(_baro_b_est.getBias() + delta_z); + _ev_hgt_b_est.setBias(_ev_hgt_b_est.getBias() - delta_z); + _gps_hgt_b_est.setBias(_gps_hgt_b_est.getBias() + delta_z); + _rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + delta_z); // Reset the timout timer _time_last_hgt_fuse = _imu_sample_delayed.time_us; @@ -1384,9 +1417,6 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance) _state.quat_nominal = quat_after_reset; uncorrelateQuatFromOtherStates(); - // record the state change - _state_reset_status.quat_change = q_error; - // update the yaw angle variance if (yaw_variance > FLT_EPSILON) { increaseQuatYawErrVariance(yaw_variance); @@ -1394,17 +1424,26 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance) // 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 = _state_reset_status.quat_change * _output_buffer[i].quat_nominal; + _output_buffer[i].quat_nominal = q_error * _output_buffer[i].quat_nominal; } // apply the change in attitude quaternion to our newest quaternion estimate // which was already taken out from the output buffer - _output_new.quat_nominal = _state_reset_status.quat_change * _output_new.quat_nominal; + _output_new.quat_nominal = q_error * _output_new.quat_nominal; + + // record the state change + if (_state_reset_status.reset_count.quat == _state_reset_count_prev.quat) { + _state_reset_status.quat_change = q_error; + + } else { + // there's already a reset this update, accumulate total delta + _state_reset_status.quat_change = q_error * _state_reset_status.quat_change; + _state_reset_status.quat_change.normalize(); + } + + _state_reset_status.reset_count.quat++; _last_static_yaw = NAN; - - // capture the reset event - _state_reset_status.quat_counter++; } // Resets the main Nav EKf yaw to the estimator from the EKF-GSF yaw estimator diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 8bac7f06c1..26eabc6124 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1340,11 +1340,11 @@ void EKF2::PublishStatus(const hrt_abstime ×tamp) _ekf.get_ekf_soln_status(&status.solution_status_flags); // reset counters - status.reset_count_vel_ne = _ekf.state_reset_status().velNE_counter; - status.reset_count_vel_d = _ekf.state_reset_status().velD_counter; - status.reset_count_pos_ne = _ekf.state_reset_status().posNE_counter; - status.reset_count_pod_d = _ekf.state_reset_status().posD_counter; - status.reset_count_quat = _ekf.state_reset_status().quat_counter; + status.reset_count_vel_ne = _ekf.state_reset_status().reset_count.velNE; + status.reset_count_vel_d = _ekf.state_reset_status().reset_count.velD; + status.reset_count_pos_ne = _ekf.state_reset_status().reset_count.posNE; + status.reset_count_pod_d = _ekf.state_reset_status().reset_count.posD; + status.reset_count_quat = _ekf.state_reset_status().reset_count.quat; status.time_slip = _last_time_slip_us * 1e-6f;