diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py index c67d1f007271..5cbe1a127f3b 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py @@ -408,74 +408,6 @@ def compute_yaw_innov_var_and_h( return (innov_var, H.T) -def compute_yaw_321_innov_var_and_h( - state: VState, - P: MTangent, - R: sf.Scalar, - epsilon: sf.Scalar -) -> (sf.Scalar, VTangent): - - state = vstate_to_state(state) - R_to_earth = state["quat_nominal"].to_rotation_matrix() - # Fix the singularity at pi/2 by inserting epsilon - meas_pred = sf.atan2(R_to_earth[1,0], R_to_earth[0,0], epsilon=epsilon) - - H = sf.V1(meas_pred).jacobian(state) - innov_var = (H * P * H.T + R)[0,0] - - return (innov_var, H.T) - -def compute_yaw_321_innov_var_and_h_alternate( - state: VState, - P: MTangent, - R: sf.Scalar, - epsilon: sf.Scalar -) -> (sf.Scalar, VTangent): - - state = vstate_to_state(state) - R_to_earth = state["quat_nominal"].to_rotation_matrix() - # Alternate form that has a singularity at yaw 0 instead of pi/2 - meas_pred = sf.pi/2 - sf.atan2(R_to_earth[0,0], R_to_earth[1,0], epsilon=epsilon) - - H = sf.V1(meas_pred).jacobian(state) - innov_var = (H * P * H.T + R)[0,0] - - return (innov_var, H.T) - -def compute_yaw_312_innov_var_and_h( - state: VState, - P: MTangent, - R: sf.Scalar, - epsilon: sf.Scalar -) -> (sf.Scalar, VTangent): - - state = vstate_to_state(state) - R_to_earth = state["quat_nominal"].to_rotation_matrix() - # Alternate form to be used when close to pitch +-pi/2 - meas_pred = sf.atan2(-R_to_earth[0,1], R_to_earth[1,1], epsilon=epsilon) - - H = sf.V1(meas_pred).jacobian(state) - innov_var = (H * P * H.T + R)[0,0] - - return (innov_var, H.T) - -def compute_yaw_312_innov_var_and_h_alternate( - state: VState, - P: MTangent, - R: sf.Scalar, - epsilon: sf.Scalar -) -> (sf.Scalar, VTangent): - - state = vstate_to_state(state) - R_to_earth = state["quat_nominal"].to_rotation_matrix() - # Alternate form to be used when close to pitch +-pi/2 - meas_pred = sf.pi/2 - sf.atan2(-R_to_earth[1,1], R_to_earth[0,1], epsilon=epsilon) - - H = sf.V1(meas_pred).jacobian(state) - innov_var = (H * P * H.T + R)[0,0] - - return (innov_var, H.T) - def compute_mag_declination_pred_innov_var_and_h( state: VState, P: MTangent, @@ -697,10 +629,6 @@ def compute_gravity_z_innov_var_and_h( generate_px4_function(compute_wind_init_and_cov_from_airspeed, output_names=["wind", "P_wind"]) generate_px4_function(compute_yaw_innov_var_and_h, output_names=["innov_var", "H"]) -generate_px4_function(compute_yaw_312_innov_var_and_h, output_names=["innov_var", "H"]) -generate_px4_function(compute_yaw_312_innov_var_and_h_alternate, output_names=["innov_var", "H"]) -generate_px4_function(compute_yaw_321_innov_var_and_h, output_names=["innov_var", "H"]) -generate_px4_function(compute_yaw_321_innov_var_and_h_alternate, output_names=["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_pred_innov_var_and_h, output_names=["meas_pred", "innov_var", "H"]) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h.h deleted file mode 100644 index e6fd32157ef7..000000000000 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h.h +++ /dev/null @@ -1,76 +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_yaw_312_innov_var_and_h - * - * Args: - * state: Matrix24_1 - * P: Matrix23_23 - * R: Scalar - * epsilon: Scalar - * - * Outputs: - * innov_var: Scalar - * H: Matrix23_1 - */ -template -void ComputeYaw312InnovVarAndH(const matrix::Matrix& state, - const matrix::Matrix& P, const Scalar R, - const Scalar epsilon, Scalar* const innov_var = nullptr, - matrix::Matrix* const H = nullptr) { - // Total ops: 53 - - // Input arrays - - // Intermediate terms (15) - const Scalar _tmp0 = 2 * state(0, 0); - const Scalar _tmp1 = -_tmp0 * state(3, 0); - const Scalar _tmp2 = 2 * state(1, 0); - const Scalar _tmp3 = _tmp2 * state(2, 0); - const Scalar _tmp4 = _tmp1 + _tmp3; - const Scalar _tmp5 = std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp6 = std::pow(state(1, 0), Scalar(2)); - const Scalar _tmp7 = -2 * _tmp5 - 2 * _tmp6 + 1; - const Scalar _tmp8 = _tmp7 + epsilon * ((((_tmp7) > 0) - ((_tmp7) < 0)) + Scalar(0.5)); - const Scalar _tmp9 = std::pow(_tmp8, Scalar(2)); - const Scalar _tmp10 = _tmp4 / _tmp9; - const Scalar _tmp11 = Scalar(1.0) / (_tmp8); - const Scalar _tmp12 = _tmp9 / (std::pow(_tmp4, Scalar(2)) + _tmp9); - const Scalar _tmp13 = _tmp12 * (_tmp10 * (_tmp1 - _tmp3) - - _tmp11 * (_tmp5 - _tmp6 - std::pow(state(0, 0), Scalar(2)) + - std::pow(state(2, 0), Scalar(2)))); - const Scalar _tmp14 = _tmp12 * (_tmp10 * (-_tmp2 * state(0, 0) + 2 * state(2, 0) * state(3, 0)) - - _tmp11 * (_tmp0 * state(2, 0) + _tmp2 * state(3, 0))); - - // Output terms (2) - if (innov_var != nullptr) { - Scalar& _innov_var = (*innov_var); - - _innov_var = R + _tmp13 * (P(0, 2) * _tmp14 + P(2, 2) * _tmp13) + - _tmp14 * (P(0, 0) * _tmp14 + P(2, 0) * _tmp13); - } - - if (H != nullptr) { - matrix::Matrix& _h = (*H); - - _h.setZero(); - - _h(0, 0) = _tmp14; - _h(2, 0) = _tmp13; - } -} // NOLINT(readability/fn_size) - -// NOLINTNEXTLINE(readability/fn_size) -} // namespace sym diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h_alternate.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h_alternate.h deleted file mode 100644 index 07fd1091819a..000000000000 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h_alternate.h +++ /dev/null @@ -1,76 +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_yaw_312_innov_var_and_h_alternate - * - * Args: - * state: Matrix24_1 - * P: Matrix23_23 - * R: Scalar - * epsilon: Scalar - * - * Outputs: - * innov_var: Scalar - * H: Matrix23_1 - */ -template -void ComputeYaw312InnovVarAndHAlternate(const matrix::Matrix& state, - const matrix::Matrix& P, const Scalar R, - const Scalar epsilon, Scalar* const innov_var = nullptr, - matrix::Matrix* const H = nullptr) { - // Total ops: 57 - - // Input arrays - - // Intermediate terms (15) - const Scalar _tmp0 = 2 * state(0, 0); - const Scalar _tmp1 = 2 * state(1, 0); - const Scalar _tmp2 = std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp3 = std::pow(state(1, 0), Scalar(2)); - const Scalar _tmp4 = -2 * _tmp2 - 2 * _tmp3 + 1; - const Scalar _tmp5 = -_tmp0 * state(3, 0); - const Scalar _tmp6 = _tmp1 * state(2, 0); - const Scalar _tmp7 = _tmp5 + _tmp6; - const Scalar _tmp8 = _tmp7 + epsilon * ((((_tmp7) > 0) - ((_tmp7) < 0)) + Scalar(0.5)); - const Scalar _tmp9 = std::pow(_tmp8, Scalar(2)); - const Scalar _tmp10 = _tmp4 / _tmp9; - const Scalar _tmp11 = Scalar(1.0) / (_tmp8); - const Scalar _tmp12 = _tmp9 / (std::pow(_tmp4, Scalar(2)) + _tmp9); - const Scalar _tmp13 = _tmp12 * (_tmp10 * (_tmp0 * state(2, 0) + _tmp1 * state(3, 0)) - - _tmp11 * (-_tmp1 * state(0, 0) + 2 * state(2, 0) * state(3, 0))); - const Scalar _tmp14 = _tmp12 * (_tmp10 * (_tmp2 - _tmp3 - std::pow(state(0, 0), Scalar(2)) + - std::pow(state(2, 0), Scalar(2))) - - _tmp11 * (_tmp5 - _tmp6)); - - // Output terms (2) - if (innov_var != nullptr) { - Scalar& _innov_var = (*innov_var); - - _innov_var = R - _tmp13 * (-P(0, 0) * _tmp13 - P(2, 0) * _tmp14) - - _tmp14 * (-P(0, 2) * _tmp13 - P(2, 2) * _tmp14); - } - - if (H != nullptr) { - matrix::Matrix& _h = (*H); - - _h.setZero(); - - _h(0, 0) = -_tmp13; - _h(2, 0) = -_tmp14; - } -} // NOLINT(readability/fn_size) - -// NOLINTNEXTLINE(readability/fn_size) -} // namespace sym diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h.h deleted file mode 100644 index 3677dbac87c3..000000000000 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h.h +++ /dev/null @@ -1,76 +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_yaw_321_innov_var_and_h - * - * Args: - * state: Matrix24_1 - * P: Matrix23_23 - * R: Scalar - * epsilon: Scalar - * - * Outputs: - * innov_var: Scalar - * H: Matrix23_1 - */ -template -void ComputeYaw321InnovVarAndH(const matrix::Matrix& state, - const matrix::Matrix& P, const Scalar R, - const Scalar epsilon, Scalar* const innov_var = nullptr, - matrix::Matrix* const H = nullptr) { - // Total ops: 53 - - // Input arrays - - // Intermediate terms (15) - const Scalar _tmp0 = 2 * state(0, 0); - const Scalar _tmp1 = _tmp0 * state(3, 0); - const Scalar _tmp2 = 2 * state(1, 0); - const Scalar _tmp3 = _tmp2 * state(2, 0); - const Scalar _tmp4 = _tmp1 + _tmp3; - const Scalar _tmp5 = std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp6 = std::pow(state(2, 0), Scalar(2)); - const Scalar _tmp7 = -2 * _tmp5 - 2 * _tmp6 + 1; - const Scalar _tmp8 = _tmp7 + epsilon * ((((_tmp7) > 0) - ((_tmp7) < 0)) + Scalar(0.5)); - const Scalar _tmp9 = std::pow(_tmp8, Scalar(2)); - const Scalar _tmp10 = _tmp4 / _tmp9; - const Scalar _tmp11 = Scalar(1.0) / (_tmp8); - const Scalar _tmp12 = _tmp9 / (std::pow(_tmp4, Scalar(2)) + _tmp9); - const Scalar _tmp13 = _tmp12 * (-_tmp10 * (-_tmp1 + _tmp3) + - _tmp11 * (-_tmp5 + _tmp6 + std::pow(state(0, 0), Scalar(2)) - - std::pow(state(1, 0), Scalar(2)))); - const Scalar _tmp14 = _tmp12 * (-_tmp10 * (-_tmp0 * state(2, 0) - _tmp2 * state(3, 0)) + - _tmp11 * (_tmp2 * state(0, 0) - 2 * state(2, 0) * state(3, 0))); - - // Output terms (2) - if (innov_var != nullptr) { - Scalar& _innov_var = (*innov_var); - - _innov_var = R + _tmp13 * (P(1, 2) * _tmp14 + P(2, 2) * _tmp13) + - _tmp14 * (P(1, 1) * _tmp14 + P(2, 1) * _tmp13); - } - - if (H != nullptr) { - matrix::Matrix& _h = (*H); - - _h.setZero(); - - _h(1, 0) = _tmp14; - _h(2, 0) = _tmp13; - } -} // NOLINT(readability/fn_size) - -// NOLINTNEXTLINE(readability/fn_size) -} // namespace sym diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h_alternate.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h_alternate.h deleted file mode 100644 index 3f1876623740..000000000000 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h_alternate.h +++ /dev/null @@ -1,76 +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_yaw_321_innov_var_and_h_alternate - * - * Args: - * state: Matrix24_1 - * P: Matrix23_23 - * R: Scalar - * epsilon: Scalar - * - * Outputs: - * innov_var: Scalar - * H: Matrix23_1 - */ -template -void ComputeYaw321InnovVarAndHAlternate(const matrix::Matrix& state, - const matrix::Matrix& P, const Scalar R, - const Scalar epsilon, Scalar* const innov_var = nullptr, - matrix::Matrix* const H = nullptr) { - // Total ops: 57 - - // Input arrays - - // Intermediate terms (15) - const Scalar _tmp0 = 2 * state(2, 0); - const Scalar _tmp1 = 2 * state(1, 0); - const Scalar _tmp2 = 2 * state(0, 0) * state(3, 0); - const Scalar _tmp3 = _tmp1 * state(2, 0); - const Scalar _tmp4 = _tmp2 + _tmp3; - const Scalar _tmp5 = _tmp4 + epsilon * ((((_tmp4) > 0) - ((_tmp4) < 0)) + Scalar(0.5)); - const Scalar _tmp6 = Scalar(1.0) / (_tmp5); - const Scalar _tmp7 = std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp8 = std::pow(state(2, 0), Scalar(2)); - const Scalar _tmp9 = -2 * _tmp7 - 2 * _tmp8 + 1; - const Scalar _tmp10 = std::pow(_tmp5, Scalar(2)); - const Scalar _tmp11 = _tmp9 / _tmp10; - const Scalar _tmp12 = _tmp10 / (_tmp10 + std::pow(_tmp9, Scalar(2))); - const Scalar _tmp13 = _tmp12 * (-_tmp11 * (-_tmp0 * state(3, 0) + _tmp1 * state(0, 0)) + - _tmp6 * (-_tmp0 * state(0, 0) - _tmp1 * state(3, 0))); - const Scalar _tmp14 = _tmp12 * (-_tmp11 * (-_tmp7 + _tmp8 + std::pow(state(0, 0), Scalar(2)) - - std::pow(state(1, 0), Scalar(2))) + - _tmp6 * (-_tmp2 + _tmp3)); - - // Output terms (2) - if (innov_var != nullptr) { - Scalar& _innov_var = (*innov_var); - - _innov_var = R - _tmp13 * (-P(1, 1) * _tmp13 - P(2, 1) * _tmp14) - - _tmp14 * (-P(1, 2) * _tmp13 - P(2, 2) * _tmp14); - } - - if (H != nullptr) { - matrix::Matrix& _h = (*H); - - _h.setZero(); - - _h(1, 0) = -_tmp13; - _h(2, 0) = -_tmp14; - } -} // NOLINT(readability/fn_size) - -// NOLINTNEXTLINE(readability/fn_size) -} // namespace sym diff --git a/src/modules/ekf2/EKF/yaw_fusion.cpp b/src/modules/ekf2/EKF/yaw_fusion.cpp index 939824a0fe91..80ce35210b14 100644 --- a/src/modules/ekf2/EKF/yaw_fusion.cpp +++ b/src/modules/ekf2/EKF/yaw_fusion.cpp @@ -33,8 +33,6 @@ #include "ekf.h" -#include -#include #include #include diff --git a/src/modules/ekf2/test/test_EKF_yaw_fusion_generated.cpp b/src/modules/ekf2/test/test_EKF_yaw_fusion_generated.cpp index 2469635e8114..7e536e62bb79 100644 --- a/src/modules/ekf2/test/test_EKF_yaw_fusion_generated.cpp +++ b/src/modules/ekf2/test/test_EKF_yaw_fusion_generated.cpp @@ -35,68 +35,10 @@ #include "EKF/ekf.h" #include "test_helper/comparison_helper.h" -#include "../EKF/python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h.h" -#include "../EKF/python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h_alternate.h" -#include "../EKF/python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h.h" -#include "../EKF/python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h_alternate.h" #include "../EKF/python/ekf_derivation/generated/compute_yaw_innov_var_and_h.h" using namespace matrix; -TEST(YawFusionGenerated, yawSingularity) -{ - // GIVEN: an attitude that should give a singularity when transforming the - // rotation matrix to Euler yaw - StateSample state{}; - state.quat_nominal = Eulerf(M_PI_F, 0.f, M_PI_F); - - const float R = sq(radians(10.f)); - SquareMatrixState P = createRandomCovarianceMatrix(); - - VectorState H_a; - VectorState H_b; - float innov_var_a; - float innov_var_b; - - // WHEN: computing the innovation variance and H using two different methods - sym::ComputeYaw321InnovVarAndH(state.vector(), P, R, FLT_EPSILON, &innov_var_a, &H_a); - sym::ComputeYawInnovVarAndH(state.vector(), P, R, &innov_var_b, &H_b); - - // THEN: Even at the singularity point, the result is still correct - EXPECT_TRUE(isEqual(H_a, H_b)); - - EXPECT_NEAR(innov_var_a, innov_var_b, 1e-5f); - EXPECT_TRUE(innov_var_a < 50.f && innov_var_a > R) << "innov_var = " << innov_var_a; -} - -TEST(YawFusionGenerated, gimbalLock321vs312vsTangent) -{ - // GIVEN: an attitude at gimbal lock position - StateSample state{}; - state.quat_nominal = Eulerf(0.f, -M_PI_F / 2.f, M_PI_F); - - const float R = sq(radians(10.f)); - SquareMatrixState P = createRandomCovarianceMatrix(); - - VectorState H_321; - VectorState H_312; - VectorState H_tangent; - float innov_var_321; - float innov_var_312; - float innov_var_tangent; - sym::ComputeYaw321InnovVarAndH(state.vector(), P, R, FLT_EPSILON, &innov_var_321, &H_321); - - sym::ComputeYaw312InnovVarAndH(state.vector(), P, R, FLT_EPSILON, &innov_var_312, &H_312); - sym::ComputeYawInnovVarAndH(state.vector(), P, R, &innov_var_tangent, &H_tangent); - - // THEN: both computation are not equivalent, 321 is undefined but 312 and "tangent" are valid - EXPECT_FALSE(isEqual(H_321, H_312)); - EXPECT_TRUE(isEqual(H_312, H_tangent)); - EXPECT_GT(fabsf(innov_var_321 - innov_var_312), 1e6f); - EXPECT_NEAR(innov_var_312, innov_var_tangent, 1e-6f); - EXPECT_TRUE(innov_var_312 < 50.f && innov_var_312 > R) << "innov_var = " << innov_var_312; -} - Vector3f getRotVarNed(const Quatf &q, const SquareMatrixState &P) { constexpr auto S = State::quat_nominal; @@ -138,6 +80,8 @@ TEST(YawFusionGenerated, positiveVarianceAllOrientations) << " roll = " << degrees(roll) << " innov_var = " << innov_var << " innov_var_true = " << innov_var_true; + + EXPECT_TRUE(H.isAllFinite()); } } }