Skip to content
This repository has been archived by the owner on May 1, 2024. It is now read-only.

EKF: Convert GPS yaw observation method to use SymPy generated code #880

Merged
merged 7 commits into from Aug 13, 2020
Merged

EKF: Convert GPS yaw observation method to use SymPy generated code #880

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
a05ddcc
EKF: add test comparison program for GPS yaw fusion equations
priseborough Jul 30, 2020
6b389ca
EKF: convert GPS yaw fusion method to use SymPy generated equations
priseborough Jul 30, 2020
768177c
Replace if/else with simple if
kamilritz Aug 1, 2020
e078e0b
EKF: fix formatting of GPS yaw fusion auto-code comparison test
priseborough Aug 4, 2020
8c1c7cf
EKF: Use SparseVector type for GPS yaw fusion Jacobians
priseborough Aug 11, 2020
8e52aed
EKF: Fix bug in GPS yaw derivation comparison check
priseborough Aug 11, 2020
2c4c11b
Add gps_yaw auto generated code file
kamilritz Aug 11, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
197 changes: 78 additions & 119 deletions EKF/gps_yaw_fusion.cpp
View file
Open in desktop
Expand Up @@ -34,6 +34,7 @@
/**
* @file gps_yaw_fusion.cpp
* Definition of functions required to use yaw obtained from GPS dual antenna measurements.
* Equations generated using EKF/python/ekf_derivation/main.py
*
* @author Paul Riseborough <p_riseborough@live.com.au>
*
Expand All @@ -48,10 +49,10 @@
void Ekf::fuseGpsYaw()
{
// 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);
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_delayed.yaw + _gps_yaw_offset);
Expand All @@ -68,91 +69,61 @@ void Ekf::fuseGpsYaw()
// calculate predicted antenna yaw angle
const float predicted_hdg = atan2f(ant_vec_ef(1),ant_vec_ef(0));

// calculate observation jacobian
float t2 = sinf(_gps_yaw_offset);
float t3 = cosf(_gps_yaw_offset);
float t4 = q0*q3*2.0f;
float t5 = q0*q0;
float t6 = q1*q1;
float t7 = q2*q2;
float t8 = q3*q3;
float t9 = q1*q2*2.0f;
float t10 = t5+t6-t7-t8;
float t11 = t3*t10;
float t12 = t4+t9;
float t13 = t3*t12;
float t14 = t5-t6+t7-t8;
float t15 = t2*t14;
float t16 = t13+t15;
float t17 = t4-t9;
float t19 = t2*t17;
float t20 = t11-t19;
float t18 = (t20*t20);
if (t18 < 1e-6f) {
// using magnetic heading process noise
// TODO extend interface to use yaw uncertainty provided by GPS if available
const float R_YAW = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f));

// 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 = powf(q1, 2);
const float HK6 = powf(q2, 2);
const float HK7 = powf(q0, 2) - powf(q3, 2);
const float HK8 = HK4*(HK5 - HK6 + HK7);
const float HK9 = HK3 - HK8;
if (fabsf(HK9) < 1e-3f) {
return;
}
t18 = 1.0f / t18;
float t21 = t16*t16;
float t22 = sq(t11-t19);
if (t22 < 1e-6f) {
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 = powf(HK13, 2)/powf(HK9, 2) + 1;
if (fabsf(HK17) < 1e-3f) {
return;
}
t22 = 1.0f/t22;
float t23 = q1*t3*2.0f;
float t24 = q2*t2*2.0f;
float t25 = t23+t24;
float t26 = 1.0f/t20;
float t27 = q1*t2*2.0f;
float t28 = t21*t22;
float t29 = t28+1.0f;
if (fabsf(t29) < 1e-6f) {
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;
}
float t30 = 1.0f/t29;
float t31 = q0*t3*2.0f;
float t32 = t31-q3*t2*2.0f;
float t33 = q3*t3*2.0f;
float t34 = q0*t2*2.0f;
float t35 = t33+t34;

float H_YAW[4];
H_YAW[0] = (t35/(t11-t2*(t4-q1*q2*2.0f))-t16*t18*t32)/(t18*t21+1.0f);
H_YAW[1] = -t30*(t26*(t27-q2*t3*2.0f)+t16*t22*t25);
H_YAW[2] = t30*(t25*t26-t16*t22*(t27-q2*t3*2.0f));
H_YAW[3] = t30*(t26*t32+t16*t22*t35);

// using magnetic heading tuning parameter
const float R_YAW = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f));

// calculate the innovation and define the innovation gate
const float innov_gate = math::max(_params.heading_innov_gate, 1.0f);
_heading_innov = predicted_hdg - measured_hdg;

// wrap the innovation to the interval between +-pi
_heading_innov = wrap_pi(_heading_innov);

// Calculate innovation variance and Kalman gains, taking advantage of the fact that only the first 3 elements in H are non zero
// calculate the innovation variance
float PH[4];
_heading_innov_var = R_YAW;

for (unsigned row = 0; row <= 3; row++) {
PH[row] = 0.0f;

for (uint8_t col = 0; col <= 3; col++) {
PH[row] += P(row,col) * H_YAW[col];
}

_heading_innov_var += H_YAW[row] * PH[row];
}
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/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
if (_heading_innov_var >= R_YAW) {
// the innovation variance contribution from the state covariances is not negative, no fault
_fault_status.flags.bad_hdg = false;
_heading_innov_var = (-HK16*HK27*HK29 - HK24*HK28*HK29 - HK25*HK29*HK30 + HK26*HK29*HK31 + R_YAW);

} else {
// the innovation variance contribution from the state covariances is negative which means the covariance matrix 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;

// we reinitialise the covariance matrix and abort this fusion step
Expand All @@ -161,29 +132,15 @@ void Ekf::fuseGpsYaw()
return;
}

const float heading_innov_var_inv = 1.f / _heading_innov_var;

// calculate the Kalman gains
// only calculate gains for states we are using
Vector24f Kfusion;

for (uint8_t row = 0; row <= 15; row++) {
for (uint8_t col = 0; col <= 3; col++) {
Kfusion(row) += P(row,col) * H_YAW[col];
}

Kfusion(row) *= heading_innov_var_inv;
}
_fault_status.flags.bad_hdg = false;
const float HK32 = HK18/_heading_innov_var;

if (_control_status.flags.wind) {
for (uint8_t row = 22; row <= 23; row++) {
for (uint8_t col = 0; col <= 3; col++) {
Kfusion(row) += P(row,col) * H_YAW[col];
}
// calculate the innovation and define the innovation gate
const float innov_gate = math::max(_params.heading_innov_gate, 1.0f);
_heading_innov = predicted_hdg - measured_hdg;

Kfusion(row) *= heading_innov_var_inv;
}
}
// wrap the innovation to the interval between +-pi
_heading_innov = wrap_pi(_heading_innov);

// innovation test ratio
_yaw_test_ratio = sq(_heading_innov) / (sq(innov_gate) * _heading_innov_var);
Expand All @@ -210,27 +167,29 @@ void Ekf::fuseGpsYaw()
_innov_check_fail_status.flags.reject_yaw = false;
}

// 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));
}

// apply covariance correction via P_new = (I -K*H)*P
// first calculate expression for KHP
// then calculate P - KHP
SquareMatrix24f KHP;
float KH[4];

for (unsigned row = 0; row < _k_num_states; row++) {

KH[0] = Kfusion(row) * H_YAW[0];
KH[1] = Kfusion(row) * H_YAW[1];
KH[2] = Kfusion(row) * H_YAW[2];
KH[3] = Kfusion(row) * H_YAW[3];

for (unsigned column = 0; column < _k_num_states; column++) {
float tmp = KH[0] * P(0,column);
tmp += KH[1] * P(1,column);
tmp += KH[2] * P(2,column);
tmp += KH[3] * P(3,column);
KHP(row,column) = tmp;
}
}
const SquareMatrix24f KHP = computeKHP(Kfusion, Hfusion);

const bool healthy = checkAndFixCovarianceUpdate(KHP);

Expand Down