fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 11:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: migrate GNSS yaw fusion to SymForce

Browse Source
This commit is contained in:
bresch
2022-11-22 09:59:31 -05:00
committed by Daniel Agar
parent 19bca47b9e
commit eff2c6adcf
3 changed files with 143 additions and 91 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/gps_yaw_fusion.cpp
+11 -91
View File
@@ -41,99 +41,37 @@
*/
#include "ekf.h"
#include "python/ekf_derivation/generated/compute_gnss_yaw_innon_innov_var_and_h.h"
#include <mathlib/mathlib.h>
#include <cstdlib>
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;