diff --git a/src/modules/ekf2/EKF/mag_fusion.cpp b/src/modules/ekf2/EKF/mag_fusion.cpp index 77e662e918..3a8b97836d 100644 --- a/src/modules/ekf2/EKF/mag_fusion.cpp +++ b/src/modules/ekf2/EKF/mag_fusion.cpp @@ -47,7 +47,7 @@ #include "python/ekf_derivation/generated/compute_mag_z_innov_var_and_h.h" #include "python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h.h" #include "python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h.h" -#include "python/ekf_derivation/generated/compute_mag_declination_innov_innov_var_and_h.h" +#include "python/ekf_derivation/generated/compute_mag_declination_pred_innov_var_and_h.h" #include @@ -335,10 +335,12 @@ bool Ekf::fuseDeclination(float decl_sigma) const float R_DECL = sq(decl_sigma); Vector24f H; - float innovation; + float decl_pred; float innovation_variance; - sym::ComputeMagDeclinationInnovInnovVarAndH(getStateAtFusionHorizonAsVector(), P, getMagDeclination(), R_DECL, FLT_EPSILON, &innovation, &innovation_variance, &H); + sym::ComputeMagDeclinationPredInnovVarAndH(getStateAtFusionHorizonAsVector(), P, R_DECL, FLT_EPSILON, &decl_pred, &innovation_variance, &H); + + const float innovation = wrap_pi(decl_pred - getMagDeclination()); if (innovation_variance < R_DECL) { // variance calculation is badly conditioned diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py index 0fd396cd43..d4d094207d 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py @@ -321,21 +321,19 @@ def compute_yaw_312_innov_var_and_h_alternate( return (innov_var, H.T) -def compute_mag_declination_innov_innov_var_and_h( +def compute_mag_declination_pred_innov_var_and_h( state: VState, P: MState, - meas: sf.Scalar, R: sf.Scalar, epsilon: sf.Scalar -) -> (sf.Scalar, VState): +) -> (sf.Scalar, sf.Scalar, VState): meas_pred = sf.atan2(state[State.iy], state[State.ix], epsilon=epsilon) - innov = meas_pred - meas H = sf.V1(meas_pred).jacobian(state) innov_var = (H * P * H.T + R)[0,0] - return (innov, innov_var, H.T) + return (meas_pred, innov_var, H.T) def predict_opt_flow(state, distance, epsilon): q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz]) @@ -518,7 +516,7 @@ generate_px4_function(compute_yaw_321_innov_var_and_h, output_names=["innov_var" generate_px4_function(compute_yaw_321_innov_var_and_h_alternate, 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_mag_declination_innov_innov_var_and_h, output_names=["innov", "innov_var", "H"]) +generate_px4_function(compute_mag_declination_pred_innov_var_and_h, output_names=["pred", "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_mag_declination_innov_innov_var_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_declination_pred_innov_var_and_h.h similarity index 65% rename from src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_declination_innov_innov_var_and_h.h rename to src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_declination_pred_innov_var_and_h.h index 4fd26103e4..a68c1a5210 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_declination_innov_innov_var_and_h.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_declination_pred_innov_var_and_h.h @@ -13,28 +13,26 @@ namespace sym { /** * This function was autogenerated from a symbolic function. Do not modify by hand. * - * Symbolic function: compute_mag_declination_innov_innov_var_and_h + * Symbolic function: compute_mag_declination_pred_innov_var_and_h * * Args: * state: Matrix24_1 * P: Matrix24_24 - * meas: Scalar * R: Scalar * epsilon: Scalar * * Outputs: - * innov: Scalar + * pred: Scalar * innov_var: Scalar * H: Matrix24_1 */ template -void ComputeMagDeclinationInnovInnovVarAndH(const matrix::Matrix& state, - const matrix::Matrix& P, - 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: 23 +void ComputeMagDeclinationPredInnovVarAndH(const matrix::Matrix& state, + const matrix::Matrix& P, const Scalar R, + const Scalar epsilon, Scalar* const pred = nullptr, + Scalar* const innov_var = nullptr, + matrix::Matrix* const H = nullptr) { + // Total ops: 22 // Input arrays @@ -47,10 +45,10 @@ void ComputeMagDeclinationInnovInnovVarAndH(const matrix::Matrix& const Scalar _tmp3 = _tmp0 * _tmp1; // Output terms (3) - if (innov != nullptr) { - Scalar& _innov = (*innov); + if (pred != nullptr) { + Scalar& _pred = (*pred); - _innov = -meas + std::atan2(state(17, 0), _tmp0); + _pred = std::atan2(state(17, 0), _tmp0); } if (innov_var != nullptr) { diff --git a/src/modules/ekf2/test/test_EKF_mag_declination_generated.cpp b/src/modules/ekf2/test/test_EKF_mag_declination_generated.cpp index 8b07a75a35..5751912be0 100644 --- a/src/modules/ekf2/test/test_EKF_mag_declination_generated.cpp +++ b/src/modules/ekf2/test/test_EKF_mag_declination_generated.cpp @@ -35,7 +35,7 @@ #include "EKF/ekf.h" #include "test_helper/comparison_helper.h" -#include "../EKF/python/ekf_derivation/generated/compute_mag_declination_innov_innov_var_and_h.h" +#include "../EKF/python/ekf_derivation/generated/compute_mag_declination_pred_innov_var_and_h.h" using namespace matrix; @@ -50,15 +50,15 @@ TEST(MagDeclinationGenerated, declination90deg) SquareMatrix24f P = createRandomCovarianceMatrix24f(); Vector24f H; - float innov; + float decl_pred; float innov_var; const float decl = radians(90.f); - sym::ComputeMagDeclinationInnovInnovVarAndH(state_vector, P, decl, R, FLT_EPSILON, &innov, &innov_var, &H); + sym::ComputeMagDeclinationPredInnovVarAndH(state_vector, P, R, FLT_EPSILON, &decl_pred, &innov_var, &H); // THEN: Even at the singularity point, atan2 is still defined EXPECT_TRUE(innov_var < 5000.f && innov_var > R) << "innov_var = " << innov_var; - EXPECT_LT(fabsf(innov), 1e-6f); + EXPECT_LT(fabsf(wrap_pi(decl_pred - decl)), 1e-6f); } TEST(MagDeclinationGenerated, declinationUndefined) @@ -72,15 +72,15 @@ TEST(MagDeclinationGenerated, declinationUndefined) SquareMatrix24f P = createRandomCovarianceMatrix24f(); Vector24f H; - float innov; + float decl_pred; float innov_var; const float decl = radians(0.f); - sym::ComputeMagDeclinationInnovInnovVarAndH(state_vector, P, decl, R, FLT_EPSILON, &innov, &innov_var, &H); + sym::ComputeMagDeclinationPredInnovVarAndH(state_vector, P, R, FLT_EPSILON, &decl_pred, &innov_var, &H); // THEN: the innovation variance is gigantic but finite EXPECT_TRUE(PX4_ISFINITE(innov_var) && innov_var > R && innov_var > 1e9f) << "innov_var = " << innov_var; - EXPECT_LT(fabsf(innov), 1e-6f); + EXPECT_LT(fabsf(wrap_pi(decl_pred - decl)), 1e-6f); } void sympyMagDeclInnovVarHAndK(float magN, float magE, const SquareMatrix24f &P, float R_DECL, @@ -156,14 +156,15 @@ TEST(MagDeclinationGenerated, SympyVsSymforce) Vector24f K_sympy; Vector24f K_symforce; float innov_sympy; - float innov_symforce; + float pred_symforce; float innov_var_sympy; float innov_var_symforce; sympyMagDeclInnovVarHAndK(mag_n, mag_e, P, R_DECL, innov_var_sympy, H_sympy, K_sympy); - innov_sympy = std::atan2(mag_e, mag_n) - decl; - sym::ComputeMagDeclinationInnovInnovVarAndH(state_vector, P, decl, R_DECL, FLT_EPSILON, &innov_symforce, + innov_sympy = wrap_pi(std::atan2(mag_e, mag_n) - decl); + sym::ComputeMagDeclinationPredInnovVarAndH(state_vector, P, R_DECL, FLT_EPSILON, &pred_symforce, &innov_var_symforce, &H_symforce); + const float innov_symforce = wrap_pi(pred_symforce - decl); K_symforce = P * H_symforce / innov_var_symforce; EXPECT_NEAR(innov_sympy, innov_symforce, 1e-5f);