From 14a967e2caa1a38ac60afe38f40e06529be8387f Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 25 Sep 2023 23:39:27 -0400 Subject: [PATCH] ekf2: remove aid src status fusion_enabled flag --- msg/EstimatorAidSource1d.msg | 1 - msg/EstimatorAidSource2d.msg | 1 - msg/EstimatorAidSource3d.msg | 1 - src/modules/ekf2/EKF/airspeed_fusion.cpp | 2 - src/modules/ekf2/EKF/auxvel_fusion.cpp | 1 - src/modules/ekf2/EKF/baro_height_control.cpp | 1 - src/modules/ekf2/EKF/ekf.h | 2 - src/modules/ekf2/EKF/ev_height_control.cpp | 3 - src/modules/ekf2/EKF/ev_pos_control.cpp | 1 - src/modules/ekf2/EKF/ev_vel_control.cpp | 1 - src/modules/ekf2/EKF/ev_yaw_control.cpp | 2 - src/modules/ekf2/EKF/fake_height_control.cpp | 6 +- src/modules/ekf2/EKF/fake_pos_control.cpp | 9 +- src/modules/ekf2/EKF/gnss_height_control.cpp | 2 - src/modules/ekf2/EKF/gps_control.cpp | 53 +++++--- src/modules/ekf2/EKF/gps_yaw_fusion.cpp | 2 - src/modules/ekf2/EKF/gravity_fusion.cpp | 4 +- src/modules/ekf2/EKF/mag_3d_control.cpp | 3 - src/modules/ekf2/EKF/mag_fusion.cpp | 2 - src/modules/ekf2/EKF/mag_heading_control.cpp | 3 - src/modules/ekf2/EKF/optflow_fusion.cpp | 2 - src/modules/ekf2/EKF/range_height_control.cpp | 2 - src/modules/ekf2/EKF/sideslip_fusion.cpp | 2 - src/modules/ekf2/EKF/terrain_estimator.cpp | 6 - src/modules/ekf2/EKF/vel_pos_fusion.cpp | 8 +- src/modules/ekf2/EKF/yaw_fusion.cpp | 121 +++++++++--------- .../EKF/zero_innovation_heading_update.cpp | 1 - 27 files changed, 105 insertions(+), 137 deletions(-) diff --git a/msg/EstimatorAidSource1d.msg b/msg/EstimatorAidSource1d.msg index 42be25881d..78273d6b06 100644 --- a/msg/EstimatorAidSource1d.msg +++ b/msg/EstimatorAidSource1d.msg @@ -14,7 +14,6 @@ float32 innovation float32 innovation_variance float32 test_ratio -bool fusion_enabled # true when measurements are being fused bool innovation_rejected # true if the observation has been rejected bool fused # true if the sample was successfully fused diff --git a/msg/EstimatorAidSource2d.msg b/msg/EstimatorAidSource2d.msg index 9fdba8fefa..2d33fc361d 100644 --- a/msg/EstimatorAidSource2d.msg +++ b/msg/EstimatorAidSource2d.msg @@ -14,7 +14,6 @@ float32[2] innovation float32[2] innovation_variance float32[2] test_ratio -bool fusion_enabled # true when measurements are being fused bool innovation_rejected # true if the observation has been rejected bool fused # true if the sample was successfully fused diff --git a/msg/EstimatorAidSource3d.msg b/msg/EstimatorAidSource3d.msg index 5d143bc04d..deb4c05bd0 100644 --- a/msg/EstimatorAidSource3d.msg +++ b/msg/EstimatorAidSource3d.msg @@ -14,7 +14,6 @@ float32[3] innovation float32[3] innovation_variance float32[3] test_ratio -bool fusion_enabled # true when measurements are being fused bool innovation_rejected # true if the observation has been rejected bool fused # true if the sample was successfully fused diff --git a/src/modules/ekf2/EKF/airspeed_fusion.cpp b/src/modules/ekf2/EKF/airspeed_fusion.cpp index 2668147e2b..5ad89e76cb 100644 --- a/src/modules/ekf2/EKF/airspeed_fusion.cpp +++ b/src/modules/ekf2/EKF/airspeed_fusion.cpp @@ -151,8 +151,6 @@ void Ekf::updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_so aid_src.innovation = innov; aid_src.innovation_variance = innov_var; - aid_src.fusion_enabled = _control_status.flags.fuse_aspd; - aid_src.timestamp_sample = airspeed_sample.time_us; const float innov_gate = fmaxf(_params.tas_innov_gate, 1.f); diff --git a/src/modules/ekf2/EKF/auxvel_fusion.cpp b/src/modules/ekf2/EKF/auxvel_fusion.cpp index 680f40cc7d..dbe88ccb87 100644 --- a/src/modules/ekf2/EKF/auxvel_fusion.cpp +++ b/src/modules/ekf2/EKF/auxvel_fusion.cpp @@ -45,7 +45,6 @@ void Ekf::controlAuxVelFusion() updateVelocityAidSrcStatus(auxvel_sample_delayed.time_us, auxvel_sample_delayed.vel, auxvel_sample_delayed.velVar, fmaxf(_params.auxvel_gate, 1.f), _aid_src_aux_vel); if (isHorizontalAidingActive()) { - _aid_src_aux_vel.fusion_enabled = true; fuseVelocity(_aid_src_aux_vel); } } diff --git a/src/modules/ekf2/EKF/baro_height_control.cpp b/src/modules/ekf2/EKF/baro_height_control.cpp index 16cfa3ef53..f3c5b0df64 100644 --- a/src/modules/ekf2/EKF/baro_height_control.cpp +++ b/src/modules/ekf2/EKF/baro_height_control.cpp @@ -121,7 +121,6 @@ void Ekf::controlBaroHeightFusion() && isNewestSampleRecent(_time_last_baro_buffer_push, 2 * BARO_MAX_INTERVAL); if (_control_status.flags.baro_hgt) { - aid_src.fusion_enabled = true; if (continuing_conditions_passing) { diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 87b39db92f..9c8f391d32 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -1241,7 +1241,6 @@ private: status.innovation_variance = 0; status.test_ratio = INFINITY; - status.fusion_enabled = false; status.innovation_rejected = true; status.fused = false; } @@ -1265,7 +1264,6 @@ private: status.test_ratio[i] = INFINITY; } - status.fusion_enabled = false; status.innovation_rejected = true; status.fused = false; } diff --git a/src/modules/ekf2/EKF/ev_height_control.cpp b/src/modules/ekf2/EKF/ev_height_control.cpp index d4b7f8fa16..4ad4dd7e0f 100644 --- a/src/modules/ekf2/EKF/ev_height_control.cpp +++ b/src/modules/ekf2/EKF/ev_height_control.cpp @@ -104,10 +104,7 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com && continuing_conditions_passing; if (_control_status.flags.ev_hgt) { - aid_src.fusion_enabled = true; - if (continuing_conditions_passing) { - if (ev_reset) { if (quality_sufficient) { diff --git a/src/modules/ekf2/EKF/ev_pos_control.cpp b/src/modules/ekf2/EKF/ev_pos_control.cpp index ec906f17a9..e969af287c 100644 --- a/src/modules/ekf2/EKF/ev_pos_control.cpp +++ b/src/modules/ekf2/EKF/ev_pos_control.cpp @@ -175,7 +175,6 @@ void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common && continuing_conditions_passing; if (_control_status.flags.ev_pos) { - aid_src.fusion_enabled = true; if (continuing_conditions_passing) { const bool bias_estimator_change = (bias_fusion_was_active != _ev_pos_b_est.fusionActive()); diff --git a/src/modules/ekf2/EKF/ev_vel_control.cpp b/src/modules/ekf2/EKF/ev_vel_control.cpp index 0160ca9fc9..a2431689d9 100644 --- a/src/modules/ekf2/EKF/ev_vel_control.cpp +++ b/src/modules/ekf2/EKF/ev_vel_control.cpp @@ -138,7 +138,6 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common && ((Vector3f(aid_src.test_ratio).max() < 0.1f) || !isHorizontalAidingActive()); if (_control_status.flags.ev_vel) { - aid_src.fusion_enabled = true; if (continuing_conditions_passing) { diff --git a/src/modules/ekf2/EKF/ev_yaw_control.cpp b/src/modules/ekf2/EKF/ev_yaw_control.cpp index d55d7f814e..d7d4eee92a 100644 --- a/src/modules/ekf2/EKF/ev_yaw_control.cpp +++ b/src/modules/ekf2/EKF/ev_yaw_control.cpp @@ -76,8 +76,6 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common && isTimedOut(aid_src.time_last_fuse, (uint32_t)1e6); if (_control_status.flags.ev_yaw) { - aid_src.fusion_enabled = true; - if (continuing_conditions_passing) { if (ev_reset) { diff --git a/src/modules/ekf2/EKF/fake_height_control.cpp b/src/modules/ekf2/EKF/fake_height_control.cpp index ef8c318081..8b7eba5664 100644 --- a/src/modules/ekf2/EKF/fake_height_control.cpp +++ b/src/modules/ekf2/EKF/fake_height_control.cpp @@ -63,9 +63,9 @@ void Ekf::controlFakeHgtFusion() if (continuing_conditions_passing) { // always protect against extreme values that could result in a NaN - aid_src.fusion_enabled = aid_src.test_ratio < sq(100.0f / innov_gate); - - fuseVerticalPosition(aid_src); + if (aid_src.test_ratio < sq(100.0f / innov_gate)) { + fuseVerticalPosition(aid_src); + } const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, (uint64_t)4e5); diff --git a/src/modules/ekf2/EKF/fake_pos_control.cpp b/src/modules/ekf2/EKF/fake_pos_control.cpp index 4c876c4597..5ff14f8862 100644 --- a/src/modules/ekf2/EKF/fake_pos_control.cpp +++ b/src/modules/ekf2/EKF/fake_pos_control.cpp @@ -76,10 +76,11 @@ void Ekf::controlFakePosFusion() if (continuing_conditions_passing) { // always protect against extreme values that could result in a NaN - aid_src.fusion_enabled = (aid_src.test_ratio[0] < sq(100.0f / innov_gate)) - && (aid_src.test_ratio[1] < sq(100.0f / innov_gate)); - - fuseHorizontalPosition(aid_src); + if ((aid_src.test_ratio[0] < sq(100.0f / innov_gate)) + && (aid_src.test_ratio[1] < sq(100.0f / innov_gate)) + ) { + fuseHorizontalPosition(aid_src); + } const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, (uint64_t)4e5); diff --git a/src/modules/ekf2/EKF/gnss_height_control.cpp b/src/modules/ekf2/EKF/gnss_height_control.cpp index 011118a7e6..100d4150f1 100644 --- a/src/modules/ekf2/EKF/gnss_height_control.cpp +++ b/src/modules/ekf2/EKF/gnss_height_control.cpp @@ -98,8 +98,6 @@ void Ekf::controlGnssHeightFusion(const gpsSample &gps_sample) && !gps_checks_failing; if (_control_status.flags.gps_hgt) { - aid_src.fusion_enabled = true; - if (continuing_conditions_passing) { fuseVerticalPosition(aid_src); diff --git a/src/modules/ekf2/EKF/gps_control.cpp b/src/modules/ekf2/EKF/gps_control.cpp index c5db7851da..4c620de17f 100644 --- a/src/modules/ekf2/EKF/gps_control.cpp +++ b/src/modules/ekf2/EKF/gps_control.cpp @@ -95,7 +95,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) vel_obs_var, // observation variance math::max(_params.gps_vel_innov_gate, 1.f), // innovation gate _aid_src_gnss_vel); - _aid_src_gnss_vel.fusion_enabled = (_params.gnss_ctrl & GnssCtrl::VEL); + const bool gnss_vel_enabled = (_params.gnss_ctrl & GnssCtrl::VEL); // GNSS position const Vector2f position{gps_sample.pos}; @@ -117,13 +117,13 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) pos_obs_var, // observation variance math::max(_params.gps_pos_innov_gate, 1.f), // innovation gate _aid_src_gnss_pos); - _aid_src_gnss_pos.fusion_enabled = (_params.gnss_ctrl & GnssCtrl::HPOS); + const bool gnss_pos_enabled = (_params.gnss_ctrl & GnssCtrl::HPOS); // Determine if we should use GPS aiding for velocity and horizontal position // To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently bool mandatory_conditions_passing = false; - if (((_params.gnss_ctrl & GnssCtrl::HPOS) || (_params.gnss_ctrl & GnssCtrl::VEL)) + if ((gnss_pos_enabled || gnss_vel_enabled) && _control_status.flags.tilt_align && _NED_origin_initialised ) { @@ -148,8 +148,13 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) if (continuing_conditions_passing || !isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) { - fuseVelocity(_aid_src_gnss_vel); - fuseHorizontalPosition(_aid_src_gnss_pos); + if (gnss_vel_enabled) { + fuseVelocity(_aid_src_gnss_vel); + } + + if (gnss_pos_enabled) { + fuseHorizontalPosition(_aid_src_gnss_pos); + } bool do_vel_pos_reset = shouldResetGpsFusion(); @@ -196,15 +201,19 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) if (do_vel_pos_reset) { ECL_WARN("GPS fusion timeout, resetting velocity and position"); - // reset velocity - _information_events.flags.reset_vel_to_gps = true; - resetVelocityTo(velocity, vel_obs_var); - _aid_src_gnss_vel.time_last_fuse = _time_delayed_us; + if (gnss_vel_enabled) { + // reset velocity + _information_events.flags.reset_vel_to_gps = true; + resetVelocityTo(velocity, vel_obs_var); + _aid_src_gnss_vel.time_last_fuse = _time_delayed_us; + } - // reset position - _information_events.flags.reset_pos_to_gps = true; - resetHorizontalPositionTo(position, pos_obs_var); - _aid_src_gnss_pos.time_last_fuse = _time_delayed_us; + if (gnss_pos_enabled) { + // reset position + _information_events.flags.reset_pos_to_gps = true; + resetHorizontalPositionTo(position, pos_obs_var); + _aid_src_gnss_pos.time_last_fuse = _time_delayed_us; + } } } else { @@ -228,15 +237,19 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) || !_control_status_prev.flags.yaw_align ) { // reset velocity - _information_events.flags.reset_vel_to_gps = true; - resetVelocityTo(velocity, vel_obs_var); - _aid_src_gnss_vel.time_last_fuse = _time_delayed_us; + if (gnss_vel_enabled) { + _information_events.flags.reset_vel_to_gps = true; + resetVelocityTo(velocity, vel_obs_var); + _aid_src_gnss_vel.time_last_fuse = _time_delayed_us; + } } - // reset position - _information_events.flags.reset_pos_to_gps = true; - resetHorizontalPositionTo(position, pos_obs_var); - _aid_src_gnss_pos.time_last_fuse = _time_delayed_us; + if (gnss_pos_enabled) { + // reset position + _information_events.flags.reset_pos_to_gps = true; + resetHorizontalPositionTo(position, pos_obs_var); + _aid_src_gnss_pos.time_last_fuse = _time_delayed_us; + } _control_status.flags.gps = true; } diff --git a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp index 9f01d2f380..e8e6c2bfef 100644 --- a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp +++ b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp @@ -73,8 +73,6 @@ void Ekf::updateGpsYaw(const gpsSample &gps_sample) gnss_yaw.innovation = wrap_pi(heading_pred - measured_hdg); gnss_yaw.innovation_variance = heading_innov_var; - gnss_yaw.fusion_enabled = _control_status.flags.gps_yaw; - gnss_yaw.timestamp_sample = gps_sample.time_us; const float innov_gate = math::max(_params.heading_innov_gate, 1.0f); diff --git a/src/modules/ekf2/EKF/gravity_fusion.cpp b/src/modules/ekf2/EKF/gravity_fusion.cpp index 635b53e9f5..d004a9b3ec 100644 --- a/src/modules/ekf2/EKF/gravity_fusion.cpp +++ b/src/modules/ekf2/EKF/gravity_fusion.cpp @@ -80,11 +80,9 @@ void Ekf::controlGravityFusion(const imuSample &imu) float innovation_gate = 1.f; setEstimatorAidStatusTestRatio(_aid_src_gravity, innovation_gate); - _aid_src_gravity.fusion_enabled = _control_status.flags.gravity_vector; - const bool accel_clipping = imu.delta_vel_clipping[0] || imu.delta_vel_clipping[1] || imu.delta_vel_clipping[2]; - if (_aid_src_gravity.fusion_enabled && !_aid_src_gravity.innovation_rejected && !accel_clipping) { + if (_control_status.flags.gravity_vector && !_aid_src_gravity.innovation_rejected && !accel_clipping) { // perform fusion for each axis _aid_src_gravity.fused = measurementUpdate(Kx, innovation_variance(0), innovation(0)) && measurementUpdate(Ky, innovation_variance(1), innovation(1)) diff --git a/src/modules/ekf2/EKF/mag_3d_control.cpp b/src/modules/ekf2/EKF/mag_3d_control.cpp index 4834ec37ab..738441cf27 100644 --- a/src/modules/ekf2/EKF/mag_3d_control.cpp +++ b/src/modules/ekf2/EKF/mag_3d_control.cpp @@ -89,7 +89,6 @@ void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_star if (_control_status.flags.mag) { aid_src.timestamp_sample = mag_sample.time_us; - aid_src.fusion_enabled = true; if (continuing_conditions_passing && _control_status.flags.yaw_align) { @@ -190,8 +189,6 @@ void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_star _nb_mag_3d_reset_available = 2; } } - - aid_src.fusion_enabled = _control_status.flags.mag; } void Ekf::stopMagFusion() diff --git a/src/modules/ekf2/EKF/mag_fusion.cpp b/src/modules/ekf2/EKF/mag_fusion.cpp index 3ca71359a4..8d18e12f5b 100644 --- a/src/modules/ekf2/EKF/mag_fusion.cpp +++ b/src/modules/ekf2/EKF/mag_fusion.cpp @@ -133,8 +133,6 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo aid_src_mag.innovation[i] = mag_innov(i); } - aid_src_mag.fusion_enabled = _control_status.flags.mag; - // do not use the synthesized measurement for the magnetomter Z component for 3D fusion if (_control_status.flags.synthetic_mag_z) { aid_src_mag.innovation[2] = 0.0f; diff --git a/src/modules/ekf2/EKF/mag_heading_control.cpp b/src/modules/ekf2/EKF/mag_heading_control.cpp index dab53aeb0e..612474937e 100644 --- a/src/modules/ekf2/EKF/mag_heading_control.cpp +++ b/src/modules/ekf2/EKF/mag_heading_control.cpp @@ -97,7 +97,6 @@ void Ekf::controlMagHeadingFusion(const magSample &mag_sample, const bool common && isTimedOut(aid_src.time_last_fuse, 3e6); if (_control_status.flags.mag_hdg) { - aid_src.fusion_enabled = true; aid_src.timestamp_sample = mag_sample.time_us; if (continuing_conditions_passing && _control_status.flags.yaw_align) { @@ -185,8 +184,6 @@ void Ekf::controlMagHeadingFusion(const magSample &mag_sample, const bool common } } - aid_src.fusion_enabled = _control_status.flags.mag_hdg; - // record corresponding mag heading and yaw state for future mag heading delta heading innovation (logging only) _mag_heading_prev = measured_hdg; _mag_heading_pred_prev = getEulerYaw(_state.quat_nominal); diff --git a/src/modules/ekf2/EKF/optflow_fusion.cpp b/src/modules/ekf2/EKF/optflow_fusion.cpp index 2db9f1c8d7..e2f1379d06 100644 --- a/src/modules/ekf2/EKF/optflow_fusion.cpp +++ b/src/modules/ekf2/EKF/optflow_fusion.cpp @@ -90,8 +90,6 @@ void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src) void Ekf::fuseOptFlow() { - _aid_src_optical_flow.fusion_enabled = true; - const float R_LOS = _aid_src_optical_flow.observation_variance[0]; // calculate the height above the ground of the optical flow camera. Since earth frame is NED diff --git a/src/modules/ekf2/EKF/range_height_control.cpp b/src/modules/ekf2/EKF/range_height_control.cpp index a9841bcc76..177b841393 100644 --- a/src/modules/ekf2/EKF/range_height_control.cpp +++ b/src/modules/ekf2/EKF/range_height_control.cpp @@ -131,8 +131,6 @@ void Ekf::controlRangeHeightFusion() && _range_sensor.isRegularlySendingData(); if (_control_status.flags.rng_hgt) { - aid_src.fusion_enabled = true; - if (continuing_conditions_passing) { fuseVerticalPosition(aid_src); diff --git a/src/modules/ekf2/EKF/sideslip_fusion.cpp b/src/modules/ekf2/EKF/sideslip_fusion.cpp index b1bfc4db3d..3ccdf2a7b6 100644 --- a/src/modules/ekf2/EKF/sideslip_fusion.cpp +++ b/src/modules/ekf2/EKF/sideslip_fusion.cpp @@ -94,8 +94,6 @@ void Ekf::updateSideslip(estimator_aid_source1d_s &sideslip) const sideslip.innovation = innov; sideslip.innovation_variance = innov_var; - sideslip.fusion_enabled = _control_status.flags.fuse_aspd; - sideslip.timestamp_sample = _time_delayed_us; const float innov_gate = fmaxf(_params.beta_innov_gate, 1.f); diff --git a/src/modules/ekf2/EKF/terrain_estimator.cpp b/src/modules/ekf2/EKF/terrain_estimator.cpp index 9ffc5d3f65..62cf8367c3 100644 --- a/src/modules/ekf2/EKF/terrain_estimator.cpp +++ b/src/modules/ekf2/EKF/terrain_estimator.cpp @@ -176,8 +176,6 @@ void Ekf::controlHaglRngFusion() // No data anymore. Stop until it comes back. stopHaglRngFusion(); } - - _aid_src_terrain_range_finder.fusion_enabled = _hagl_sensor_status.flags.range_finder; } float Ekf::getRngVar() const @@ -241,7 +239,6 @@ void Ekf::updateHaglRng(estimator_aid_source1d_s &aid_src) const setEstimatorAidStatusTestRatio(aid_src, innov_gate); - aid_src.fusion_enabled = false; aid_src.fused = false; } @@ -319,8 +316,6 @@ void Ekf::controlHaglFlowFusion() // No data anymore. Stop until it comes back. stopHaglFlowFusion(); } - - _aid_src_terrain_optical_flow.fusion_enabled = _hagl_sensor_status.flags.flow; } void Ekf::stopHaglFlowFusion() @@ -345,7 +340,6 @@ void Ekf::resetHaglFlow() void Ekf::fuseFlowForTerrain(estimator_aid_source2d_s &flow) { - flow.fusion_enabled = true; flow.fused = true; const float R_LOS = flow.observation_variance[0]; diff --git a/src/modules/ekf2/EKF/vel_pos_fusion.cpp b/src/modules/ekf2/EKF/vel_pos_fusion.cpp index 071f01ed5e..e615dc2d20 100644 --- a/src/modules/ekf2/EKF/vel_pos_fusion.cpp +++ b/src/modules/ekf2/EKF/vel_pos_fusion.cpp @@ -134,7 +134,7 @@ void Ekf::updateHorizontalPositionAidSrcStatus(const uint64_t &time_us, const Ve void Ekf::fuseVelocity(estimator_aid_source2d_s &aid_src) { - if (aid_src.fusion_enabled && !aid_src.innovation_rejected) { + if (!aid_src.innovation_rejected) { // vx, vy if (fuseVelPosHeight(aid_src.innovation[0], aid_src.innovation_variance[0], State::vel.idx) && fuseVelPosHeight(aid_src.innovation[1], aid_src.innovation_variance[1], State::vel.idx + 1) @@ -150,7 +150,7 @@ void Ekf::fuseVelocity(estimator_aid_source2d_s &aid_src) void Ekf::fuseVelocity(estimator_aid_source3d_s &aid_src) { - if (aid_src.fusion_enabled && !aid_src.innovation_rejected) { + if (!aid_src.innovation_rejected) { // vx, vy, vz if (fuseVelPosHeight(aid_src.innovation[0], aid_src.innovation_variance[0], State::vel.idx) && fuseVelPosHeight(aid_src.innovation[1], aid_src.innovation_variance[1], State::vel.idx + 1) @@ -168,7 +168,7 @@ void Ekf::fuseVelocity(estimator_aid_source3d_s &aid_src) void Ekf::fuseHorizontalPosition(estimator_aid_source2d_s &aid_src) { // x & y - if (aid_src.fusion_enabled && !aid_src.innovation_rejected) { + if (!aid_src.innovation_rejected) { if (fuseVelPosHeight(aid_src.innovation[0], aid_src.innovation_variance[0], State::pos.idx) && fuseVelPosHeight(aid_src.innovation[1], aid_src.innovation_variance[1], State::pos.idx + 1) ) { @@ -184,7 +184,7 @@ void Ekf::fuseHorizontalPosition(estimator_aid_source2d_s &aid_src) void Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src) { // z - if (aid_src.fusion_enabled && !aid_src.innovation_rejected) { + if (!aid_src.innovation_rejected) { if (fuseVelPosHeight(aid_src.innovation, aid_src.innovation_variance, State::pos.idx + 2)) { aid_src.fused = true; aid_src.time_last_fuse = _time_delayed_us; diff --git a/src/modules/ekf2/EKF/yaw_fusion.cpp b/src/modules/ekf2/EKF/yaw_fusion.cpp index d6eec32a60..05615585ce 100644 --- a/src/modules/ekf2/EKF/yaw_fusion.cpp +++ b/src/modules/ekf2/EKF/yaw_fusion.cpp @@ -55,80 +55,77 @@ bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H // innovation test ratio setEstimatorAidStatusTestRatio(aid_src_status, gate_sigma); - if (aid_src_status.fusion_enabled) { + // check if the innovation variance calculation is badly conditioned + if (aid_src_status.innovation_variance >= aid_src_status.observation_variance) { + // the innovation variance contribution from the state covariances is not negative, no fault + _fault_status.flags.bad_hdg = false; - // check if the innovation variance calculation is badly conditioned - if (aid_src_status.innovation_variance >= aid_src_status.observation_variance) { - // the innovation variance contribution from the state covariances is not negative, no fault - _fault_status.flags.bad_hdg = false; + } else { + // the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned + _fault_status.flags.bad_hdg = true; + + // we reinitialise the covariance matrix and abort this fusion step + initialiseCovariance(); + ECL_ERR("yaw fusion numerical error - covariance reset"); + + return false; + } + + // calculate the Kalman gains + // only calculate gains for states we are using + VectorState Kfusion; + const float heading_innov_var_inv = 1.f / aid_src_status.innovation_variance; + + for (uint8_t row = 0; row < State::size; row++) { + for (uint8_t col = 0; col <= 3; col++) { + Kfusion(row) += P(row, col) * H_YAW(col); + } + + Kfusion(row) *= heading_innov_var_inv; + } + + // set the magnetometer unhealthy if the test fails + if (aid_src_status.innovation_rejected) { + _innov_check_fail_status.flags.reject_yaw = true; + + // if we are in air we don't want to fuse the measurement + // we allow to use it when on the ground because the large innovation could be caused + // by interference or a large initial gyro bias + if (!_control_status.flags.in_air + && isTimedOut(_time_last_in_air, (uint64_t)5e6) + && isTimedOut(aid_src_status.time_last_fuse, (uint64_t)1e6) + ) { + // constrain the innovation to the maximum set by the gate + // we need to delay this forced fusion to avoid starting it + // immediately after touchdown, when the drone is still armed + float gate_limit = sqrtf((sq(gate_sigma) * aid_src_status.innovation_variance)); + aid_src_status.innovation = math::constrain(aid_src_status.innovation, -gate_limit, gate_limit); + + // also reset the yaw gyro variance to converge faster and avoid + // being stuck on a previous bad estimate + resetGyroBiasZCov(); } else { - // the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned - _fault_status.flags.bad_hdg = true; - - // we reinitialise the covariance matrix and abort this fusion step - initialiseCovariance(); - ECL_ERR("yaw fusion numerical error - covariance reset"); - return false; } - // calculate the Kalman gains - // only calculate gains for states we are using - VectorState Kfusion; - const float heading_innov_var_inv = 1.f / aid_src_status.innovation_variance; + } else { + _innov_check_fail_status.flags.reject_yaw = false; + } - for (uint8_t row = 0; row < State::size; row++) { - for (uint8_t col = 0; col <= 3; col++) { - Kfusion(row) += P(row, col) * H_YAW(col); - } + if (measurementUpdate(Kfusion, aid_src_status.innovation_variance, aid_src_status.innovation)) { - Kfusion(row) *= heading_innov_var_inv; - } + _time_last_heading_fuse = _time_delayed_us; - // set the magnetometer unhealthy if the test fails - if (aid_src_status.innovation_rejected) { - _innov_check_fail_status.flags.reject_yaw = true; + aid_src_status.time_last_fuse = _time_delayed_us; + aid_src_status.fused = true; - // if we are in air we don't want to fuse the measurement - // we allow to use it when on the ground because the large innovation could be caused - // by interference or a large initial gyro bias - if (!_control_status.flags.in_air - && isTimedOut(_time_last_in_air, (uint64_t)5e6) - && isTimedOut(aid_src_status.time_last_fuse, (uint64_t)1e6) - ) { - // constrain the innovation to the maximum set by the gate - // we need to delay this forced fusion to avoid starting it - // immediately after touchdown, when the drone is still armed - float gate_limit = sqrtf((sq(gate_sigma) * aid_src_status.innovation_variance)); - aid_src_status.innovation = math::constrain(aid_src_status.innovation, -gate_limit, gate_limit); + _fault_status.flags.bad_hdg = false; - // also reset the yaw gyro variance to converge faster and avoid - // being stuck on a previous bad estimate - resetGyroBiasZCov(); + return true; - } else { - return false; - } - - } else { - _innov_check_fail_status.flags.reject_yaw = false; - } - - if (measurementUpdate(Kfusion, aid_src_status.innovation_variance, aid_src_status.innovation)) { - - _time_last_heading_fuse = _time_delayed_us; - - aid_src_status.time_last_fuse = _time_delayed_us; - aid_src_status.fused = true; - - _fault_status.flags.bad_hdg = false; - - return true; - - } else { - _fault_status.flags.bad_hdg = true; - } + } else { + _fault_status.flags.bad_hdg = true; } // otherwise diff --git a/src/modules/ekf2/EKF/zero_innovation_heading_update.cpp b/src/modules/ekf2/EKF/zero_innovation_heading_update.cpp index 9bb0b61a8d..1bae77c7c8 100644 --- a/src/modules/ekf2/EKF/zero_innovation_heading_update.cpp +++ b/src/modules/ekf2/EKF/zero_innovation_heading_update.cpp @@ -62,7 +62,6 @@ void Ekf::controlZeroInnovationHeadingUpdate() if (!_control_status.flags.tilt_align || (aid_src_status.innovation_variance - obs_var) > sq(_params.mag_heading_noise)) { // The yaw variance is too large, fuse fake measurement - aid_src_status.fusion_enabled = true; fuseYaw(aid_src_status, H_YAW); } }