From 2f3ea8809924408cceaee4a2befd555eaf8f0f29 Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 19 Oct 2022 11:57:13 +0200 Subject: [PATCH] ekf2: migrate mag declination to SymForce --- src/modules/ekf2/EKF/mag_fusion.cpp | 54 ++---------- .../EKF/python/ekf_derivation/derivation.py | 17 ++++ ...te_mag_declination_innov_innov_var_and_h.h | 74 ++++++++++++++++ src/modules/ekf2/test/CMakeLists.txt | 1 + .../test_EKF_mag_declination_generated.cpp | 84 +++++++++++++++++++ 5 files changed, 184 insertions(+), 46 deletions(-) create mode 100644 src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_declination_innov_innov_var_and_h.h create mode 100644 src/modules/ekf2/test/test_EKF_mag_declination_generated.cpp diff --git a/src/modules/ekf2/EKF/mag_fusion.cpp b/src/modules/ekf2/EKF/mag_fusion.cpp index 7bf93699f8..45ee59006a 100644 --- a/src/modules/ekf2/EKF/mag_fusion.cpp +++ b/src/modules/ekf2/EKF/mag_fusion.cpp @@ -47,6 +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 @@ -333,63 +334,24 @@ bool Ekf::fuseYaw(const float innovation, const float variance, estimator_aid_so bool Ekf::fuseDeclination(float decl_sigma) { - // assign intermediate state variables - const float magN = _state.mag_I(0); - const float magE = _state.mag_I(1); - - // minimum North field strength before calculation becomes badly conditioned (T) - constexpr float N_field_min = 0.001f; - // observation variance (rad**2) const float R_DECL = sq(decl_sigma); - // Calculate intermediate variables - if (fabsf(magN) < sq(N_field_min)) { - // calculation is badly conditioned close to +-90 deg declination - return false; - } + Vector24f H; + float innovation; + float innovation_variance; - const float HK0 = ecl::powf(magN, -2); - const float HK1 = HK0*ecl::powf(magE, 2) + 1.0F; - const float HK2 = 1.0F/HK1; - const float HK3 = 1.0F/magN; - const float HK4 = HK2*HK3; - const float HK5 = HK3*magE; - const float HK6 = HK5*P(16,17) - P(17,17); - const float HK7 = ecl::powf(HK1, -2); - const float HK8 = HK5*P(16,16) - P(16,17); - const float innovation_variance = -HK0*HK6*HK7 + HK7*HK8*magE/ecl::powf(magN, 3) + R_DECL; - float HK9; + sym::ComputeMagDeclinationInnovInnovVarAndH(getStateAtFusionHorizonAsVector(), P, getMagDeclination(), R_DECL, FLT_EPSILON, &innovation, &innovation_variance, &H); - if (innovation_variance > R_DECL) { - HK9 = HK4/innovation_variance; - } else { + if (innovation_variance < R_DECL) { // variance calculation is badly conditioned return false; } - // Calculate the observation Jacobian - // Note only 2 terms are non-zero which can be used in matrix operations for calculation of Kalman gains and covariance update to significantly reduce cost - // Note Hfusion indices do not match state indices - SparseVector24f<16,17> Hfusion; - Hfusion.at<16>() = -HK0*HK2*magE; - Hfusion.at<17>() = HK4; + SparseVector24f<16,17> Hfusion(H); // Calculate the Kalman gains - Vector24f Kfusion; - - for (unsigned row = 0; row <= 15; row++) { - Kfusion(row) = -HK9*(HK5*P(row,16) - P(row,17)); - } - - Kfusion(16) = -HK8*HK9; - Kfusion(17) = -HK6*HK9; - - for (unsigned row = 18; row <= 23; row++) { - Kfusion(row) = -HK9*(HK5*P(16,row) - P(17,row)); - } - - const float innovation = math::constrain(atan2f(magE, magN) - getMagDeclination(), -0.5f, 0.5f); + Vector24f Kfusion = P * Hfusion / innovation_variance; const bool is_fused = measurementUpdate(Kfusion, Hfusion, innovation); diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py index ae0d791575..9965260e54 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py @@ -321,6 +321,22 @@ def compute_yaw_312_innov_var_and_h_alternate( return (innov_var, H.T) +def compute_mag_declination_innov_innov_var_and_h( + state: VState, + P: MState, + meas: sf.Scalar, + R: sf.Scalar, + epsilon: 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) + print("Derive EKF2 equations...") generate_px4_function(compute_airspeed_innov_and_innov_var, output_names=["innov", "innov_var"]) generate_px4_function(compute_airspeed_h_and_k, output_names=["H", "K"]) @@ -335,3 +351,4 @@ 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"]) 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_innov_innov_var_and_h.h new file mode 100644 index 0000000000..b75b801c0a --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_declination_innov_innov_var_and_h.h @@ -0,0 +1,74 @@ +// ----------------------------------------------------------------------------- +// 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_mag_declination_innov_innov_var_and_h + * + * Args: + * state: Matrix24_1 + * P: Matrix24_24 + * meas: Scalar + * R: Scalar + * epsilon: Scalar + * + * Outputs: + * innov: 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 + + // Input arrays + + // Intermediate terms (4) + const Scalar _tmp0 = + epsilon * ((((state(16, 0)) > 0) - ((state(16, 0)) < 0)) + Scalar(0.5)) + state(16, 0); + const Scalar _tmp1 = + Scalar(1.0) / (std::pow(_tmp0, Scalar(2)) + std::pow(state(17, 0), Scalar(2))); + const Scalar _tmp2 = _tmp1 * state(17, 0); + const Scalar _tmp3 = _tmp0 * _tmp1; + + // Output terms (3) + if (innov != nullptr) { + Scalar& _innov = (*innov); + + _innov = -meas + std::atan2(state(17, 0), _tmp0); + } + + if (innov_var != nullptr) { + Scalar& _innov_var = (*innov_var); + + _innov_var = R - _tmp2 * (-P(16, 16) * _tmp2 + P(17, 16) * _tmp3) + + _tmp3 * (-P(16, 17) * _tmp2 + P(17, 17) * _tmp3); + } + + if (H != nullptr) { + matrix::Matrix& _H = (*H); + + _H.setZero(); + + _H(16, 0) = -_tmp2; + _H(17, 0) = _tmp3; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/src/modules/ekf2/test/CMakeLists.txt b/src/modules/ekf2/test/CMakeLists.txt index dfaec34426..670cda7057 100644 --- a/src/modules/ekf2/test/CMakeLists.txt +++ b/src/modules/ekf2/test/CMakeLists.txt @@ -50,6 +50,7 @@ px4_add_unit_gtest(SRC test_EKF_height_fusion.cpp LINKLIBS ecl_EKF ecl_sensor_si px4_add_unit_gtest(SRC test_EKF_imuSampling.cpp LINKLIBS ecl_EKF ecl_sensor_sim) px4_add_unit_gtest(SRC test_EKF_initialization.cpp LINKLIBS ecl_EKF ecl_sensor_sim) px4_add_unit_gtest(SRC test_EKF_mag_3d_fusion_generated.cpp LINKLIBS ecl_EKF ecl_test_helper) +px4_add_unit_gtest(SRC test_EKF_mag_declination_generated.cpp LINKLIBS ecl_EKF ecl_test_helper) px4_add_unit_gtest(SRC test_EKF_measurementSampling.cpp LINKLIBS ecl_EKF ecl_sensor_sim) px4_add_unit_gtest(SRC test_EKF_ringbuffer.cpp LINKLIBS ecl_EKF ecl_sensor_sim) px4_add_unit_gtest(SRC test_EKF_sideslip_fusion_generated.cpp LINKLIBS ecl_EKF ecl_test_helper) diff --git a/src/modules/ekf2/test/test_EKF_mag_declination_generated.cpp b/src/modules/ekf2/test/test_EKF_mag_declination_generated.cpp new file mode 100644 index 0000000000..92a318b85a --- /dev/null +++ b/src/modules/ekf2/test/test_EKF_mag_declination_generated.cpp @@ -0,0 +1,84 @@ +/**************************************************************************** + * + * Copyright (C) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#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" + +using namespace matrix; + +TEST(MagDeclinationGenerated, declination90deg) +{ + // GIVEN: an estimated mag declination of 90 degrees + Vector24f state_vector{}; + state_vector(16) = 0.f; // North mag field + state_vector(17) = 0.2f; // East mag field + + const float R = sq(radians(sq(0.5f))); + SquareMatrix24f P = createRandomCovarianceMatrix24f(); + + Vector24f H; + float innov; + float innov_var; + + const float decl = radians(90.f); + sym::ComputeMagDeclinationInnovInnovVarAndH(state_vector, P, decl, R, FLT_EPSILON, &innov, &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); +} + +TEST(MagDeclinationGenerated, declinationUndefined) +{ + // GIVEN: an undefined declination + Vector24f state_vector{}; + state_vector(16) = 0.f; // North mag field + state_vector(17) = 0.f; // East mag field + + const float R = sq(radians(sq(0.5f))); + SquareMatrix24f P = createRandomCovarianceMatrix24f(); + + Vector24f H; + float innov; + float innov_var; + + const float decl = radians(0.f); + sym::ComputeMagDeclinationInnovInnovVarAndH(state_vector, P, decl, R, FLT_EPSILON, &innov, &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); +}