diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py index 7e99388fce..c67d1f0072 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py @@ -390,6 +390,24 @@ def compute_mag_z_innov_var_and_h( return (innov_var, H.T) +def compute_yaw_innov_var_and_h( + state: VState, + P: MTangent, + R: sf.Scalar +) -> (sf.Scalar, VTangent): + + state = vstate_to_state(state) + q = sf.Quaternion.from_storage(state["quat_nominal"].to_storage()) + r = sf.Quaternion.symbolic('r') + delta_q = q * r.conj() # create a quaternion error of the measurement at the origin + delta_meas_pred = 2 * delta_q.z # Use small angle approximation to obtain a simpler jacobian + + H = sf.V1(delta_meas_pred).jacobian(state) + H = H.subs({r.w: q.w, r.x: q.x, r.y: q.y, r.z: q.z}) # assume innovation is small + innov_var = (H * P * H.T + R)[0,0] + + return (innov_var, H.T) + def compute_yaw_321_innov_var_and_h( state: VState, P: MTangent, @@ -678,6 +696,7 @@ if not args.disable_wind: generate_px4_function(compute_sideslip_innov_and_innov_var, output_names=["innov", "innov_var"]) 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"]) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_innov_var_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_innov_var_and_h.h new file mode 100644 index 0000000000..443fb0b03c --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_innov_var_and_h.h @@ -0,0 +1,65 @@ +// ----------------------------------------------------------------------------- +// 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_innov_var_and_h + * + * Args: + * state: Matrix24_1 + * P: Matrix23_23 + * R: Scalar + * + * Outputs: + * innov_var: Scalar + * H: Matrix23_1 + */ +template +void ComputeYawInnovVarAndH(const matrix::Matrix& state, + const matrix::Matrix& P, const Scalar R, + Scalar* const innov_var = nullptr, + matrix::Matrix* const H = nullptr) { + // Total ops: 36 + + // Input arrays + + // Intermediate terms (5) + const Scalar _tmp0 = 2 * state(2, 0); + const Scalar _tmp1 = 2 * state(1, 0); + const Scalar _tmp2 = -_tmp0 * state(0, 0) + _tmp1 * state(3, 0); + const Scalar _tmp3 = _tmp0 * state(3, 0) + _tmp1 * state(0, 0); + const Scalar _tmp4 = std::pow(state(0, 0), Scalar(2)) - std::pow(state(1, 0), Scalar(2)) - + std::pow(state(2, 0), Scalar(2)) + std::pow(state(3, 0), Scalar(2)); + + // Output terms (2) + if (innov_var != nullptr) { + Scalar& _innov_var = (*innov_var); + + _innov_var = R + _tmp2 * (P(0, 0) * _tmp2 + P(1, 0) * _tmp3 + P(2, 0) * _tmp4) + + _tmp3 * (P(0, 1) * _tmp2 + P(1, 1) * _tmp3 + P(2, 1) * _tmp4) + + _tmp4 * (P(0, 2) * _tmp2 + P(1, 2) * _tmp3 + P(2, 2) * _tmp4); + } + + if (H != nullptr) { + matrix::Matrix& _h = (*H); + + _h.setZero(); + + _h(0, 0) = _tmp2; + _h(1, 0) = _tmp3; + _h(2, 0) = _tmp4; + } +} // 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 e5f94938aa..939824a0fe 100644 --- a/src/modules/ekf2/EKF/yaw_fusion.cpp +++ b/src/modules/ekf2/EKF/yaw_fusion.cpp @@ -35,6 +35,7 @@ #include #include +#include #include @@ -42,7 +43,7 @@ bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status) { VectorState H_YAW; - computeYawInnovVarAndH(aid_src_status.observation_variance, aid_src_status.innovation_variance, H_YAW); + sym::ComputeYawInnovVarAndH(_state.vector(), P, aid_src_status.observation_variance, &aid_src_status.innovation_variance, &H_YAW); return fuseYaw(aid_src_status, H_YAW); } @@ -135,10 +136,5 @@ bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H void Ekf::computeYawInnovVarAndH(float variance, float &innovation_variance, VectorState &H_YAW) const { - if (shouldUse321RotationSequence(_R_to_earth)) { - sym::ComputeYaw321InnovVarAndH(_state.vector(), P, variance, FLT_EPSILON, &innovation_variance, &H_YAW); - - } else { - sym::ComputeYaw312InnovVarAndH(_state.vector(), P, variance, FLT_EPSILON, &innovation_variance, &H_YAW); - } + sym::ComputeYawInnovVarAndH(_state.vector(), P, variance, &innovation_variance, &H_YAW); } 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 ab02fd3cac..2469635e81 100644 --- a/src/modules/ekf2/test/test_EKF_yaw_fusion_generated.cpp +++ b/src/modules/ekf2/test/test_EKF_yaw_fusion_generated.cpp @@ -39,10 +39,11 @@ #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, singularityYawEquivalence) +TEST(YawFusionGenerated, yawSingularity) { // GIVEN: an attitude that should give a singularity when transforming the // rotation matrix to Euler yaw @@ -57,18 +58,18 @@ TEST(YawFusionGenerated, singularityYawEquivalence) float innov_var_a; float innov_var_b; - // WHEN: computing the innovation variance and H using two different - // alternate forms (one is singular at pi/2 and the other one at 0) + // 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::ComputeYaw321InnovVarAndHAlternate(state.vector(), P, R, FLT_EPSILON, &innov_var_b, &H_b); + sym::ComputeYawInnovVarAndH(state.vector(), P, R, &innov_var_b, &H_b); - // THEN: Even at the singularity point, the result is still correct, thanks to epsilon + // 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, gimbalLock321vs312) +TEST(YawFusionGenerated, gimbalLock321vs312vsTangent) { // GIVEN: an attitude at gimbal lock position StateSample state{}; @@ -79,18 +80,31 @@ TEST(YawFusionGenerated, gimbalLock321vs312) 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 is valid + // 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; + matrix::SquareMatrix3f rot_cov_body = P.slice(S.idx, S.idx); + auto R_to_earth = Dcmf(q); + return matrix::SquareMatrix(R_to_earth * rot_cov_body * R_to_earth.T()).diag(); +} + TEST(YawFusionGenerated, positiveVarianceAllOrientations) { const float R = sq(radians(10.f)); @@ -99,19 +113,14 @@ TEST(YawFusionGenerated, positiveVarianceAllOrientations) VectorState H; float innov_var; - // GIVEN: all orientations (90 deg steps) - for (float yaw = 0.f; yaw < 2.f * M_PI_F; yaw += M_PI_F / 2.f) { - for (float pitch = 0.f; pitch < 2.f * M_PI_F; pitch += M_PI_F / 2.f) { - for (float roll = 0.f; roll < 2.f * M_PI_F; roll += M_PI_F / 2.f) { + // GIVEN: all orientations + for (float yaw = 0.f; yaw < 2.f * M_PI_F; yaw += M_PI_F / 4.f) { + for (float pitch = 0.f; pitch < 2.f * M_PI_F; pitch += M_PI_F / 4.f) { + for (float roll = 0.f; roll < 2.f * M_PI_F; roll += M_PI_F / 4.f) { StateSample state{}; state.quat_nominal = Eulerf(roll, pitch, yaw); - if (shouldUse321RotationSequence(Dcmf(state.quat_nominal))) { - sym::ComputeYaw321InnovVarAndH(state.vector(), P, R, FLT_EPSILON, &innov_var, &H); - - } else { - sym::ComputeYaw312InnovVarAndH(state.vector(), P, R, FLT_EPSILON, &innov_var, &H); - } + sym::ComputeYawInnovVarAndH(state.vector(), P, R, &innov_var, &H); // THEN: the innovation variance must be positive and finite EXPECT_TRUE(innov_var < 100.f && innov_var > R) @@ -119,6 +128,16 @@ TEST(YawFusionGenerated, positiveVarianceAllOrientations) << " pitch = " << degrees(pitch) << " roll = " << degrees(roll) << " innov_var = " << innov_var; + + // AND: it should be the same as the "true" innovation variance obtained by summing + // the Z rotation variance in NED and the measurement variance + const float innov_var_true = getRotVarNed(state.quat_nominal, P)(2) + R; + EXPECT_NEAR(innov_var, innov_var_true, 1e-5f) + << "yaw = " << degrees(yaw) + << " pitch = " << degrees(pitch) + << " roll = " << degrees(roll) + << " innov_var = " << innov_var + << " innov_var_true = " << innov_var_true; } } }