From 6969e5b6b4d89888a50a294beeea14fa2a9b617b Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 16 Dec 2024 15:23:06 +0100 Subject: [PATCH] ekf2: do not pre-compute airspeed Kalman gain The generated code is not much faster than the simple matrix-vector multiplication --- .../aid_sources/airspeed/airspeed_fusion.cpp | 8 +- .../EKF/python/ekf_derivation/derivation.py | 10 +- .../generated/compute_airspeed_h.h | 56 +++++++++ .../generated/compute_airspeed_h_and_k.h | 116 ------------------ 4 files changed, 62 insertions(+), 128 deletions(-) create mode 100644 src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_h.h delete mode 100644 src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_h_and_k.h 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 51f6fc8f707f..14873831fcee 100644 --- a/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp @@ -44,7 +44,7 @@ #include "ekf.h" -#include +#include #include #include @@ -204,10 +204,8 @@ void Ekf::fuseAirspeed(const airspeedSample &airspeed_sample, estimator_aid_sour _fault_status.flags.bad_airspeed = false; - VectorState H; // Observation jacobian - VectorState K; // Kalman gain vector - - sym::ComputeAirspeedHAndK(_state.vector(), P, innov_var, FLT_EPSILON, &H, &K); + const VectorState H = sym::ComputeAirspeedH(_state.vector(), FLT_EPSILON); + VectorState K = P * H / aid_src.innovation_variance; if (update_wind_only) { const Vector2f K_wind = K.slice(State::wind_vel.idx, 0); diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py index 86651f9d656c..0ad3fef4ed0e 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py @@ -268,10 +268,8 @@ def compute_airspeed_innov_and_innov_var( return (innov, innov_var) -def compute_airspeed_h_and_k( +def compute_airspeed_h( state: VState, - P: MTangent, - innov_var: sf.Scalar, epsilon: sf.Scalar ) -> (VTangent, VTangent): @@ -281,9 +279,7 @@ def compute_airspeed_h_and_k( airspeed_pred = vel_rel.norm(epsilon=epsilon) H = jacobian_chain_rule(airspeed_pred, state) - K = P * H.T / sf.Max(innov_var, epsilon) - - return (H.T, K) + return H.T def compute_wind_init_and_cov_from_airspeed( v_local: sf.V3, @@ -739,7 +735,7 @@ def compute_gravity_z_innov_var_and_h( generate_px4_function(compute_mag_z_innov_var_and_h, output_names=["innov_var", "H"]) if not args.disable_wind: - generate_px4_function(compute_airspeed_h_and_k, output_names=["H", "K"]) + generate_px4_function(compute_airspeed_h, output_names=None) generate_px4_function(compute_airspeed_innov_and_innov_var, output_names=["innov", "innov_var"]) generate_px4_function(compute_drag_x_innov_var_and_h, output_names=["innov_var", "Hx"]) generate_px4_function(compute_drag_y_innov_var_and_h, output_names=["innov_var", "Hy"]) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_h.h new file mode 100644 index 000000000000..999c01d1eebc --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_h.h @@ -0,0 +1,56 @@ +// ----------------------------------------------------------------------------- +// 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_airspeed_h + * + * Args: + * state: Matrix25_1 + * epsilon: Scalar + * + * Outputs: + * res: Matrix24_1 + */ +template +matrix::Matrix ComputeAirspeedH(const matrix::Matrix& state, + const Scalar epsilon) { + // Total ops: 14 + + // Input arrays + + // Intermediate terms (5) + const Scalar _tmp0 = -state(23, 0) + state(5, 0); + const Scalar _tmp1 = -state(22, 0) + state(4, 0); + const Scalar _tmp2 = std::pow(Scalar(std::pow(_tmp0, Scalar(2)) + std::pow(_tmp1, Scalar(2)) + + epsilon + std::pow(state(6, 0), Scalar(2))), + Scalar(Scalar(-1) / Scalar(2))); + const Scalar _tmp3 = _tmp1 * _tmp2; + const Scalar _tmp4 = _tmp0 * _tmp2; + + // Output terms (1) + matrix::Matrix _res; + + _res.setZero(); + + _res(3, 0) = _tmp3; + _res(4, 0) = _tmp4; + _res(5, 0) = _tmp2 * state(6, 0); + _res(21, 0) = -_tmp3; + _res(22, 0) = -_tmp4; + + return _res; +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_h_and_k.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_h_and_k.h deleted file mode 100644 index 52bb16353d36..000000000000 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_h_and_k.h +++ /dev/null @@ -1,116 +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_airspeed_h_and_k - * - * Args: - * state: Matrix25_1 - * P: Matrix24_24 - * innov_var: Scalar - * epsilon: Scalar - * - * Outputs: - * H: Matrix24_1 - * K: Matrix24_1 - */ -template -void ComputeAirspeedHAndK(const matrix::Matrix& state, - const matrix::Matrix& P, const Scalar innov_var, - const Scalar epsilon, matrix::Matrix* const H = nullptr, - matrix::Matrix* const K = nullptr) { - // Total ops: 256 - - // Input arrays - - // Intermediate terms (7) - const Scalar _tmp0 = -state(23, 0) + state(5, 0); - const Scalar _tmp1 = -state(22, 0) + state(4, 0); - const Scalar _tmp2 = std::pow(Scalar(std::pow(_tmp0, Scalar(2)) + std::pow(_tmp1, Scalar(2)) + - epsilon + std::pow(state(6, 0), Scalar(2))), - Scalar(Scalar(-1) / Scalar(2))); - const Scalar _tmp3 = _tmp1 * _tmp2; - const Scalar _tmp4 = _tmp0 * _tmp2; - const Scalar _tmp5 = _tmp2 * state(6, 0); - const Scalar _tmp6 = Scalar(1.0) / (math::max(epsilon, innov_var)); - - // Output terms (2) - if (H != nullptr) { - matrix::Matrix& _h = (*H); - - _h.setZero(); - - _h(3, 0) = _tmp3; - _h(4, 0) = _tmp4; - _h(5, 0) = _tmp5; - _h(21, 0) = -_tmp3; - _h(22, 0) = -_tmp4; - } - - if (K != nullptr) { - matrix::Matrix& _k = (*K); - - _k(0, 0) = _tmp6 * (-P(0, 21) * _tmp3 - P(0, 22) * _tmp4 + P(0, 3) * _tmp3 + P(0, 4) * _tmp4 + - P(0, 5) * _tmp5); - _k(1, 0) = _tmp6 * (-P(1, 21) * _tmp3 - P(1, 22) * _tmp4 + P(1, 3) * _tmp3 + P(1, 4) * _tmp4 + - P(1, 5) * _tmp5); - _k(2, 0) = _tmp6 * (-P(2, 21) * _tmp3 - P(2, 22) * _tmp4 + P(2, 3) * _tmp3 + P(2, 4) * _tmp4 + - P(2, 5) * _tmp5); - _k(3, 0) = _tmp6 * (-P(3, 21) * _tmp3 - P(3, 22) * _tmp4 + P(3, 3) * _tmp3 + P(3, 4) * _tmp4 + - P(3, 5) * _tmp5); - _k(4, 0) = _tmp6 * (-P(4, 21) * _tmp3 - P(4, 22) * _tmp4 + P(4, 3) * _tmp3 + P(4, 4) * _tmp4 + - P(4, 5) * _tmp5); - _k(5, 0) = _tmp6 * (-P(5, 21) * _tmp3 - P(5, 22) * _tmp4 + P(5, 3) * _tmp3 + P(5, 4) * _tmp4 + - P(5, 5) * _tmp5); - _k(6, 0) = _tmp6 * (-P(6, 21) * _tmp3 - P(6, 22) * _tmp4 + P(6, 3) * _tmp3 + P(6, 4) * _tmp4 + - P(6, 5) * _tmp5); - _k(7, 0) = _tmp6 * (-P(7, 21) * _tmp3 - P(7, 22) * _tmp4 + P(7, 3) * _tmp3 + P(7, 4) * _tmp4 + - P(7, 5) * _tmp5); - _k(8, 0) = _tmp6 * (-P(8, 21) * _tmp3 - P(8, 22) * _tmp4 + P(8, 3) * _tmp3 + P(8, 4) * _tmp4 + - P(8, 5) * _tmp5); - _k(9, 0) = _tmp6 * (-P(9, 21) * _tmp3 - P(9, 22) * _tmp4 + P(9, 3) * _tmp3 + P(9, 4) * _tmp4 + - P(9, 5) * _tmp5); - _k(10, 0) = _tmp6 * (-P(10, 21) * _tmp3 - P(10, 22) * _tmp4 + P(10, 3) * _tmp3 + - P(10, 4) * _tmp4 + P(10, 5) * _tmp5); - _k(11, 0) = _tmp6 * (-P(11, 21) * _tmp3 - P(11, 22) * _tmp4 + P(11, 3) * _tmp3 + - P(11, 4) * _tmp4 + P(11, 5) * _tmp5); - _k(12, 0) = _tmp6 * (-P(12, 21) * _tmp3 - P(12, 22) * _tmp4 + P(12, 3) * _tmp3 + - P(12, 4) * _tmp4 + P(12, 5) * _tmp5); - _k(13, 0) = _tmp6 * (-P(13, 21) * _tmp3 - P(13, 22) * _tmp4 + P(13, 3) * _tmp3 + - P(13, 4) * _tmp4 + P(13, 5) * _tmp5); - _k(14, 0) = _tmp6 * (-P(14, 21) * _tmp3 - P(14, 22) * _tmp4 + P(14, 3) * _tmp3 + - P(14, 4) * _tmp4 + P(14, 5) * _tmp5); - _k(15, 0) = _tmp6 * (-P(15, 21) * _tmp3 - P(15, 22) * _tmp4 + P(15, 3) * _tmp3 + - P(15, 4) * _tmp4 + P(15, 5) * _tmp5); - _k(16, 0) = _tmp6 * (-P(16, 21) * _tmp3 - P(16, 22) * _tmp4 + P(16, 3) * _tmp3 + - P(16, 4) * _tmp4 + P(16, 5) * _tmp5); - _k(17, 0) = _tmp6 * (-P(17, 21) * _tmp3 - P(17, 22) * _tmp4 + P(17, 3) * _tmp3 + - P(17, 4) * _tmp4 + P(17, 5) * _tmp5); - _k(18, 0) = _tmp6 * (-P(18, 21) * _tmp3 - P(18, 22) * _tmp4 + P(18, 3) * _tmp3 + - P(18, 4) * _tmp4 + P(18, 5) * _tmp5); - _k(19, 0) = _tmp6 * (-P(19, 21) * _tmp3 - P(19, 22) * _tmp4 + P(19, 3) * _tmp3 + - P(19, 4) * _tmp4 + P(19, 5) * _tmp5); - _k(20, 0) = _tmp6 * (-P(20, 21) * _tmp3 - P(20, 22) * _tmp4 + P(20, 3) * _tmp3 + - P(20, 4) * _tmp4 + P(20, 5) * _tmp5); - _k(21, 0) = _tmp6 * (-P(21, 21) * _tmp3 - P(21, 22) * _tmp4 + P(21, 3) * _tmp3 + - P(21, 4) * _tmp4 + P(21, 5) * _tmp5); - _k(22, 0) = _tmp6 * (-P(22, 21) * _tmp3 - P(22, 22) * _tmp4 + P(22, 3) * _tmp3 + - P(22, 4) * _tmp4 + P(22, 5) * _tmp5); - _k(23, 0) = _tmp6 * (-P(23, 21) * _tmp3 - P(23, 22) * _tmp4 + P(23, 3) * _tmp3 + - P(23, 4) * _tmp4 + P(23, 5) * _tmp5); - } -} // NOLINT(readability/fn_size) - -// NOLINTNEXTLINE(readability/fn_size) -} // namespace sym