diff --git a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp index e12bf1bf26..d85d08cba0 100644 --- a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp +++ b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp @@ -41,7 +41,7 @@ */ #include "ekf.h" -#include "python/ekf_derivation/generated/compute_gnss_yaw_innon_innov_var_and_h.h" +#include "python/ekf_derivation/generated/compute_gnss_yaw_pred_innov_var_and_h.h" #include #include @@ -59,17 +59,17 @@ void Ekf::updateGpsYaw(const gpsSample &gps_sample) const float R_YAW = sq(fmaxf(gps_sample.yaw_acc, _params.gps_heading_noise)); - float heading_innov; + float heading_pred; float heading_innov_var; { Vector24f H; - sym::ComputeGnssYawInnonInnovVarAndH(getStateAtFusionHorizonAsVector(), P, _gps_yaw_offset, measured_hdg, R_YAW, FLT_EPSILON, &heading_innov, &heading_innov_var, &H); + sym::ComputeGnssYawPredInnovVarAndH(getStateAtFusionHorizonAsVector(), P, _gps_yaw_offset, R_YAW, FLT_EPSILON, &heading_pred, &heading_innov_var, &H); } gnss_yaw.observation = measured_hdg; gnss_yaw.observation_variance = R_YAW; - gnss_yaw.innovation = heading_innov; + gnss_yaw.innovation = wrap_pi(heading_pred - measured_hdg); gnss_yaw.innovation_variance = heading_innov_var; gnss_yaw.fusion_enabled = _control_status.flags.gps_yaw; @@ -93,12 +93,12 @@ void Ekf::fuseGpsYaw() Vector24f H; { - float heading_innov; + float heading_pred; float heading_innov_var; // Note: we recompute innov and innov_var because it doesn't cost much more than just computing H // making a separate function just for H uses more flash space without reducing CPU load significantly - sym::ComputeGnssYawInnonInnovVarAndH(getStateAtFusionHorizonAsVector(), P, _gps_yaw_offset, gnss_yaw.observation, gnss_yaw.observation_variance, FLT_EPSILON, &heading_innov, &heading_innov_var, &H); + sym::ComputeGnssYawPredInnovVarAndH(getStateAtFusionHorizonAsVector(), P, _gps_yaw_offset, gnss_yaw.observation_variance, FLT_EPSILON, &heading_pred, &heading_innov_var, &H); } const SparseVector24f<0,1,2,3> Hfusion(H); diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py index 3d15be6d82..0fd396cd43 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py @@ -387,11 +387,10 @@ def compute_flow_y_innov_var_and_h( return (innov_var, Hy.T) -def compute_gnss_yaw_innon_innov_var_and_h( +def compute_gnss_yaw_pred_innov_var_and_h( state: VState, P: MState, antenna_yaw_offset: sf.Scalar, - meas: sf.Scalar, R: sf.Scalar, epsilon: sf.Scalar ) -> (sf.Scalar, sf.Scalar, VState): @@ -411,9 +410,7 @@ def compute_gnss_yaw_innon_innov_var_and_h( H = sf.V1(meas_pred).jacobian(state) innov_var = (H * P * H.T + R)[0,0] - innov = meas_pred - meas - - return (innov, innov_var, H.T) + return (meas_pred, innov_var, H.T) def predict_drag( state: VState, @@ -524,7 +521,7 @@ generate_px4_function(compute_yaw_312_innov_var_and_h_alternate, output_names=[" generate_px4_function(compute_mag_declination_innov_innov_var_and_h, output_names=["innov", "innov_var", "H"]) generate_px4_function(compute_flow_xy_innov_var_and_hx, output_names=["innov_var", "H"]) generate_px4_function(compute_flow_y_innov_var_and_h, output_names=["innov_var", "H"]) -generate_px4_function(compute_gnss_yaw_innon_innov_var_and_h, output_names=["innov", "innov_var", "H"]) +generate_px4_function(compute_gnss_yaw_pred_innov_var_and_h, output_names=["meas_pred", "innov_var", "H"]) generate_px4_function(compute_drag_x_innov_var_and_k, output_names=["innov_var", "K"]) generate_px4_function(compute_drag_y_innov_var_and_k, output_names=["innov_var", "K"]) generate_px4_function(compute_gravity_innov_var_and_k_and_h, output_names=["innov", "innov_var", "Kx", "Ky", "Kz"]) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gnss_yaw_innon_innov_var_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gnss_yaw_innon_innov_var_and_h.h deleted file mode 100644 index 0836399f9d..0000000000 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gnss_yaw_innon_innov_var_and_h.h +++ /dev/null @@ -1,103 +0,0 @@ -// ----------------------------------------------------------------------------- -// 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: compute_gnss_yaw_innon_innov_var_and_h - * - * Args: - * state: Matrix24_1 - * P: Matrix24_24 - * antenna_yaw_offset: Scalar - * meas: Scalar - * R: Scalar - * epsilon: Scalar - * - * Outputs: - * innov: Scalar - * innov_var: Scalar - * H: Matrix24_1 - */ -template -void ComputeGnssYawInnonInnovVarAndH(const matrix::Matrix& state, - const matrix::Matrix& P, - const Scalar antenna_yaw_offset, const Scalar meas, - const Scalar R, const Scalar epsilon, - Scalar* const innov = nullptr, - Scalar* const innov_var = nullptr, - matrix::Matrix* const H = nullptr) { - // Total ops: 106 - - // Input arrays - - // Intermediate terms (28) - const Scalar _tmp0 = std::pow(state(2, 0), Scalar(2)); - const Scalar _tmp1 = std::pow(state(1, 0), Scalar(2)); - const Scalar _tmp2 = std::pow(state(0, 0), Scalar(2)) - std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp3 = std::sin(antenna_yaw_offset); - const Scalar _tmp4 = state(0, 0) * state(3, 0); - const Scalar _tmp5 = state(1, 0) * state(2, 0); - const Scalar _tmp6 = std::cos(antenna_yaw_offset); - const Scalar _tmp7 = _tmp3 * (_tmp0 - _tmp1 + _tmp2) + 2 * _tmp6 * (_tmp4 + _tmp5); - const Scalar _tmp8 = 2 * _tmp3 * (-_tmp4 + _tmp5) + _tmp6 * (-_tmp0 + _tmp1 + _tmp2); - const Scalar _tmp9 = _tmp8 + epsilon * ((((_tmp8) > 0) - ((_tmp8) < 0)) + Scalar(0.5)); - const Scalar _tmp10 = 2 * state(3, 0); - const Scalar _tmp11 = 2 * state(0, 0); - const Scalar _tmp12 = -_tmp10 * _tmp3 + _tmp11 * _tmp6; - const Scalar _tmp13 = Scalar(1.0) / (_tmp9); - const Scalar _tmp14 = _tmp10 * _tmp6; - const Scalar _tmp15 = _tmp11 * _tmp3; - const Scalar _tmp16 = std::pow(_tmp9, Scalar(2)); - const Scalar _tmp17 = _tmp7 / _tmp16; - const Scalar _tmp18 = _tmp16 / (_tmp16 + std::pow(_tmp7, Scalar(2))); - const Scalar _tmp19 = _tmp18 * (_tmp12 * _tmp13 - _tmp17 * (-_tmp14 - _tmp15)); - const Scalar _tmp20 = 2 * state(1, 0); - const Scalar _tmp21 = 2 * state(2, 0); - const Scalar _tmp22 = _tmp20 * _tmp6 + _tmp21 * _tmp3; - const Scalar _tmp23 = _tmp20 * _tmp3; - const Scalar _tmp24 = _tmp21 * _tmp6; - const Scalar _tmp25 = _tmp18 * (_tmp13 * (-_tmp23 + _tmp24) - _tmp17 * _tmp22); - const Scalar _tmp26 = _tmp18 * (-_tmp12 * _tmp17 + _tmp13 * (_tmp14 + _tmp15)); - const Scalar _tmp27 = _tmp18 * (_tmp13 * _tmp22 - _tmp17 * (_tmp23 - _tmp24)); - - // Output terms (3) - if (innov != nullptr) { - Scalar& _innov = (*innov); - - _innov = -meas + std::atan2(_tmp7, _tmp9); - } - - if (innov_var != nullptr) { - Scalar& _innov_var = (*innov_var); - - _innov_var = - R + _tmp19 * (P(0, 3) * _tmp26 + P(1, 3) * _tmp25 + P(2, 3) * _tmp27 + P(3, 3) * _tmp19) + - _tmp25 * (P(0, 1) * _tmp26 + P(1, 1) * _tmp25 + P(2, 1) * _tmp27 + P(3, 1) * _tmp19) + - _tmp26 * (P(0, 0) * _tmp26 + P(1, 0) * _tmp25 + P(2, 0) * _tmp27 + P(3, 0) * _tmp19) + - _tmp27 * (P(0, 2) * _tmp26 + P(1, 2) * _tmp25 + P(2, 2) * _tmp27 + P(3, 2) * _tmp19); - } - - if (H != nullptr) { - matrix::Matrix& _h = (*H); - - _h.setZero(); - - _h(0, 0) = _tmp26; - _h(1, 0) = _tmp25; - _h(2, 0) = _tmp27; - _h(3, 0) = _tmp19; - } -} // NOLINT(readability/fn_size) - -// NOLINTNEXTLINE(readability/fn_size) -} // namespace sym diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gnss_yaw_pred_innov_var_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gnss_yaw_pred_innov_var_and_h.h new file mode 100644 index 0000000000..9fd7866e54 --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gnss_yaw_pred_innov_var_and_h.h @@ -0,0 +1,99 @@ +// ----------------------------------------------------------------------------- +// 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: compute_gnss_yaw_pred_innov_var_and_h + * + * Args: + * state: Matrix24_1 + * P: Matrix24_24 + * antenna_yaw_offset: Scalar + * R: Scalar + * epsilon: Scalar + * + * Outputs: + * meas_pred: Scalar + * innov_var: Scalar + * H: Matrix24_1 + */ +template +void ComputeGnssYawPredInnovVarAndH(const matrix::Matrix& state, + const matrix::Matrix& P, + const Scalar antenna_yaw_offset, const Scalar R, + const Scalar epsilon, Scalar* const meas_pred = nullptr, + Scalar* const innov_var = nullptr, + matrix::Matrix* const H = nullptr) { + // Total ops: 101 + + // Input arrays + + // Intermediate terms (26) + const Scalar _tmp0 = std::pow(state(2, 0), Scalar(2)); + const Scalar _tmp1 = std::pow(state(1, 0), Scalar(2)); + const Scalar _tmp2 = std::pow(state(0, 0), Scalar(2)) - std::pow(state(3, 0), Scalar(2)); + const Scalar _tmp3 = std::sin(antenna_yaw_offset); + const Scalar _tmp4 = state(0, 0) * state(3, 0); + const Scalar _tmp5 = state(1, 0) * state(2, 0); + const Scalar _tmp6 = std::cos(antenna_yaw_offset); + const Scalar _tmp7 = 2 * _tmp6; + const Scalar _tmp8 = _tmp3 * (_tmp0 - _tmp1 + _tmp2) + _tmp7 * (_tmp4 + _tmp5); + const Scalar _tmp9 = 2 * _tmp3; + const Scalar _tmp10 = _tmp6 * (-_tmp0 + _tmp1 + _tmp2) + _tmp9 * (-_tmp4 + _tmp5); + const Scalar _tmp11 = _tmp10 + epsilon * ((((_tmp10) > 0) - ((_tmp10) < 0)) + Scalar(0.5)); + const Scalar _tmp12 = _tmp7 * state(0, 0) - _tmp9 * state(3, 0); + const Scalar _tmp13 = Scalar(1.0) / (_tmp11); + const Scalar _tmp14 = _tmp7 * state(3, 0); + const Scalar _tmp15 = _tmp9 * state(0, 0); + const Scalar _tmp16 = std::pow(_tmp11, Scalar(2)); + const Scalar _tmp17 = _tmp8 / _tmp16; + const Scalar _tmp18 = _tmp16 / (_tmp16 + std::pow(_tmp8, Scalar(2))); + const Scalar _tmp19 = _tmp18 * (_tmp12 * _tmp13 - _tmp17 * (-_tmp14 - _tmp15)); + const Scalar _tmp20 = _tmp7 * state(1, 0) + _tmp9 * state(2, 0); + const Scalar _tmp21 = _tmp9 * state(1, 0); + const Scalar _tmp22 = _tmp7 * state(2, 0); + const Scalar _tmp23 = _tmp18 * (_tmp13 * (-_tmp21 + _tmp22) - _tmp17 * _tmp20); + const Scalar _tmp24 = _tmp18 * (-_tmp12 * _tmp17 + _tmp13 * (_tmp14 + _tmp15)); + const Scalar _tmp25 = _tmp18 * (_tmp13 * _tmp20 - _tmp17 * (_tmp21 - _tmp22)); + + // Output terms (3) + if (meas_pred != nullptr) { + Scalar& _meas_pred = (*meas_pred); + + _meas_pred = std::atan2(_tmp8, _tmp11); + } + + if (innov_var != nullptr) { + Scalar& _innov_var = (*innov_var); + + _innov_var = + R + _tmp19 * (P(0, 3) * _tmp24 + P(1, 3) * _tmp23 + P(2, 3) * _tmp25 + P(3, 3) * _tmp19) + + _tmp23 * (P(0, 1) * _tmp24 + P(1, 1) * _tmp23 + P(2, 1) * _tmp25 + P(3, 1) * _tmp19) + + _tmp24 * (P(0, 0) * _tmp24 + P(1, 0) * _tmp23 + P(2, 0) * _tmp25 + P(3, 0) * _tmp19) + + _tmp25 * (P(0, 2) * _tmp24 + P(1, 2) * _tmp23 + P(2, 2) * _tmp25 + P(3, 2) * _tmp19); + } + + if (H != nullptr) { + matrix::Matrix& _h = (*H); + + _h.setZero(); + + _h(0, 0) = _tmp24; + _h(1, 0) = _tmp23; + _h(2, 0) = _tmp25; + _h(3, 0) = _tmp19; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/src/modules/ekf2/test/test_EKF_gnss_yaw_generated.cpp b/src/modules/ekf2/test/test_EKF_gnss_yaw_generated.cpp index eeaa8e9c32..afada74b5b 100644 --- a/src/modules/ekf2/test/test_EKF_gnss_yaw_generated.cpp +++ b/src/modules/ekf2/test/test_EKF_gnss_yaw_generated.cpp @@ -35,7 +35,7 @@ #include "EKF/ekf.h" #include "test_helper/comparison_helper.h" -#include "../EKF/python/ekf_derivation/generated/compute_gnss_yaw_innon_innov_var_and_h.h" +#include "../EKF/python/ekf_derivation/generated/compute_gnss_yaw_pred_innov_var_and_h.h" using namespace matrix; @@ -140,11 +140,11 @@ TEST(GnssYawFusionGenerated, SympyVsSymforce) Vector24f K_sympy; sympyGnssYawInnovVarHAndK(q(0), q(1), q(2), q(3), P, yaw_offset, R_YAW, innov_var_sympy, H_sympy, K_sympy); - float innov_symforce; + float meas_pred_symforce; float innov_var_symforce; Vector24f H_symforce; - sym::ComputeGnssYawInnonInnovVarAndH(state_vector, P, yaw_offset, 0.f, R_YAW, FLT_EPSILON, &innov_symforce, - &innov_var_symforce, &H_symforce); + sym::ComputeGnssYawPredInnovVarAndH(state_vector, P, yaw_offset, R_YAW, FLT_EPSILON, &meas_pred_symforce, + &innov_var_symforce, &H_symforce); // K isn't generated from symbolic anymore to save flash space Vector24f K_symforce = P * H_symforce / innov_var_symforce; @@ -177,11 +177,11 @@ TEST(GnssYawFusionGenerated, SingularityPitch90) SquareMatrix24f P = createRandomCovarianceMatrix24f(); const float R_YAW = sq(0.3f); - float innov; + float meas_pred; float innov_var; Vector24f H; - sym::ComputeGnssYawInnonInnovVarAndH(state_vector, P, yaw_offset, 0.f, R_YAW, FLT_EPSILON, &innov, - &innov_var, &H); + sym::ComputeGnssYawPredInnovVarAndH(state_vector, P, yaw_offset, R_YAW, FLT_EPSILON, &meas_pred, + &innov_var, &H); Vector24f K = P * H / innov_var; // THEN: the arctan is singular, the attitude isn't observable, so the innovation variance @@ -205,11 +205,11 @@ TEST(GnssYawFusionGenerated, SingularityRoll90) SquareMatrix24f P = createRandomCovarianceMatrix24f(); const float R_YAW = sq(0.3f); - float innov; + float meas_pred; float innov_var; Vector24f H; - sym::ComputeGnssYawInnonInnovVarAndH(state_vector, P, yaw_offset, 0.f, R_YAW, FLT_EPSILON, &innov, - &innov_var, &H); + sym::ComputeGnssYawPredInnovVarAndH(state_vector, P, yaw_offset, R_YAW, FLT_EPSILON, &meas_pred, + &innov_var, &H); Vector24f K = P * H / innov_var; // THEN: the arctan is singular, the attitude isn't observable, so the innovation variance