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 80c51e0979..9153856009 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 78cc2bd30a..cac1db3f3d 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 a69906eef4..0684536ae5 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 53ee21e09b..590a289da3 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 62781e5081..a5ba562cad 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 a285d3e455..ad15c4f6bb 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 d5bca4f46a..83f7ef624d 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 5215570d14..37d13a32e3 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 4479dc37b8..1b439c8879 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 1b20c433e2..60c869c575 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -566,8 +566,6 @@ private: 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 @@ private: #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 @@ private: // 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 @@ private: 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 5ad5d7db34..56a667840a 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)