diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 1c58ee8473..65f49d5c72 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -40,6 +40,7 @@ */ #include "ekf.h" +#include "python/ekf_derivation/generated/quat_var_to_rot_var.h" #include #include @@ -882,46 +883,9 @@ void Ekf::updateVerticalDeadReckoningStatus() // calculate the variances for the rotation vector equivalent Vector3f Ekf::calcRotVecVariances() const { - Vector3f rot_var_vec; - float q0, q1, q2, q3; - - if (_state.quat_nominal(0) >= 0.0f) { - q0 = _state.quat_nominal(0); - q1 = _state.quat_nominal(1); - q2 = _state.quat_nominal(2); - q3 = _state.quat_nominal(3); - - } else { - q0 = -_state.quat_nominal(0); - q1 = -_state.quat_nominal(1); - q2 = -_state.quat_nominal(2); - q3 = -_state.quat_nominal(3); - } - float t2 = q0*q0; - float t3 = acosf(q0); - float t4 = -t2+1.0f; - float t5 = t2-1.0f; - if ((t4 > 1e-9f) && (t5 < -1e-9f)) { - float t6 = 1.0f/t5; - float t7 = q1*t6*2.0f; - float t8 = 1.0f/powf(t4,1.5f); - float t9 = q0*q1*t3*t8*2.0f; - float t10 = t7+t9; - float t11 = 1.0f/sqrtf(t4); - float t12 = q2*t6*2.0f; - float t13 = q0*q2*t3*t8*2.0f; - float t14 = t12+t13; - float t15 = q3*t6*2.0f; - float t16 = q0*q3*t3*t8*2.0f; - float t17 = t15+t16; - rot_var_vec(0) = t10*(P(0,0)*t10+P(1,0)*t3*t11*2.0f)+t3*t11*(P(0,1)*t10+P(1,1)*t3*t11*2.0f)*2.0f; - rot_var_vec(1) = t14*(P(0,0)*t14+P(2,0)*t3*t11*2.0f)+t3*t11*(P(0,2)*t14+P(2,2)*t3*t11*2.0f)*2.0f; - rot_var_vec(2) = t17*(P(0,0)*t17+P(3,0)*t3*t11*2.0f)+t3*t11*(P(0,3)*t17+P(3,3)*t3*t11*2.0f)*2.0f; - } else { - rot_var_vec = 4.0f * P.slice<3,3>(1,1).diag(); - } - - return rot_var_vec; + Vector3f rot_var; + sym::QuatVarToRotVar(getStateAtFusionHorizonAsVector(), P, FLT_EPSILON, &rot_var); + return rot_var; } // initialise the quaternion covariances using rotation vector variances diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py index 5b1c4c59ec..80a8edb5c2 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py @@ -502,6 +502,15 @@ def compute_gravity_innov_var_and_k_and_h( return (innov, innov_var, K[0], K[1], K[2]) +def quat_var_to_rot_var( + state: VState, + P: MState, + epsilon: sf.Scalar +): + J = sf.V3(state_to_rot3(state).to_tangent(epsilon=epsilon)).jacobian(state) + rot_cov = J * P * J.T + return sf.V3(rot_cov[0, 0], rot_cov[1, 1], rot_cov[2, 2]) + 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"]) @@ -523,3 +532,4 @@ generate_px4_function(compute_gnss_yaw_pred_innov_var_and_h, output_names=["meas generate_px4_function(compute_drag_x_innov_var_and_k, output_names=["innov_var", "K"]) generate_px4_function(compute_drag_y_innov_var_and_k, output_names=["innov_var", "K"]) generate_px4_function(compute_gravity_innov_var_and_k_and_h, output_names=["innov", "innov_var", "Kx", "Ky", "Kz"]) +generate_px4_function(quat_var_to_rot_var, output_names=["rot_var"]) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/quat_var_to_rot_var.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/quat_var_to_rot_var.h new file mode 100644 index 0000000000..a7a1831f71 --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/quat_var_to_rot_var.h @@ -0,0 +1,67 @@ +// ----------------------------------------------------------------------------- +// 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: quat_var_to_rot_var + * + * Args: + * state: Matrix24_1 + * P: Matrix24_24 + * epsilon: Scalar + * + * Outputs: + * rot_var: Matrix31 + */ +template +void QuatVarToRotVar(const matrix::Matrix& state, + const matrix::Matrix& P, const Scalar epsilon, + matrix::Matrix* const rot_var = nullptr) { + // Total ops: 61 + + // Input arrays + + // Intermediate terms (17) + const Scalar _tmp0 = std::fabs(state(0, 0)); + const Scalar _tmp1 = 1 - epsilon; + const Scalar _tmp2 = math::min(_tmp0, _tmp1); + const Scalar _tmp3 = 1 - std::pow(_tmp2, Scalar(2)); + const Scalar _tmp4 = (((state(0, 0)) > 0) - ((state(0, 0)) < 0)); + const Scalar _tmp5 = 2 * math::min(0, _tmp4) + 1; + const Scalar _tmp6 = _tmp5 * std::acos(_tmp2); + const Scalar _tmp7 = 2 * _tmp6 / std::sqrt(_tmp3); + const Scalar _tmp8 = _tmp4 * ((((-_tmp0 + _tmp1) > 0) - ((-_tmp0 + _tmp1) < 0)) + 1); + const Scalar _tmp9 = _tmp8 * state(1, 0); + const Scalar _tmp10 = _tmp2 * _tmp6 / (_tmp3 * std::sqrt(_tmp3)); + const Scalar _tmp11 = _tmp5 / _tmp3; + const Scalar _tmp12 = _tmp10 * _tmp9 - _tmp11 * _tmp9; + const Scalar _tmp13 = _tmp10 * _tmp8; + const Scalar _tmp14 = _tmp11 * _tmp8; + const Scalar _tmp15 = _tmp13 * state(2, 0) - _tmp14 * state(2, 0); + const Scalar _tmp16 = _tmp13 * state(3, 0) - _tmp14 * state(3, 0); + + // Output terms (1) + if (rot_var != nullptr) { + matrix::Matrix& _rot_var = (*rot_var); + + _rot_var(0, 0) = _tmp12 * (P(0, 0) * _tmp12 + P(1, 0) * _tmp7) + + _tmp7 * (P(0, 1) * _tmp12 + P(1, 1) * _tmp7); + _rot_var(1, 0) = _tmp15 * (P(0, 0) * _tmp15 + P(2, 0) * _tmp7) + + _tmp7 * (P(0, 2) * _tmp15 + P(2, 2) * _tmp7); + _rot_var(2, 0) = _tmp16 * (P(0, 0) * _tmp16 + P(3, 0) * _tmp7) + + _tmp7 * (P(0, 3) * _tmp16 + P(3, 3) * _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 4412be2fa3..152aafc944 100644 --- a/src/modules/ekf2/test/CMakeLists.txt +++ b/src/modules/ekf2/test/CMakeLists.txt @@ -37,6 +37,7 @@ add_subdirectory(sensor_simulator) add_subdirectory(test_helper) px4_add_unit_gtest(SRC test_EKF_accelerometer.cpp LINKLIBS ecl_EKF ecl_sensor_sim) +px4_add_unit_gtest(SRC test_EKF_attitude_variance.cpp LINKLIBS ecl_EKF ecl_test_helper) px4_add_unit_gtest(SRC test_EKF_airspeed_fusion_generated.cpp LINKLIBS ecl_EKF ecl_test_helper) px4_add_unit_gtest(SRC test_EKF_airspeed.cpp LINKLIBS ecl_EKF ecl_sensor_sim) px4_add_unit_gtest(SRC test_EKF_basics.cpp LINKLIBS ecl_EKF ecl_sensor_sim) diff --git a/src/modules/ekf2/test/test_EKF_attitude_variance.cpp b/src/modules/ekf2/test/test_EKF_attitude_variance.cpp new file mode 100644 index 0000000000..761685b002 --- /dev/null +++ b/src/modules/ekf2/test/test_EKF_attitude_variance.cpp @@ -0,0 +1,132 @@ +/**************************************************************************** + * + * Copyright (C) 2023 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/quat_var_to_rot_var.h" + +using namespace matrix; + +Vector3f calcRotVarMatlab(const Quatf &q, const SquareMatrix24f &P) +{ + Vector3f rot_var_vec; + float q0, q1, q2, q3; + + if (q(0) >= 0.0f) { + q0 = q(0); + q1 = q(1); + q2 = q(2); + q3 = q(3); + + } else { + q0 = -q(0); + q1 = -q(1); + q2 = -q(2); + q3 = -q(3); + } + + float t2 = q0 * q0; + float t3 = acosf(q0); + float t4 = -t2 + 1.0f; + float t5 = t2 - 1.0f; + + if ((t4 > 1e-9f) && (t5 < -1e-9f)) { + float t6 = 1.0f / t5; + float t7 = q1 * t6 * 2.0f; + float t8 = 1.0f / powf(t4, 1.5f); + float t9 = q0 * q1 * t3 * t8 * 2.0f; + float t10 = t7 + t9; + float t11 = 1.0f / sqrtf(t4); + float t12 = q2 * t6 * 2.0f; + float t13 = q0 * q2 * t3 * t8 * 2.0f; + float t14 = t12 + t13; + float t15 = q3 * t6 * 2.0f; + float t16 = q0 * q3 * t3 * t8 * 2.0f; + float t17 = t15 + t16; + rot_var_vec(0) = t10 * (P(0, 0) * t10 + P(1, 0) * t3 * t11 * 2.0f) + t3 * t11 * (P(0, 1) * t10 + P(1, + 1) * t3 * t11 * 2.0f) * 2.0f; + rot_var_vec(1) = t14 * (P(0, 0) * t14 + P(2, 0) * t3 * t11 * 2.0f) + t3 * t11 * (P(0, 2) * t14 + P(2, + 2) * t3 * t11 * 2.0f) * 2.0f; + rot_var_vec(2) = t17 * (P(0, 0) * t17 + P(3, 0) * t3 * t11 * 2.0f) + t3 * t11 * (P(0, 3) * t17 + P(3, + 3) * t3 * t11 * 2.0f) * 2.0f; + + } else { + rot_var_vec = 4.0f * P.slice<3, 3>(1, 1).diag(); + } + + return rot_var_vec; +} + +TEST(AttitudeVariance, matlabVsSymforce) +{ + Quatf q(Eulerf(M_PI_F / 4.f, -M_PI_F / 6.f, M_PI_F)); + q = -q; // use non-canonical quaternion to cover special case + + const SquareMatrix24f P = createRandomCovarianceMatrix24f(); + Vector3f rot_var_matlab = calcRotVarMatlab(q, P); + + Vector24f state_vector{}; + state_vector(0) = q(0); + state_vector(1) = q(1); + state_vector(2) = q(2); + state_vector(3) = q(3); + + Vector3f rot_var_symforce; + sym::QuatVarToRotVar(state_vector, P, FLT_EPSILON, &rot_var_symforce); + + EXPECT_EQ(rot_var_matlab, rot_var_symforce); +} + +TEST(AttitudeVariance, matlabVsSymforceZeroTilt) +{ + Quatf q; + + const SquareMatrix24f P = createRandomCovarianceMatrix24f(); + Vector3f rot_var_matlab = calcRotVarMatlab(q, P); + + Vector24f state_vector{}; + state_vector(0) = q(0); + state_vector(1) = q(1); + state_vector(2) = q(2); + state_vector(3) = q(3); + + Vector3f rot_var_symforce; + sym::QuatVarToRotVar(state_vector, P, FLT_EPSILON, &rot_var_symforce); + + EXPECT_EQ(rot_var_matlab, rot_var_symforce); + + const Vector3f rot_var_true = 4.0f * P.slice<3, 3>(1, 1).diag(); // special case + EXPECT_EQ(rot_var_symforce, rot_var_true); +}