From b6c86d841a640d2724fb7b2ea70bb56e9ddb8152 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Mon, 17 Jun 2024 14:13:59 +0200 Subject: [PATCH] added support for resetting wind states to external observation Signed-off-by: RomanBapst --- src/modules/ekf2/EKF/common.h | 1 + src/modules/ekf2/EKF/ekf.cpp | 33 ++++++++++ src/modules/ekf2/EKF/ekf.h | 12 ++++ .../EKF/python/ekf_derivation/derivation.py | 18 +++++ ...it_and_cov_from_wind_speed_and_direction.h | 65 +++++++++++++++++++ 5 files changed, 129 insertions(+) create mode 100644 src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_wind_init_and_cov_from_wind_speed_and_direction.h diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index ae1d185860..8f43e18deb 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -637,6 +637,7 @@ union information_event_status_u { bool reset_hgt_to_rng : 1; ///< 15 - true when the vertical position state is reset to the rng measurement bool reset_hgt_to_ev : 1; ///< 16 - true when the vertical position state is reset to the ev measurement bool reset_pos_to_ext_obs : 1; ///< 17 - true when horizontal position was reset to an external observation while deadreckoning + bool reset_wind_to_ext_obs : 1; ///< 18 - true when wind states were reset to an external observation } flags; uint32_t value; }; diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index 1571aa06ca..e7d04ca0f2 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -40,6 +40,7 @@ */ #include "ekf.h" +#include #include @@ -299,6 +300,38 @@ void Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, fl ECL_INFO("reset position to external observation"); _information_events.flags.reset_pos_to_ext_obs = true; } +void Ekf::resetWindToExternalObservation(float wind_speed, float wind_direction, float wind_speed_accuracy, float wind_direction_accuracy) +{ + const float wind_speed_constrained = math::max(wind_speed, 0.0f); + + // 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); + + matrix::SquareMatrix P_wind; + Vector2f wind; + + sym::ComputeWindInitAndCovFromWindSpeedAndDirection(wind_speed_constrained, wind_direction_rad, wind_speed_accuracy, wind_direction_accuracy, &wind, &P_wind); + + const Vector2f wind_var = P_wind.diag(); + + ECL_INFO("reset wind states to external observation"); + _information_events.flags.reset_wind_to_ext_obs = true; + + resetWindTo(wind, wind_var); +} + +void Ekf::resetWindTo(const Vector2f &wind, const Vector2f &wind_var) +{ + _state.wind_vel = wind; + + if (PX4_ISFINITE(wind_var(0))) { + P.uncorrelateCovarianceSetVariance<1>(State::wind_vel.idx, math::max(sq(_params.initial_wind_uncertainty), wind_var(0))); + } + + if (PX4_ISFINITE(wind_var(1))) { + P.uncorrelateCovarianceSetVariance<1>(State::wind_vel.idx + 1, math::max(sq(_params.initial_wind_uncertainty), wind_var(1))); + } +} void Ekf::updateParameters() { diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 2a23d1d283..c59f8350a3 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -503,6 +503,16 @@ public: void resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy, uint64_t timestamp_observation); + /** + * @brief Resets the wind states to an external observation + * + * @param wind_speed The wind speed in m/s + * @param wind_direction The azimuth (from true north) from where the wind is flowing from in degrees (0 to 360) + * @param wind_speed_accuracy The 1 sigma accuracy of the wind speed estimate in m/s + * @param wind_direction_accuracy The 1 sigma accuracy of the wind direction estimate in degrees + */ + void resetWindToExternalObservation(float wind_speed, float wind_direction, float wind_speed_accuracy, float wind_direction_accuracy); + void updateParameters(); friend class AuxGlobalPosition; @@ -788,6 +798,8 @@ 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 4d8b5278db..f1cb9cd046 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py @@ -313,6 +313,23 @@ def compute_wind_init_and_cov_from_airspeed( 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, @@ -687,6 +704,7 @@ 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 new file mode 100644 index 0000000000..46c7a37a8e --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_wind_init_and_cov_from_wind_speed_and_direction.h @@ -0,0 +1,65 @@ +// ----------------------------------------------------------------------------- +// 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