From 049c0e669601c8437b6fd215fa0c67c71b38e356 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 27 Jun 2024 12:23:00 -0400 Subject: [PATCH 1/3] ekf2: use bias corrected angular velocity - avoid unnecessarily storing _ang_rate_delayed_raw --- .../barometer/baro_height_control.cpp | 9 +++--- .../external_vision/ev_control.cpp | 11 +++---- .../external_vision/ev_height_control.cpp | 8 +++-- .../external_vision/ev_pos_control.cpp | 5 ++-- .../external_vision/ev_vel_control.cpp | 8 +++-- .../external_vision/ev_yaw_control.cpp | 5 ++-- .../ekf2/EKF/aid_sources/gnss/gps_control.cpp | 7 +++-- src/modules/ekf2/EKF/control.cpp | 2 +- src/modules/ekf2/EKF/ekf.cpp | 9 ------ src/modules/ekf2/EKF/ekf.h | 30 ++++++++++--------- src/modules/ekf2/EKF/height_control.cpp | 2 +- 11 files changed, 49 insertions(+), 47 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp index 80c51e097954..9153856009fd 100644 --- a/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp @@ -38,7 +38,7 @@ #include "ekf.h" -void Ekf::controlBaroHeightFusion() +void Ekf::controlBaroHeightFusion(const imuSample &imu_sample) { static constexpr const char *HGT_SRC_NAME = "baro"; @@ -52,7 +52,7 @@ void Ekf::controlBaroHeightFusion() if (_baro_buffer && _baro_buffer->pop_first_older_than(_time_delayed_us, &baro_sample)) { #if defined(CONFIG_EKF2_BARO_COMPENSATION) - const float measurement = compensateBaroForDynamicPressure(baro_sample.hgt); + const float measurement = compensateBaroForDynamicPressure(imu_sample, baro_sample.hgt); #else const float measurement = baro_sample.hgt; #endif @@ -195,7 +195,7 @@ void Ekf::stopBaroHgtFusion() } #if defined(CONFIG_EKF2_BARO_COMPENSATION) -float Ekf::compensateBaroForDynamicPressure(const float baro_alt_uncompensated) const +float Ekf::compensateBaroForDynamicPressure(const imuSample &imu_sample, const float baro_alt_uncompensated) const { if (_control_status.flags.wind && local_position_is_valid()) { // calculate static pressure error = Pmeas - Ptruth @@ -203,7 +203,8 @@ float Ekf::compensateBaroForDynamicPressure(const float baro_alt_uncompensated) // negative X and Y directions. Used to correct baro data for positional errors // Calculate airspeed in body frame - const Vector3f vel_imu_rel_body_ned = _R_to_earth * (_ang_rate_delayed_raw % _params.imu_pos_body); + const Vector3f angular_velocity = (imu_sample.delta_ang / imu_sample.delta_ang_dt) - _state.gyro_bias; + const Vector3f vel_imu_rel_body_ned = _R_to_earth * (angular_velocity % _params.imu_pos_body); const Vector3f velocity_earth = _state.vel - vel_imu_rel_body_ned; const Vector3f wind_velocity_earth(_state.wind_vel(0), _state.wind_vel(1), 0.0f); diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp index 78cc2bd30a11..cac1db3f3d72 100644 --- a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp @@ -38,7 +38,7 @@ #include "ekf.h" -void Ekf::controlExternalVisionFusion() +void Ekf::controlExternalVisionFusion(const imuSample &imu_sample) { _ev_pos_b_est.predict(_dt_ekf_avg); _ev_hgt_b_est.predict(_dt_ekf_avg); @@ -62,10 +62,11 @@ void Ekf::controlExternalVisionFusion() && isNewestSampleRecent(_time_last_ext_vision_buffer_push, EV_MAX_INTERVAL); updateEvAttitudeErrorFilter(ev_sample, ev_reset); - controlEvYawFusion(ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_yaw); - controlEvVelFusion(ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_vel); - controlEvPosFusion(ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_pos); - controlEvHeightFusion(ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_hgt); + controlEvYawFusion(imu_sample, ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_yaw); + controlEvVelFusion(imu_sample, ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_vel); + controlEvPosFusion(imu_sample, ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_pos); + controlEvHeightFusion(imu_sample, ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, + _aid_src_ev_hgt); if (quality_sufficient) { _ev_sample_prev = ev_sample; diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp index a69906eef489..0684536ae587 100644 --- a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp @@ -38,8 +38,9 @@ #include "ekf.h" -void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, - const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src) +void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSample &ev_sample, + const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, + estimator_aid_source1d_s &aid_src) { static constexpr const char *AID_SRC_NAME = "EV height"; @@ -152,7 +153,8 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com if (ev_sample.vel.isAllFinite() && (_params.ev_ctrl & static_cast(EvCtrl::VEL))) { // correct velocity for offset relative to IMU - const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body; + const Vector3f angular_velocity = (imu_sample.delta_ang / imu_sample.delta_ang_dt) - _state.gyro_bias; + const Vector3f vel_offset_body = angular_velocity % pos_offset_body; const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body; switch (ev_sample.vel_frame) { diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp index 53ee21e09b8c..590a289da394 100644 --- a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp @@ -41,8 +41,9 @@ static constexpr const char *EV_AID_SRC_NAME = "EV position"; -void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, - const bool ev_reset, const bool quality_sufficient, estimator_aid_source2d_s &aid_src) +void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample &ev_sample, + const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, + estimator_aid_source2d_s &aid_src) { const bool yaw_alignment_changed = (!_control_status_prev.flags.ev_yaw && _control_status.flags.ev_yaw) || (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align); diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp index 62781e5081ac..a5ba562cad23 100644 --- a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp @@ -41,8 +41,9 @@ #include #include -void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, - const bool ev_reset, const bool quality_sufficient, estimator_aid_source3d_s &aid_src) +void Ekf::controlEvVelFusion(const imuSample &imu_sample, const extVisionSample &ev_sample, + const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, + estimator_aid_source3d_s &aid_src) { static constexpr const char *AID_SRC_NAME = "EV velocity"; @@ -55,8 +56,9 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common && ev_sample.vel.isAllFinite(); // correct velocity for offset relative to IMU + const Vector3f angular_velocity = imu_sample.delta_ang / imu_sample.delta_ang_dt - _state.gyro_bias; const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body; - const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body; + const Vector3f vel_offset_body = angular_velocity % pos_offset_body; const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body; // rotate measurement into correct earth frame if required diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_yaw_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_yaw_control.cpp index a285d3e4556a..ad15c4f6bb33 100644 --- a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_yaw_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_yaw_control.cpp @@ -38,8 +38,9 @@ #include "ekf.h" -void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, - const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src) +void Ekf::controlEvYawFusion(const imuSample &imu_sample, const extVisionSample &ev_sample, + const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, + estimator_aid_source1d_s &aid_src) { static constexpr const char *AID_SRC_NAME = "EV yaw"; diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp index d5bca4f46ac3..83f7ef624d64 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp @@ -86,7 +86,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) updateGnssPos(gnss_sample, _aid_src_gnss_pos); } - updateGnssVel(gnss_sample, _aid_src_gnss_vel); + updateGnssVel(imu_delayed, gnss_sample, _aid_src_gnss_vel); } else if (_control_status.flags.gps) { if (!isNewestSampleRecent(_time_last_gps_buffer_push, _params.reset_timeout_max)) { @@ -173,12 +173,13 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) } } -void Ekf::updateGnssVel(const gnssSample &gnss_sample, estimator_aid_source3d_s &aid_src) +void Ekf::updateGnssVel(const imuSample &imu_sample, const gnssSample &gnss_sample, estimator_aid_source3d_s &aid_src) { // correct velocity for offset relative to IMU const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body; - const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body; + const Vector3f angular_velocity = imu_sample.delta_ang / imu_sample.delta_ang_dt - _state.gyro_bias; + const Vector3f vel_offset_body = angular_velocity % pos_offset_body; const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body; const Vector3f velocity = gnss_sample.vel - vel_offset_earth; diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 5215570d14c7..37d13a32e36b 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -137,7 +137,7 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed) #if defined(CONFIG_EKF2_EXTERNAL_VISION) // Additional data odometry data from an external estimator can be fused. - controlExternalVisionFusion(); + controlExternalVisionFusion(imu_delayed); #endif // CONFIG_EKF2_EXTERNAL_VISION #if defined(CONFIG_EKF2_AUXVEL) diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index 4479dc37b8ea..1b439c887956 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -89,8 +89,6 @@ void Ekf::reset() _control_status.flags.in_air = true; _control_status_prev.flags.in_air = true; - _ang_rate_delayed_raw.zero(); - _fault_status.value = 0; _innov_check_fail_status.value = 0; @@ -264,13 +262,6 @@ void Ekf::predictState(const imuSample &imu_delayed) _state.vel = matrix::constrain(_state.vel, -1000.f, 1000.f); _state.pos = matrix::constrain(_state.pos, -1.e6f, 1.e6f); - // some calculations elsewhere in code require a raw angular rate vector so calculate here to avoid duplication - // protect against possible small timesteps resulting from timing slip on previous frame that can drive spikes into the rate - // due to insufficient averaging - if (imu_delayed.delta_ang_dt > 0.25f * _dt_ekf_avg) { - _ang_rate_delayed_raw = imu_delayed.delta_ang / imu_delayed.delta_ang_dt; - } - // calculate a filtered horizontal acceleration with a 1 sec time constant // this are used for manoeuvre detection elsewhere diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 1b20c433e2a6..60c869c575de 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -566,8 +566,6 @@ class Ekf final : public EstimatorInterface 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) - StateSample _state{}; ///< state struct of the ekf running at the delayed time horizon bool _filter_initialised{false}; ///< true when the EKF sttes and covariances been initialised @@ -936,16 +934,20 @@ class Ekf final : public EstimatorInterface #if defined(CONFIG_EKF2_EXTERNAL_VISION) // control fusion of external vision observations - void controlExternalVisionFusion(); + void controlExternalVisionFusion(const imuSample &imu_sample); void updateEvAttitudeErrorFilter(extVisionSample &ev_sample, bool ev_reset); - void controlEvHeightFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, - const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src); - void controlEvPosFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, - const bool ev_reset, const bool quality_sufficient, estimator_aid_source2d_s &aid_src); - void controlEvVelFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, - const bool ev_reset, const bool quality_sufficient, estimator_aid_source3d_s &aid_src); - void controlEvYawFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, - const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src); + void controlEvHeightFusion(const imuSample &imu_sample, const extVisionSample &ev_sample, + const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, + estimator_aid_source1d_s &aid_src); + void controlEvPosFusion(const imuSample &imu_sample, const extVisionSample &ev_sample, + const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, + estimator_aid_source2d_s &aid_src); + void controlEvVelFusion(const imuSample &imu_sample, const extVisionSample &ev_sample, + const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, + estimator_aid_source3d_s &aid_src); + void controlEvYawFusion(const imuSample &imu_sample, const extVisionSample &ev_sample, + const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, + estimator_aid_source1d_s &aid_src); void resetVelocityToEV(const Vector3f &measurement, const Vector3f &measurement_var, const VelocityFrame &vel_frame); Vector3f rotateVarianceToEkf(const Vector3f &measurement_var); @@ -968,7 +970,7 @@ class Ekf final : public EstimatorInterface // control fusion of GPS observations void controlGpsFusion(const imuSample &imu_delayed); void stopGpsFusion(); - void updateGnssVel(const gnssSample &gnss_sample, estimator_aid_source3d_s &aid_src); + void updateGnssVel(const imuSample &imu_sample, const gnssSample &gnss_sample, estimator_aid_source3d_s &aid_src); void updateGnssPos(const gnssSample &gnss_sample, estimator_aid_source2d_s &aid_src); void controlGnssYawEstimator(estimator_aid_source3d_s &aid_src_vel); bool tryYawEmergencyReset(); @@ -1063,13 +1065,13 @@ class Ekf final : public EstimatorInterface void checkHeightSensorRefFallback(); #if defined(CONFIG_EKF2_BAROMETER) - void controlBaroHeightFusion(); + void controlBaroHeightFusion(const imuSample &imu_sample); void stopBaroHgtFusion(); void updateGroundEffect(); # if defined(CONFIG_EKF2_BARO_COMPENSATION) - float compensateBaroForDynamicPressure(float baro_alt_uncompensated) const; + float compensateBaroForDynamicPressure(const imuSample &imu_sample, float baro_alt_uncompensated) const; # endif // CONFIG_EKF2_BARO_COMPENSATION #endif // CONFIG_EKF2_BAROMETER diff --git a/src/modules/ekf2/EKF/height_control.cpp b/src/modules/ekf2/EKF/height_control.cpp index 5ad5d7db34dd..56a667840a75 100644 --- a/src/modules/ekf2/EKF/height_control.cpp +++ b/src/modules/ekf2/EKF/height_control.cpp @@ -45,7 +45,7 @@ void Ekf::controlHeightFusion(const imuSample &imu_delayed) #if defined(CONFIG_EKF2_BAROMETER) updateGroundEffect(); - controlBaroHeightFusion(); + controlBaroHeightFusion(imu_delayed); #endif // CONFIG_EKF2_BAROMETER #if defined(CONFIG_EKF2_GNSS) From 172a848245c966d190638b37d413abd368b1c64f Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 27 Jun 2024 12:33:17 -0400 Subject: [PATCH 2/3] ekf2: directly use IMU sample to find corresponding aid source sample - I think this helps make it clear we're using a sensor sample corresponding with a particular IMU sample --- .../ekf2/EKF/aid_sources/auxvel/auxvel_fusion.cpp | 4 ++-- .../aid_sources/barometer/baro_height_control.cpp | 6 +++--- .../EKF/aid_sources/external_vision/ev_control.cpp | 2 +- .../EKF/aid_sources/magnetometer/mag_control.cpp | 10 +++++----- .../range_finder/range_height_control.cpp | 12 ++++++------ src/modules/ekf2/EKF/control.cpp | 4 ++-- src/modules/ekf2/EKF/ekf.h | 6 +++--- src/modules/ekf2/EKF/height_control.cpp | 2 +- 8 files changed, 23 insertions(+), 23 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/auxvel/auxvel_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/auxvel/auxvel_fusion.cpp index 14d333f3642d..6531c5c65a07 100644 --- a/src/modules/ekf2/EKF/aid_sources/auxvel/auxvel_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/auxvel/auxvel_fusion.cpp @@ -33,12 +33,12 @@ #include "ekf.h" -void Ekf::controlAuxVelFusion() +void Ekf::controlAuxVelFusion(const imuSample &imu_sample) { if (_auxvel_buffer) { auxVelSample sample; - if (_auxvel_buffer->pop_first_older_than(_time_delayed_us, &sample)) { + if (_auxvel_buffer->pop_first_older_than(imu_sample.time_us, &sample)) { updateAidSourceStatus(_aid_src_aux_vel, sample.time_us, // sample timestamp diff --git a/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp index 9153856009fd..c6c6f220de70 100644 --- a/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp @@ -49,7 +49,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample) baroSample baro_sample; - if (_baro_buffer && _baro_buffer->pop_first_older_than(_time_delayed_us, &baro_sample)) { + if (_baro_buffer && _baro_buffer->pop_first_older_than(imu_sample.time_us, &baro_sample)) { #if defined(CONFIG_EKF2_BARO_COMPENSATION) const float measurement = compensateBaroForDynamicPressure(imu_sample, baro_sample.hgt); @@ -137,7 +137,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample) // reset vertical velocity resetVerticalVelocityToZero(); - aid_src.time_last_fuse = _time_delayed_us; + aid_src.time_last_fuse = imu_sample.time_us; } else if (is_fusion_failing) { // Some other height source is still working @@ -166,7 +166,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample) bias_est.setBias(_state.pos(2) + _baro_lpf.getState()); } - aid_src.time_last_fuse = _time_delayed_us; + aid_src.time_last_fuse = imu_sample.time_us; bias_est.setFusionActive(); _control_status.flags.baro_hgt = true; } diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp index cac1db3f3d72..68022677ba64 100644 --- a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp @@ -46,7 +46,7 @@ void Ekf::controlExternalVisionFusion(const imuSample &imu_sample) // Check for new external vision data extVisionSample ev_sample; - if (_ext_vision_buffer && _ext_vision_buffer->pop_first_older_than(_time_delayed_us, &ev_sample)) { + if (_ext_vision_buffer && _ext_vision_buffer->pop_first_older_than(imu_sample.time_us, &ev_sample)) { bool ev_reset = (ev_sample.reset_counter != _ev_sample_prev.reset_counter); diff --git a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp index 979807f116a5..2dab0da57eaf 100644 --- a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp @@ -41,7 +41,7 @@ #include -void Ekf::controlMagFusion() +void Ekf::controlMagFusion(const imuSample &imu_sample) { static constexpr const char *AID_SRC_NAME = "mag"; estimator_aid_source3d_s &aid_src = _aid_src_mag; @@ -59,7 +59,7 @@ void Ekf::controlMagFusion() magSample mag_sample; - if (_mag_buffer && _mag_buffer->pop_first_older_than(_time_delayed_us, &mag_sample)) { + if (_mag_buffer && _mag_buffer->pop_first_older_than(imu_sample.time_us, &mag_sample)) { if (mag_sample.reset || (_mag_counter == 0)) { // sensor or calibration has changed, reset low pass filter @@ -185,7 +185,7 @@ void Ekf::controlMagFusion() if (mag_sample.reset || checkHaglYawResetReq() || (wmm_updated && no_ne_aiding_or_pre_takeoff)) { ECL_INFO("reset to %s", AID_SRC_NAME); resetMagStates(_mag_lpf.getState(), _control_status.flags.mag_hdg || _control_status.flags.mag_3D); - aid_src.time_last_fuse = _time_delayed_us; + aid_src.time_last_fuse = imu_sample.time_us; } else { // The normal sequence is to fuse the magnetometer data first before fusing @@ -213,7 +213,7 @@ void Ekf::controlMagFusion() if (no_ne_aiding_or_pre_takeoff) { ECL_WARN("%s fusion failing, resetting", AID_SRC_NAME); resetMagStates(_mag_lpf.getState(), _control_status.flags.mag_hdg || _control_status.flags.mag_3D); - aid_src.time_last_fuse = _time_delayed_us; + aid_src.time_last_fuse = imu_sample.time_us; } else { ECL_WARN("stopping %s, fusion failing", AID_SRC_NAME); @@ -242,7 +242,7 @@ void Ekf::controlMagFusion() bool reset_heading = !_control_status.flags.yaw_align; resetMagStates(_mag_lpf.getState(), reset_heading); - aid_src.time_last_fuse = _time_delayed_us; + aid_src.time_last_fuse = imu_sample.time_us; if (reset_heading) { _control_status.flags.yaw_align = true; diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp index 1b7db97e3020..94a44883eecf 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp @@ -39,7 +39,7 @@ #include "ekf.h" #include "ekf_derivation/generated/compute_hagl_innov_var.h" -void Ekf::controlRangeHaglFusion() +void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) { static constexpr const char *HGT_SRC_NAME = "RNG"; @@ -47,7 +47,7 @@ void Ekf::controlRangeHaglFusion() if (_range_buffer) { // Get range data from buffer and check validity - rng_data_ready = _range_buffer->pop_first_older_than(_time_delayed_us, _range_sensor.getSampleAddress()); + rng_data_ready = _range_buffer->pop_first_older_than(imu_sample.time_us, _range_sensor.getSampleAddress()); _range_sensor.setDataReadiness(rng_data_ready); // update range sensor angle parameters in case they have changed @@ -55,7 +55,7 @@ void Ekf::controlRangeHaglFusion() _range_sensor.setCosMaxTilt(_params.range_cos_max_tilt); _range_sensor.setQualityHysteresis(_params.range_valid_quality_s); - _range_sensor.runChecks(_time_delayed_us, _R_to_earth); + _range_sensor.runChecks(imu_sample.time_us, _R_to_earth); if (_range_sensor.isDataHealthy()) { // correct the range data for position offset relative to the IMU @@ -72,7 +72,7 @@ void Ekf::controlRangeHaglFusion() _rng_consistency_check.setGate(_params.range_kin_consistency_gate); _rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2), - P(State::vel.idx + 2, State::vel.idx + 2), horizontal_motion, _time_delayed_us); + P(State::vel.idx + 2, State::vel.idx + 2), horizontal_motion, imu_sample.time_us); } } else { @@ -151,7 +151,7 @@ void Ekf::controlRangeHaglFusion() _control_status.flags.rng_hgt = true; stopRngTerrFusion(); - aid_src.time_last_fuse = _time_delayed_us; + aid_src.time_last_fuse = imu_sample.time_us; } } else { @@ -183,7 +183,7 @@ void Ekf::controlRangeHaglFusion() // reset vertical velocity resetVerticalVelocityToZero(); - aid_src.time_last_fuse = _time_delayed_us; + aid_src.time_last_fuse = imu_sample.time_us; } else if (is_fusion_failing) { // Some other height source is still working diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 37d13a32e36b..b8aca027e769 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -102,7 +102,7 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed) #if defined(CONFIG_EKF2_MAGNETOMETER) // control use of observations for aiding - controlMagFusion(); + controlMagFusion(imu_delayed); #endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_OPTICAL_FLOW) @@ -142,7 +142,7 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed) #if defined(CONFIG_EKF2_AUXVEL) // Additional horizontal velocity data from an auxiliary sensor can be fused - controlAuxVelFusion(); + controlAuxVelFusion(imu_delayed); #endif // CONFIG_EKF2_AUXVEL // #if defined(CONFIG_EKF2_TERRAIN) diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 60c869c575de..85ea4a7625bc 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -841,7 +841,7 @@ class Ekf final : public EstimatorInterface #if defined(CONFIG_EKF2_RANGE_FINDER) // range height - void controlRangeHaglFusion(); + void controlRangeHaglFusion(const imuSample &imu_delayed); bool isConditionalRangeAidSuitable(); void stopRngHgtFusion(); void stopRngTerrFusion(); @@ -1019,7 +1019,7 @@ class Ekf final : public EstimatorInterface #if defined(CONFIG_EKF2_MAGNETOMETER) // control fusion of magnetometer observations - void controlMagFusion(); + void controlMagFusion(const imuSample &imu_sample); bool checkHaglYawResetReq() const; @@ -1052,7 +1052,7 @@ class Ekf final : public EstimatorInterface #if defined(CONFIG_EKF2_AUXVEL) // control fusion of auxiliary velocity observations - void controlAuxVelFusion(); + void controlAuxVelFusion(const imuSample &imu_sample); void stopAuxVelFusion(); #endif // CONFIG_EKF2_AUXVEL diff --git a/src/modules/ekf2/EKF/height_control.cpp b/src/modules/ekf2/EKF/height_control.cpp index 56a667840a75..4db232ca051f 100644 --- a/src/modules/ekf2/EKF/height_control.cpp +++ b/src/modules/ekf2/EKF/height_control.cpp @@ -53,7 +53,7 @@ void Ekf::controlHeightFusion(const imuSample &imu_delayed) #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_RANGE_FINDER) - controlRangeHaglFusion(); + controlRangeHaglFusion(imu_delayed); #endif // CONFIG_EKF2_RANGE_FINDER checkHeightSensorRefFallback(); From 1c6810ea5ea1e4b38f2d807d342b33853a1e77b8 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 8 Jul 2024 11:25:21 -0400 Subject: [PATCH 3/3] ekf2: add IMU delta_ang_dt/delta_vel_dt safety constrain before pushing into buffer --- src/modules/ekf2/EKF/covariance.cpp | 4 ++-- src/modules/ekf2/EKF/estimator_interface.cpp | 12 +++++++++++- 2 files changed, 13 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index 8108d26c1381..58e83b3bd757 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -134,8 +134,8 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) // calculate variances and upper diagonal covariances for quaternion, velocity, position and gyro bias states P = sym::PredictCovariance(_state.vector(), P, - imu_delayed.delta_vel / math::max(imu_delayed.delta_vel_dt, FLT_EPSILON), accel_var, - imu_delayed.delta_ang / math::max(imu_delayed.delta_ang_dt, FLT_EPSILON), gyro_var, + imu_delayed.delta_vel / imu_delayed.delta_vel_dt, accel_var, + imu_delayed.delta_ang / imu_delayed.delta_ang_dt, gyro_var, dt); // Construct the process noise variance diagonal for those states with a stationary process model diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index bce6ec88637f..d4a579f5b8cf 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -94,7 +94,17 @@ void EstimatorInterface::setIMUData(const imuSample &imu_sample) _imu_updated = true; - _imu_buffer.push(_imu_down_sampler.getDownSampledImuAndTriggerReset()); + imuSample imu_downsampled = _imu_down_sampler.getDownSampledImuAndTriggerReset(); + + // as a precaution constrain the integration delta time to prevent numerical problems + const float filter_update_period_s = _params.filter_update_interval_us * 1e-6f; + const float imu_min_dt = 0.5f * filter_update_period_s; + const float imu_max_dt = 2.0f * filter_update_period_s; + + imu_downsampled.delta_ang_dt = math::constrain(imu_downsampled.delta_ang_dt, imu_min_dt, imu_max_dt); + imu_downsampled.delta_vel_dt = math::constrain(imu_downsampled.delta_vel_dt, imu_min_dt, imu_max_dt); + + _imu_buffer.push(imu_downsampled); // get the oldest data from the buffer _time_delayed_us = _imu_buffer.get_oldest().time_us;