diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index 9f8371b533..4b0357b14c 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -43,6 +43,7 @@ #include "ekf.h" #include "python/ekf_derivation/generated/predict_covariance.h" +#include "python/ekf_derivation/generated/rot_var_ned_to_lower_triangular_quat_cov.h" #include #include @@ -53,7 +54,7 @@ void Ekf::initialiseCovariance() { P.zero(); - resetQuatCov(); + resetQuatCov((_params.mag_fusion_type != MagFuseType::NONE) ?_params.mag_heading_noise : NAN); // velocity P(4,4) = sq(fmaxf(_params.gps_vel_noise, 0.01f)); @@ -526,27 +527,25 @@ bool Ekf::checkAndFixCovarianceUpdate(const SquareMatrix24f &KHP) return healthy; } -void Ekf::resetQuatCov() +void Ekf::resetQuatCov(const float yaw_noise) { - zeroQuatCov(); - - // define the initial angle uncertainty as variances for a rotation vector - Vector3f rot_vec_var; - rot_vec_var.setAll(sq(_params.initial_tilt_err)); - - initialiseQuatCovariances(rot_vec_var); + const float tilt_var = sq(fmaxf(_params.initial_tilt_err, 1.0e-2f)); + Vector3f rot_var_ned(tilt_var, tilt_var, 0.f); // update the yaw angle variance using the variance of the measurement - if (_params.mag_fusion_type != MagFuseType::NONE) { + if (PX4_ISFINITE(yaw_noise)) { // using magnetic heading tuning parameter - increaseQuatYawErrVariance(sq(fmaxf(_params.mag_heading_noise, 1.0e-2f))); + rot_var_ned(2) = (sq(fmaxf(yaw_noise, 1.0e-2f))); } -} -void Ekf::zeroQuatCov() -{ + // clear existing quaternion covariance P.uncorrelateCovarianceSetVariance<2>(0, 0.0f); P.uncorrelateCovarianceSetVariance<2>(2, 0.0f); + + matrix::SquareMatrix q_cov; + sym::RotVarNedToLowerTriangularQuatCov(getStateAtFusionHorizonAsVector(), rot_var_ned, &q_cov); + q_cov.copyLowerToUpperTriangle(); + P.slice<4, 4>(0, 0) = q_cov; } void Ekf::resetMagCov() diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index a42ea5bcee..a1047b97e4 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -1075,12 +1075,7 @@ private: // gravity fusion: heuristically enable / disable gravity fusion void controlGravityFusion(const imuSample &imu_delayed); - // initialise the quaternion covariances using rotation vector variances - // do not call before quaternion states are initialised - void initialiseQuatCovariances(Vector3f &rot_vec_var); - - void resetQuatCov(); - void zeroQuatCov(); + void resetQuatCov(float yaw_noise = NAN); void resetMagCov(); diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 2235a9dbb2..f30686a8f7 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -41,7 +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 "python/ekf_derivation/generated/rot_var_ned_to_lower_triangular_quat_cov.h" #include #include @@ -917,106 +917,6 @@ float Ekf::getYawVar() const return yaw_var; } -// initialise the quaternion covariances using rotation vector variances -// do not call before quaternion states are initialised -void Ekf::initialiseQuatCovariances(Vector3f &rot_vec_var) -{ - // calculate an equivalent rotation vector from the quaternion - 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 delta = 2.0f*acosf(q0); - float scaler = (delta/sinf(delta*0.5f)); - float rotX = scaler*q1; - float rotY = scaler*q2; - float rotZ = scaler*q3; - - // autocode generated using matlab symbolic toolbox - float t2 = rotX*rotX; - float t4 = rotY*rotY; - float t5 = rotZ*rotZ; - float t6 = t2+t4+t5; - if (t6 > 1e-9f) { - float t7 = sqrtf(t6); - float t8 = t7*0.5f; - float t3 = sinf(t8); - float t9 = t3*t3; - float t10 = 1.0f/t6; - float t11 = 1.0f/sqrtf(t6); - float t12 = cosf(t8); - float t13 = 1.0f/powf(t6,1.5f); - float t14 = t3*t11; - float t15 = rotX*rotY*t3*t13; - float t16 = rotX*rotZ*t3*t13; - float t17 = rotY*rotZ*t3*t13; - float t18 = t2*t10*t12*0.5f; - float t27 = t2*t3*t13; - float t19 = t14+t18-t27; - float t23 = rotX*rotY*t10*t12*0.5f; - float t28 = t15-t23; - float t20 = rotY*rot_vec_var(1)*t3*t11*t28*0.5f; - float t25 = rotX*rotZ*t10*t12*0.5f; - float t31 = t16-t25; - float t21 = rotZ*rot_vec_var(2)*t3*t11*t31*0.5f; - float t22 = t20+t21-rotX*rot_vec_var(0)*t3*t11*t19*0.5f; - float t24 = t15-t23; - float t26 = t16-t25; - float t29 = t4*t10*t12*0.5f; - float t34 = t3*t4*t13; - float t30 = t14+t29-t34; - float t32 = t5*t10*t12*0.5f; - float t40 = t3*t5*t13; - float t33 = t14+t32-t40; - float t36 = rotY*rotZ*t10*t12*0.5f; - float t39 = t17-t36; - float t35 = rotZ*rot_vec_var(2)*t3*t11*t39*0.5f; - float t37 = t15-t23; - float t38 = t17-t36; - float t41 = rot_vec_var(0)*(t15-t23)*(t16-t25); - float t42 = t41-rot_vec_var(1)*t30*t39-rot_vec_var(2)*t33*t39; - float t43 = t16-t25; - float t44 = t17-t36; - - // zero all the quaternion covariances - P.uncorrelateCovarianceSetVariance<2>(0, 0.0f); - P.uncorrelateCovarianceSetVariance<2>(2, 0.0f); - - - // Update the quaternion internal covariances using auto-code generated using matlab symbolic toolbox - P(0,0) = rot_vec_var(0)*t2*t9*t10*0.25f+rot_vec_var(1)*t4*t9*t10*0.25f+rot_vec_var(2)*t5*t9*t10*0.25f; - P(0,1) = t22; - P(0,2) = t35+rotX*rot_vec_var(0)*t3*t11*(t15-rotX*rotY*t10*t12*0.5f)*0.5f-rotY*rot_vec_var(1)*t3*t11*t30*0.5f; - P(0,3) = rotX*rot_vec_var(0)*t3*t11*(t16-rotX*rotZ*t10*t12*0.5f)*0.5f+rotY*rot_vec_var(1)*t3*t11*(t17-rotY*rotZ*t10*t12*0.5f)*0.5f-rotZ*rot_vec_var(2)*t3*t11*t33*0.5f; - P(1,0) = t22; - P(1,1) = rot_vec_var(0)*(t19*t19)+rot_vec_var(1)*(t24*t24)+rot_vec_var(2)*(t26*t26); - P(1,2) = rot_vec_var(2)*(t16-t25)*(t17-rotY*rotZ*t10*t12*0.5f)-rot_vec_var(0)*t19*t28-rot_vec_var(1)*t28*t30; - P(1,3) = rot_vec_var(1)*(t15-t23)*(t17-rotY*rotZ*t10*t12*0.5f)-rot_vec_var(0)*t19*t31-rot_vec_var(2)*t31*t33; - P(2,0) = t35-rotY*rot_vec_var(1)*t3*t11*t30*0.5f+rotX*rot_vec_var(0)*t3*t11*(t15-t23)*0.5f; - P(2,1) = rot_vec_var(2)*(t16-t25)*(t17-t36)-rot_vec_var(0)*t19*t28-rot_vec_var(1)*t28*t30; - P(2,2) = rot_vec_var(1)*(t30*t30)+rot_vec_var(0)*(t37*t37)+rot_vec_var(2)*(t38*t38); - P(2,3) = t42; - P(3,0) = rotZ*rot_vec_var(2)*t3*t11*t33*(-0.5f)+rotX*rot_vec_var(0)*t3*t11*(t16-t25)*0.5f+rotY*rot_vec_var(1)*t3*t11*(t17-t36)*0.5f; - P(3,1) = rot_vec_var(1)*(t15-t23)*(t17-t36)-rot_vec_var(0)*t19*t31-rot_vec_var(2)*t31*t33; - P(3,2) = t42; - P(3,3) = rot_vec_var(2)*(t33*t33)+rot_vec_var(0)*(t43*t43)+rot_vec_var(1)*(t44*t44); - - } else { - // the equations are badly conditioned so use a small angle approximation - P.uncorrelateCovarianceSetVariance<1>(0, 0.0f); - P.uncorrelateCovarianceSetVariance<3>(1, 0.25f * rot_vec_var); - } -} - void Ekf::updateGroundEffect() { if (_control_status.flags.in_air && !_control_status.flags.fixed_wing) { @@ -1040,7 +940,7 @@ void Ekf::updateGroundEffect() void Ekf::increaseQuatYawErrVariance(float yaw_variance) { matrix::SquareMatrix q_cov; - sym::YawVarToLowerTriangularQuatCov(getStateAtFusionHorizonAsVector(), yaw_variance, &q_cov); + sym::RotVarNedToLowerTriangularQuatCov(getStateAtFusionHorizonAsVector(), Vector3f(0.f, 0.f, yaw_variance), &q_cov); q_cov.copyLowerToUpperTriangle(); P.slice<4, 4>(0, 0) += q_cov; } diff --git a/src/modules/ekf2/EKF/mag_fusion.cpp b/src/modules/ekf2/EKF/mag_fusion.cpp index f04a2df8e8..6a1d36dea8 100644 --- a/src/modules/ekf2/EKF/mag_fusion.cpp +++ b/src/modules/ekf2/EKF/mag_fusion.cpp @@ -76,7 +76,7 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo // we need to re-initialise covariances and abort this fusion step if (update_all_states) { - resetQuatCov(); + resetQuatCov(_params.mag_heading_noise); } resetMagCov(); @@ -94,7 +94,7 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo // we need to re-initialise covariances and abort this fusion step if (update_all_states) { - resetQuatCov(); + resetQuatCov(_params.mag_heading_noise); } resetMagCov(); @@ -111,7 +111,7 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo // we need to re-initialise covariances and abort this fusion step if (update_all_states) { - resetQuatCov(); + resetQuatCov(_params.mag_heading_noise); } resetMagCov(); @@ -175,7 +175,7 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo // we need to re-initialise covariances and abort this fusion step if (update_all_states) { - resetQuatCov(); + resetQuatCov(_params.mag_heading_noise); } resetMagCov(); @@ -204,7 +204,7 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo // we need to re-initialise covariances and abort this fusion step if (update_all_states) { - resetQuatCov(); + resetQuatCov(_params.mag_heading_noise); } resetMagCov(); diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py index 83b20495db..13262ba988 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py @@ -511,21 +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( +def rot_var_ned_to_lower_triangular_quat_cov( state: VState, - yaw_var: sf.Scalar + rot_var_ned: sf.V3 ): + # This function converts an attitude variance defined by a 3D vector in NED frame + # into a 4x4 covariance matrix representing the uncertainty on each of the 4 quaternion parameters + # Note: the resulting quaternion uncertainty is defined as a perturbation + # at the tip of the quaternion (i.e.:body-frame uncertainty) 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]) + # Convert uncertainties from NED to body frame + rot_cov_ned = sf.M33.diag(rot_var_ned) 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 + rot_cov_body = adjoint.T * rot_cov_ned * adjoint # Convert yaw (body) to quaternion parameter uncertainty - q_var = J * yaw_cov_body * J.T + q_var = J * rot_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() @@ -552,4 +556,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"]) +generate_px4_function(rot_var_ned_to_lower_triangular_quat_cov, output_names=["q_cov_lower_triangle"]) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/rot_var_ned_to_lower_triangular_quat_cov.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/rot_var_ned_to_lower_triangular_quat_cov.h new file mode 100644 index 0000000000..8b2166f223 --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/rot_var_ned_to_lower_triangular_quat_cov.h @@ -0,0 +1,123 @@ +// ----------------------------------------------------------------------------- +// 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: rot_var_ned_to_lower_triangular_quat_cov + * + * Args: + * state: Matrix24_1 + * rot_var_ned: Matrix31 + * + * Outputs: + * q_cov_lower_triangle: Matrix44 + */ +template +void RotVarNedToLowerTriangularQuatCov( + const matrix::Matrix& state, const matrix::Matrix& rot_var_ned, + matrix::Matrix* const q_cov_lower_triangle = nullptr) { + // Total ops: 185 + + // Input arrays + + // Intermediate terms (54) + const Scalar _tmp0 = 2 * state(0, 0) * state(2, 0); + const Scalar _tmp1 = 2 * state(3, 0); + const Scalar _tmp2 = _tmp1 * state(1, 0); + const Scalar _tmp3 = _tmp0 + _tmp2; + const Scalar _tmp4 = _tmp1 * state(0, 0); + const Scalar _tmp5 = 2 * state(1, 0); + const Scalar _tmp6 = _tmp5 * state(2, 0); + const Scalar _tmp7 = -_tmp4 + _tmp6; + const Scalar _tmp8 = -2 * std::pow(state(3, 0), Scalar(2)); + const Scalar _tmp9 = -2 * std::pow(state(1, 0), Scalar(2)); + const Scalar _tmp10 = _tmp8 + _tmp9 + 1; + const Scalar _tmp11 = _tmp1 * state(2, 0); + const Scalar _tmp12 = _tmp5 * state(0, 0); + const Scalar _tmp13 = _tmp11 - _tmp12; + const Scalar _tmp14 = _tmp13 * rot_var_ned(1, 0); + const Scalar _tmp15 = 1 - 2 * std::pow(state(2, 0), Scalar(2)); + const Scalar _tmp16 = _tmp15 + _tmp9; + const Scalar _tmp17 = _tmp11 + _tmp12; + const Scalar _tmp18 = _tmp17 * rot_var_ned(2, 0); + const Scalar _tmp19 = _tmp10 * _tmp14 + _tmp16 * _tmp18 + _tmp3 * _tmp7 * rot_var_ned(0, 0); + const Scalar _tmp20 = (Scalar(1) / Scalar(2)) * state(3, 0); + const Scalar _tmp21 = -_tmp19 * _tmp20; + const Scalar _tmp22 = std::pow(_tmp10, Scalar(2)) * rot_var_ned(1, 0) + + std::pow(_tmp17, Scalar(2)) * rot_var_ned(2, 0) + + std::pow(_tmp7, Scalar(2)) * rot_var_ned(0, 0); + const Scalar _tmp23 = (Scalar(1) / Scalar(2)) * state(2, 0); + const Scalar _tmp24 = _tmp15 + _tmp8; + const Scalar _tmp25 = _tmp24 * rot_var_ned(0, 0); + const Scalar _tmp26 = _tmp4 + _tmp6; + const Scalar _tmp27 = -_tmp0 + _tmp2; + const Scalar _tmp28 = (Scalar(1) / Scalar(2)) * _tmp10 * _tmp26 * rot_var_ned(1, 0) + + (Scalar(1) / Scalar(2)) * _tmp18 * _tmp27 + + (Scalar(1) / Scalar(2)) * _tmp25 * _tmp7; + const Scalar _tmp29 = _tmp28 * state(1, 0); + const Scalar _tmp30 = std::pow(_tmp24, Scalar(2)) * rot_var_ned(0, 0) + + std::pow(_tmp26, Scalar(2)) * rot_var_ned(1, 0) + + std::pow(_tmp27, Scalar(2)) * rot_var_ned(2, 0); + const Scalar _tmp31 = (Scalar(1) / Scalar(2)) * state(1, 0); + const Scalar _tmp32 = _tmp14 * _tmp26 + _tmp16 * _tmp27 * rot_var_ned(2, 0) + _tmp25 * _tmp3; + const Scalar _tmp33 = _tmp20 * _tmp32; + const Scalar _tmp34 = -_tmp28 * state(2, 0); + const Scalar _tmp35 = _tmp19 * _tmp23; + const Scalar _tmp36 = std::pow(_tmp13, Scalar(2)) * rot_var_ned(1, 0) + + std::pow(_tmp16, Scalar(2)) * rot_var_ned(2, 0) + + std::pow(_tmp3, Scalar(2)) * rot_var_ned(0, 0); + const Scalar _tmp37 = -_tmp31 * _tmp32; + const Scalar _tmp38 = _tmp28 * state(0, 0); + const Scalar _tmp39 = -_tmp20 * _tmp22 + _tmp35 + _tmp38; + const Scalar _tmp40 = (Scalar(1) / Scalar(2)) * state(0, 0); + const Scalar _tmp41 = _tmp23 * _tmp32; + const Scalar _tmp42 = _tmp28 * state(3, 0); + const Scalar _tmp43 = _tmp30 * _tmp40 + _tmp41 - _tmp42; + const Scalar _tmp44 = _tmp32 * _tmp40; + const Scalar _tmp45 = _tmp21 + _tmp23 * _tmp36 + _tmp44; + const Scalar _tmp46 = _tmp19 * _tmp31; + const Scalar _tmp47 = _tmp22 * _tmp40 + _tmp42 - _tmp46; + const Scalar _tmp48 = _tmp20 * _tmp30 + _tmp37 + _tmp38; + const Scalar _tmp49 = _tmp19 * _tmp40; + const Scalar _tmp50 = -_tmp31 * _tmp36 + _tmp33 + _tmp49; + const Scalar _tmp51 = _tmp22 * _tmp31 + _tmp34 + _tmp49; + const Scalar _tmp52 = -_tmp23 * _tmp30 + _tmp29 + _tmp44; + const Scalar _tmp53 = _tmp36 * _tmp40 - _tmp41 + _tmp46; + + // 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) = -_tmp20 * (-_tmp20 * _tmp36 - _tmp35 + _tmp37) - + _tmp23 * (_tmp21 - _tmp22 * _tmp23 - _tmp29) - + _tmp31 * (-_tmp30 * _tmp31 - _tmp33 + _tmp34); + _q_cov_lower_triangle(1, 0) = -_tmp20 * _tmp45 - _tmp23 * _tmp39 - _tmp31 * _tmp43; + _q_cov_lower_triangle(2, 0) = -_tmp20 * _tmp50 - _tmp23 * _tmp47 - _tmp31 * _tmp48; + _q_cov_lower_triangle(3, 0) = -_tmp20 * _tmp53 - _tmp23 * _tmp51 - _tmp31 * _tmp52; + _q_cov_lower_triangle(0, 1) = 0; + _q_cov_lower_triangle(1, 1) = -_tmp20 * _tmp39 + _tmp23 * _tmp45 + _tmp40 * _tmp43; + _q_cov_lower_triangle(2, 1) = -_tmp20 * _tmp47 + _tmp23 * _tmp50 + _tmp40 * _tmp48; + _q_cov_lower_triangle(3, 1) = -_tmp20 * _tmp51 + _tmp23 * _tmp53 + _tmp40 * _tmp52; + _q_cov_lower_triangle(0, 2) = 0; + _q_cov_lower_triangle(1, 2) = 0; + _q_cov_lower_triangle(2, 2) = _tmp20 * _tmp48 - _tmp31 * _tmp50 + _tmp40 * _tmp47; + _q_cov_lower_triangle(3, 2) = _tmp20 * _tmp52 - _tmp31 * _tmp53 + _tmp40 * _tmp51; + _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) = -_tmp23 * _tmp52 + _tmp31 * _tmp51 + _tmp40 * _tmp53; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym 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 deleted file mode 100644 index 687f524627..0000000000 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_var_to_lower_triangular_quat_cov.h +++ /dev/null @@ -1,101 +0,0 @@ -// ----------------------------------------------------------------------------- -// 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 b1a9e14f8a..d851d6a7a6 100644 --- a/src/modules/ekf2/test/test_EKF_attitude_variance.cpp +++ b/src/modules/ekf2/test/test_EKF_attitude_variance.cpp @@ -36,7 +36,7 @@ #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/rot_var_ned_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" @@ -137,11 +137,19 @@ TEST(AttitudeVariance, matlabVsSymforceZeroTilt) void increaseYawVar(const Vector24f &state_vector, SquareMatrix24f &P, const float yaw_var) { SquareMatrix q_cov; - sym::YawVarToLowerTriangularQuatCov(state_vector, yaw_var, &q_cov); + sym::RotVarNedToLowerTriangularQuatCov(state_vector, Vector3f(0.f, 0.f, yaw_var), &q_cov); q_cov.copyLowerToUpperTriangle(); P.slice<4, 4>(0, 0) += q_cov; } +void setTiltVar(const Vector24f &state_vector, SquareMatrix24f &P, const float tilt_var) +{ + SquareMatrix q_cov; + sym::RotVarNedToLowerTriangularQuatCov(state_vector, Vector3f(tilt_var, tilt_var, 0.f), &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; @@ -210,3 +218,55 @@ TEST(AttitudeVariance, increaseYawWithTilt) const float var_2 = getYawVar(state_vector, P); EXPECT_NEAR(var_2, yaw_var_1 + yaw_var_2, 1e-6f); } + +TEST(AttitudeVariance, setRotVarNoTilt) +{ + 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 tilt_var = radians(1.2f); + setTiltVar(state_vector, P, tilt_var); + + Vector3f rot_var; + sym::QuatVarToRotVar(state_vector, P, FLT_EPSILON, &rot_var); + + EXPECT_NEAR(rot_var(0), tilt_var, 1e-6f); + EXPECT_NEAR(rot_var(1), tilt_var, 1e-6f); + EXPECT_EQ(rot_var(2), 0.f); + + // Compare against known values (special case) + EXPECT_EQ(P(0, 0), 0.f); + EXPECT_EQ(P(1, 1), 0.25f * tilt_var); + EXPECT_EQ(P(2, 2), 0.25f * tilt_var); + EXPECT_EQ(P(3, 3), 0.25f * 0.f); // no yaw var +} + +TEST(AttitudeVariance, setRotVarPitch90) +{ + Quatf q(Eulerf(0.f, M_PI_F, 0.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 tilt_var = radians(1.2f); + setTiltVar(state_vector, P, tilt_var); + + Vector3f rot_var; + sym::QuatVarToRotVar(state_vector, P, FLT_EPSILON, &rot_var); + + // TODO: FIXME, due to the nonlinearity of the quaternion parameters, + // setting the variance and getting it back is approximate. + // The correct way would be to keep the uncertainty as a 3D vector in the tangent plane + // instead of converting it to the parameter space + // EXPECT_NEAR(rot_var(0), tilt_var, 1e-6f); + // EXPECT_NEAR(rot_var(1), tilt_var, 1e-6f); + // EXPECT_EQ(rot_var(2), 0.f); +}