From eff2c6adcfcb00ebef7829fa778a6cf035ed8df0 Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 17 Nov 2022 12:11:56 +0100 Subject: [PATCH] ekf2: migrate GNSS yaw fusion to SymForce --- src/modules/ekf2/EKF/gps_yaw_fusion.cpp | 102 ++--------------- .../EKF/python/ekf_derivation/derivation.py | 29 +++++ .../compute_gnss_yaw_innon_innov_var_and_h.h | 103 ++++++++++++++++++ 3 files changed, 143 insertions(+), 91 deletions(-) create mode 100644 src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gnss_yaw_innon_innov_var_and_h.h diff --git a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp index c5017d5fed..b2847984aa 100644 --- a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp +++ b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp @@ -41,99 +41,37 @@ */ #include "ekf.h" +#include "python/ekf_derivation/generated/compute_gnss_yaw_innon_innov_var_and_h.h" #include #include void Ekf::fuseGpsYaw(const gpsSample& gps_sample) { - // assign intermediate state variables - const float q0 = _state.quat_nominal(0); - const float q1 = _state.quat_nominal(1); - const float q2 = _state.quat_nominal(2); - const float q3 = _state.quat_nominal(3); - // calculate the observed yaw angle of antenna array, converting a from body to antenna yaw measurement const float measured_hdg = wrap_pi(gps_sample.yaw + _gps_yaw_offset); - // define the predicted antenna array vector and rotate into earth frame - const Vector3f ant_vec_bf = {cosf(_gps_yaw_offset), sinf(_gps_yaw_offset), 0.0f}; - const Vector3f ant_vec_ef = _R_to_earth * ant_vec_bf; - - // calculate predicted antenna yaw angle - const float predicted_hdg = atan2f(ant_vec_ef(1), ant_vec_ef(0)); - - // wrap the innovation to the interval between +-pi - const float heading_innov = wrap_pi(predicted_hdg - measured_hdg); - // using magnetic heading process noise // TODO extend interface to use yaw uncertainty provided by GPS if available const float R_YAW = sq(fmaxf(_params.gps_heading_noise, 1.0e-2f)); + float heading_innov; + float heading_innov_var; + Vector24f H; + + sym::ComputeGnssYawInnonInnovVarAndH(getStateAtFusionHorizonAsVector(), P, _gps_yaw_offset, measured_hdg, R_YAW, FLT_EPSILON, &heading_innov, &heading_innov_var, &H); + + const SparseVector24f<0,1,2,3> Hfusion(H); + _aid_src_gnss_yaw.timestamp_sample = gps_sample.time_us; _aid_src_gnss_yaw.observation = measured_hdg; _aid_src_gnss_yaw.observation_variance = R_YAW; _aid_src_gnss_yaw.innovation = heading_innov; _aid_src_gnss_yaw.fusion_enabled = _control_status.flags.gps_yaw; - // check if antenna array vector is within 30 degrees of vertical and therefore unable to provide a reliable heading - if (fabsf(ant_vec_ef(2)) > cosf(math::radians(30.0f))) { - return; - } - - // calculate intermediate variables - const float HK0 = sinf(_gps_yaw_offset); - const float HK1 = q0*q3; - const float HK2 = q1*q2; - const float HK3 = 2*HK0*(HK1 - HK2); - const float HK4 = cosf(_gps_yaw_offset); - const float HK5 = ecl::powf(q1, 2); - const float HK6 = ecl::powf(q2, 2); - const float HK7 = ecl::powf(q0, 2) - ecl::powf(q3, 2); - const float HK8 = HK4*(HK5 - HK6 + HK7); - const float HK9 = HK3 - HK8; - - if (fabsf(HK9) < 1e-3f) { - return; - } - const float HK10 = 1.0F/HK9; - const float HK11 = HK4*q0; - const float HK12 = HK0*q3; - const float HK13 = HK0*(-HK5 + HK6 + HK7) + 2*HK4*(HK1 + HK2); - const float HK14 = HK10*HK13; - const float HK15 = HK0*q0 + HK4*q3; - const float HK16 = HK10*(HK14*(HK11 - HK12) + HK15); - const float HK17 = ecl::powf(HK13, 2)/ecl::powf(HK9, 2) + 1; - if (fabsf(HK17) < 1e-3f) { - return; - } - const float HK18 = 2/HK17; - // const float HK19 = 1.0F/(-HK3 + HK8); - const float HK19_inverse = -HK3 + HK8; - - if (fabsf(HK19_inverse) < 1e-6f) { - return; - } - const float HK19 = 1.0F/HK19_inverse; - const float HK20 = HK4*q1; - const float HK21 = HK0*q2; - const float HK22 = HK13*HK19; - const float HK23 = HK0*q1 - HK4*q2; - const float HK24 = HK19*(HK22*(HK20 + HK21) + HK23); - const float HK25 = HK19*(-HK20 - HK21 + HK22*HK23); - const float HK26 = HK10*(-HK11 + HK12 + HK14*HK15); - const float HK27 = -HK16*P(0,0) - HK24*P(0,1) - HK25*P(0,2) + HK26*P(0,3); - const float HK28 = -HK16*P(0,1) - HK24*P(1,1) - HK25*P(1,2) + HK26*P(1,3); - const float HK29 = 4/ecl::powf(HK17, 2); - const float HK30 = -HK16*P(0,2) - HK24*P(1,2) - HK25*P(2,2) + HK26*P(2,3); - const float HK31 = -HK16*P(0,3) - HK24*P(1,3) - HK25*P(2,3) + HK26*P(3,3); - // const float HK32 = HK18/(-HK16*HK27*HK29 - HK24*HK28*HK29 - HK25*HK29*HK30 + HK26*HK29*HK31 + R_YAW); - - // check if the innovation variance calculation is badly conditioned - const float heading_innov_var = (-HK16*HK27*HK29 - HK24*HK28*HK29 - HK25*HK29*HK30 + HK26*HK29*HK31 + R_YAW); - _aid_src_gnss_yaw.innovation_variance = heading_innov_var; + // check if the innovation variance calculation is badly conditioned if (heading_innov_var < R_YAW) { // the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned _fault_status.flags.bad_hdg = true; @@ -145,12 +83,9 @@ void Ekf::fuseGpsYaw(const gpsSample& gps_sample) } _fault_status.flags.bad_hdg = false; - const float HK32 = HK18 / heading_innov_var; - // calculate the innovation and define the innovation gate const float innov_gate = math::max(_params.heading_innov_gate, 1.0f); - // innovation test ratio setEstimatorAidStatusTestRatio(_aid_src_gnss_yaw, innov_gate); if (_aid_src_gnss_yaw.innovation_rejected) { @@ -172,24 +107,9 @@ void Ekf::fuseGpsYaw(const gpsSample& gps_sample) resetZDeltaAngBiasCov(); } - // calculate observation jacobian - // Observation jacobian and Kalman gain vectors - SparseVector24f<0,1,2,3> Hfusion; - Hfusion.at<0>() = -HK16*HK18; - Hfusion.at<1>() = -HK18*HK24; - Hfusion.at<2>() = -HK18*HK25; - Hfusion.at<3>() = HK18*HK26; - // calculate the Kalman gains // only calculate gains for states we are using - Vector24f Kfusion; - Kfusion(0) = HK27*HK32; - Kfusion(1) = HK28*HK32; - Kfusion(2) = HK30*HK32; - Kfusion(3) = HK31*HK32; - for (unsigned row = 4; row <= 23; row++) { - Kfusion(row) = HK32*(-HK16*P(0,row) - HK24*P(1,row) - HK25*P(2,row) + HK26*P(3,row)); - } + Vector24f Kfusion = P * Hfusion / _aid_src_gnss_yaw.innovation_variance; const bool is_fused = measurementUpdate(Kfusion, Hfusion, heading_innov); _fault_status.flags.bad_hdg = !is_fused; diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py index 003543dfa0..3a09abaaa6 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py @@ -387,6 +387,34 @@ def compute_flow_y_innov_var_and_h( return (innov_var, Hy.T) +def compute_gnss_yaw_innon_innov_var_and_h( + state: VState, + P: MState, + antenna_yaw_offset: sf.Scalar, + meas: sf.Scalar, + R: sf.Scalar, + epsilon: sf.Scalar +) -> (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) + + # define antenna vector in body frame + ant_vec_bf = sf.V3(sf.cos(antenna_yaw_offset), sf.sin(antenna_yaw_offset), 0) + + # rotate into earth frame + ant_vec_ef = R_to_earth * ant_vec_bf + + # Calculate the yaw angle from the projection + meas_pred = sf.atan2(ant_vec_ef[1], ant_vec_ef[0], epsilon=epsilon) + + H = sf.V1(meas_pred).jacobian(state) + innov_var = (H * P * H.T + R)[0,0] + + innov = meas_pred - meas + + 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"]) @@ -404,3 +432,4 @@ generate_px4_function(compute_yaw_312_innov_var_and_h_alternate, output_names=[" generate_px4_function(compute_mag_declination_innov_innov_var_and_h, output_names=["innov", "innov_var", "H"]) generate_px4_function(compute_flow_xy_innov_var_and_hx, output_names=["innov_var", "H"]) generate_px4_function(compute_flow_y_innov_var_and_h, output_names=["innov_var", "H"]) +generate_px4_function(compute_gnss_yaw_innon_innov_var_and_h, output_names=["innov", "innov_var", "H"]) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gnss_yaw_innon_innov_var_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gnss_yaw_innon_innov_var_and_h.h new file mode 100644 index 0000000000..be89340b42 --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gnss_yaw_innon_innov_var_and_h.h @@ -0,0 +1,103 @@ +// ----------------------------------------------------------------------------- +// 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_gnss_yaw_innon_innov_var_and_h + * + * Args: + * state: Matrix24_1 + * P: Matrix24_24 + * antenna_yaw_offset: Scalar + * meas: Scalar + * R: Scalar + * epsilon: Scalar + * + * Outputs: + * innov: Scalar + * innov_var: Scalar + * H: Matrix24_1 + */ +template +void ComputeGnssYawInnonInnovVarAndH(const matrix::Matrix& state, + const matrix::Matrix& P, + const Scalar antenna_yaw_offset, 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: 106 + + // Input arrays + + // Intermediate terms (28) + const Scalar _tmp0 = std::pow(state(2, 0), Scalar(2)); + const Scalar _tmp1 = std::pow(state(1, 0), Scalar(2)); + const Scalar _tmp2 = std::pow(state(0, 0), Scalar(2)) - std::pow(state(3, 0), Scalar(2)); + const Scalar _tmp3 = std::sin(antenna_yaw_offset); + const Scalar _tmp4 = state(0, 0) * state(3, 0); + const Scalar _tmp5 = state(1, 0) * state(2, 0); + const Scalar _tmp6 = std::cos(antenna_yaw_offset); + const Scalar _tmp7 = _tmp3 * (_tmp0 - _tmp1 + _tmp2) + 2 * _tmp6 * (_tmp4 + _tmp5); + const Scalar _tmp8 = 2 * _tmp3 * (-_tmp4 + _tmp5) + _tmp6 * (-_tmp0 + _tmp1 + _tmp2); + const Scalar _tmp9 = _tmp8 + epsilon * ((((_tmp8) > 0) - ((_tmp8) < 0)) + Scalar(0.5)); + const Scalar _tmp10 = 2 * state(3, 0); + const Scalar _tmp11 = 2 * state(0, 0); + const Scalar _tmp12 = -_tmp10 * _tmp3 + _tmp11 * _tmp6; + const Scalar _tmp13 = Scalar(1.0) / (_tmp9); + const Scalar _tmp14 = _tmp10 * _tmp6; + const Scalar _tmp15 = _tmp11 * _tmp3; + const Scalar _tmp16 = std::pow(_tmp9, Scalar(2)); + const Scalar _tmp17 = _tmp7 / _tmp16; + const Scalar _tmp18 = _tmp16 / (_tmp16 + std::pow(_tmp7, Scalar(2))); + const Scalar _tmp19 = _tmp18 * (_tmp12 * _tmp13 - _tmp17 * (-_tmp14 - _tmp15)); + const Scalar _tmp20 = 2 * state(1, 0); + const Scalar _tmp21 = 2 * state(2, 0); + const Scalar _tmp22 = _tmp20 * _tmp6 + _tmp21 * _tmp3; + const Scalar _tmp23 = _tmp20 * _tmp3; + const Scalar _tmp24 = _tmp21 * _tmp6; + const Scalar _tmp25 = _tmp18 * (_tmp13 * (-_tmp23 + _tmp24) - _tmp17 * _tmp22); + const Scalar _tmp26 = _tmp18 * (-_tmp12 * _tmp17 + _tmp13 * (_tmp14 + _tmp15)); + const Scalar _tmp27 = _tmp18 * (_tmp13 * _tmp22 - _tmp17 * (_tmp23 - _tmp24)); + + // Output terms (3) + if (innov != nullptr) { + Scalar& _innov = (*innov); + + _innov = -meas + std::atan2(_tmp7, _tmp9); + } + + if (innov_var != nullptr) { + Scalar& _innov_var = (*innov_var); + + _innov_var = + R + _tmp19 * (P(0, 3) * _tmp26 + P(1, 3) * _tmp25 + P(2, 3) * _tmp27 + P(3, 3) * _tmp19) + + _tmp25 * (P(0, 1) * _tmp26 + P(1, 1) * _tmp25 + P(2, 1) * _tmp27 + P(3, 1) * _tmp19) + + _tmp26 * (P(0, 0) * _tmp26 + P(1, 0) * _tmp25 + P(2, 0) * _tmp27 + P(3, 0) * _tmp19) + + _tmp27 * (P(0, 2) * _tmp26 + P(1, 2) * _tmp25 + P(2, 2) * _tmp27 + P(3, 2) * _tmp19); + } + + if (H != nullptr) { + matrix::Matrix& _H = (*H); + + _H.setZero(); + + _H(0, 0) = _tmp26; + _H(1, 0) = _tmp25; + _H(2, 0) = _tmp27; + _H(3, 0) = _tmp19; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym