From 206488b8446463ed3461dc81cdc77fcf5e344b4c Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 5 Aug 2023 20:46:27 -0400 Subject: [PATCH] ekf2: innovation sequence monitoring for all aid sources - add new 'innovation_filtered' and 'test_ratio_filtered' fields to estimator_aid_source topics --- msg/EstimatorAidSource1d.msg | 6 +- msg/EstimatorAidSource2d.msg | 14 +- msg/EstimatorAidSource3d.msg | 14 +- .../aid_sources/airspeed/airspeed_fusion.cpp | 45 ++-- .../aux_global_position.cpp | 26 +- .../EKF/aid_sources/auxvel/auxvel_fusion.cpp | 15 +- .../barometer/baro_height_control.cpp | 13 +- .../ekf2/EKF/aid_sources/drag/drag_fusion.cpp | 48 ++-- .../external_vision/ev_control.cpp | 3 - .../external_vision/ev_height_control.cpp | 11 +- .../external_vision/ev_pos_control.cpp | 22 +- .../external_vision/ev_vel_control.cpp | 39 +-- .../external_vision/ev_yaw_control.cpp | 45 ++-- .../EKF/aid_sources/fake_height_control.cpp | 5 +- .../ekf2/EKF/aid_sources/fake_pos_control.cpp | 17 +- .../aid_sources/gnss/gnss_height_control.cpp | 13 +- .../ekf2/EKF/aid_sources/gnss/gps_control.cpp | 62 +++-- .../EKF/aid_sources/gnss/gps_yaw_fusion.cpp | 78 +++--- .../aid_sources/gravity/gravity_fusion.cpp | 20 +- .../aid_sources/magnetometer/mag_control.cpp | 20 +- .../optical_flow/optical_flow_control.cpp | 2 - .../optical_flow/optical_flow_fusion.cpp | 29 +-- .../range_finder/range_height_control.cpp | 17 +- .../aid_sources/sideslip/sideslip_fusion.cpp | 35 ++- src/modules/ekf2/EKF/ekf.cpp | 53 +--- src/modules/ekf2/EKF/ekf.h | 231 +++++++++++++----- src/modules/ekf2/EKF/estimator_interface.h | 1 - src/modules/ekf2/EKF/position_fusion.cpp | 57 ++--- .../terrain_estimator/terrain_estimator.cpp | 31 +-- src/modules/ekf2/EKF/velocity_fusion.cpp | 54 +--- src/modules/ekf2/EKF/yaw_fusion.cpp | 31 +-- 31 files changed, 524 insertions(+), 533 deletions(-) diff --git a/msg/EstimatorAidSource1d.msg b/msg/EstimatorAidSource1d.msg index 1b416835d670..56ef5cfb991f 100644 --- a/msg/EstimatorAidSource1d.msg +++ b/msg/EstimatorAidSource1d.msg @@ -11,8 +11,12 @@ float32 observation float32 observation_variance float32 innovation +float32 innovation_filtered + float32 innovation_variance -float32 test_ratio + +float32 test_ratio # normalized innovation squared +float32 test_ratio_filtered # signed filtered test ratio 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 98e645a3ec92..069030eb15fd 100644 --- a/msg/EstimatorAidSource2d.msg +++ b/msg/EstimatorAidSource2d.msg @@ -1,5 +1,5 @@ -uint64 timestamp # time since system start (microseconds) -uint64 timestamp_sample # the timestamp of the raw data (microseconds) +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) uint8 estimator_instance @@ -11,11 +11,15 @@ float32[2] observation float32[2] observation_variance float32[2] innovation +float32[2] innovation_filtered + float32[2] innovation_variance -float32[2] test_ratio -bool innovation_rejected # true if the observation has been rejected -bool fused # true if the sample was successfully fused +float32[2] test_ratio # normalized innovation squared +float32[2] test_ratio_filtered # signed filtered test ratio + +bool innovation_rejected # true if the observation has been rejected +bool fused # true if the sample was successfully fused # TOPICS estimator_aid_src_ev_pos estimator_aid_src_fake_pos estimator_aid_src_gnss_pos estimator_aid_src_aux_global_position # TOPICS estimator_aid_src_aux_vel estimator_aid_src_optical_flow estimator_aid_src_terrain_optical_flow diff --git a/msg/EstimatorAidSource3d.msg b/msg/EstimatorAidSource3d.msg index deb4c05bd0fb..b89add28e57e 100644 --- a/msg/EstimatorAidSource3d.msg +++ b/msg/EstimatorAidSource3d.msg @@ -1,5 +1,5 @@ -uint64 timestamp # time since system start (microseconds) -uint64 timestamp_sample # the timestamp of the raw data (microseconds) +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) uint8 estimator_instance @@ -11,10 +11,14 @@ float32[3] observation float32[3] observation_variance float32[3] innovation +float32[3] innovation_filtered + float32[3] innovation_variance -float32[3] test_ratio -bool innovation_rejected # true if the observation has been rejected -bool fused # true if the sample was successfully fused +float32[3] test_ratio # normalized innovation squared +float32[3] test_ratio_filtered # signed filtered test ratio + +bool innovation_rejected # true if the observation has been rejected +bool fused # true if the sample was successfully fused # TOPICS estimator_aid_src_ev_vel estimator_aid_src_gnss_vel estimator_aid_src_gravity estimator_aid_src_mag diff --git a/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp index 7f8539b1cb0e..69895181b1ed 100644 --- a/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp @@ -63,6 +63,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed) } #if defined(CONFIG_EKF2_GNSS) + // clear yaw estimator airspeed (updated later with true airspeed if airspeed fusion is active) if (_control_status.flags.fixed_wing) { if (_control_status.flags.in_air && !_control_status.flags.vehicle_at_rest) { @@ -74,6 +75,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed) _yawEstimator.setTrueAirspeed(0.f); } } + #endif // CONFIG_EKF2_GNSS if (_params.arsp_thr <= 0.f) { @@ -89,11 +91,15 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed) _innov_check_fail_status.flags.reject_airspeed = _aid_src_airspeed.innovation_rejected; // TODO: remove this redundant flag - const bool continuing_conditions_passing = _control_status.flags.in_air && _control_status.flags.fixed_wing && !_control_status.flags.fake_pos; + const bool continuing_conditions_passing = _control_status.flags.in_air + && _control_status.flags.fixed_wing + && !_control_status.flags.fake_pos; + const bool is_airspeed_significant = airspeed_sample.true_airspeed > _params.arsp_thr; const bool is_airspeed_consistent = (_aid_src_airspeed.test_ratio > 0.f && _aid_src_airspeed.test_ratio < 1.f); - const bool starting_conditions_passing = continuing_conditions_passing && is_airspeed_significant - && (is_airspeed_consistent || !_control_status.flags.wind || _control_status.flags.inertial_dead_reckoning); + const bool starting_conditions_passing = continuing_conditions_passing + && is_airspeed_significant + && (is_airspeed_consistent || !_control_status.flags.wind || _control_status.flags.inertial_dead_reckoning); if (_control_status.flags.fuse_aspd) { if (continuing_conditions_passing) { @@ -139,26 +145,22 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed) void Ekf::updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source1d_s &aid_src) const { - // reset flags - resetEstimatorAidStatus(aid_src); - // Variance for true airspeed measurement - (m/sec)^2 const float R = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * math::constrain(airspeed_sample.eas2tas, 0.9f, 10.0f)); float innov = 0.f; float innov_var = 0.f; - sym::ComputeAirspeedInnovAndInnovVar(_state.vector(), P, airspeed_sample.true_airspeed, R, FLT_EPSILON, &innov, &innov_var); - - aid_src.observation = airspeed_sample.true_airspeed; - aid_src.observation_variance = R; - aid_src.innovation = innov; - aid_src.innovation_variance = innov_var; - - aid_src.timestamp_sample = airspeed_sample.time_us; - - const float innov_gate = fmaxf(_params.tas_innov_gate, 1.f); - setEstimatorAidStatusTestRatio(aid_src, innov_gate); + sym::ComputeAirspeedInnovAndInnovVar(_state.vector(), P, airspeed_sample.true_airspeed, R, FLT_EPSILON, + &innov, &innov_var); + + updateAidSourceStatus(aid_src, + airspeed_sample.time_us, // sample timestamp + airspeed_sample.true_airspeed, // observation + R, // observation variance + innov, // innovation + innov_var, // innovation variance + math::max(_params.tas_innov_gate, 1.f)); // innovation gate } void Ekf::fuseAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source1d_s &aid_src) @@ -221,7 +223,6 @@ void Ekf::stopAirspeedFusion() { if (_control_status.flags.fuse_aspd) { ECL_INFO("stopping airspeed fusion"); - resetEstimatorAidStatus(_aid_src_airspeed); _control_status.flags.fuse_aspd = false; #if defined(CONFIG_EKF2_GNSS) @@ -235,16 +236,18 @@ void Ekf::resetWindUsingAirspeed(const airspeedSample &airspeed_sample) constexpr float sideslip_var = sq(math::radians(15.0f)); const float euler_yaw = getEulerYaw(_R_to_earth); - const float airspeed_var = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * math::constrain(airspeed_sample.eas2tas, 0.9f, 10.0f)); + const float airspeed_var = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) + * math::constrain(airspeed_sample.eas2tas, 0.9f, 10.0f)); matrix::SquareMatrix P_wind; - sym::ComputeWindInitAndCovFromAirspeed(_state.vel, euler_yaw, airspeed_sample.true_airspeed, getVelocityVariance(), getYawVar(), sideslip_var, airspeed_var, &_state.wind_vel, &P_wind); + sym::ComputeWindInitAndCovFromAirspeed(_state.vel, euler_yaw, airspeed_sample.true_airspeed, getVelocityVariance(), + getYawVar(), sideslip_var, airspeed_var, &_state.wind_vel, &P_wind); resetStateCovariance(P_wind); ECL_INFO("reset wind using airspeed to (%.3f, %.3f)", (double)_state.wind_vel(0), (double)_state.wind_vel(1)); - _aid_src_airspeed.time_last_fuse = _time_delayed_us; + resetAidSourceStatusZeroInnovation(_aid_src_airspeed); } void Ekf::resetVelUsingAirspeed(const airspeedSample &airspeed_sample) diff --git a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp index a0bee2b0a3ba..15be1c3ce6b4 100644 --- a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp +++ b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp @@ -80,23 +80,27 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed position = ekf.global_origin().project(sample.latitude, sample.longitude); //const float hgt = ekf.getEkfGlobalOriginAltitude() - (float)sample.altitude; // relax the upper observation noise limit which prevents bad measurements perturbing the position estimate - float pos_noise = math::max(sample.eph, _param_ekf2_agp_noise.get()); + float pos_noise = math::max(sample.eph, _param_ekf2_agp_noise.get(), 0.01f); const float pos_var = sq(pos_noise); const Vector2f pos_obs_var(pos_var, pos_var); - ekf.updateHorizontalPositionAidSrcStatus(sample.time_us, - position, // observation - pos_obs_var, // observation variance - math::max(_param_ekf2_agp_gate.get(), 1.f), // innovation gate - aid_src); + + ekf.updateAidSourceStatus(aid_src, + sample.time_us, // sample timestamp + position, // observation + pos_obs_var, // observation variance + Vector2f(ekf.state().pos) - position, // innovation + Vector2f(ekf.getPositionVariance()) + pos_obs_var, // innovation variance + math::max(_param_ekf2_agp_gate.get(), 1.f)); // innovation gate } const bool starting_conditions = PX4_ISFINITE(sample.latitude) && PX4_ISFINITE(sample.longitude) - && ekf.control_status_flags().yaw_align; + && ekf.control_status_flags().yaw_align; const bool continuing_conditions = starting_conditions && ekf.global_origin_valid(); switch (_state) { case State::stopped: + /* FALLTHROUGH */ case State::starting: if (starting_conditions) { @@ -116,6 +120,7 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed } } } + break; case State::active: @@ -124,8 +129,11 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed if (isTimedOut(aid_src.time_last_fuse, imu_delayed.time_us, ekf._params.no_aid_timeout_max) || (_reset_counters.lat_lon != sample.lat_lon_reset_counter)) { + ekf.resetHorizontalPositionTo(Vector2f(aid_src.observation), Vector2f(aid_src.observation_variance)); - aid_src.time_last_fuse = imu_delayed.time_us; + + ekf.resetAidSourceStatusZeroInnovation(aid_src); + _reset_counters.lat_lon = sample.lat_lon_reset_counter; } @@ -133,6 +141,7 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed ekf.disableControlStatusAuxGpos(); _state = State::stopped; } + break; default: @@ -143,6 +152,7 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed aid_src.timestamp = hrt_absolute_time(); _estimator_aid_src_aux_global_position_pub.publish(aid_src); #endif // MODULE_NAME + } else if ((_state != State::stopped) && isTimedOut(_time_last_buffer_push, imu_delayed.time_us, (uint64_t)5e6)) { ekf.disableControlStatusAuxGpos(); _state = State::stopped; 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 4ef94e31ee9e..132aaa10559b 100644 --- a/src/modules/ekf2/EKF/aid_sources/auxvel/auxvel_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/auxvel/auxvel_fusion.cpp @@ -36,13 +36,17 @@ void Ekf::controlAuxVelFusion() { if (_auxvel_buffer) { - auxVelSample auxvel_sample_delayed; + auxVelSample sample; - if (_auxvel_buffer->pop_first_older_than(_time_delayed_us, &auxvel_sample_delayed)) { + if (_auxvel_buffer->pop_first_older_than(_time_delayed_us, &sample)) { - resetEstimatorAidStatus(_aid_src_aux_vel); - - updateHorizontalVelocityAidSrcStatus(auxvel_sample_delayed.time_us, auxvel_sample_delayed.vel, auxvel_sample_delayed.velVar, fmaxf(_params.auxvel_gate, 1.f), _aid_src_aux_vel); + updateAidSourceStatus(_aid_src_aux_vel, + sample.time_us, // sample timestamp + sample.vel, // observation + sample.velVar, // observation variance + Vector2f(_state.vel.xy()) - sample.vel, // innovation + Vector2f(getStateVariance()) + sample.velVar, // innovation variance + math::max(_params.auxvel_gate, 1.f)); // innovation gate if (isHorizontalAidingActive()) { fuseHorizontalVelocity(_aid_src_aux_vel); @@ -55,5 +59,4 @@ void Ekf::stopAuxVelFusion() { ECL_INFO("stopping aux vel fusion"); //_control_status.flags.aux_vel = false; - resetEstimatorAidStatus(_aid_src_aux_vel); } 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 f8786c60f974..80c51e097954 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 @@ -59,8 +59,6 @@ void Ekf::controlBaroHeightFusion() const float measurement_var = sq(_params.baro_noise); - const float innov_gate = fmaxf(_params.baro_innov_gate, 1.f); - const bool measurement_valid = PX4_ISFINITE(measurement) && PX4_ISFINITE(measurement_var); if (measurement_valid) { @@ -80,11 +78,11 @@ void Ekf::controlBaroHeightFusion() } // vertical position innovation - baro measurement has opposite sign to earth z axis - updateVerticalPositionAidSrcStatus(baro_sample.time_us, - -(measurement - bias_est.getBias()), - measurement_var + bias_est.getBiasVar(), - innov_gate, - aid_src); + updateVerticalPositionAidStatus(aid_src, + baro_sample.time_us, + -(measurement - bias_est.getBias()), // observation + measurement_var + bias_est.getBiasVar(), // observation variance + math::max(_params.baro_innov_gate, 1.f)); // innovation gate // Compensate for positive static pressure transients (negative vertical position innovations) // caused by rotor wash ground interaction by applying a temporary deadzone to baro innovations. @@ -191,7 +189,6 @@ void Ekf::stopBaroHgtFusion() } _baro_b_est.setFusionInactive(); - resetEstimatorAidStatus(_aid_src_baro_hgt); _control_status.flags.baro_hgt = false; } diff --git a/src/modules/ekf2/EKF/aid_sources/drag/drag_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/drag/drag_fusion.cpp index 5b4baedf123f..c55ed14aaf4d 100644 --- a/src/modules/ekf2/EKF/aid_sources/drag/drag_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/drag/drag_fusion.cpp @@ -105,11 +105,16 @@ void Ekf::fuseDrag(const dragSample &drag_sample) bcoef_inv(1) = bcoef_inv(0); } - _aid_src_drag.timestamp_sample = drag_sample.time_us; - _aid_src_drag.fused = false; - bool fused[] {false, false}; + Vector2f observation{}; + Vector2f observation_variance{R_ACC, R_ACC}; + Vector2f innovation{}; + Vector2f innovation_variance{}; + + // Apply an innovation consistency check with a 5 Sigma threshold + const float innov_gate = 5.f; + VectorState H; // perform sequential fusion of XY specific forces @@ -120,16 +125,15 @@ void Ekf::fuseDrag(const dragSample &drag_sample) // Drag is modelled as an arbitrary combination of bluff body drag that proportional to // equivalent airspeed squared, and rotor momentum drag that is proportional to true airspeed // parallel to the rotor disc and mass flow through the rotor disc. - const float pred_acc = -0.5f * bcoef_inv(axis_index) * rho * rel_wind_body(axis_index) * rel_wind_speed - rel_wind_body(axis_index) * mcoef_corrrected; + const float pred_acc = -0.5f * bcoef_inv(axis_index) * rho * rel_wind_body(axis_index) * rel_wind_speed + - rel_wind_body(axis_index) * mcoef_corrrected; - _aid_src_drag.observation[axis_index] = mea_acc; - _aid_src_drag.observation_variance[axis_index] = R_ACC; - _aid_src_drag.innovation[axis_index] = pred_acc - mea_acc; - _aid_src_drag.innovation_variance[axis_index] = NAN; // reset + observation(axis_index) = mea_acc; + innovation(axis_index) = pred_acc - mea_acc; if (axis_index == 0) { sym::ComputeDragXInnovVarAndH(state_vector_prev, P, rho, bcoef_inv(axis_index), mcoef_corrrected, R_ACC, FLT_EPSILON, - &_aid_src_drag.innovation_variance[axis_index], &H); + &innovation_variance(axis_index), &H); if (!using_bcoef_x && !using_mcoef) { continue; @@ -137,35 +141,41 @@ void Ekf::fuseDrag(const dragSample &drag_sample) } else if (axis_index == 1) { sym::ComputeDragYInnovVarAndH(state_vector_prev, P, rho, bcoef_inv(axis_index), mcoef_corrrected, R_ACC, FLT_EPSILON, - &_aid_src_drag.innovation_variance[axis_index], &H); + &innovation_variance(axis_index), &H); if (!using_bcoef_y && !using_mcoef) { continue; } } - if (_aid_src_drag.innovation_variance[axis_index] < R_ACC) { + if (innovation_variance(axis_index) < R_ACC) { // calculation is badly conditioned - return; + break; } - // Apply an innovation consistency check with a 5 Sigma threshold - const float innov_gate = 5.f; - setEstimatorAidStatusTestRatio(_aid_src_drag, innov_gate); + const float test_ratio = sq(innovation(axis_index)) / (sq(innov_gate) * innovation_variance(axis_index)); if (_control_status.flags.in_air && _control_status.flags.wind && !_control_status.flags.fake_pos - && PX4_ISFINITE(_aid_src_drag.innovation_variance[axis_index]) && PX4_ISFINITE(_aid_src_drag.innovation[axis_index]) - && (_aid_src_drag.test_ratio[axis_index] < 1.f) + && PX4_ISFINITE(innovation_variance(axis_index)) && PX4_ISFINITE(innovation(axis_index)) + && (test_ratio < 1.f) ) { - VectorState K = P * H / _aid_src_drag.innovation_variance[axis_index]; + VectorState K = P * H / innovation_variance(axis_index); - if (measurementUpdate(K, H, R_ACC, _aid_src_drag.innovation[axis_index])) { + if (measurementUpdate(K, H, R_ACC, innovation(axis_index))) { fused[axis_index] = true; } } } + updateAidSourceStatus(_aid_src_drag, + drag_sample.time_us, // sample timestamp + observation, // observation + observation_variance, // observation variance + innovation, // innovation + innovation_variance, // innovation variance + innov_gate); // innovation gate + if (fused[0] && fused[1]) { _aid_src_drag.fused = true; _aid_src_drag.time_last_fuse = _time_delayed_us; 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 0780f4111814..f757b59a788e 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 @@ -69,9 +69,6 @@ void Ekf::controlExternalVisionFusion() _ev_sample_prev = ev_sample; } - // record corresponding yaw state for future EV delta heading innovation (logging only) - _ev_yaw_pred_prev = getEulerYaw(_state.quat_nominal); - } else if ((_control_status.flags.ev_pos || _control_status.flags.ev_vel || _control_status.flags.ev_yaw || _control_status.flags.ev_hgt) && isTimedOut(_ev_sample_prev.time_us, 2 * EV_MAX_INTERVAL)) { 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 0552ace53c76..e4308c4fcf76 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 @@ -85,11 +85,11 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com const bool measurement_valid = PX4_ISFINITE(measurement) && PX4_ISFINITE(measurement_var); - updateVerticalPositionAidSrcStatus(ev_sample.time_us, - measurement - bias_est.getBias(), - measurement_var + bias_est.getBiasVar(), - math::max(_params.ev_pos_innov_gate, 1.f), - aid_src); + updateVerticalPositionAidStatus(aid_src, + ev_sample.time_us, + measurement - bias_est.getBias(), // observation + measurement_var + bias_est.getBiasVar(), // observation variance + math::max(_params.ev_pos_innov_gate, 1.f)); // innovation gate // update the bias estimator before updating the main filter but after // using its current state to compute the vertical position innovation @@ -220,7 +220,6 @@ void Ekf::stopEvHgtFusion() } _ev_hgt_b_est.setFusionInactive(); - resetEstimatorAidStatus(_aid_src_ev_hgt); _control_status.flags.ev_hgt = false; } 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 3f6027400ade..1e6e7904ddce 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 @@ -126,12 +126,14 @@ void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common } #if defined(CONFIG_EKF2_GNSS) + // increase minimum variance if GPS active (position reference) if (_control_status.flags.gps) { for (int i = 0; i < 2; i++) { pos_cov(i, i) = math::max(pos_cov(i, i), sq(_params.gps_pos_noise)); } } + #endif // CONFIG_EKF2_GNSS const Vector2f measurement{pos(0), pos(1)}; @@ -155,18 +157,24 @@ void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common } } - updateHorizontalPositionAidSrcStatus(ev_sample.time_us, - measurement - _ev_pos_b_est.getBias(), // observation - measurement_var + _ev_pos_b_est.getBiasVar(), // observation variance - math::max(_params.ev_pos_innov_gate, 1.f), // innovation gate - aid_src); + const Vector2f position = measurement - _ev_pos_b_est.getBias(); + const Vector2f pos_obs_var = measurement_var + _ev_pos_b_est.getBiasVar(); + + updateAidSourceStatus(aid_src, + ev_sample.time_us, // sample timestamp + position, // observation + pos_obs_var, // observation variance + Vector2f(_state.pos) - position, // innovation + Vector2f(getStateVariance()) + pos_obs_var, // innovation variance + math::max(_params.ev_pos_innov_gate, 1.f)); // innovation gate // update the bias estimator before updating the main filter but after // using its current state to compute the vertical position innovation if (measurement_valid && quality_sufficient) { _ev_pos_b_est.setMaxStateNoise(Vector2f(sqrtf(measurement_var(0)), sqrtf(measurement_var(1)))); _ev_pos_b_est.setProcessNoiseSpectralDensity(_params.ev_hgt_bias_nsd); // TODO - _ev_pos_b_est.fuseBias(measurement - Vector2f(_state.pos.xy()), measurement_var + Vector2f(getStateVariance())); + _ev_pos_b_est.fuseBias(measurement - Vector2f(_state.pos.xy()), + measurement_var + Vector2f(getStateVariance())); } if (!measurement_valid) { @@ -297,8 +305,6 @@ void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measure void Ekf::stopEvPosFusion() { if (_control_status.flags.ev_pos) { - resetEstimatorAidStatus(_aid_src_ev_pos); - _control_status.flags.ev_pos = false; } } 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 aff345ac1140..e612452b762b 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 @@ -107,12 +107,14 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common } #if defined(CONFIG_EKF2_GNSS) + // increase minimum variance if GPS active (position reference) if (_control_status.flags.gps) { for (int i = 0; i < 2; i++) { vel_cov(i, i) = math::max(vel_cov(i, i), sq(_params.gps_vel_noise)); } } + #endif // CONFIG_EKF2_GNSS const Vector3f measurement{vel}; @@ -125,11 +127,13 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common const bool measurement_valid = measurement.isAllFinite() && measurement_var.isAllFinite(); - updateVelocityAidSrcStatus(ev_sample.time_us, - measurement, // observation - measurement_var, // observation variance - math::max(_params.ev_vel_innov_gate, 1.f), // innovation gate - aid_src); + updateAidSourceStatus(aid_src, + ev_sample.time_us, // sample timestamp + measurement, // observation + measurement_var, // observation variance + _state.vel - measurement, // innovation + getVelocityVariance() + measurement_var, // innovation variance + math::max(_params.ev_vel_innov_gate, 1.f)); // innovation gate if (!measurement_valid) { continuing_conditions_passing = false; @@ -149,7 +153,7 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common ECL_INFO("reset to %s", AID_SRC_NAME); _information_events.flags.reset_vel_to_vision = true; resetVelocityTo(measurement, measurement_var); - aid_src.time_last_fuse = _time_delayed_us; + resetAidSourceStatusZeroInnovation(aid_src); } else { // EV has reset, but quality isn't sufficient @@ -174,7 +178,7 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common _information_events.flags.reset_vel_to_vision = true; ECL_WARN("%s fusion failing, resetting", AID_SRC_NAME); resetVelocityTo(measurement, measurement_var); - aid_src.time_last_fuse = _time_delayed_us; + resetAidSourceStatusZeroInnovation(aid_src); if (_control_status.flags.in_air) { _nb_ev_vel_reset_available--; @@ -205,19 +209,25 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common if (starting_conditions_passing) { // activate fusion, only reset if necessary if (!isHorizontalAidingActive() || yaw_alignment_changed) { - ECL_INFO("starting %s fusion, resetting velocity to (%.3f, %.3f, %.3f)", AID_SRC_NAME, (double)measurement(0), (double)measurement(1), (double)measurement(2)); + ECL_INFO("starting %s fusion, resetting velocity to (%.3f, %.3f, %.3f)", AID_SRC_NAME, + (double)measurement(0), (double)measurement(1), (double)measurement(2)); + _information_events.flags.reset_vel_to_vision = true; + resetVelocityTo(measurement, measurement_var); + resetAidSourceStatusZeroInnovation(aid_src); - } else { + _control_status.flags.ev_vel = true; + + } else if (fuseVelocity(aid_src)) { ECL_INFO("starting %s fusion", AID_SRC_NAME); + _control_status.flags.ev_vel = true; } - aid_src.time_last_fuse = _time_delayed_us; - - _nb_ev_vel_reset_available = 5; - _information_events.flags.starting_vision_vel_fusion = true; - _control_status.flags.ev_vel = true; + if (_control_status.flags.ev_vel) { + _nb_ev_vel_reset_available = 5; + _information_events.flags.starting_vision_vel_fusion = true; + } } } } @@ -225,7 +235,6 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common void Ekf::stopEvVelFusion() { if (_control_status.flags.ev_vel) { - resetEstimatorAidStatus(_aid_src_ev_vel); _control_status.flags.ev_vel = false; } 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 3afb3b78fced..d974a5e87b81 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 @@ -43,11 +43,22 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common { static constexpr const char *AID_SRC_NAME = "EV yaw"; - resetEstimatorAidStatus(aid_src); - aid_src.timestamp_sample = ev_sample.time_us; - aid_src.observation = getEulerYaw(ev_sample.quat); - aid_src.observation_variance = math::max(ev_sample.orientation_var(2), _params.ev_att_noise, sq(0.01f)); - aid_src.innovation = wrap_pi(getEulerYaw(_R_to_earth) - aid_src.observation); + float obs = getEulerYaw(ev_sample.quat); + float obs_var = math::max(ev_sample.orientation_var(2), _params.ev_att_noise, sq(0.01f)); + + float innov = wrap_pi(getEulerYaw(_R_to_earth) - obs); + float innov_var = 0.f; + + VectorState H_YAW; + computeYawInnovVarAndH(obs_var, innov_var, H_YAW); + + updateAidSourceStatus(aid_src, + ev_sample.time_us, // sample timestamp + obs, // observation + obs_var, // observation variance + innov, // innovation + innov_var, // innovation variance + math::max(_params.heading_innov_gate, 1.f)); // innovation gate if (ev_reset) { _control_status.flags.ev_yaw_fault = false; @@ -65,10 +76,6 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common && (ev_sample.pos_frame != PositionFrame::LOCAL_FRAME_NED) ) { continuing_conditions_passing = false; - - // use delta yaw for innovation logging - aid_src.innovation = wrap_pi(wrap_pi(getEulerYaw(_R_to_earth) - _ev_yaw_pred_prev) - - wrap_pi(getEulerYaw(ev_sample.quat) - getEulerYaw(_ev_sample_prev.quat))); } const bool starting_conditions_passing = common_starting_conditions_passing @@ -94,7 +101,7 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common } } else if (quality_sufficient) { - fuseYaw(aid_src); + fuseYaw(aid_src, H_YAW); } else { aid_src.innovation_rejected = true; @@ -141,18 +148,25 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common if (ev_sample.pos_frame == PositionFrame::LOCAL_FRAME_NED) { if (_control_status.flags.yaw_align) { - ECL_INFO("starting %s fusion", AID_SRC_NAME); + + if (fuseYaw(aid_src, H_YAW)) { + ECL_INFO("starting %s fusion", AID_SRC_NAME); + _information_events.flags.starting_vision_yaw_fusion = true; + + _control_status.flags.ev_yaw = true; + } } else { // reset yaw to EV and set yaw_align ECL_INFO("starting %s fusion, resetting state", AID_SRC_NAME); + _information_events.flags.starting_vision_yaw_fusion = true; + resetQuatStateYaw(aid_src.observation, aid_src.observation_variance); _control_status.flags.yaw_align = true; - } + _control_status.flags.ev_yaw = true; - aid_src.time_last_fuse = _time_delayed_us; - _information_events.flags.starting_vision_yaw_fusion = true; - _control_status.flags.ev_yaw = true; + aid_src.time_last_fuse = _time_delayed_us; + } } else if (ev_sample.pos_frame == PositionFrame::LOCAL_FRAME_FRD) { // turn on fusion of external vision yaw measurements @@ -178,7 +192,6 @@ void Ekf::stopEvYawFusion() { #if defined(CONFIG_EKF2_EXTERNAL_VISION) if (_control_status.flags.ev_yaw) { - resetEstimatorAidStatus(_aid_src_ev_yaw); _control_status.flags.ev_yaw = false; } diff --git a/src/modules/ekf2/EKF/aid_sources/fake_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/fake_height_control.cpp index 8b7eba566413..4ecb5b532564 100644 --- a/src/modules/ekf2/EKF/aid_sources/fake_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/fake_height_control.cpp @@ -51,8 +51,7 @@ void Ekf::controlFakeHgtFusion() const float obs_var = sq(_params.pos_noaid_noise); const float innov_gate = 3.f; - updateVerticalPositionAidSrcStatus(_time_delayed_us, _last_known_pos(2), obs_var, innov_gate, aid_src); - + updateVerticalPositionAidStatus(aid_src, _time_delayed_us, _last_known_pos(2), obs_var, innov_gate); const bool continuing_conditions_passing = !isVerticalAidingActive(); const bool starting_conditions_passing = continuing_conditions_passing @@ -113,7 +112,5 @@ void Ekf::stopFakeHgtFusion() if (_control_status.flags.fake_hgt) { ECL_INFO("stop fake height fusion"); _control_status.flags.fake_hgt = false; - - resetEstimatorAidStatus(_aid_src_fake_hgt); } } diff --git a/src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp b/src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp index cb3d246f235b..33f05ac68e8f 100644 --- a/src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp @@ -63,14 +63,21 @@ void Ekf::controlFakePosFusion() obs_var(0) = obs_var(1) = sq(0.5f); } - const float innov_gate = 3.f; + const Vector2f position(_last_known_pos); - updateHorizontalPositionAidSrcStatus(_time_delayed_us, Vector2f(_last_known_pos), obs_var, innov_gate, aid_src); + const float innov_gate = 3.f; + updateAidSourceStatus(aid_src, + _time_delayed_us, + position, // observation + obs_var, // observation variance + Vector2f(_state.pos) - position, // innovation + Vector2f(getStateVariance()) + obs_var, // innovation variance + innov_gate); // innovation gate const bool continuing_conditions_passing = !isHorizontalAidingActive() - && ((getTiltVariance() > sq(math::radians(3.f))) || _control_status.flags.vehicle_at_rest) - && (!(_params.imu_ctrl & static_cast(ImuCtrl::GravityVector)) || _control_status.flags.vehicle_at_rest); + && ((getTiltVariance() > sq(math::radians(3.f))) || _control_status.flags.vehicle_at_rest) + && (!(_params.imu_ctrl & static_cast(ImuCtrl::GravityVector)) || _control_status.flags.vehicle_at_rest); const bool starting_conditions_passing = continuing_conditions_passing && _horizontal_deadreckon_time_exceeded; @@ -131,7 +138,5 @@ void Ekf::stopFakePosFusion() if (_control_status.flags.fake_pos) { ECL_INFO("stop fake position fusion"); _control_status.flags.fake_pos = false; - - resetEstimatorAidStatus(_aid_src_fake_pos); } } diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp index aafefd8bff1c..ff04cc3b3977 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp @@ -67,16 +67,14 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample) const float measurement = gnss_alt - getEkfGlobalOriginAltitude(); const float measurement_var = sq(noise); - const float innov_gate = math::max(_params.gps_pos_innov_gate, 1.f); - const bool measurement_valid = PX4_ISFINITE(measurement) && PX4_ISFINITE(measurement_var); // GNSS position, vertical position GNSS measurement has opposite sign to earth z axis - updateVerticalPositionAidSrcStatus(gps_sample.time_us, - -(measurement - bias_est.getBias()), - measurement_var + bias_est.getBiasVar(), - innov_gate, - aid_src); + updateVerticalPositionAidStatus(aid_src, + gps_sample.time_us, + -(measurement - bias_est.getBias()), + measurement_var + bias_est.getBiasVar(), + math::max(_params.gps_pos_innov_gate, 1.f)); // update the bias estimator before updating the main filter but after // using its current state to compute the vertical position innovation @@ -171,7 +169,6 @@ void Ekf::stopGpsHgtFusion() } _gps_hgt_b_est.setFusionInactive(); - resetEstimatorAidStatus(_aid_src_gnss_hgt); _control_status.flags.gps_hgt = false; } 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 ffa85706767b..a6bb067f4b73 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp @@ -52,8 +52,8 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) // run EKF-GSF yaw estimator once per imu_delayed update _yawEstimator.predict(imu_delayed.delta_ang, imu_delayed.delta_ang_dt, - imu_delayed.delta_vel, imu_delayed.delta_vel_dt, - (_control_status.flags.in_air && !_control_status.flags.vehicle_at_rest)); + imu_delayed.delta_vel, imu_delayed.delta_vel_dt, + (_control_status.flags.in_air && !_control_status.flags.vehicle_at_rest)); _gps_intermittent = !isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GNSS_MAX_INTERVAL); @@ -183,13 +183,26 @@ void Ekf::updateGnssVel(const gnssSample &gnss_sample, estimator_aid_source3d_s const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body; const Vector3f velocity = gnss_sample.vel - vel_offset_earth; - const float vel_var = sq(math::max(gnss_sample.sacc, _params.gps_vel_noise)); + const float vel_var = sq(math::max(gnss_sample.sacc, _params.gps_vel_noise, 0.01f)); const Vector3f vel_obs_var(vel_var, vel_var, vel_var * sq(1.5f)); - updateVelocityAidSrcStatus(gnss_sample.time_us, - velocity, // observation - vel_obs_var, // observation variance - math::max(_params.gps_vel_innov_gate, 1.f), // innovation gate - aid_src); + + const float innovation_gate = math::max(_params.gps_vel_innov_gate, 1.f); + + updateAidSourceStatus(aid_src, + gnss_sample.time_us, // sample timestamp + velocity, // observation + vel_obs_var, // observation variance + _state.vel - velocity, // innovation + getVelocityVariance() + vel_obs_var, // innovation variance + innovation_gate); // innovation gate + + // vz special case if there is bad vertical acceleration data, then don't reject measurement if GNSS reports velocity accuracy is acceptable, + // but limit innovation to prevent spikes that could destabilise the filter + if (aid_src.innovation_rejected && _fault_status.flags.bad_acc_vertical && (gnss_sample.sacc < _params.req_sacc)) { + const float innov_limit = innovation_gate * sqrtf(aid_src.innovation_variance[2]); + aid_src.innovation[2] = math::constrain(aid_src.innovation[2], -innov_limit, innov_limit); + aid_src.innovation_rejected = false; + } } void Ekf::updateGnssPos(const gnssSample &gnss_sample, estimator_aid_source2d_s &aid_src) @@ -210,13 +223,16 @@ void Ekf::updateGnssPos(const gnssSample &gnss_sample, estimator_aid_source2d_s } } - const float pos_var = sq(pos_noise); + const float pos_var = math::max(sq(pos_noise), sq(0.01f)); const Vector2f pos_obs_var(pos_var, pos_var); - updateHorizontalPositionAidSrcStatus(gnss_sample.time_us, - position, // observation - pos_obs_var, // observation variance - math::max(_params.gps_pos_innov_gate, 1.f), // innovation gate - aid_src); + + updateAidSourceStatus(aid_src, + gnss_sample.time_us, // sample timestamp + position, // observation + pos_obs_var, // observation variance + Vector2f(_state.pos) - position, // innovation + Vector2f(getStateVariance()) + pos_obs_var, // innovation variance + math::max(_params.gps_pos_innov_gate, 1.f)); // innovation gate } void Ekf::controlGnssYawEstimator(estimator_aid_source3d_s &aid_src_vel) @@ -289,7 +305,8 @@ void Ekf::resetVelocityToGnss(estimator_aid_source3d_s &aid_src) { _information_events.flags.reset_vel_to_gps = true; resetVelocityTo(Vector3f(aid_src.observation), Vector3f(aid_src.observation_variance)); - aid_src.time_last_fuse = _time_delayed_us; + + resetAidSourceStatusZeroInnovation(aid_src); } void Ekf::resetHorizontalPositionToGnss(estimator_aid_source2d_s &aid_src) @@ -297,7 +314,8 @@ void Ekf::resetHorizontalPositionToGnss(estimator_aid_source2d_s &aid_src) _information_events.flags.reset_pos_to_gps = true; resetHorizontalPositionTo(Vector2f(aid_src.observation), Vector2f(aid_src.observation_variance)); _gpos_origin_eph = 0.f; // The uncertainty of the global origin is now contained in the local position uncertainty - aid_src.time_last_fuse = _time_delayed_us; + + resetAidSourceStatusZeroInnovation(aid_src); } bool Ekf::shouldResetGpsFusion() const @@ -342,12 +360,12 @@ void Ekf::controlGpsYawFusion(const gnssSample &gps_sample) return; } - updateGpsYaw(gps_sample); - const bool is_new_data_available = PX4_ISFINITE(gps_sample.yaw); if (is_new_data_available) { + updateGpsYaw(gps_sample); + const bool continuing_conditions_passing = _control_status.flags.tilt_align; const bool is_gps_yaw_data_intermittent = !isNewestSampleRecent(_time_last_gps_yaw_buffer_push, @@ -392,7 +410,9 @@ void Ekf::controlGpsYawFusion(const gnssSample &gps_sample) // Reset before starting the fusion if (resetYawToGps(gps_sample.yaw, gps_sample.yaw_offset)) { - _aid_src_gnss_yaw.time_last_fuse = _time_delayed_us; + + resetAidSourceStatusZeroInnovation(_aid_src_gnss_yaw); + _control_status.flags.gps_yaw = true; _control_status.flags.yaw_align = true; } @@ -422,7 +442,6 @@ void Ekf::stopGpsYawFusion() if (_control_status.flags.gps_yaw) { _control_status.flags.gps_yaw = false; - resetEstimatorAidStatus(_aid_src_gnss_yaw); ECL_INFO("stopping GPS yaw fusion"); } @@ -433,8 +452,7 @@ void Ekf::stopGpsFusion() { if (_control_status.flags.gps) { ECL_INFO("stopping GPS position and velocity fusion"); - resetEstimatorAidStatus(_aid_src_gnss_pos); - resetEstimatorAidStatus(_aid_src_gnss_vel); + _last_gps_fail_us = 0; _last_gps_pass_us = 0; diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_yaw_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_yaw_fusion.cpp index 13c057f26653..2e135f357f20 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_yaw_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_yaw_fusion.cpp @@ -48,44 +48,33 @@ void Ekf::updateGpsYaw(const gnssSample &gps_sample) { - if (PX4_ISFINITE(gps_sample.yaw)) { + // calculate the observed yaw angle of antenna array, converting a from body to antenna yaw measurement + const float measured_hdg = wrap_pi(gps_sample.yaw + gps_sample.yaw_offset); - auto &gnss_yaw = _aid_src_gnss_yaw; - resetEstimatorAidStatus(gnss_yaw); + const float yaw_acc = PX4_ISFINITE(gps_sample.yaw_acc) ? gps_sample.yaw_acc : 0.f; + const float R_YAW = sq(fmaxf(yaw_acc, _params.gps_heading_noise)); - // initially populate for estimator_aid_src_gnss_yaw logging - - // calculate the observed yaw angle of antenna array, converting a from body to antenna yaw measurement - const float measured_hdg = wrap_pi(gps_sample.yaw + gps_sample.yaw_offset); - - const float yaw_acc = PX4_ISFINITE(gps_sample.yaw_acc) ? gps_sample.yaw_acc : 0.f; - const float R_YAW = sq(fmaxf(yaw_acc, _params.gps_heading_noise)); - - float heading_pred; - float heading_innov_var; - - { - VectorState H; - sym::ComputeGnssYawPredInnovVarAndH(_state.vector(), P, gps_sample.yaw_offset, R_YAW, FLT_EPSILON, &heading_pred, &heading_innov_var, &H); - } - - gnss_yaw.observation = measured_hdg; - gnss_yaw.observation_variance = R_YAW; - gnss_yaw.innovation = wrap_pi(heading_pred - measured_hdg); - gnss_yaw.innovation_variance = heading_innov_var; - - gnss_yaw.timestamp_sample = gps_sample.time_us; + float heading_pred; + float heading_innov_var; - const float innov_gate = math::max(_params.heading_innov_gate, 1.0f); - setEstimatorAidStatusTestRatio(gnss_yaw, innov_gate); - } + VectorState H; + sym::ComputeGnssYawPredInnovVarAndH(_state.vector(), P, gps_sample.yaw_offset, R_YAW, FLT_EPSILON, + &heading_pred, &heading_innov_var, &H); + + updateAidSourceStatus(_aid_src_gnss_yaw, + gps_sample.time_us, // sample timestamp + measured_hdg, // observation + R_YAW, // observation variance + wrap_pi(heading_pred - measured_hdg), // innovation + heading_innov_var, // innovation variance + math::max(_params.heading_innov_gate, 1.f)); // innovation gate } void Ekf::fuseGpsYaw(float antenna_yaw_offset) { - auto &gnss_yaw = _aid_src_gnss_yaw; + auto &aid_src = _aid_src_gnss_yaw; - if (gnss_yaw.innovation_rejected) { + if (aid_src.innovation_rejected) { _innov_check_fail_status.flags.reject_yaw = true; return; } @@ -94,19 +83,17 @@ void Ekf::fuseGpsYaw(float antenna_yaw_offset) antenna_yaw_offset = 0.f; } - VectorState H; - - { float heading_pred; float heading_innov_var; + VectorState H; // Note: we recompute innov and innov_var because it doesn't cost much more than just computing H // making a separate function just for H uses more flash space without reducing CPU load significantly - sym::ComputeGnssYawPredInnovVarAndH(_state.vector(), P, antenna_yaw_offset, gnss_yaw.observation_variance, FLT_EPSILON, &heading_pred, &heading_innov_var, &H); - } + sym::ComputeGnssYawPredInnovVarAndH(_state.vector(), P, antenna_yaw_offset, aid_src.observation_variance, FLT_EPSILON, + &heading_pred, &heading_innov_var, &H); // check if the innovation variance calculation is badly conditioned - if (gnss_yaw.innovation_variance < gnss_yaw.observation_variance) { + if (aid_src.innovation_variance < aid_src.observation_variance) { // the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned _fault_status.flags.bad_hdg = true; @@ -119,11 +106,9 @@ void Ekf::fuseGpsYaw(float antenna_yaw_offset) _fault_status.flags.bad_hdg = false; _innov_check_fail_status.flags.reject_yaw = false; - _gnss_yaw_signed_test_ratio_lpf.update(matrix::sign(gnss_yaw.innovation) * gnss_yaw.test_ratio); - - if ((fabsf(_gnss_yaw_signed_test_ratio_lpf.getState()) > 0.2f) - && !_control_status.flags.in_air && isTimedOut(gnss_yaw.time_last_fuse, (uint64_t)1e6)) { - + if ((fabsf(aid_src.test_ratio_filtered) > 0.2f) + && !_control_status.flags.in_air && isTimedOut(aid_src.time_last_fuse, (uint64_t)1e6) + ) { // A constant large signed test ratio is a sign of wrong gyro bias // Reset the yaw gyro variance to converge faster and avoid // being stuck on a previous bad estimate @@ -132,15 +117,15 @@ void Ekf::fuseGpsYaw(float antenna_yaw_offset) // calculate the Kalman gains // only calculate gains for states we are using - VectorState Kfusion = P * H / gnss_yaw.innovation_variance; + VectorState Kfusion = P * H / aid_src.innovation_variance; - const bool is_fused = measurementUpdate(Kfusion, H, gnss_yaw.observation_variance, gnss_yaw.innovation); + const bool is_fused = measurementUpdate(Kfusion, H, aid_src.observation_variance, aid_src.innovation); _fault_status.flags.bad_hdg = !is_fused; - gnss_yaw.fused = is_fused; + aid_src.fused = is_fused; if (is_fused) { _time_last_heading_fuse = _time_delayed_us; - gnss_yaw.time_last_fuse = _time_delayed_us; + aid_src.time_last_fuse = _time_delayed_us; } } @@ -161,8 +146,5 @@ bool Ekf::resetYawToGps(const float gnss_yaw, const float gnss_yaw_offset) const float yaw_variance = sq(fmaxf(_params.gps_heading_noise, 1.e-2f)); resetQuatStateYaw(measured_yaw, yaw_variance); - _aid_src_gnss_yaw.time_last_fuse = _time_delayed_us; - _gnss_yaw_signed_test_ratio_lpf.reset(0.f); - return true; } diff --git a/src/modules/ekf2/EKF/aid_sources/gravity/gravity_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/gravity/gravity_fusion.cpp index 70a0a8ded200..40127a37548e 100644 --- a/src/modules/ekf2/EKF/aid_sources/gravity/gravity_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gravity/gravity_fusion.cpp @@ -69,19 +69,13 @@ void Ekf::controlGravityFusion(const imuSample &imu) sym::ComputeGravityXyzInnovVarAndHx(state_vector, P, measurement_var, &innovation_variance, &H); // fill estimator aid source status - resetEstimatorAidStatus(_aid_src_gravity); - _aid_src_gravity.timestamp_sample = imu.time_us; - measurement.copyTo(_aid_src_gravity.observation); - - for (auto &var : _aid_src_gravity.observation_variance) { - var = measurement_var; - } - - innovation.copyTo(_aid_src_gravity.innovation); - innovation_variance.copyTo(_aid_src_gravity.innovation_variance); - - float innovation_gate = 0.25f; - setEstimatorAidStatusTestRatio(_aid_src_gravity, innovation_gate); + updateAidSourceStatus(_aid_src_gravity, + imu.time_us, // sample timestamp + measurement, // observation + Vector3f{measurement_var, measurement_var, measurement_var}, // observation variance + innovation, // innovation + innovation_variance, // innovation variance + 0.25f); // innovation gate bool fused[3] {false, false, false}; 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 5bc5370f4859..8bf976b8fa5e 100644 --- a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp @@ -97,10 +97,6 @@ void Ekf::controlMagFusion() _fault_status.flags.bad_mag_y = false; _fault_status.flags.bad_mag_z = false; - - resetEstimatorAidStatus(aid_src); - aid_src.timestamp_sample = mag_sample.time_us; - // XYZ Measurement uncertainty. Need to consider timing errors for fast rotations const float R_MAG = math::max(sq(_params.mag_noise), sq(0.01f)); @@ -112,15 +108,13 @@ void Ekf::controlMagFusion() VectorState H; sym::ComputeMagInnovInnovVarAndHx(_state.vector(), P, mag_sample.mag, R_MAG, FLT_EPSILON, &mag_innov, &innov_var, &H); - for (int i = 0; i < 3; i++) { - aid_src.observation[i] = mag_sample.mag(i); - aid_src.observation_variance[i] = R_MAG; - aid_src.innovation[i] = mag_innov(i); - aid_src.innovation_variance[i] = innov_var(i); - } - - const float innov_gate = math::max(_params.mag_innov_gate, 1.f); - setEstimatorAidStatusTestRatio(aid_src, innov_gate); + updateAidSourceStatus(aid_src, + mag_sample.time_us, // sample timestamp + mag_sample.mag, // observation + Vector3f(R_MAG, R_MAG, R_MAG), // observation variance + mag_innov, // innovation + innov_var, // innovation variance + math::max(_params.mag_innov_gate, 1.f)); // innovation gate // Perform an innovation consistency check and report the result _innov_check_fail_status.flags.reject_mag_x = (aid_src.test_ratio[0] > 1.f); diff --git a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp index 160b0b24308e..7758a64da360 100644 --- a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp @@ -164,8 +164,6 @@ void Ekf::stopFlowFusion() if (_control_status.flags.opt_flow) { ECL_INFO("stopping optical flow fusion"); _control_status.flags.opt_flow = false; - - resetEstimatorAidStatus(_aid_src_optical_flow); } } diff --git a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp index 5dd257c7251d..38186ebadf25 100644 --- a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp @@ -50,9 +50,6 @@ void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src) { - resetEstimatorAidStatus(aid_src); - aid_src.timestamp_sample = _flow_sample_delayed.time_us; - const Vector2f vel_body = predictFlowVelBody(); const float range = predictFlowRange(); @@ -66,24 +63,26 @@ void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src) _flow_vel_body(1) = opt_flow_rate(0) * range; _flow_vel_ne = Vector2f(_R_to_earth * Vector3f(_flow_vel_body(0), _flow_vel_body(1), 0.f)); - aid_src.observation[0] = opt_flow_rate(0); // flow around the X axis - aid_src.observation[1] = opt_flow_rate(1); // flow around the Y axis - - aid_src.innovation[0] = (vel_body(1) / range) - aid_src.observation[0]; - aid_src.innovation[1] = (-vel_body(0) / range) - aid_src.observation[1]; + Vector2f innovation{ + (vel_body(1) / range) - opt_flow_rate(0), + (-vel_body(0) / range) - opt_flow_rate(1) + }; // calculate the optical flow observation variance const float R_LOS = calcOptFlowMeasVar(_flow_sample_delayed); - aid_src.observation_variance[0] = R_LOS; - aid_src.observation_variance[1] = R_LOS; Vector2f innov_var; VectorState H; sym::ComputeFlowXyInnovVarAndHx(_state.vector(), P, range, R_LOS, FLT_EPSILON, &innov_var, &H); - innov_var.copyTo(aid_src.innovation_variance); // run the innovation consistency check and record result - setEstimatorAidStatusTestRatio(aid_src, math::max(_params.flow_innov_gate, 1.f)); + updateAidSourceStatus(aid_src, + _flow_sample_delayed.time_us, // sample timestamp + opt_flow_rate, // observation + Vector2f{R_LOS, R_LOS}, // observation variance + innovation, // innovation + innov_var, // innovation variance + math::max(_params.flow_innov_gate, 1.f)); // innovation gate } void Ekf::fuseOptFlow() @@ -101,17 +100,13 @@ void Ekf::fuseOptFlow() sym::ComputeFlowXyInnovVarAndHx(state_vector, P, range, R_LOS, FLT_EPSILON, &innov_var, &H); innov_var.copyTo(_aid_src_optical_flow.innovation_variance); - if ((_aid_src_optical_flow.innovation_variance[0] < R_LOS) - || (_aid_src_optical_flow.innovation_variance[1] < R_LOS)) { + if ((innov_var(0) < R_LOS) || (innov_var(1) < R_LOS)) { // we need to reinitialise the covariance matrix and abort this fusion step ECL_ERR("Opt flow error - covariance reset"); initialiseCovariance(); return; } - // run the innovation consistency check and record result - setEstimatorAidStatusTestRatio(_aid_src_optical_flow, math::max(_params.flow_innov_gate, 1.f)); - _innov_check_fail_status.flags.reject_optflow_X = (_aid_src_optical_flow.test_ratio[0] > 1.f); _innov_check_fail_status.flags.reject_optflow_Y = (_aid_src_optical_flow.test_ratio[1] > 1.f); 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 8b4b96712d47..76e815ac738c 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 @@ -102,16 +102,14 @@ void Ekf::controlRangeHeightFusion() const float measurement = math::max(_range_sensor.getDistBottom(), _params.rng_gnd_clearance); const float measurement_var = sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sensor.getDistBottom()); - const float innov_gate = math::max(_params.range_innov_gate, 1.f); - const bool measurement_valid = PX4_ISFINITE(measurement) && PX4_ISFINITE(measurement_var); // vertical position innovation - baro measurement has opposite sign to earth z axis - updateVerticalPositionAidSrcStatus(_range_sensor.getSampleAddress()->time_us, - -(measurement - bias_est.getBias()), - measurement_var + bias_est.getBiasVar(), - innov_gate, - aid_src); + updateVerticalPositionAidStatus(aid_src, + _range_sensor.getSampleAddress()->time_us, // sample timestamp + -(measurement - bias_est.getBias()), // observation + measurement_var + bias_est.getBiasVar(), // observation variance + math::max(_params.range_innov_gate, 1.f)); // innovation gate // update the bias estimator before updating the main filter but after // using its current state to compute the vertical position innovation @@ -170,7 +168,7 @@ void Ekf::controlRangeHeightFusion() if (starting_conditions_passing) { if ((_params.height_sensor_ref == static_cast(HeightSensor::RANGE)) && (_params.rng_ctrl == static_cast(RngCtrl::CONDITIONAL)) - ) { + ) { // Range finder is used while hovering to stabilize the height estimate. Don't reset but use it as height reference. ECL_INFO("starting conditional %s height fusion", HGT_SRC_NAME); _height_sensor_ref = HeightSensor::RANGE; @@ -178,7 +176,7 @@ void Ekf::controlRangeHeightFusion() } else if ((_params.height_sensor_ref == static_cast(HeightSensor::RANGE)) && (_params.rng_ctrl != static_cast(RngCtrl::CONDITIONAL)) - ) { + ) { // Range finder is the primary height source, the ground is now the datum used // to compute the local vertical position ECL_INFO("starting %s height fusion, resetting height", HGT_SRC_NAME); @@ -259,7 +257,6 @@ void Ekf::stopRngHgtFusion() } _rng_hgt_b_est.setFusionInactive(); - resetEstimatorAidStatus(_aid_src_rng_hgt); _control_status.flags.rng_hgt = false; } diff --git a/src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp index c35501f241c9..997b474793b5 100644 --- a/src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp @@ -49,8 +49,10 @@ void Ekf::controlBetaFusion(const imuSample &imu_delayed) { - _control_status.flags.fuse_beta = _params.beta_fusion_enabled && _control_status.flags.fixed_wing - && _control_status.flags.in_air && !_control_status.flags.fake_pos; + _control_status.flags.fuse_beta = _params.beta_fusion_enabled + && _control_status.flags.fixed_wing + && _control_status.flags.in_air + && !_control_status.flags.fake_pos; if (_control_status.flags.fuse_beta) { @@ -76,26 +78,22 @@ void Ekf::controlBetaFusion(const imuSample &imu_delayed) } } -void Ekf::updateSideslip(estimator_aid_source1d_s &sideslip) const +void Ekf::updateSideslip(estimator_aid_source1d_s &aid_src) const { - // reset flags - resetEstimatorAidStatus(sideslip); + float observation = 0.f; + const float R = math::max(sq(_params.beta_noise), sq(0.01f)); // observation noise variance - const float R = sq(_params.beta_noise); // observation noise variance - - float innov = 0.f; - float innov_var = 0.f; + float innov; + float innov_var; sym::ComputeSideslipInnovAndInnovVar(_state.vector(), P, R, FLT_EPSILON, &innov, &innov_var); - sideslip.observation = 0.f; - sideslip.observation_variance = R; - sideslip.innovation = innov; - sideslip.innovation_variance = innov_var; - - sideslip.timestamp_sample = _time_delayed_us; - - const float innov_gate = fmaxf(_params.beta_innov_gate, 1.f); - setEstimatorAidStatusTestRatio(sideslip, innov_gate); + updateAidSourceStatus(aid_src, + _time_delayed_us, // sample timestamp + observation, // observation + R, // observation variance + innov, // innovation + innov_var, // innovation variance + math::max(_params.beta_innov_gate, 1.f)); // innovation gate } void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip) @@ -103,6 +101,7 @@ void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip) if (sideslip.innovation_rejected) { return; } + // determine if we need the sideslip fusion to correct states other than wind bool update_wind_only = !_control_status.flags.wind_dead_reckoning; diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index 7b75211edd7e..8dc7e35d6844 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -124,53 +124,6 @@ void Ekf::reset() _time_good_vert_accel = 0; _clip_counter = 0; -#if defined(CONFIG_EKF2_BAROMETER) - resetEstimatorAidStatus(_aid_src_baro_hgt); -#endif // CONFIG_EKF2_BAROMETER -#if defined(CONFIG_EKF2_AIRSPEED) - resetEstimatorAidStatus(_aid_src_airspeed); -#endif // CONFIG_EKF2_AIRSPEED -#if defined(CONFIG_EKF2_SIDESLIP) - resetEstimatorAidStatus(_aid_src_sideslip); -#endif // CONFIG_EKF2_SIDESLIP - - resetEstimatorAidStatus(_aid_src_fake_pos); - resetEstimatorAidStatus(_aid_src_fake_hgt); - -#if defined(CONFIG_EKF2_EXTERNAL_VISION) - resetEstimatorAidStatus(_aid_src_ev_hgt); - resetEstimatorAidStatus(_aid_src_ev_pos); - resetEstimatorAidStatus(_aid_src_ev_vel); - resetEstimatorAidStatus(_aid_src_ev_yaw); -#endif // CONFIG_EKF2_EXTERNAL_VISION - -#if defined(CONFIG_EKF2_GNSS) - resetEstimatorAidStatus(_aid_src_gnss_hgt); - resetEstimatorAidStatus(_aid_src_gnss_pos); - resetEstimatorAidStatus(_aid_src_gnss_vel); - -# if defined(CONFIG_EKF2_GNSS_YAW) - resetEstimatorAidStatus(_aid_src_gnss_yaw); -# endif // CONFIG_EKF2_GNSS_YAW -#endif // CONFIG_EKF2_GNSS - -#if defined(CONFIG_EKF2_MAGNETOMETER) - resetEstimatorAidStatus(_aid_src_mag); -#endif // CONFIG_EKF2_MAGNETOMETER - -#if defined(CONFIG_EKF2_AUXVEL) - resetEstimatorAidStatus(_aid_src_aux_vel); -#endif // CONFIG_EKF2_AUXVEL - -#if defined(CONFIG_EKF2_OPTICAL_FLOW) - resetEstimatorAidStatus(_aid_src_optical_flow); - resetEstimatorAidStatus(_aid_src_terrain_optical_flow); -#endif // CONFIG_EKF2_OPTICAL_FLOW - -#if defined(CONFIG_EKF2_RANGE_FINDER) - resetEstimatorAidStatus(_aid_src_rng_hgt); -#endif // CONFIG_EKF2_RANGE_FINDER - _zero_velocity_update.reset(); } @@ -332,7 +285,6 @@ void Ekf::predictState(const imuSample &imu_delayed) void Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy, uint64_t timestamp_observation) { - if (!_pos_ref.isInitialized()) { return; } @@ -344,7 +296,10 @@ void Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, fl Vector2f pos_corrected = _pos_ref.project(lat_deg, lon_deg) + _state.vel.xy() * dt; - resetHorizontalPositionToExternal(pos_corrected, math::max(accuracy, FLT_EPSILON)); + resetHorizontalPositionTo(pos_corrected, sq(math::max(accuracy, 0.01f))); + + ECL_INFO("reset position to external observation"); + _information_events.flags.reset_pos_to_ext_obs = true; } void Ekf::updateParameters() diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 258ef6d7e865..bf62b542f42c 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -641,8 +641,6 @@ class Ekf final : public EstimatorInterface estimator_aid_source3d_s _aid_src_ev_vel{}; estimator_aid_source1d_s _aid_src_ev_yaw{}; - float _ev_yaw_pred_prev{}; ///< previous value of yaw state used by odometry fusion (m) - uint8_t _nb_ev_pos_reset_available{0}; uint8_t _nb_ev_vel_reset_available{0}; uint8_t _nb_ev_yaw_reset_available{0}; @@ -747,7 +745,6 @@ class Ekf final : public EstimatorInterface } // update quaternion states and covariances using an innovation, observation variance and Jacobian vector - bool fuseYaw(estimator_aid_source1d_s &aid_src_status); bool fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H_YAW); void computeYawInnovVarAndH(float variance, float &innovation_variance, VectorState &H_YAW) const; @@ -802,8 +799,9 @@ class Ekf final : public EstimatorInterface void resetHorizontalVelocityToZero(); void resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var); + + void resetHorizontalPositionToLastKnown(); - void resetHorizontalPositionToExternal(const Vector2f &new_horiz_pos, float horiz_accuracy); void resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f &new_horz_pos_var); void resetHorizontalPositionTo(const Vector2f &new_horz_pos, const float pos_var = NAN) { resetHorizontalPositionTo(new_horz_pos, Vector2f(pos_var, pos_var)); } @@ -815,20 +813,15 @@ class Ekf final : public EstimatorInterface void resetVerticalVelocityToZero(); // horizontal and vertical position aid source - void updateHorizontalPositionAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, const float innov_gate, estimator_aid_source2d_s &aid_src) const; - void updateVerticalPositionAidSrcStatus(const uint64_t &time_us, const float obs, const float obs_var, const float innov_gate, estimator_aid_source1d_s &aid_src) const; - - // 2d & 3d velocity aid source - void updateHorizontalVelocityAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, const float innov_gate, estimator_aid_source2d_s &aid_src) const; - void updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector3f &obs, const Vector3f &obs_var, const float innov_gate, estimator_aid_source3d_s &aid_src) const; + void updateVerticalPositionAidStatus(estimator_aid_source1d_s &aid_src, const uint64_t &time_us, const float observation, const float observation_variance, const float innovation_gate = 1.f) const; // horizontal and vertical position fusion - void fuseHorizontalPosition(estimator_aid_source2d_s &pos_aid_src); - void fuseVerticalPosition(estimator_aid_source1d_s &hgt_aid_src); + bool fuseHorizontalPosition(estimator_aid_source2d_s &pos_aid_src); + bool fuseVerticalPosition(estimator_aid_source1d_s &hgt_aid_src); // 2d & 3d velocity fusion - void fuseHorizontalVelocity(estimator_aid_source2d_s &vel_aid_src); - void fuseVelocity(estimator_aid_source3d_s &vel_aid_src); + bool fuseHorizontalVelocity(estimator_aid_source2d_s &vel_aid_src); + bool fuseVelocity(estimator_aid_source3d_s &vel_aid_src); #if defined(CONFIG_EKF2_TERRAIN) // terrain vertical position estimator @@ -1130,88 +1123,200 @@ class Ekf final : public EstimatorInterface bool _ev_q_error_initialized{false}; #endif // CONFIG_EKF2_EXTERNAL_VISION - void resetEstimatorAidStatus(estimator_aid_source1d_s &status) const + // state was reset to aid source, keep observation and update all other fields appropriately (zero innovation, etc) + void resetAidSourceStatusZeroInnovation(estimator_aid_source1d_s &status) const { - // only bother resetting if timestamp_sample is set - if (status.timestamp_sample != 0) { - status.timestamp_sample = 0; + status.time_last_fuse = _time_delayed_us; - // preserve status.time_last_fuse + status.innovation = 0.f; + status.innovation_filtered = 0.f; + status.innovation_variance = status.observation_variance; - status.observation = 0; - status.observation_variance = 0; + status.test_ratio = 0.f; + status.test_ratio_filtered = 0.f; - status.innovation = 0; - status.innovation_variance = 0; - status.test_ratio = INFINITY; - - status.innovation_rejected = true; - status.fused = false; - } + status.innovation_rejected = false; + status.fused = true; } - template - void resetEstimatorAidStatus(T &status) const + // helper used for populating and filtering estimator aid source struct for logging + void updateAidSourceStatus(estimator_aid_source1d_s &status, const uint64_t ×tamp_sample, + const float &observation, const float &observation_variance, + const float &innovation, const float &innovation_variance, + float innovation_gate = 1.f) const { - // only bother resetting if timestamp_sample is set - if (status.timestamp_sample != 0) { - status.timestamp_sample = 0; + bool innovation_rejected = false; - // preserve status.time_last_fuse + const float test_ratio = sq(innovation) / (sq(innovation_gate) * innovation_variance); - for (size_t i = 0; i < (sizeof(status.observation) / sizeof(status.observation[0])); i++) { - status.observation[i] = 0; - status.observation_variance[i] = 0; + if ((status.timestamp_sample > 0) && (timestamp_sample > status.timestamp_sample)) { - status.innovation[i] = 0; - status.innovation_variance[i] = 0; - status.test_ratio[i] = INFINITY; + const float dt_s = math::constrain((timestamp_sample - status.timestamp_sample) * 1e-6f, 0.001f, 1.f); + + static constexpr float tau = 0.5f; + const float alpha = math::constrain(dt_s / (dt_s + tau), 0.f, 1.f); + + // test_ratio_filtered + if (PX4_ISFINITE(status.test_ratio_filtered)) { + status.test_ratio_filtered += alpha * (matrix::sign(innovation) * test_ratio - status.test_ratio_filtered); + + } else { + // otherwise, init the filtered test ratio + status.test_ratio_filtered = test_ratio; + } + + // innovation_filtered + if (PX4_ISFINITE(status.innovation_filtered)) { + status.innovation_filtered += alpha * (innovation - status.innovation_filtered); + + } else { + // otherwise, init the filtered innovation + status.innovation_filtered = innovation; } - status.innovation_rejected = true; - status.fused = false; + + // limit extremes in filtered values + static constexpr float kNormalizedInnovationLimit = 2.f; + static constexpr float kTestRatioLimit = sq(kNormalizedInnovationLimit); + + if (test_ratio > kTestRatioLimit) { + + status.test_ratio_filtered = math::constrain(status.test_ratio_filtered, -kTestRatioLimit, kTestRatioLimit); + + const float innov_limit = kNormalizedInnovationLimit * innovation_gate * sqrtf(innovation_variance); + status.innovation_filtered = math::constrain(status.innovation_filtered, -innov_limit, innov_limit); + } + + } else { + // invalid timestamp_sample, reset + status.test_ratio_filtered = test_ratio; + status.innovation_filtered = innovation; } + + status.test_ratio = test_ratio; + + status.observation = observation; + status.observation_variance = observation_variance; + + status.innovation = innovation; + status.innovation_variance = innovation_variance; + + if ((test_ratio > 1.f) + || !PX4_ISFINITE(test_ratio) + || !PX4_ISFINITE(status.innovation) + || !PX4_ISFINITE(status.innovation_variance) + ) { + innovation_rejected = true; + } + + status.timestamp_sample = timestamp_sample; + + // if any of the innovations are rejected, then the overall innovation is rejected + status.innovation_rejected = innovation_rejected; + + // reset + status.fused = false; } - void setEstimatorAidStatusTestRatio(estimator_aid_source1d_s &status, float innovation_gate) const + // state was reset to aid source, keep observation and update all other fields appropriately (zero innovation, etc) + template + void resetAidSourceStatusZeroInnovation(T &status) const { - if (PX4_ISFINITE(status.innovation) - && PX4_ISFINITE(status.innovation_variance) - && (status.innovation_variance > 0.f) - ) { - status.test_ratio = sq(status.innovation) / (sq(innovation_gate) * status.innovation_variance); - status.innovation_rejected = (status.test_ratio > 1.f); + status.time_last_fuse = _time_delayed_us; - } else { - status.test_ratio = INFINITY; - status.innovation_rejected = true; + for (size_t i = 0; i < (sizeof(status.observation) / sizeof(status.observation[0])); i++) { + status.innovation[i] = 0.f; + status.innovation_filtered[i] = 0.f; + status.innovation_variance[i] = status.observation_variance[i]; + + status.test_ratio[i] = 0.f; + status.test_ratio_filtered[i] = 0.f; } + + status.innovation_rejected = false; + status.fused = true; } - template - void setEstimatorAidStatusTestRatio(T &status, float innovation_gate) const + // helper used for populating and filtering estimator aid source struct for logging + template + void updateAidSourceStatus(T &status, const uint64_t ×tamp_sample, + const S &observation, const S &observation_variance, + const S &innovation, const S &innovation_variance, + float innovation_gate = 1.f) const { bool innovation_rejected = false; - for (size_t i = 0; i < (sizeof(status.test_ratio) / sizeof(status.test_ratio[0])); i++) { - if (PX4_ISFINITE(status.innovation[i]) - && PX4_ISFINITE(status.innovation_variance[i]) - && (status.innovation_variance[i] > 0.f) - ) { - status.test_ratio[i] = sq(status.innovation[i]) / (sq(innovation_gate) * status.innovation_variance[i]); + const float dt_s = math::constrain((timestamp_sample - status.timestamp_sample) * 1e-6f, 0.001f, 1.f); + + static constexpr float tau = 0.5f; + const float alpha = math::constrain(dt_s / (dt_s + tau), 0.f, 1.f); + + for (size_t i = 0; i < (sizeof(status.observation) / sizeof(status.observation[0])); i++) { - if (status.test_ratio[i] > 1.f) { - innovation_rejected = true; + const float test_ratio = sq(innovation(i)) / (sq(innovation_gate) * innovation_variance(i)); + + if ((status.timestamp_sample > 0) && (timestamp_sample > status.timestamp_sample)) { + + // test_ratio_filtered + if (PX4_ISFINITE(status.test_ratio_filtered[i])) { + status.test_ratio_filtered[i] += alpha * (matrix::sign(innovation(i)) * test_ratio - status.test_ratio_filtered[i]); + + } else { + // otherwise, init the filtered test ratio + status.test_ratio_filtered[i] = test_ratio; + } + + // innovation_filtered + if (PX4_ISFINITE(status.innovation_filtered[i])) { + status.innovation_filtered[i] += alpha * (innovation(i) - status.innovation_filtered[i]); + + } else { + // otherwise, init the filtered innovation + status.innovation_filtered[i] = innovation(i); + } + + // limit extremes in filtered values + static constexpr float kNormalizedInnovationLimit = 2.f; + static constexpr float kTestRatioLimit = sq(kNormalizedInnovationLimit); + + if (test_ratio > kTestRatioLimit) { + + status.test_ratio_filtered[i] = math::constrain(status.test_ratio_filtered[i], -kTestRatioLimit, kTestRatioLimit); + + const float innov_limit = kNormalizedInnovationLimit * innovation_gate * sqrtf(innovation_variance(i)); + status.innovation_filtered[i] = math::constrain(status.innovation_filtered[i], -innov_limit, innov_limit); } } else { - status.test_ratio[i] = INFINITY; + // invalid timestamp_sample, reset + status.test_ratio_filtered[i] = test_ratio; + status.innovation_filtered[i] = innovation(i); + } + + status.test_ratio[i] = test_ratio; + + status.observation[i] = observation(i); + status.observation_variance[i] = observation_variance(i); + + status.innovation[i] = innovation(i); + status.innovation_variance[i] = innovation_variance(i); + + if ((test_ratio > 1.f) + || !PX4_ISFINITE(test_ratio) + || !PX4_ISFINITE(status.innovation[i]) + || !PX4_ISFINITE(status.innovation_variance[i]) + ) { innovation_rejected = true; } } + status.timestamp_sample = timestamp_sample; + // if any of the innovations are rejected, then the overall innovation is rejected status.innovation_rejected = innovation_rejected; + + // reset + status.fused = false; } ZeroGyroUpdate _zero_gyro_update{}; diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index b19936ff1ae8..e390f41ea1b1 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -401,7 +401,6 @@ class EstimatorInterface # if defined(CONFIG_EKF2_GNSS_YAW) // innovation consistency check monitoring ratios - AlphaFilter _gnss_yaw_signed_test_ratio_lpf{0.1f}; // average signed test ratio used to detect a bias in the state uint64_t _time_last_gps_yaw_buffer_push{0}; # endif // CONFIG_EKF2_GNSS_YAW #endif // CONFIG_EKF2_GNSS diff --git a/src/modules/ekf2/EKF/position_fusion.cpp b/src/modules/ekf2/EKF/position_fusion.cpp index a1d2f85c273d..d8362b5fceda 100644 --- a/src/modules/ekf2/EKF/position_fusion.cpp +++ b/src/modules/ekf2/EKF/position_fusion.cpp @@ -33,50 +33,27 @@ #include "ekf.h" -void Ekf::updateHorizontalPositionAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, - const float innov_gate, estimator_aid_source2d_s &aid_src) const +void Ekf::updateVerticalPositionAidStatus(estimator_aid_source1d_s &aid_src, const uint64_t &time_us, + const float observation, const float observation_variance, const float innovation_gate) const { - resetEstimatorAidStatus(aid_src); + float innovation = _state.pos(2) - observation; + float innovation_variance = getStateVariance()(2) + observation_variance; - for (int i = 0; i < 2; i++) { - aid_src.observation[i] = obs(i); - aid_src.innovation[i] = _state.pos(i) - aid_src.observation[i]; - - aid_src.observation_variance[i] = math::max(sq(0.01f), obs_var(i)); - const int state_index = State::pos.idx + i; - aid_src.innovation_variance[i] = P(state_index, state_index) + aid_src.observation_variance[i]; - } - - setEstimatorAidStatusTestRatio(aid_src, innov_gate); - - aid_src.timestamp_sample = time_us; -} - -void Ekf::updateVerticalPositionAidSrcStatus(const uint64_t &time_us, const float obs, const float obs_var, - const float innov_gate, estimator_aid_source1d_s &aid_src) const -{ - resetEstimatorAidStatus(aid_src); - - aid_src.observation = obs; - aid_src.innovation = _state.pos(2) - aid_src.observation; - - aid_src.observation_variance = math::max(sq(0.01f), obs_var); - aid_src.innovation_variance = P(State::pos.idx + 2, State::pos.idx + 2) + aid_src.observation_variance; - - setEstimatorAidStatusTestRatio(aid_src, innov_gate); + updateAidSourceStatus(aid_src, time_us, + observation, observation_variance, + innovation, innovation_variance, + innovation_gate); // z special case if there is bad vertical acceleration data, then don't reject measurement, // but limit innovation to prevent spikes that could destabilise the filter if (_fault_status.flags.bad_acc_vertical && aid_src.innovation_rejected) { - const float innov_limit = innov_gate * sqrtf(aid_src.innovation_variance); + const float innov_limit = innovation_gate * sqrtf(aid_src.innovation_variance); aid_src.innovation = math::constrain(aid_src.innovation, -innov_limit, innov_limit); aid_src.innovation_rejected = false; } - - aid_src.timestamp_sample = time_us; } -void Ekf::fuseHorizontalPosition(estimator_aid_source2d_s &aid_src) +bool Ekf::fuseHorizontalPosition(estimator_aid_source2d_s &aid_src) { // x & y if (!aid_src.innovation_rejected @@ -91,9 +68,11 @@ void Ekf::fuseHorizontalPosition(estimator_aid_source2d_s &aid_src) } else { aid_src.fused = false; } + + return aid_src.fused; } -void Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src) +bool Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src) { // z if (!aid_src.innovation_rejected @@ -107,6 +86,8 @@ void Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src) } else { aid_src.fused = false; } + + return aid_src.fused; } void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f &new_horz_pos_var) @@ -200,11 +181,3 @@ void Ekf::resetHorizontalPositionToLastKnown() // Used when falling back to non-aiding mode of operation resetHorizontalPositionTo(_last_known_pos.xy(), sq(_params.pos_noaid_noise)); } - -void Ekf::resetHorizontalPositionToExternal(const Vector2f &new_horiz_pos, float horiz_accuracy) -{ - ECL_INFO("reset position to external observation"); - _information_events.flags.reset_pos_to_ext_obs = true; - - resetHorizontalPositionTo(new_horiz_pos, sq(horiz_accuracy)); -} diff --git a/src/modules/ekf2/EKF/terrain_estimator/terrain_estimator.cpp b/src/modules/ekf2/EKF/terrain_estimator/terrain_estimator.cpp index 64890cd14fa5..dad3c322f105 100644 --- a/src/modules/ekf2/EKF/terrain_estimator/terrain_estimator.cpp +++ b/src/modules/ekf2/EKF/terrain_estimator/terrain_estimator.cpp @@ -205,8 +205,6 @@ void Ekf::stopHaglRngFusion() ECL_INFO("stopping HAGL range fusion"); // reset flags - resetEstimatorAidStatus(_aid_src_terrain_range_finder); - _innov_check_fail_status.flags.reject_hagl = false; _hagl_sensor_status.flags.range_finder = false; @@ -221,30 +219,19 @@ void Ekf::updateHaglRng(estimator_aid_source1d_s &aid_src) const // predict the hagl from the vehicle position and terrain height const float pred_hagl = _terrain_vpos - _state.pos(2); - // calculate the innovation - const float hagl_innov = pred_hagl - meas_hagl; - // calculate the observation variance adding the variance of the vehicles own height uncertainty const float obs_variance = getRngVar(); // calculate the innovation variance - limiting it to prevent a badly conditioned fusion const float hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance); - // perform an innovation consistency check and only fuse data if it passes - const float innov_gate = fmaxf(_params.range_innov_gate, 1.0f); - - - aid_src.timestamp_sample = _time_delayed_us; // TODO - - aid_src.observation = meas_hagl; - aid_src.observation_variance = obs_variance; - - aid_src.innovation = hagl_innov; - aid_src.innovation_variance = hagl_innov_var; - - setEstimatorAidStatusTestRatio(aid_src, innov_gate); - - aid_src.fused = false; + updateAidSourceStatus(aid_src, + _time_delayed_us, // sample timestamp + meas_hagl, // observation + obs_variance, // observation variance + pred_hagl - meas_hagl, // innovation + hagl_innov_var, // innovation variance + math::max(_params.range_innov_gate, 1.f)); // innovation gate } void Ekf::fuseHaglRng(estimator_aid_source1d_s &aid_src) @@ -329,7 +316,6 @@ void Ekf::stopHaglFlowFusion() ECL_INFO("stopping HAGL flow fusion"); _hagl_sensor_status.flags.flow = false; - resetEstimatorAidStatus(_aid_src_terrain_optical_flow); } } @@ -367,9 +353,6 @@ void Ekf::fuseFlowForTerrain(estimator_aid_source2d_s &flow) return; } - // run the innovation consistency check and record result - setEstimatorAidStatusTestRatio(flow, math::max(_params.flow_innov_gate, 1.f)); - _innov_check_fail_status.flags.reject_optflow_X = (flow.test_ratio[0] > 1.f); _innov_check_fail_status.flags.reject_optflow_Y = (flow.test_ratio[1] > 1.f); diff --git a/src/modules/ekf2/EKF/velocity_fusion.cpp b/src/modules/ekf2/EKF/velocity_fusion.cpp index fbb079c1fdee..07e590c2111a 100644 --- a/src/modules/ekf2/EKF/velocity_fusion.cpp +++ b/src/modules/ekf2/EKF/velocity_fusion.cpp @@ -33,53 +33,7 @@ #include "ekf.h" -void Ekf::updateHorizontalVelocityAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, - const float innov_gate, estimator_aid_source2d_s &aid_src) const -{ - resetEstimatorAidStatus(aid_src); - - for (int i = 0; i < 2; i++) { - aid_src.observation[i] = obs(i); - aid_src.innovation[i] = _state.vel(i) - aid_src.observation[i]; - - aid_src.observation_variance[i] = math::max(sq(0.01f), obs_var(i)); - const int state_index = State::vel.idx + i; - aid_src.innovation_variance[i] = P(state_index, state_index) + aid_src.observation_variance[i]; - } - - setEstimatorAidStatusTestRatio(aid_src, innov_gate); - - aid_src.timestamp_sample = time_us; -} - -void Ekf::updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector3f &obs, const Vector3f &obs_var, - const float innov_gate, estimator_aid_source3d_s &aid_src) const -{ - resetEstimatorAidStatus(aid_src); - - for (int i = 0; i < 3; i++) { - aid_src.observation[i] = obs(i); - aid_src.innovation[i] = _state.vel(i) - aid_src.observation[i]; - - aid_src.observation_variance[i] = math::max(sq(0.01f), obs_var(i)); - const int state_index = State::vel.idx + i; - aid_src.innovation_variance[i] = P(state_index, state_index) + aid_src.observation_variance[i]; - } - - setEstimatorAidStatusTestRatio(aid_src, innov_gate); - - // vz special case if there is bad vertical acceleration data, then don't reject measurement, - // but limit innovation to prevent spikes that could destabilise the filter - if (_fault_status.flags.bad_acc_vertical && aid_src.innovation_rejected) { - const float innov_limit = innov_gate * sqrtf(aid_src.innovation_variance[2]); - aid_src.innovation[2] = math::constrain(aid_src.innovation[2], -innov_limit, innov_limit); - aid_src.innovation_rejected = false; - } - - aid_src.timestamp_sample = time_us; -} - -void Ekf::fuseHorizontalVelocity(estimator_aid_source2d_s &aid_src) +bool Ekf::fuseHorizontalVelocity(estimator_aid_source2d_s &aid_src) { // vx, vy if (!aid_src.innovation_rejected @@ -94,9 +48,11 @@ void Ekf::fuseHorizontalVelocity(estimator_aid_source2d_s &aid_src) } else { aid_src.fused = false; } + + return aid_src.fused; } -void Ekf::fuseVelocity(estimator_aid_source3d_s &aid_src) +bool Ekf::fuseVelocity(estimator_aid_source3d_s &aid_src) { // vx, vy, vz if (!aid_src.innovation_rejected @@ -113,6 +69,8 @@ void Ekf::fuseVelocity(estimator_aid_source3d_s &aid_src) } else { aid_src.fused = false; } + + return aid_src.fused; } void Ekf::resetHorizontalVelocityTo(const Vector2f &new_horz_vel, const Vector2f &new_horz_vel_var) diff --git a/src/modules/ekf2/EKF/yaw_fusion.cpp b/src/modules/ekf2/EKF/yaw_fusion.cpp index e843f782ea6e..7714170a62e0 100644 --- a/src/modules/ekf2/EKF/yaw_fusion.cpp +++ b/src/modules/ekf2/EKF/yaw_fusion.cpp @@ -37,23 +37,8 @@ #include -// update quaternion states and covariances using the yaw innovation and yaw observation variance -bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status) -{ - VectorState H_YAW; - sym::ComputeYawInnovVarAndH(_state.vector(), P, aid_src_status.observation_variance, &aid_src_status.innovation_variance, &H_YAW); - - return fuseYaw(aid_src_status, H_YAW); -} - bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H_YAW) { - // define the innovation gate size - float gate_sigma = math::max(_params.heading_innov_gate, 1.f); - - // innovation test ratio - setEstimatorAidStatusTestRatio(aid_src_status, gate_sigma); - // 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 @@ -83,7 +68,7 @@ bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H Kfusion(row) *= heading_innov_var_inv; } - // set the magnetometer unhealthy if the test fails + // set the heading unhealthy if the test fails if (aid_src_status.innovation_rejected) { _innov_check_fail_status.flags.reject_yaw = true; @@ -91,13 +76,14 @@ bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H // 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) - ) { + && 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)); + const float gate_sigma = math::max(_params.heading_innov_gate, 1.f); + const 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 @@ -122,13 +108,10 @@ bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H _fault_status.flags.bad_hdg = false; return true; - - } else { - _fault_status.flags.bad_hdg = true; } // otherwise - aid_src_status.fused = false; + _fault_status.flags.bad_hdg = true; return false; }