diff --git a/src/lib/wind_estimator/python/derivation.py b/src/lib/wind_estimator/python/derivation.py index fdc14ecee5..7d53e699b0 100755 --- a/src/lib/wind_estimator/python/derivation.py +++ b/src/lib/wind_estimator/python/derivation.py @@ -96,10 +96,10 @@ def init_wind_using_airspeed( airspeed_var: sf.Scalar, ) -> (sf.V2, sf.M22): - # Initialise wind states assuming zero side slip and horizontal flight - wind = sf.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]) + # Initialise wind states assuming horizontal flight + sideslip = sm.Symbol("beta") + wind = sf.V2(v_local[0] - airspeed * sm.cos(heading + sideslip), v_local[1] - airspeed * sm.sin(heading + sideslip)) + J = wind.jacobian([v_local[0], v_local[1], heading, sideslip, airspeed]) R = sf.M55() R[0,0] = v_var @@ -110,6 +110,10 @@ def init_wind_using_airspeed( P = J * R * J.T + # Assume zero sideslip + P = P.subs({sideslip: 0.0}) + wind = wind.subs({sideslip: 0.0}) + return (wind, P) generate_px4_function(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 index cbfa6fa152..8fca830af2 100644 --- a/src/lib/wind_estimator/python/generated/init_wind_using_airspeed.h +++ b/src/lib/wind_estimator/python/generated/init_wind_using_airspeed.h @@ -34,21 +34,20 @@ void InitWindUsingAirspeed(const matrix::Matrix& v_local, const Sc const Scalar sideslip_var, const Scalar airspeed_var, matrix::Matrix* const wind = nullptr, matrix::Matrix* const P = nullptr) { - // Total ops: 22 - - // Unused inputs - (void)heading_var; + // Total ops: 29 // Input arrays - // Intermediate terms (7) + // Intermediate terms (9) 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; + const Scalar _tmp3 = std::pow(airspeed, Scalar(2)); + const Scalar _tmp4 = _tmp3 * sideslip_var; + const Scalar _tmp5 = _tmp3 * heading_var; + const Scalar _tmp6 = std::pow(_tmp0, Scalar(2)); + const Scalar _tmp7 = _tmp0 * _tmp1; + const Scalar _tmp8 = -_tmp4 * _tmp7 - _tmp5 * _tmp7 + _tmp7 * airspeed_var; // Output terms (2) if (wind != nullptr) { @@ -61,10 +60,10 @@ void InitWindUsingAirspeed(const matrix::Matrix& v_local, const Sc 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; + _p(0, 0) = _tmp2 * _tmp4 + _tmp2 * _tmp5 + _tmp6 * airspeed_var + v_var; + _p(1, 0) = _tmp8; + _p(0, 1) = _tmp8; + _p(1, 1) = _tmp2 * airspeed_var + _tmp4 * _tmp6 + _tmp5 * _tmp6 + v_var; } } // NOLINT(readability/fn_size)