From f11908a266a3c099b9ec138b496fcfbe6a88b160 Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 14 Oct 2022 17:05:41 +0200 Subject: [PATCH] ekf2: generate yaw fusion jacobians using symforce --- .../EKF/python/ekf_derivation/derivation.py | 72 +++++++++++++ .../compute_yaw_312_innov_var_and_h.h | 75 +++++++++++++ ...ompute_yaw_312_innov_var_and_h_alternate.h | 75 +++++++++++++ .../compute_yaw_321_innov_var_and_h.h | 75 +++++++++++++ ...ompute_yaw_321_innov_var_and_h_alternate.h | 75 +++++++++++++ src/modules/ekf2/test/CMakeLists.txt | 1 + .../test/test_EKF_yaw_fusion_generated.cpp | 102 ++++++++++++++++++ 7 files changed, 475 insertions(+) create mode 100644 src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h.h create mode 100644 src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h_alternate.h create mode 100644 src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h.h create mode 100644 src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h_alternate.h create mode 100644 src/modules/ekf2/test/test_EKF_yaw_fusion_generated.cpp diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py index 346a4223f2..ae0d791575 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py @@ -253,6 +253,74 @@ def compute_mag_z_innov_var_and_h( return (innov_var, H.T) +def compute_yaw_321_innov_var_and_h( + state: VState, + P: MState, + R: sf.Scalar, + epsilon: sf.Scalar +) -> (sf.Scalar, VState): + + q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz]) + R_to_earth = quat_to_rot(q_att) + # 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: MState, + R: sf.Scalar, + epsilon: sf.Scalar +) -> (sf.Scalar, VState): + + q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz]) + R_to_earth = quat_to_rot(q_att) + # 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: MState, + R: sf.Scalar, + epsilon: sf.Scalar +) -> (sf.Scalar, VState): + + q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz]) + R_to_earth = quat_to_rot(q_att) + # 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: MState, + R: sf.Scalar, + epsilon: sf.Scalar +) -> (sf.Scalar, VState): + + q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz]) + R_to_earth = quat_to_rot(q_att) + # 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) + 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"]) @@ -263,3 +331,7 @@ generate_px4_function(predict_covariance, output_names=["P_new"]) generate_px4_function(compute_mag_innov_innov_var_and_hx, output_names=["innov", "innov_var", "Hx"]) generate_px4_function(compute_mag_y_innov_var_and_h, output_names=["innov_var", "H"]) generate_px4_function(compute_mag_z_innov_var_and_h, 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_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"]) 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 new file mode 100644 index 0000000000..2ef1864a97 --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h.h @@ -0,0 +1,75 @@ +// ----------------------------------------------------------------------------- +// 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_yaw_312_innov_var_and_h + * + * Args: + * state: Matrix24_1 + * P: Matrix24_24 + * R: Scalar + * epsilon: Scalar + * + * Outputs: + * innov_var: Scalar + * H: Matrix24_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: 75 + + // Input arrays + + // Intermediate terms (11) + const Scalar _tmp0 = -state(0, 0) * state(3, 0) + state(1, 0) * state(2, 0); + const Scalar _tmp1 = 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)); + const Scalar _tmp2 = _tmp1 + epsilon * ((((_tmp1) > 0) - ((_tmp1) < 0)) + Scalar(0.5)); + const Scalar _tmp3 = std::pow(_tmp2, Scalar(2)); + const Scalar _tmp4 = 4 * _tmp0 / _tmp3; + const Scalar _tmp5 = 2 / _tmp2; + const Scalar _tmp6 = _tmp3 / (4 * std::pow(_tmp0, Scalar(2)) + _tmp3); + const Scalar _tmp7 = _tmp6 * (-_tmp4 * state(1, 0) - _tmp5 * state(2, 0)); + const Scalar _tmp8 = _tmp6 * (_tmp4 * state(2, 0) - _tmp5 * state(1, 0)); + const Scalar _tmp9 = _tmp6 * (-_tmp4 * state(3, 0) + _tmp5 * state(0, 0)); + const Scalar _tmp10 = _tmp6 * (_tmp4 * state(0, 0) + _tmp5 * state(3, 0)); + + // Output terms (2) + if (innov_var != nullptr) { + Scalar& _innov_var = (*innov_var); + + _innov_var = R + + _tmp10 * (P(0, 0) * _tmp10 + P(1, 0) * _tmp7 + P(2, 0) * _tmp8 + P(3, 0) * _tmp9) + + _tmp7 * (P(0, 1) * _tmp10 + P(1, 1) * _tmp7 + P(2, 1) * _tmp8 + P(3, 1) * _tmp9) + + _tmp8 * (P(0, 2) * _tmp10 + P(1, 2) * _tmp7 + P(2, 2) * _tmp8 + P(3, 2) * _tmp9) + + _tmp9 * (P(0, 3) * _tmp10 + P(1, 3) * _tmp7 + P(2, 3) * _tmp8 + P(3, 3) * _tmp9); + } + + if (H != nullptr) { + matrix::Matrix& _H = (*H); + + _H.setZero(); + + _H(0, 0) = _tmp10; + _H(1, 0) = _tmp7; + _H(2, 0) = _tmp8; + _H(3, 0) = _tmp9; + } +} // 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 new file mode 100644 index 0000000000..e5b76ed267 --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h_alternate.h @@ -0,0 +1,75 @@ +// ----------------------------------------------------------------------------- +// 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_yaw_312_innov_var_and_h_alternate + * + * Args: + * state: Matrix24_1 + * P: Matrix24_24 + * R: Scalar + * epsilon: Scalar + * + * Outputs: + * innov_var: Scalar + * H: Matrix24_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: 83 + + // Input arrays + + // Intermediate terms (11) + const Scalar _tmp0 = 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)); + const Scalar _tmp1 = -state(0, 0) * state(3, 0) + state(1, 0) * state(2, 0); + const Scalar _tmp2 = 2 * _tmp1 + epsilon * ((((_tmp1) > 0) - ((_tmp1) < 0)) + Scalar(0.5)); + const Scalar _tmp3 = std::pow(_tmp2, Scalar(2)); + const Scalar _tmp4 = 2 * _tmp0 / _tmp3; + const Scalar _tmp5 = 2 / _tmp2; + const Scalar _tmp6 = _tmp3 / (std::pow(_tmp0, Scalar(2)) + _tmp3); + const Scalar _tmp7 = _tmp6 * (_tmp4 * state(1, 0) - _tmp5 * state(2, 0)); + const Scalar _tmp8 = _tmp6 * (_tmp4 * state(2, 0) + _tmp5 * state(1, 0)); + const Scalar _tmp9 = _tmp6 * (-_tmp4 * state(0, 0) + _tmp5 * state(3, 0)); + const Scalar _tmp10 = _tmp6 * (-_tmp4 * state(3, 0) - _tmp5 * state(0, 0)); + + // Output terms (2) + if (innov_var != nullptr) { + Scalar& _innov_var = (*innov_var); + + _innov_var = + R - _tmp10 * (-P(0, 0) * _tmp10 - P(1, 0) * _tmp8 - P(2, 0) * _tmp7 - P(3, 0) * _tmp9) - + _tmp7 * (-P(0, 2) * _tmp10 - P(1, 2) * _tmp8 - P(2, 2) * _tmp7 - P(3, 2) * _tmp9) - + _tmp8 * (-P(0, 1) * _tmp10 - P(1, 1) * _tmp8 - P(2, 1) * _tmp7 - P(3, 1) * _tmp9) - + _tmp9 * (-P(0, 3) * _tmp10 - P(1, 3) * _tmp8 - P(2, 3) * _tmp7 - P(3, 3) * _tmp9); + } + + if (H != nullptr) { + matrix::Matrix& _H = (*H); + + _H.setZero(); + + _H(0, 0) = -_tmp10; + _H(1, 0) = -_tmp8; + _H(2, 0) = -_tmp7; + _H(3, 0) = -_tmp9; + } +} // 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 new file mode 100644 index 0000000000..eb2caad5c4 --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h.h @@ -0,0 +1,75 @@ +// ----------------------------------------------------------------------------- +// 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_yaw_321_innov_var_and_h + * + * Args: + * state: Matrix24_1 + * P: Matrix24_24 + * R: Scalar + * epsilon: Scalar + * + * Outputs: + * innov_var: Scalar + * H: Matrix24_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: 74 + + // Input arrays + + // Intermediate terms (11) + const Scalar _tmp0 = state(0, 0) * state(3, 0) + state(1, 0) * state(2, 0); + const Scalar _tmp1 = 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)); + const Scalar _tmp2 = _tmp1 + epsilon * ((((_tmp1) > 0) - ((_tmp1) < 0)) + Scalar(0.5)); + const Scalar _tmp3 = std::pow(_tmp2, Scalar(2)); + const Scalar _tmp4 = 4 * _tmp0 / _tmp3; + const Scalar _tmp5 = 2 / _tmp2; + const Scalar _tmp6 = _tmp3 / (4 * std::pow(_tmp0, Scalar(2)) + _tmp3); + const Scalar _tmp7 = _tmp6 * (_tmp4 * state(2, 0) + _tmp5 * state(1, 0)); + const Scalar _tmp8 = _tmp6 * (-_tmp4 * state(1, 0) + _tmp5 * state(2, 0)); + const Scalar _tmp9 = _tmp6 * (-_tmp4 * state(0, 0) + _tmp5 * state(3, 0)); + const Scalar _tmp10 = _tmp6 * (_tmp4 * state(3, 0) + _tmp5 * state(0, 0)); + + // Output terms (2) + if (innov_var != nullptr) { + Scalar& _innov_var = (*innov_var); + + _innov_var = R + + _tmp10 * (P(0, 3) * _tmp9 + P(1, 3) * _tmp8 + P(2, 3) * _tmp7 + P(3, 3) * _tmp10) + + _tmp7 * (P(0, 2) * _tmp9 + P(1, 2) * _tmp8 + P(2, 2) * _tmp7 + P(3, 2) * _tmp10) + + _tmp8 * (P(0, 1) * _tmp9 + P(1, 1) * _tmp8 + P(2, 1) * _tmp7 + P(3, 1) * _tmp10) + + _tmp9 * (P(0, 0) * _tmp9 + P(1, 0) * _tmp8 + P(2, 0) * _tmp7 + P(3, 0) * _tmp10); + } + + if (H != nullptr) { + matrix::Matrix& _H = (*H); + + _H.setZero(); + + _H(0, 0) = _tmp9; + _H(1, 0) = _tmp8; + _H(2, 0) = _tmp7; + _H(3, 0) = _tmp10; + } +} // 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 new file mode 100644 index 0000000000..240d725470 --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h_alternate.h @@ -0,0 +1,75 @@ +// ----------------------------------------------------------------------------- +// 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_yaw_321_innov_var_and_h_alternate + * + * Args: + * state: Matrix24_1 + * P: Matrix24_24 + * R: Scalar + * epsilon: Scalar + * + * Outputs: + * innov_var: Scalar + * H: Matrix24_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: 84 + + // Input arrays + + // Intermediate terms (11) + const Scalar _tmp0 = 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)); + const Scalar _tmp1 = state(0, 0) * state(3, 0) + state(1, 0) * state(2, 0); + const Scalar _tmp2 = 2 * _tmp1 + epsilon * ((((_tmp1) > 0) - ((_tmp1) < 0)) + Scalar(0.5)); + const Scalar _tmp3 = std::pow(_tmp2, Scalar(2)); + const Scalar _tmp4 = 2 * _tmp0 / _tmp3; + const Scalar _tmp5 = 2 / _tmp2; + const Scalar _tmp6 = _tmp3 / (std::pow(_tmp0, Scalar(2)) + _tmp3); + const Scalar _tmp7 = _tmp6 * (-_tmp4 * state(0, 0) - _tmp5 * state(3, 0)); + const Scalar _tmp8 = _tmp6 * (-_tmp4 * state(2, 0) + _tmp5 * state(1, 0)); + const Scalar _tmp9 = _tmp6 * (-_tmp4 * state(3, 0) + _tmp5 * state(0, 0)); + const Scalar _tmp10 = _tmp6 * (-_tmp4 * state(1, 0) - _tmp5 * state(2, 0)); + + // Output terms (2) + if (innov_var != nullptr) { + Scalar& _innov_var = (*innov_var); + + _innov_var = + R - _tmp10 * (-P(0, 2) * _tmp9 - P(1, 2) * _tmp8 - P(2, 2) * _tmp10 - P(3, 2) * _tmp7) - + _tmp7 * (-P(0, 3) * _tmp9 - P(1, 3) * _tmp8 - P(2, 3) * _tmp10 - P(3, 3) * _tmp7) - + _tmp8 * (-P(0, 1) * _tmp9 - P(1, 1) * _tmp8 - P(2, 1) * _tmp10 - P(3, 1) * _tmp7) - + _tmp9 * (-P(0, 0) * _tmp9 - P(1, 0) * _tmp8 - P(2, 0) * _tmp10 - P(3, 0) * _tmp7); + } + + if (H != nullptr) { + matrix::Matrix& _H = (*H); + + _H.setZero(); + + _H(0, 0) = -_tmp9; + _H(1, 0) = -_tmp8; + _H(2, 0) = -_tmp10; + _H(3, 0) = -_tmp7; + } +} // 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 c20144dec4..dfaec34426 100644 --- a/src/modules/ekf2/test/CMakeLists.txt +++ b/src/modules/ekf2/test/CMakeLists.txt @@ -57,4 +57,5 @@ px4_add_unit_gtest(SRC test_EKF_terrain_estimator.cpp LINKLIBS ecl_EKF ecl_senso px4_add_unit_gtest(SRC test_EKF_utils.cpp LINKLIBS ecl_EKF ecl_sensor_sim) px4_add_unit_gtest(SRC test_EKF_withReplayData.cpp LINKLIBS ecl_EKF ecl_sensor_sim) px4_add_unit_gtest(SRC test_EKF_yaw_estimator.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper) +px4_add_unit_gtest(SRC test_EKF_yaw_fusion_generated.cpp LINKLIBS ecl_EKF ecl_test_helper) px4_add_unit_gtest(SRC test_SensorRangeFinder.cpp LINKLIBS ecl_EKF ecl_sensor_sim) diff --git a/src/modules/ekf2/test/test_EKF_yaw_fusion_generated.cpp b/src/modules/ekf2/test/test_EKF_yaw_fusion_generated.cpp new file mode 100644 index 0000000000..67d9d4ca7b --- /dev/null +++ b/src/modules/ekf2/test/test_EKF_yaw_fusion_generated.cpp @@ -0,0 +1,102 @@ +/**************************************************************************** + * + * 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_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" + +using namespace matrix; + +TEST(YawFusionGenerated, singularityYawEquivalence) +{ + // GIVEN: an attitude that should give a singularity when transforming the + // rotation matrix to Euler yaw + const Quatf q(Eulerf(M_PI_F, 0.f, M_PI_F)); + + Vector24f state_vector{}; + state_vector(0) = q(0); + state_vector(1) = q(1); + state_vector(2) = q(2); + state_vector(3) = q(3); + + const float R = sq(radians(10.f)); + SquareMatrix24f P = createRandomCovarianceMatrix24f(); + + Vector24f H_a; + Vector24f H_b; + 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) + 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); + + // THEN: Even at the singularity point, the result is still correct, thanks to epsilon + EXPECT_TRUE(isEqual(H_a, H_b)); + EXPECT_NEAR(innov_var_a, innov_var_b, 1e-5f); + EXPECT_TRUE(innov_var_a < 5.f && innov_var_a > R) << "innov_var = " << innov_var_a; +} + +TEST(YawFusionGenerated, gimbalLock321vs312) +{ + // GIVEN: an attitude at gimbal lock position + const Quatf q(Eulerf(0.f, -M_PI_F / 2.f, M_PI_F)); + + Vector24f state_vector{}; + state_vector(0) = q(0); + state_vector(1) = q(1); + state_vector(2) = q(2); + state_vector(3) = q(3); + + const float R = sq(radians(10.f)); + SquareMatrix24f P = createRandomCovarianceMatrix24f(); + + Vector24f H_321; + Vector24f H_312; + float innov_var_321; + float innov_var_312; + 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); + + // THEN: both computation are not equivalent, 321 is undefined but 312 is valid + EXPECT_FALSE(isEqual(H_321, H_312)); + EXPECT_GT(fabsf(innov_var_321 - innov_var_312), 1e6f); + EXPECT_TRUE(innov_var_312 < 5.f && innov_var_312 > R) << "innov_var = " << innov_var_312; +}