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 69895181b1..b3223a5ab3 100644 --- a/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp @@ -127,7 +127,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed) if (_control_status.flags.inertial_dead_reckoning && !is_airspeed_consistent) { resetVelUsingAirspeed(airspeed_sample); - } else if (!_control_status.flags.wind || getWindVelocityVariance().longerThan(_params.initial_wind_uncertainty)) { + } else if (!_control_status.flags.wind) { // If starting wind state estimation, reset the wind states and covariances before fusing any data // Also catch the case where sideslip fusion enabled wind estimation recently and didn't converge yet. resetWindUsingAirspeed(airspeed_sample); 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 c55ed14aaf..5af3510cd1 100644 --- a/src/modules/ekf2/EKF/aid_sources/drag/drag_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/drag/drag_fusion.cpp @@ -57,6 +57,10 @@ void Ekf::controlDragFusion(const imuSample &imu_delayed) if (_drag_buffer->pop_first_older_than(imu_delayed.time_us, &drag_sample)) { fuseDrag(drag_sample); + + if (!_aid_src_drag.fused && !_control_status.flags.fuse_aspd && !_control_status.flags.fuse_beta) { + resetWind(); + } } } } 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 997b474793..81f681535e 100644 --- a/src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp @@ -64,16 +64,10 @@ void Ekf::controlBetaFusion(const imuSample &imu_delayed) updateSideslip(_aid_src_sideslip); _innov_check_fail_status.flags.reject_sideslip = _aid_src_sideslip.innovation_rejected; - // If starting wind state estimation, reset the wind states and covariances before fusing any data - if (!_control_status.flags.wind) { - // activate the wind states - _control_status.flags.wind = true; - // reset the timeout timers to prevent repeated resets - _aid_src_sideslip.time_last_fuse = imu_delayed.time_us; + if (!fuseSideslip(_aid_src_sideslip) && !_control_status.flags.wind) { resetWindToZero(); } - - fuseSideslip(_aid_src_sideslip); + _control_status.flags.wind = true; } } } @@ -96,10 +90,10 @@ void Ekf::updateSideslip(estimator_aid_source1d_s &aid_src) const math::max(_params.beta_innov_gate, 1.f)); // innovation gate } -void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip) +bool Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip) { if (sideslip.innovation_rejected) { - return; + return false; } // determine if we need the sideslip fusion to correct states other than wind @@ -125,7 +119,7 @@ void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip) ECL_ERR("sideslip badly conditioned - %s covariance reset", action_string); - return; + return false; } _fault_status.flags.bad_sideslip = false; @@ -149,4 +143,5 @@ void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip) if (is_fused) { sideslip.time_last_fuse = _time_delayed_us; } + return is_fused; } diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index e76ca01153..9693df150b 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -195,15 +195,14 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) #if defined(CONFIG_EKF2_WIND) - if (_control_status.flags.wind) { - // wind vel: add process noise - float wind_vel_nsd_scaled = math::constrain(_params.wind_vel_nsd, 0.f, 1.f) * (1.f + _params.wind_vel_nsd_scaler * fabsf(_height_rate_lpf)); - float wind_vel_process_noise = sq(wind_vel_nsd_scaled) * dt; + // wind vel: add process noise + float wind_vel_nsd_scaled = math::constrain(_params.wind_vel_nsd, 0.f, + 1.f) * (1.f + _params.wind_vel_nsd_scaler * fabsf(_height_rate_lpf)); + float wind_vel_process_noise = sq(wind_vel_nsd_scaled) * dt; - for (unsigned index = 0; index < State::wind_vel.dof; index++) { - const unsigned i = State::wind_vel.idx + index; - P(i, i) += wind_vel_process_noise; - } + for (unsigned index = 0; index < State::wind_vel.dof; index++) { + const unsigned i = State::wind_vel.idx + index; + P(i, i) = fminf(P(i, i) + wind_vel_process_noise, _params.initial_wind_uncertainty); } #endif // CONFIG_EKF2_WIND diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index c59f8350a3..9580076405 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -772,7 +772,7 @@ private: // fuse synthetic zero sideslip measurement void updateSideslip(estimator_aid_source1d_s &_aid_src_sideslip) const; - void fuseSideslip(estimator_aid_source1d_s &_aid_src_sideslip); + bool fuseSideslip(estimator_aid_source1d_s &_aid_src_sideslip); #endif // CONFIG_EKF2_SIDESLIP #if defined(CONFIG_EKF2_DRAG_FUSION) @@ -798,8 +798,6 @@ private: 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)); } - void resetWindTo(const Vector2f &wind, const Vector2f &wind_var); - bool isHeightResetRequired() const; void resetVerticalPositionTo(float new_vert_pos, float new_vert_pos_var = NAN); diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py index f1cb9cd046..fa87995ba1 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py @@ -298,38 +298,15 @@ def compute_wind_init_and_cov_from_airspeed( # Initialise wind states assuming horizontal flight sideslip = sf.Symbol("beta") wind = sf.V2(v_local[0] - airspeed * sf.cos(heading + sideslip), v_local[1] - airspeed * sf.sin(heading + sideslip)) - J = wind.jacobian([v_local[0], v_local[1], heading, sideslip, airspeed]) + H = wind.jacobian([v_local[0], v_local[1], heading, sideslip, airspeed]) - R = sf.M55() - R[0,0] = v_var[0] - R[1,1] = v_var[1] - R[2,2] = heading_var - R[3,3] = sideslip_var - R[4,4] = airspeed_var - - P = J * R * J.T + P = sf.Matrix.diag([v_var[0], v_var[1], heading_var, sideslip_var, airspeed_var]) + P = H * P * H.T # Assume zero sideslip P = P.subs({sideslip: 0.0}) wind = wind.subs({sideslip: 0.0}) return (wind, P) -def compute_wind_init_and_cov_from_wind_speed_and_direction( - wind_speed: sf.Scalar, - wind_direction: sf.Scalar, - wind_speed_var: sf.Scalar, - wind_direction_var: sf.Scalar -)-> (sf.V2, sf.M22): - wind = sf.V2(wind_speed * sf.cos(wind_direction), wind_speed * sf.sin(wind_direction)) - J = wind.jacobian([wind_speed, wind_direction]) - - R = sf.M22() - R[0,0] = wind_speed_var - R[1,1] = wind_direction_var - - P = J * R * J.T - - return (wind, P) - def predict_sideslip( state: State, @@ -704,7 +681,6 @@ if not args.disable_wind: generate_px4_function(compute_sideslip_h_and_k, output_names=["H", "K"]) generate_px4_function(compute_sideslip_innov_and_innov_var, output_names=["innov", "innov_var"]) generate_px4_function(compute_wind_init_and_cov_from_airspeed, output_names=["wind", "P_wind"]) - generate_px4_function(compute_wind_init_and_cov_from_wind_speed_and_direction, output_names=["wind", "P_wind"]) generate_px4_function(compute_yaw_innov_var_and_h, output_names=["innov_var", "H"]) generate_px4_function(compute_flow_xy_innov_var_and_hx, output_names=["innov_var", "H"]) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_wind_init_and_cov_from_wind_speed_and_direction.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_wind_init_and_cov_from_wind_speed_and_direction.h deleted file mode 100644 index 46c7a37a8e..0000000000 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_wind_init_and_cov_from_wind_speed_and_direction.h +++ /dev/null @@ -1,65 +0,0 @@ -// ----------------------------------------------------------------------------- -// This file was autogenerated by symforce from template: -// function/FUNCTION.h.jinja -// Do NOT modify by hand. -// ----------------------------------------------------------------------------- - -#pragma once - -#include - -namespace sym { - -/** - * This function was autogenerated from a symbolic function. Do not modify by hand. - * - * Symbolic function: compute_wind_init_and_cov_from_wind_speed_and_direction - * - * Args: - * wind_speed: Scalar - * wind_direction: Scalar - * wind_speed_var: Scalar - * wind_direction_var: Scalar - * - * Outputs: - * wind: Matrix21 - * P_wind: Matrix22 - */ -template -void ComputeWindInitAndCovFromWindSpeedAndDirection( - const Scalar wind_speed, const Scalar wind_direction, const Scalar wind_speed_var, - const Scalar wind_direction_var, matrix::Matrix* const wind = nullptr, - matrix::Matrix* const P_wind = nullptr) { - // Total ops: 18 - - // Input arrays - - // Intermediate terms (7) - const Scalar _tmp0 = std::cos(wind_direction); - const Scalar _tmp1 = std::sin(wind_direction); - const Scalar _tmp2 = std::pow(_tmp0, Scalar(2)); - const Scalar _tmp3 = std::pow(_tmp1, Scalar(2)); - const Scalar _tmp4 = wind_direction_var * std::pow(wind_speed, Scalar(2)); - const Scalar _tmp5 = _tmp0 * _tmp1; - const Scalar _tmp6 = -_tmp4 * _tmp5 + _tmp5 * wind_speed_var; - - // Output terms (2) - if (wind != nullptr) { - matrix::Matrix& _wind = (*wind); - - _wind(0, 0) = _tmp0 * wind_speed; - _wind(1, 0) = _tmp1 * wind_speed; - } - - if (P_wind != nullptr) { - matrix::Matrix& _p_wind = (*P_wind); - - _p_wind(0, 0) = _tmp2 * wind_speed_var + _tmp3 * _tmp4; - _p_wind(1, 0) = _tmp6; - _p_wind(0, 1) = _tmp6; - _p_wind(1, 1) = _tmp2 * _tmp4 + _tmp3 * wind_speed_var; - } -} // NOLINT(readability/fn_size) - -// NOLINTNEXTLINE(readability/fn_size) -} // namespace sym diff --git a/src/modules/ekf2/EKF/wind.cpp b/src/modules/ekf2/EKF/wind.cpp index c1aa518047..ee937333a9 100644 --- a/src/modules/ekf2/EKF/wind.cpp +++ b/src/modules/ekf2/EKF/wind.cpp @@ -37,7 +37,6 @@ */ #include "ekf.h" -#include void Ekf::resetWindToExternalObservation(float wind_speed, float wind_direction, float wind_speed_accuracy, float wind_direction_accuracy) { @@ -45,21 +44,19 @@ void Ekf::resetWindToExternalObservation(float wind_speed, float wind_direction, // wind direction is given as azimuth where wind blows FROM, we need direction where wind blows TO const float wind_direction_rad = wrap_pi(math::radians(wind_direction) + M_PI_F); - - const float wind_direction_var = sq(math::radians(wind_direction_accuracy)); - const float wind_speed_var = sq(wind_speed_accuracy); - - matrix::SquareMatrix P_wind; - Vector2f wind; - - sym::ComputeWindInitAndCovFromWindSpeedAndDirection(wind_speed_constrained, wind_direction_rad, wind_speed_var, wind_direction_var, &wind, &P_wind); - - const Vector2f wind_var = P_wind.diag(); + Vector2f wind = wind_speed_constrained * Vector2f(cosf(wind_direction_rad), sinf(wind_direction_rad)); ECL_INFO("reset wind states to external observation"); _information_events.flags.reset_wind_to_ext_obs = true; - resetWindTo(wind, wind_var); + Vector2f innov = _state.wind_vel - wind; + float R = sq(0.1f); + Vector2f innov_var{P(State::wind_vel.idx, State::wind_vel.idx) + R, P(State::wind_vel.idx + 1, State::wind_vel.idx + 1) + R}; + const bool control_status_wind_prev = _control_status.flags.wind; + _control_status.flags.wind = true; + fuseDirectStateMeasurement(innov(0), innov_var(0), R, State::wind_vel.idx); + fuseDirectStateMeasurement(innov(1), innov_var(1), R, State::wind_vel.idx + 1); + _control_status.flags.wind = control_status_wind_prev; // reset the horizontal velocity variances to allow the velocity states to be pulled towards // a solution that is aligned with the newly set wind estimates diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 24b92a1ad4..58958effad 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -536,7 +536,7 @@ void EKF2::Run() } if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_EXTERNAL_WIND_ESTIMATE) { - if (_ekf.control_status_flags().wind_dead_reckoning) { + if (_ekf.control_status_flags().wind_dead_reckoning || !_ekf.control_status_flags().in_air) { _ekf.resetWindToExternalObservation(vehicle_command.param1, vehicle_command.param3, vehicle_command.param2, vehicle_command.param4); }