diff --git a/src/lib/wind_estimator/WindEstimator.cpp b/src/lib/wind_estimator/WindEstimator.cpp index df9c2bb7ad..84a5ba6bc3 100644 --- a/src/lib/wind_estimator/WindEstimator.cpp +++ b/src/lib/wind_estimator/WindEstimator.cpp @@ -37,6 +37,7 @@ */ #include "WindEstimator.hpp" +#include "python/generated/init_wind_using_airspeed.h" bool WindEstimator::initialise(const matrix::Vector3f &velI, const float hor_vel_variance, const float heading_rad, @@ -44,34 +45,19 @@ WindEstimator::initialise(const matrix::Vector3f &velI, const float hor_vel_vari { if (PX4_ISFINITE(tas_meas) && PX4_ISFINITE(tas_variance)) { - const float cos_heading = cosf(heading_rad); - const float sin_heading = sinf(heading_rad); + constexpr float initial_heading_var = sq(math::radians(INITIAL_HEADING_ERROR_DEG)); + constexpr float initial_sideslip_var = sq(math::radians(INITIAL_BETA_ERROR_DEG)); - // initialise wind states assuming zero side slip and horizontal flight - _state(INDEX_W_N) = velI(0) - tas_meas * cos_heading; - _state(INDEX_W_E) = velI(1) - tas_meas * sin_heading; + matrix::SquareMatrix P_wind_init; + matrix::Vector2f wind_init; + + sym::InitWindUsingAirspeed(velI, heading_rad, tas_meas, hor_vel_variance, initial_heading_var, initial_sideslip_var, + tas_variance, + &wind_init, &P_wind_init); + + _state.xy() = wind_init; _state(INDEX_TAS_SCALE) = _scale_init; - - constexpr float initial_sideslip_uncertainty = math::radians(INITIAL_BETA_ERROR_DEG); - const float initial_wind_var_body_y = sq(tas_meas * sinf(initial_sideslip_uncertainty)); - constexpr float heading_variance = sq(math::radians(INITIAL_HEADING_ERROR_DEG)); - - // rotate wind velocity into earth frame aligned with vehicle yaw - const float Wx = _state(INDEX_W_N) * cos_heading + _state(INDEX_W_E) * sin_heading; - const float Wy = -_state(INDEX_W_N) * sin_heading + _state(INDEX_W_E) * cos_heading; - - _P(INDEX_W_N, INDEX_W_N) = tas_variance * sq(cos_heading) + heading_variance * sq(-Wx * sin_heading - Wy * cos_heading) - + initial_wind_var_body_y * sq(sin_heading); - _P(INDEX_W_N, INDEX_W_E) = tas_variance * sin_heading * cos_heading + heading_variance * - (-Wx * sin_heading - Wy * cos_heading) * (Wx * cos_heading - Wy * sin_heading) - - initial_wind_var_body_y * sin_heading * cos_heading; - _P(INDEX_W_E, INDEX_W_N) = _P(INDEX_W_N, INDEX_W_E); - _P(INDEX_W_E, INDEX_W_E) = tas_variance * sq(sin_heading) + heading_variance * sq(Wx * cos_heading - Wy * sin_heading) + - initial_wind_var_body_y * sq(cos_heading); - - // Now add the variance due to uncertainty in vehicle velocity that was used to calculate the initial wind speed - _P(INDEX_W_N, INDEX_W_N) += hor_vel_variance; - _P(INDEX_W_E, INDEX_W_E) += hor_vel_variance; + _P.slice<2, 2>(0, 0) = P_wind_init; } else { // no airspeed available diff --git a/src/lib/wind_estimator/WindEstimator.hpp b/src/lib/wind_estimator/WindEstimator.hpp index 98e41194ec..9fc6a182b6 100644 --- a/src/lib/wind_estimator/WindEstimator.hpp +++ b/src/lib/wind_estimator/WindEstimator.hpp @@ -45,6 +45,7 @@ #include "python/generated/fuse_airspeed.h" #include "python/generated/fuse_beta.h" +#include "python/generated/init_wind_using_airspeed.h" using namespace time_literals; diff --git a/src/lib/wind_estimator/python/derivation.py b/src/lib/wind_estimator/python/derivation.py index cd76a06c17..04a2b06376 100644 --- a/src/lib/wind_estimator/python/derivation.py +++ b/src/lib/wind_estimator/python/derivation.py @@ -73,6 +73,31 @@ def fuse_beta( return (geo.V3(H), K, innov_var, innov) +def init_wind_using_airspeed( + v_local: geo.V3, + heading: T.Scalar, + airspeed: T.Scalar, + v_var: T.Scalar, + heading_var: T.Scalar, + sideslip_var: T.Scalar, + airspeed_var: T.Scalar, +) -> (geo.V2, geo.M22): + + # Initialise wind states assuming zero side slip and horizontal flight + wind = geo.V2(v_local[0] - airspeed * sm.cos(heading), v_local[1] - airspeed * sm.sin(heading)) + # Zero sideslip, propagate the sideslip variance using partial derivatives w.r.t heading + J = wind.jacobian([v_local[0], v_local[1], heading, heading, airspeed]) + + R = geo.M55() + R[0,0] = v_var + R[1,1] = v_var + R[2,2] = heading_var + R[3,3] = sideslip_var + R[4,4] = airspeed_var + + P = J * R * J.T + + return (wind, P) def generatePx4Function(function_name, output_names): from symforce.codegen import Codegen, CppConfig @@ -111,5 +136,7 @@ def generatePythonFunction(function_name, output_names): skip_directory_nesting=True) generatePx4Function(fuse_airspeed, output_names=["H", "K", "innov_var", "innov"]) -generatePythonFunction(fuse_airspeed, output_names=["H", "K", "innov_var", "innov"]) generatePx4Function(fuse_beta, output_names=["H", "K", "innov_var", "innov"]) +generatePx4Function(init_wind_using_airspeed, output_names=["wind", "P"]) + +generatePythonFunction(fuse_airspeed, output_names=["H", "K", "innov_var", "innov"]) diff --git a/src/lib/wind_estimator/python/generated/init_wind_using_airspeed.h b/src/lib/wind_estimator/python/generated/init_wind_using_airspeed.h new file mode 100644 index 0000000000..7d53eb5f6c --- /dev/null +++ b/src/lib/wind_estimator/python/generated/init_wind_using_airspeed.h @@ -0,0 +1,69 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// backends/cpp/templates/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: init_wind_using_airspeed + * + * Args: + * v_local: Matrix31 + * heading: Scalar + * airspeed: Scalar + * v_var: Scalar + * heading_var: Scalar + * sideslip_var: Scalar + * airspeed_var: Scalar + * + * Outputs: + * wind: Matrix21 + * P: Matrix22 + */ +template +void InitWindUsingAirspeed(const matrix::Matrix& v_local, const Scalar heading, + const Scalar airspeed, const Scalar v_var, const Scalar heading_var, + const Scalar sideslip_var, const Scalar airspeed_var, + matrix::Matrix* const wind = nullptr, + matrix::Matrix* const P = nullptr) { + // Total ops: 22 + + // Input arrays + + // Intermediate terms (7) + const Scalar _tmp0 = std::cos(heading); + const Scalar _tmp1 = std::sin(heading); + const Scalar _tmp2 = std::pow(_tmp1, Scalar(2)); + const Scalar _tmp3 = std::pow(airspeed, Scalar(2)) * sideslip_var; + const Scalar _tmp4 = std::pow(_tmp0, Scalar(2)); + const Scalar _tmp5 = _tmp0 * _tmp1; + const Scalar _tmp6 = -_tmp3 * _tmp5 + _tmp5 * airspeed_var; + + // Output terms (2) + if (wind != nullptr) { + matrix::Matrix& _wind = (*wind); + + _wind(0, 0) = -_tmp0 * airspeed + v_local(0, 0); + _wind(1, 0) = -_tmp1 * airspeed + v_local(1, 0); + } + + if (P != nullptr) { + matrix::Matrix& _P = (*P); + + _P(0, 0) = _tmp2 * _tmp3 + _tmp4 * airspeed_var + v_var; + _P(1, 0) = _tmp6; + _P(0, 1) = _tmp6; + _P(1, 1) = _tmp2 * airspeed_var + _tmp3 * _tmp4 + v_var; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym