diff --git a/src/modules/ekf2/EKF/airspeed_fusion.cpp b/src/modules/ekf2/EKF/airspeed_fusion.cpp index fcdc9c095c..63e4cdee7f 100644 --- a/src/modules/ekf2/EKF/airspeed_fusion.cpp +++ b/src/modules/ekf2/EKF/airspeed_fusion.cpp @@ -46,6 +46,7 @@ #include "python/ekf_derivation/generated/compute_airspeed_h_and_k.h" #include "python/ekf_derivation/generated/compute_airspeed_innov_and_innov_var.h" +#include "python/ekf_derivation/generated/compute_wind_init_and_cov_from_airspeed.h" #include @@ -225,46 +226,17 @@ void Ekf::stopAirspeedFusion() void Ekf::resetWindUsingAirspeed(const airspeedSample &airspeed_sample) { - const float euler_yaw = getEulerYaw(_R_to_earth); + constexpr float sideslip_var = sq(math::radians(15.0f)); - // estimate wind using zero sideslip assumption and airspeed measurement if airspeed available - _state.wind_vel(0) = _state.vel(0) - airspeed_sample.true_airspeed * cosf(euler_yaw); - _state.wind_vel(1) = _state.vel(1) - airspeed_sample.true_airspeed * sinf(euler_yaw); + 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)); + + 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); + + resetStateCovariance(P_wind); ECL_INFO("reset wind using airspeed to (%.3f, %.3f)", (double)_state.wind_vel(0), (double)_state.wind_vel(1)); - resetWindCovarianceUsingAirspeed(airspeed_sample); - _aid_src_airspeed.time_last_fuse = _time_delayed_us; } - -void Ekf::resetWindCovarianceUsingAirspeed(const airspeedSample &airspeed_sample) -{ - // Derived using EKF/matlab/scripts/Inertial Nav EKF/wind_cov.py - // TODO: explicitly include the sideslip angle in the derivation - const float euler_yaw = getEulerYaw(_R_to_earth); - const float R_TAS = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * math::constrain(airspeed_sample.eas2tas, 0.9f, 10.0f)); - constexpr float initial_sideslip_uncertainty = math::radians(15.0f); - const float initial_wind_var_body_y = sq(airspeed_sample.true_airspeed * sinf(initial_sideslip_uncertainty)); - constexpr float R_yaw = sq(math::radians(10.0f)); - - const float cos_yaw = cosf(euler_yaw); - const float sin_yaw = sinf(euler_yaw); - - // rotate wind velocity into earth frame aligned with vehicle yaw - const float Wx = _state.wind_vel(0) * cos_yaw + _state.wind_vel(1) * sin_yaw; - const float Wy = -_state.wind_vel(0) * sin_yaw + _state.wind_vel(1) * cos_yaw; - - // it is safer to remove all existing correlations to other states at this time - P.uncorrelateCovarianceSetVariance(State::wind_vel.idx, 0.0f); - - P(22, 22) = R_TAS * sq(cos_yaw) + R_yaw * sq(-Wx * sin_yaw - Wy * cos_yaw) + initial_wind_var_body_y * sq(sin_yaw); - P(22, 23) = R_TAS * sin_yaw * cos_yaw + R_yaw * (-Wx * sin_yaw - Wy * cos_yaw) * (Wx * cos_yaw - Wy * sin_yaw) - - initial_wind_var_body_y * sin_yaw * cos_yaw; - P(23, 22) = P(22, 23); - P(23, 23) = R_TAS * sq(sin_yaw) + R_yaw * sq(Wx * cos_yaw - Wy * sin_yaw) + initial_wind_var_body_y * sq(cos_yaw); - - // Now add the variance due to uncertainty in vehicle velocity that was used to calculate the initial wind speed - P(22, 22) += P(4, 4); - P(23, 23) += P(5, 5); -} diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index de5dc0c5ce..a6195b9810 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -865,9 +865,6 @@ private: // Reset the wind states using the current airspeed measurement, ground relative nav velocity, yaw angle and assumption of zero sideslip void resetWindUsingAirspeed(const airspeedSample &airspeed_sample); - - // perform a limited reset of the wind state covariances - void resetWindCovarianceUsingAirspeed(const airspeedSample &airspeed_sample); #endif // CONFIG_EKF2_AIRSPEED #if defined(CONFIG_EKF2_SIDESLIP) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py index 1a17dda154..a5a945f98a 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py @@ -175,6 +175,35 @@ def compute_airspeed_h_and_k( return (H.T, K) +def compute_wind_init_and_cov_from_airspeed( + v_local: sf.V3, + heading: sf.Scalar, + airspeed: sf.Scalar, + v_var: sf.V3, + heading_var: sf.Scalar, + sideslip_var: sf.Scalar, + airspeed_var: sf.Scalar, +) -> (sf.V2, sf.M22): + + # 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]) + + 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 + + # Assume zero sideslip + P = P.subs({sideslip: 0.0}) + wind = wind.subs({sideslip: 0.0}) + return (wind, P) + def predict_sideslip( state: State, epsilon: sf.Scalar @@ -568,6 +597,7 @@ print("Derive EKF2 equations...") generate_px4_function(predict_covariance, output_names=["P_new"]) generate_px4_function(compute_airspeed_innov_and_innov_var, output_names=["innov", "innov_var"]) generate_px4_function(compute_airspeed_h_and_k, output_names=["H", "K"]) +generate_px4_function(compute_wind_init_and_cov_from_airspeed, output_names=["wind", "P_wind"]) generate_px4_function(compute_sideslip_innov_and_innov_var, output_names=["innov", "innov_var"]) generate_px4_function(compute_sideslip_h_and_k, output_names=["H", "K"]) generate_px4_function(compute_mag_innov_innov_var_and_hx, output_names=["innov", "innov_var", "Hx"]) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_wind_init_and_cov_from_airspeed.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_wind_init_and_cov_from_airspeed.h new file mode 100644 index 0000000000..95144609e6 --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_wind_init_and_cov_from_airspeed.h @@ -0,0 +1,73 @@ +// ----------------------------------------------------------------------------- +// 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_airspeed + * + * Args: + * v_local: Matrix31 + * heading: Scalar + * airspeed: Scalar + * v_var: Matrix31 + * heading_var: Scalar + * sideslip_var: Scalar + * airspeed_var: Scalar + * + * Outputs: + * wind: Matrix21 + * P_wind: Matrix22 + */ +template +void ComputeWindInitAndCovFromAirspeed(const matrix::Matrix& v_local, + const Scalar heading, const Scalar airspeed, + const matrix::Matrix& v_var, + const Scalar heading_var, const Scalar sideslip_var, + const Scalar airspeed_var, + matrix::Matrix* const wind = nullptr, + matrix::Matrix* const P_wind = nullptr) { + // Total ops: 29 + + // Input arrays + + // 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)); + 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) { + matrix::Matrix& _wind = (*wind); + + _wind(0, 0) = -_tmp0 * airspeed + v_local(0, 0); + _wind(1, 0) = -_tmp1 * airspeed + v_local(1, 0); + } + + if (P_wind != nullptr) { + matrix::Matrix& _p_wind = (*P_wind); + + _p_wind(0, 0) = _tmp2 * _tmp4 + _tmp2 * _tmp5 + _tmp6 * airspeed_var + v_var(0, 0); + _p_wind(1, 0) = _tmp8; + _p_wind(0, 1) = _tmp8; + _p_wind(1, 1) = _tmp2 * airspeed_var + _tmp4 * _tmp6 + _tmp5 * _tmp6 + v_var(1, 0); + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym