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

EKF: Convert airspeed fusion to use SymPy generated auto-code #890

Merged
merged 4 commits into from Aug 15, 2020
Merged

EKF: Convert airspeed fusion to use SymPy generated auto-code #890

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
e7e511a
EKF: Convert airspeed fusion to use SymPy generated auto-code
priseborough Aug 13, 2020
0d89616
EKF: airspeed fusion cleanup
priseborough Aug 14, 2020
b51ce69
EKF: airspeed fusion derivation test comparison cleanup
priseborough Aug 14, 2020
69d898c
EKF: Only record airspeed fusion event when states are updated
priseborough Aug 14, 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
234 changes: 107 additions & 127 deletions EKF/airspeed_fusion.cpp
View file
Open in desktop
Expand Up @@ -34,6 +34,7 @@
/**
* @file airspeed_fusion.cpp
* airspeed fusion methods.
* equations generated using EKF/python/ekf_derivation/main.py
*
* @author Carl Olsson <carlolsson.co@gmail.com>
* @author Roman Bast <bapstroman@gmail.com>
Expand All @@ -46,152 +47,131 @@

void Ekf::fuseAirspeed()
{
const float vn = _state.vel(0); // Velocity in north direction
const float ve = _state.vel(1); // Velocity in east direction
const float vd = _state.vel(2); // Velocity in downwards direction
const float vwn = _state.wind_vel(0); // Wind speed in north direction
const float vwe = _state.wind_vel(1); // Wind speed in east direction

// Calculate the predicted airspeed
const float v_tas_pred = sqrtf((ve - vwe) * (ve - vwe) + (vn - vwn) * (vn - vwn) + vd * vd);
const float &vn = _state.vel(0); // Velocity in north direction
const float &ve = _state.vel(1); // Velocity in east direction
const float &vd = _state.vel(2); // Velocity in downwards direction
const float &vwn = _state.wind_vel(0); // Wind speed in north direction
const float &vwe = _state.wind_vel(1); // Wind speed in east direction

// Variance for true airspeed measurement - (m/sec)^2
const float R_TAS = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) *
math::constrain(_airspeed_sample_delayed.eas2tas, 0.9f, 10.0f));

// Perform fusion of True Airspeed measurement
if (v_tas_pred > 1.0f) {
// determine if we need the sideslip fusion to correct states other than wind
const bool update_wind_only = !_is_wind_dead_reckoning;

// intermediate variable from algebraic optimisation
float SH_TAS[3];
SH_TAS[0] = 1.0f/v_tas_pred;
SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2.0f*vwe))*0.5f;
SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2.0f*vwn))*0.5f;

// Observation Jacobian
Vector24f H_TAS;
H_TAS(4) = SH_TAS[2];
H_TAS(5) = SH_TAS[1];
H_TAS(6) = vd*SH_TAS[0];
H_TAS(22) = -SH_TAS[2];
H_TAS(23) = -SH_TAS[1];

_airspeed_innov_var = (R_TAS + SH_TAS[2]*(P(4,4)*SH_TAS[2] + P(5,4)*SH_TAS[1] - P(22,4)*SH_TAS[2] - P(23,4)*SH_TAS[1] + P(6,4)*vd*SH_TAS[0]) + SH_TAS[1]*(P(4,5)*SH_TAS[2] + P(5,5)*SH_TAS[1] - P(22,5)*SH_TAS[2] - P(23,5)*SH_TAS[1] + P(6,5)*vd*SH_TAS[0]) - SH_TAS[2]*(P(4,22)*SH_TAS[2] + P(5,22)*SH_TAS[1] - P(22,22)*SH_TAS[2] - P(23,22)*SH_TAS[1] + P(6,22)*vd*SH_TAS[0]) - SH_TAS[1]*(P(4,23)*SH_TAS[2] + P(5,23)*SH_TAS[1] - P(22,23)*SH_TAS[2] - P(23,23)*SH_TAS[1] + P(6,23)*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P(4,6)*SH_TAS[2] + P(5,6)*SH_TAS[1] - P(22,6)*SH_TAS[2] - P(23,6)*SH_TAS[1] + P(6,6)*vd*SH_TAS[0]));

if (_airspeed_innov_var >= R_TAS) { // Check for badly conditioned calculation
_fault_status.flags.bad_airspeed = false;

} else { // Reset the estimator covariance matrix
_fault_status.flags.bad_airspeed = true;

// if we are getting aiding from other sources, warn and reset the wind states and covariances only
const char* action_string = nullptr;
if (update_wind_only) {
resetWindStates();
resetWindCovariance();
action_string = "wind";

} else {
initialiseCovariance();
_state.wind_vel.setZero();
action_string = "full";
}
ECL_ERR("airspeed badly conditioned - %s covariance reset", action_string);

return;
}
// determine if we need the sideslip fusion to correct states other than wind
const bool update_wind_only = !_is_wind_dead_reckoning;

// Intermediate variables
const float HK0 = vn - vwn;
const float HK1 = ve - vwe;
const float HK2 = ecl::powf(HK0, 2) + ecl::powf(HK1, 2) + ecl::powf(vd, 2);
priseborough marked this conversation as resolved.
Show resolved Hide resolved
const float v_tas_pred = sqrtf(HK2); // predicted airspeed
//const float HK3 = powf(HK2, -1.0F/2.0F);
if (v_tas_pred < 1.0f) {
// calculation can be badly conditioned for very low airspeed values so don't fuse this time
return;
}
const float HK3 = 1.0f / v_tas_pred;
const float HK4 = HK0*HK3;
const float HK5 = HK1*HK3;
const float HK6 = 1.0F/HK2;
const float HK7 = HK0*P(4,6) - HK0*P(6,22) + HK1*P(5,6) - HK1*P(6,23) + P(6,6)*vd;
const float HK8 = HK1*P(5,23);
const float HK9 = HK0*P(4,5) - HK0*P(5,22) + HK1*P(5,5) - HK8 + P(5,6)*vd;
const float HK10 = HK1*HK6;
const float HK11 = HK0*P(4,22);
const float HK12 = HK0*P(4,4) - HK1*P(4,23) + HK1*P(4,5) - HK11 + P(4,6)*vd;
const float HK13 = HK0*HK6;
const float HK14 = -HK0*P(22,23) + HK0*P(4,23) - HK1*P(23,23) + HK8 + P(6,23)*vd;
const float HK15 = -HK0*P(22,22) - HK1*P(22,23) + HK1*P(5,22) + HK11 + P(6,22)*vd;
//const float HK16 = HK3/(-HK10*HK14 + HK10*HK9 + HK12*HK13 - HK13*HK15 + HK6*HK7*vd + R_TAS);

// innovation variance - check for badly conditioned calculation
_airspeed_innov_var = (-HK10*HK14 + HK10*HK9 + HK12*HK13 - HK13*HK15 + HK6*HK7*vd + R_TAS);
if (_airspeed_innov_var < R_TAS) { //
// Reset the estimator covariance matrix
// if we are getting aiding from other sources, warn and reset the wind states and covariances only
const char* action_string = nullptr;
if (update_wind_only) {
resetWindStates();
resetWindCovariance();
action_string = "wind";

// Variable used to optimise calculations of the Kalman gain
float SK_TAS[2];
SK_TAS[0] = 1.0f / _airspeed_innov_var;
SK_TAS[1] = SH_TAS[1];

Vector24f Kfusion; // Kalman gain
if (!update_wind_only) {
// we have no other source of aiding, so use airspeed measurements to correct states
Kfusion(0) = SK_TAS[0]*(P(0,4)*SH_TAS[2] - P(0,22)*SH_TAS[2] + P(0,5)*SK_TAS[1] - P(0,23)*SK_TAS[1] + P(0,6)*vd*SH_TAS[0]);
Kfusion(1) = SK_TAS[0]*(P(1,4)*SH_TAS[2] - P(1,22)*SH_TAS[2] + P(1,5)*SK_TAS[1] - P(1,23)*SK_TAS[1] + P(1,6)*vd*SH_TAS[0]);
Kfusion(2) = SK_TAS[0]*(P(2,4)*SH_TAS[2] - P(2,22)*SH_TAS[2] + P(2,5)*SK_TAS[1] - P(2,23)*SK_TAS[1] + P(2,6)*vd*SH_TAS[0]);
Kfusion(3) = SK_TAS[0]*(P(3,4)*SH_TAS[2] - P(3,22)*SH_TAS[2] + P(3,5)*SK_TAS[1] - P(3,23)*SK_TAS[1] + P(3,6)*vd*SH_TAS[0]);
Kfusion(4) = SK_TAS[0]*(P(4,4)*SH_TAS[2] - P(4,22)*SH_TAS[2] + P(4,5)*SK_TAS[1] - P(4,23)*SK_TAS[1] + P(4,6)*vd*SH_TAS[0]);
Kfusion(5) = SK_TAS[0]*(P(5,4)*SH_TAS[2] - P(5,22)*SH_TAS[2] + P(5,5)*SK_TAS[1] - P(5,23)*SK_TAS[1] + P(5,6)*vd*SH_TAS[0]);
Kfusion(6) = SK_TAS[0]*(P(6,4)*SH_TAS[2] - P(6,22)*SH_TAS[2] + P(6,5)*SK_TAS[1] - P(6,23)*SK_TAS[1] + P(6,6)*vd*SH_TAS[0]);
Kfusion(7) = SK_TAS[0]*(P(7,4)*SH_TAS[2] - P(7,22)*SH_TAS[2] + P(7,5)*SK_TAS[1] - P(7,23)*SK_TAS[1] + P(7,6)*vd*SH_TAS[0]);
Kfusion(8) = SK_TAS[0]*(P(8,4)*SH_TAS[2] - P(8,22)*SH_TAS[2] + P(8,5)*SK_TAS[1] - P(8,23)*SK_TAS[1] + P(8,6)*vd*SH_TAS[0]);
Kfusion(9) = SK_TAS[0]*(P(9,4)*SH_TAS[2] - P(9,22)*SH_TAS[2] + P(9,5)*SK_TAS[1] - P(9,23)*SK_TAS[1] + P(9,6)*vd*SH_TAS[0]);
Kfusion(10) = SK_TAS[0]*(P(10,4)*SH_TAS[2] - P(10,22)*SH_TAS[2] + P(10,5)*SK_TAS[1] - P(10,23)*SK_TAS[1] + P(10,6)*vd*SH_TAS[0]);
Kfusion(11) = SK_TAS[0]*(P(11,4)*SH_TAS[2] - P(11,22)*SH_TAS[2] + P(11,5)*SK_TAS[1] - P(11,23)*SK_TAS[1] + P(11,6)*vd*SH_TAS[0]);
Kfusion(12) = SK_TAS[0]*(P(12,4)*SH_TAS[2] - P(12,22)*SH_TAS[2] + P(12,5)*SK_TAS[1] - P(12,23)*SK_TAS[1] + P(12,6)*vd*SH_TAS[0]);
Kfusion(13) = SK_TAS[0]*(P(13,4)*SH_TAS[2] - P(13,22)*SH_TAS[2] + P(13,5)*SK_TAS[1] - P(13,23)*SK_TAS[1] + P(13,6)*vd*SH_TAS[0]);
Kfusion(14) = SK_TAS[0]*(P(14,4)*SH_TAS[2] - P(14,22)*SH_TAS[2] + P(14,5)*SK_TAS[1] - P(14,23)*SK_TAS[1] + P(14,6)*vd*SH_TAS[0]);
Kfusion(15) = SK_TAS[0]*(P(15,4)*SH_TAS[2] - P(15,22)*SH_TAS[2] + P(15,5)*SK_TAS[1] - P(15,23)*SK_TAS[1] + P(15,6)*vd*SH_TAS[0]);
Kfusion(16) = SK_TAS[0]*(P(16,4)*SH_TAS[2] - P(16,22)*SH_TAS[2] + P(16,5)*SK_TAS[1] - P(16,23)*SK_TAS[1] + P(16,6)*vd*SH_TAS[0]);
Kfusion(17) = SK_TAS[0]*(P(17,4)*SH_TAS[2] - P(17,22)*SH_TAS[2] + P(17,5)*SK_TAS[1] - P(17,23)*SK_TAS[1] + P(17,6)*vd*SH_TAS[0]);
Kfusion(18) = SK_TAS[0]*(P(18,4)*SH_TAS[2] - P(18,22)*SH_TAS[2] + P(18,5)*SK_TAS[1] - P(18,23)*SK_TAS[1] + P(18,6)*vd*SH_TAS[0]);
Kfusion(19) = SK_TAS[0]*(P(19,4)*SH_TAS[2] - P(19,22)*SH_TAS[2] + P(19,5)*SK_TAS[1] - P(19,23)*SK_TAS[1] + P(19,6)*vd*SH_TAS[0]);
Kfusion(20) = SK_TAS[0]*(P(20,4)*SH_TAS[2] - P(20,22)*SH_TAS[2] + P(20,5)*SK_TAS[1] - P(20,23)*SK_TAS[1] + P(20,6)*vd*SH_TAS[0]);
Kfusion(21) = SK_TAS[0]*(P(21,4)*SH_TAS[2] - P(21,22)*SH_TAS[2] + P(21,5)*SK_TAS[1] - P(21,23)*SK_TAS[1] + P(21,6)*vd*SH_TAS[0]);
} else {
initialiseCovariance();
_state.wind_vel.setZero();
action_string = "full";
}
Kfusion(22) = SK_TAS[0]*(P(22,4)*SH_TAS[2] - P(22,22)*SH_TAS[2] + P(22,5)*SK_TAS[1] - P(22,23)*SK_TAS[1] + P(22,6)*vd*SH_TAS[0]);
Kfusion(23) = SK_TAS[0]*(P(23,4)*SH_TAS[2] - P(23,22)*SH_TAS[2] + P(23,5)*SK_TAS[1] - P(23,23)*SK_TAS[1] + P(23,6)*vd*SH_TAS[0]);
ECL_ERR("airspeed badly conditioned - %s covariance reset", action_string);

// Calculate measurement innovation
_airspeed_innov = v_tas_pred -
_airspeed_sample_delayed.true_airspeed;
_fault_status.flags.bad_airspeed = true;

// Compute the ratio of innovation to gate size
_tas_test_ratio = sq(_airspeed_innov) / (sq(fmaxf(_params.tas_innov_gate, 1.0f)) * _airspeed_innov_var);
return;
}
const float HK16 = HK3 / _airspeed_innov_var;
_fault_status.flags.bad_airspeed = false;

// Observation Jacobians
SparseVector24f<4,5,6,22,23> Hfusion;
Hfusion.at<4>() = HK4;
Hfusion.at<5>() = HK5;
Hfusion.at<6>() = HK3*vd;
Hfusion.at<22>() = -HK4;
Hfusion.at<23>() = -HK5;

Vector24f Kfusion; // Kalman gain vector
if (!update_wind_only) {
// we have no other source of aiding, so use airspeed measurements to correct states
for (unsigned row = 0; row <= 3; row++) {
Kfusion(row) = HK16*(HK0*P(4,row) - HK0*P(row,22) + HK1*P(5,row) - HK1*P(row,23) + P(6,row)*vd);
}

// If the innovation consistency check fails then don't fuse the sample and indicate bad airspeed health
if (_tas_test_ratio > 1.0f) {
_innov_check_fail_status.flags.reject_airspeed = true;
return;
Kfusion(4) = HK12*HK16;
Kfusion(5) = HK16*HK9;
Kfusion(6) = HK16*HK7;

} else {
_innov_check_fail_status.flags.reject_airspeed = false;
for (unsigned row = 7; row <= 21; row++) {
Kfusion(row) = HK16*(HK0*P(4,row) - HK0*P(row,22) + HK1*P(5,row) - HK1*P(row,23) + P(6,row)*vd);
}
}

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

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

KH[0] = Kfusion(row) * H_TAS(4);
KH[1] = Kfusion(row) * H_TAS(5);
KH[2] = Kfusion(row) * H_TAS(6);
KH[3] = Kfusion(row) * H_TAS(22);
KH[4] = Kfusion(row) * H_TAS(23);

for (unsigned column = 0; column < _k_num_states; column++) {
float tmp = KH[0] * P(4,column);
tmp += KH[1] * P(5,column);
tmp += KH[2] * P(6,column);
tmp += KH[3] * P(22,column);
tmp += KH[4] * P(23,column);
KHP(row,column) = tmp;
}
}
Kfusion(22) = HK15*HK16;
Kfusion(23) = HK14*HK16;

const bool healthy = checkAndFixCovarianceUpdate(KHP);
// Calculate measurement innovation
_airspeed_innov = v_tas_pred - _airspeed_sample_delayed.true_airspeed;

_fault_status.flags.bad_airspeed = !healthy;
// Compute the ratio of innovation to gate size
_tas_test_ratio = sq(_airspeed_innov) / (sq(fmaxf(_params.tas_innov_gate, 1.0f)) * _airspeed_innov_var);

if (healthy) {
// apply the covariance corrections
P -= KHP;
// If the innovation consistency check fails then don't fuse the sample and indicate bad airspeed health
if (_tas_test_ratio > 1.0f) {
_innov_check_fail_status.flags.reject_airspeed = true;
return;

fixCovarianceErrors(true);
} else {
_innov_check_fail_status.flags.reject_airspeed = false;
}

// apply the state corrections
fuse(Kfusion, _airspeed_innov);
// apply covariance correction via P_new = (I -K*H)*P
// first calculate expression for KHP
// then calculate P - KHP
const SquareMatrix24f KHP = computeKHP(Kfusion, Hfusion);

const bool healthy = checkAndFixCovarianceUpdate(KHP);

_fault_status.flags.bad_airspeed = !healthy;

if (healthy) {
// apply the covariance corrections
P -= KHP;

fixCovarianceErrors(true);

// apply the state corrections
fuse(Kfusion, _airspeed_innov);

_time_last_arsp_fuse = _time_last_imu;

_time_last_arsp_fuse = _time_last_imu;
}
}
}

Expand Down