mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-29 19:34:07 +08:00
ekf2: migrate GNSS yaw fusion to SymForce
This commit is contained in:
bresch
Daniel Agar
parent
19bca47b9e
commit
eff2c6adcf
@ -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;
|
||||
|
||||
@ -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"])
|
||||
|
||||
@ -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 <matrix/math.hpp>
|
||||
|
||||
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 <typename Scalar>
|
||||
void ComputeGnssYawInnonInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& 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<Scalar, 24, 1>* 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<Scalar, 24, 1>& _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
|
||||
Loading…
x
Reference in New Issue
Block a user