diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 65f49d5c72..b93106977a 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -41,6 +41,7 @@ #include "ekf.h" #include "python/ekf_derivation/generated/quat_var_to_rot_var.h" +#include "python/ekf_derivation/generated/yaw_var_to_lower_triangular_quat_cov.h" #include #include @@ -1008,46 +1009,12 @@ void Ekf::updateGroundEffect() } } -// Increase the yaw error variance of the quaternions -// Argument is additional yaw variance in rad**2 void Ekf::increaseQuatYawErrVariance(float yaw_variance) { - // See DeriveYawResetEquations.m for derivation which produces code fragments in C_code4.txt file - // The auto-code was cleaned up and had terms multiplied by zero removed to give the following: - - // Intermediate variables - float SG[3]; - SG[0] = sq(_state.quat_nominal(0)) - sq(_state.quat_nominal(1)) - sq(_state.quat_nominal(2)) + sq(_state.quat_nominal(3)); - SG[1] = 2*_state.quat_nominal(0)*_state.quat_nominal(2) - 2*_state.quat_nominal(1)*_state.quat_nominal(3); - SG[2] = 2*_state.quat_nominal(0)*_state.quat_nominal(1) + 2*_state.quat_nominal(2)*_state.quat_nominal(3); - - float SQ[4]; - SQ[0] = 0.5f * ((_state.quat_nominal(1)*SG[0]) - (_state.quat_nominal(0)*SG[2]) + (_state.quat_nominal(3)*SG[1])); - SQ[1] = 0.5f * ((_state.quat_nominal(0)*SG[1]) - (_state.quat_nominal(2)*SG[0]) + (_state.quat_nominal(3)*SG[2])); - SQ[2] = 0.5f * ((_state.quat_nominal(3)*SG[0]) - (_state.quat_nominal(1)*SG[1]) + (_state.quat_nominal(2)*SG[2])); - SQ[3] = 0.5f * ((_state.quat_nominal(0)*SG[0]) + (_state.quat_nominal(1)*SG[2]) + (_state.quat_nominal(2)*SG[1])); - - // Limit yaw variance increase to prevent a badly conditioned covariance matrix - yaw_variance = fminf(yaw_variance, 1.0e-2f); - - // Add covariances for additonal yaw uncertainty to existing covariances. - // This assumes that the additional yaw error is uncorrrelated to existing errors - P(0,0) += yaw_variance*sq(SQ[2]); - P(0,1) += yaw_variance*SQ[1]*SQ[2]; - P(1,1) += yaw_variance*sq(SQ[1]); - P(0,2) += yaw_variance*SQ[0]*SQ[2]; - P(1,2) += yaw_variance*SQ[0]*SQ[1]; - P(2,2) += yaw_variance*sq(SQ[0]); - P(0,3) -= yaw_variance*SQ[2]*SQ[3]; - P(1,3) -= yaw_variance*SQ[1]*SQ[3]; - P(2,3) -= yaw_variance*SQ[0]*SQ[3]; - P(3,3) += yaw_variance*sq(SQ[3]); - P(1,0) += yaw_variance*SQ[1]*SQ[2]; - P(2,0) += yaw_variance*SQ[0]*SQ[2]; - P(2,1) += yaw_variance*SQ[0]*SQ[1]; - P(3,0) -= yaw_variance*SQ[2]*SQ[3]; - P(3,1) -= yaw_variance*SQ[1]*SQ[3]; - P(3,2) -= yaw_variance*SQ[0]*SQ[3]; + matrix::SquareMatrix q_cov; + sym::YawVarToLowerTriangularQuatCov(getStateAtFusionHorizonAsVector(), yaw_variance, &q_cov); + q_cov.copyLowerToUpperTriangle(); + P.slice<4, 4>(0, 0) += q_cov; } void Ekf::saveMagCovData() diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py index 80a8edb5c2..6bcc542c4f 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py @@ -511,6 +511,25 @@ def quat_var_to_rot_var( rot_cov = J * P * J.T return sf.V3(rot_cov[0, 0], rot_cov[1, 1], rot_cov[2, 2]) +def yaw_var_to_lower_triangular_quat_cov( + state: VState, + yaw_var: sf.Scalar +): + q = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz]) + attitude = state_to_rot3(state) + J = q.jacobian(attitude) + + # Convert yaw uncertainty from NED to body frame + yaw_cov_ned = sf.M33.diag([0, 0, yaw_var]) + adjoint = attitude.to_rotation_matrix() # the adjoint of SO(3) is simply the rotation matrix itself + yaw_cov_body = adjoint.T * yaw_cov_ned * adjoint + + # Convert yaw (body) to quaternion parameter uncertainty + q_var = J * yaw_cov_body * J.T + + # Generate lower trangle only and copy it to the upper part in implementation (produces less code) + return q_var.lower_triangle() + 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"]) @@ -533,3 +552,4 @@ generate_px4_function(compute_drag_x_innov_var_and_k, output_names=["innov_var", 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"]) +generate_px4_function(yaw_var_to_lower_triangular_quat_cov, output_names=["q_cov_lower_triangle"]) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_var_to_lower_triangular_quat_cov.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_var_to_lower_triangular_quat_cov.h new file mode 100644 index 0000000000..687f524627 --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_var_to_lower_triangular_quat_cov.h @@ -0,0 +1,101 @@ +// ----------------------------------------------------------------------------- +// 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: yaw_var_to_lower_triangular_quat_cov + * + * Args: + * state: Matrix24_1 + * yaw_var: Scalar + * + * Outputs: + * q_cov_lower_triangle: Matrix44 + */ +template +void YawVarToLowerTriangularQuatCov( + const matrix::Matrix& state, const Scalar yaw_var, + matrix::Matrix* const q_cov_lower_triangle = nullptr) { + // Total ops: 136 + + // Input arrays + + // Intermediate terms (39) + const Scalar _tmp0 = (Scalar(1) / Scalar(2)) * state(3, 0); + const Scalar _tmp1 = + -2 * std::pow(state(1, 0), Scalar(2)) - 2 * std::pow(state(2, 0), Scalar(2)) + 1; + const Scalar _tmp2 = std::pow(_tmp1, Scalar(2)) * yaw_var; + const Scalar _tmp3 = (Scalar(1) / Scalar(2)) * state(1, 0); + const Scalar _tmp4 = 2 * state(2, 0); + const Scalar _tmp5 = 2 * state(1, 0); + const Scalar _tmp6 = -_tmp4 * state(0, 0) + _tmp5 * state(3, 0); + const Scalar _tmp7 = _tmp1 * _tmp6 * yaw_var; + const Scalar _tmp8 = -_tmp3 * _tmp7; + const Scalar _tmp9 = (Scalar(1) / Scalar(2)) * state(2, 0); + const Scalar _tmp10 = _tmp4 * state(3, 0) + _tmp5 * state(0, 0); + const Scalar _tmp11 = _tmp10 * yaw_var; + const Scalar _tmp12 = _tmp1 * _tmp11; + const Scalar _tmp13 = _tmp12 * _tmp9; + const Scalar _tmp14 = std::pow(_tmp10, Scalar(2)) * yaw_var; + const Scalar _tmp15 = _tmp11 * _tmp6; + const Scalar _tmp16 = _tmp15 * _tmp3; + const Scalar _tmp17 = -_tmp0 * _tmp12; + const Scalar _tmp18 = std::pow(_tmp6, Scalar(2)) * yaw_var; + const Scalar _tmp19 = _tmp0 * _tmp7; + const Scalar _tmp20 = -_tmp15 * _tmp9; + const Scalar _tmp21 = (Scalar(1) / Scalar(2)) * state(0, 0); + const Scalar _tmp22 = _tmp21 * _tmp7; + const Scalar _tmp23 = _tmp17 + _tmp2 * _tmp9 + _tmp22; + const Scalar _tmp24 = _tmp15 * _tmp21; + const Scalar _tmp25 = -_tmp0 * _tmp14 + _tmp13 + _tmp24; + const Scalar _tmp26 = _tmp7 * _tmp9; + const Scalar _tmp27 = _tmp0 * _tmp15; + const Scalar _tmp28 = _tmp18 * _tmp21 + _tmp26 - _tmp27; + const Scalar _tmp29 = _tmp12 * _tmp21; + const Scalar _tmp30 = _tmp19 - _tmp2 * _tmp3 + _tmp29; + const Scalar _tmp31 = _tmp12 * _tmp3; + const Scalar _tmp32 = _tmp14 * _tmp21 + _tmp27 - _tmp31; + const Scalar _tmp33 = _tmp0 * _tmp18 + _tmp24 + _tmp8; + const Scalar _tmp34 = _tmp2 * _tmp21 - _tmp26 + _tmp31; + const Scalar _tmp35 = _tmp14 * _tmp3 + _tmp20 + _tmp29; + const Scalar _tmp36 = (Scalar(1) / Scalar(2)) * _tmp35; + const Scalar _tmp37 = _tmp16 - _tmp18 * _tmp9 + _tmp22; + const Scalar _tmp38 = (Scalar(1) / Scalar(2)) * _tmp34; + + // Output terms (1) + if (q_cov_lower_triangle != nullptr) { + matrix::Matrix& _q_cov_lower_triangle = (*q_cov_lower_triangle); + + _q_cov_lower_triangle(0, 0) = -_tmp0 * (-_tmp0 * _tmp2 - _tmp13 + _tmp8) - + _tmp3 * (-_tmp18 * _tmp3 - _tmp19 + _tmp20) - + _tmp9 * (-_tmp14 * _tmp9 - _tmp16 + _tmp17); + _q_cov_lower_triangle(1, 0) = -_tmp0 * _tmp23 - _tmp25 * _tmp9 - _tmp28 * _tmp3; + _q_cov_lower_triangle(2, 0) = -_tmp0 * _tmp30 - _tmp3 * _tmp33 - _tmp32 * _tmp9; + _q_cov_lower_triangle(3, 0) = -_tmp0 * _tmp34 - _tmp3 * _tmp37 - _tmp36 * state(2, 0); + _q_cov_lower_triangle(0, 1) = 0; + _q_cov_lower_triangle(1, 1) = -_tmp0 * _tmp25 + _tmp21 * _tmp28 + _tmp23 * _tmp9; + _q_cov_lower_triangle(2, 1) = -_tmp0 * _tmp32 + _tmp21 * _tmp33 + _tmp30 * _tmp9; + _q_cov_lower_triangle(3, 1) = -_tmp0 * _tmp35 + _tmp21 * _tmp37 + _tmp34 * _tmp9; + _q_cov_lower_triangle(0, 2) = 0; + _q_cov_lower_triangle(1, 2) = 0; + _q_cov_lower_triangle(2, 2) = _tmp0 * _tmp33 + _tmp21 * _tmp32 - _tmp3 * _tmp30; + _q_cov_lower_triangle(3, 2) = _tmp0 * _tmp37 + _tmp36 * state(0, 0) - _tmp38 * state(1, 0); + _q_cov_lower_triangle(0, 3) = 0; + _q_cov_lower_triangle(1, 3) = 0; + _q_cov_lower_triangle(2, 3) = 0; + _q_cov_lower_triangle(3, 3) = _tmp36 * state(1, 0) - _tmp37 * _tmp9 + _tmp38 * state(0, 0); + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/src/modules/ekf2/test/test_EKF_attitude_variance.cpp b/src/modules/ekf2/test/test_EKF_attitude_variance.cpp index 761685b002..b1a9e14f8a 100644 --- a/src/modules/ekf2/test/test_EKF_attitude_variance.cpp +++ b/src/modules/ekf2/test/test_EKF_attitude_variance.cpp @@ -36,6 +36,9 @@ #include "test_helper/comparison_helper.h" #include "../EKF/python/ekf_derivation/generated/quat_var_to_rot_var.h" +#include "../EKF/python/ekf_derivation/generated/yaw_var_to_lower_triangular_quat_cov.h" +#include "../EKF/python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h.h" +#include "../EKF/python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h.h" using namespace matrix; @@ -130,3 +133,80 @@ TEST(AttitudeVariance, matlabVsSymforceZeroTilt) const Vector3f rot_var_true = 4.0f * P.slice<3, 3>(1, 1).diag(); // special case EXPECT_EQ(rot_var_symforce, rot_var_true); } + +void increaseYawVar(const Vector24f &state_vector, SquareMatrix24f &P, const float yaw_var) +{ + SquareMatrix q_cov; + sym::YawVarToLowerTriangularQuatCov(state_vector, yaw_var, &q_cov); + q_cov.copyLowerToUpperTriangle(); + P.slice<4, 4>(0, 0) += q_cov; +} + +float getYawVar(const Vector24f &state_vector, const SquareMatrix24f &P) +{ + Vector24f H_YAW; + float yaw_var = 0.f; + sym::ComputeYaw312InnovVarAndH(state_vector, P, 0.f, FLT_EPSILON, &yaw_var, &H_YAW); + return yaw_var; +} + +TEST(AttitudeVariance, increaseYawVarNoTilt) +{ + Quatf q; + SquareMatrix24f P; + 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 yaw_var = radians(25.f); + increaseYawVar(state_vector, P, yaw_var); + + const float var_new = getYawVar(state_vector, P); + + EXPECT_NEAR(var_new, yaw_var, 1e-6f); +} + +TEST(AttitudeVariance, increaseYawVarPitch90) +{ + Quatf q(Eulerf(M_PI_F / 2.f, M_PI_F / 2.f, M_PI_F / 3.f)); + SquareMatrix24f P; + 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 yaw_var = radians(25.f); + increaseYawVar(state_vector, P, yaw_var); + + const float var_new = getYawVar(state_vector, P); + + EXPECT_NEAR(var_new, yaw_var, 1e-6f); +} + +TEST(AttitudeVariance, increaseYawWithTilt) +{ + Quatf q(Eulerf(-M_PI_F, M_PI_F / 3.f, -M_PI_F / 5.f)); + SquareMatrix24f P; + Vector24f state_vector{}; + state_vector(0) = q(0); + state_vector(1) = q(1); + state_vector(2) = q(2); + state_vector(3) = q(3); + + // Set the yaw variance (from 0) + const float yaw_var_1 = radians(25.f); + increaseYawVar(state_vector, P, yaw_var_1); + + const float var_1 = getYawVar(state_vector, P); + EXPECT_NEAR(var_1, yaw_var_1, 1e-6f); + + // Increase it even more + const float yaw_var_2 = radians(12.f); + increaseYawVar(state_vector, P, yaw_var_2); + + const float var_2 = getYawVar(state_vector, P); + EXPECT_NEAR(var_2, yaw_var_1 + yaw_var_2, 1e-6f); +}