mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-02 03:24:07 +08:00
EKF: Convert airspeed fusion to use SymPy generated auto-code
This commit is contained in:
Paul Riseborough
Paul Riseborough
parent
5907e81ef7
commit
a5a43dbc6c
@ -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>
|
||||
@ -46,152 +47,137 @@
|
||||
|
||||
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
|
||||
SparseVector24f<4,5,6,22,23> Hfusion; // Observation Jacobians
|
||||
Vector24f Kfusion; // Kalman gain vector
|
||||
|
||||
// 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;
|
||||
// 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;
|
||||
// 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);
|
||||
if (HK2 < 1.0f) {
|
||||
// calculation can be badly conditioned for very low airspeed values so don't fuse this time
|
||||
return;
|
||||
}
|
||||
const float v_tas_pred = sqrtf(HK2); // predicted airspeed
|
||||
//const float HK3 = powf(HK2, -1.0F/2.0F);
|
||||
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);
|
||||
float HK16;
|
||||
|
||||
// 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];
|
||||
// innovation variance
|
||||
_airspeed_innov_var = (-HK10*HK14 + HK10*HK9 + HK12*HK13 - HK13*HK15 + HK6*HK7*vd + R_TAS);
|
||||
if (_airspeed_innov_var >= R_TAS) { // Check for badly conditioned calculation
|
||||
HK16 = HK3 / _airspeed_innov_var;
|
||||
_fault_status.flags.bad_airspeed = false;
|
||||
|
||||
_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]));
|
||||
} else { // Reset the estimator covariance matrix
|
||||
_fault_status.flags.bad_airspeed = true;
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
// 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]);
|
||||
}
|
||||
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]);
|
||||
|
||||
// Calculate measurement innovation
|
||||
_airspeed_innov = v_tas_pred -
|
||||
_airspeed_sample_delayed.true_airspeed;
|
||||
|
||||
// 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 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;
|
||||
// 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 {
|
||||
_innov_check_fail_status.flags.reject_airspeed = false;
|
||||
initialiseCovariance();
|
||||
_state.wind_vel.setZero();
|
||||
action_string = "full";
|
||||
}
|
||||
ECL_ERR("airspeed badly conditioned - %s covariance reset", action_string);
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
// Observation Jacobians
|
||||
Hfusion.at<4>() = HK4;
|
||||
Hfusion.at<5>() = HK5;
|
||||
Hfusion.at<6>() = HK3*vd;
|
||||
Hfusion.at<22>() = -HK4;
|
||||
Hfusion.at<23>() = -HK5;
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
// apply covariance correction via P_new = (I -K*H)*P
|
||||
// first calculate expression for KHP
|
||||
// then calculate P - KHP
|
||||
SquareMatrix24f KHP;
|
||||
float KH[5];
|
||||
Kfusion(4) = HK12*HK16;
|
||||
Kfusion(5) = HK16*HK9;
|
||||
Kfusion(6) = HK16*HK7;
|
||||
|
||||
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;
|
||||
}
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
const bool healthy = checkAndFixCovarianceUpdate(KHP);
|
||||
Kfusion(22) = HK15*HK16;
|
||||
Kfusion(23) = HK14*HK16;
|
||||
|
||||
_fault_status.flags.bad_airspeed = !healthy;
|
||||
// Calculate measurement innovation
|
||||
_airspeed_innov = v_tas_pred - _airspeed_sample_delayed.true_airspeed;
|
||||
|
||||
if (healthy) {
|
||||
// apply the covariance corrections
|
||||
P -= KHP;
|
||||
// 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);
|
||||
|
||||
fixCovarianceErrors(true);
|
||||
// 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;
|
||||
|
||||
// apply the state corrections
|
||||
fuse(Kfusion, _airspeed_innov);
|
||||
} else {
|
||||
_innov_check_fail_status.flags.reject_airspeed = false;
|
||||
}
|
||||
|
||||
// Airspeed measurement sample has passed check so record it
|
||||
_time_last_arsp_fuse = _time_last_imu;
|
||||
|
||||
// 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -0,0 +1,228 @@
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <cstdlib>
|
||||
#include "../../../../../matrix/matrix/math.hpp"
|
||||
#include "util.h"
|
||||
|
||||
typedef matrix::Vector<float, 24> Vector24f;
|
||||
typedef matrix::SquareMatrix<float, 24> SquareMatrix24f;
|
||||
template<int ... Idxs>
|
||||
using SparseVector24f = matrix::SparseVectorf<24, Idxs...>;
|
||||
|
||||
int main()
|
||||
{
|
||||
// Compare calculation of observation Jacobians and Kalman gains for sympy and matlab generated equations
|
||||
|
||||
SparseVector24f<4,5,6,22,23> Hfusion;
|
||||
Vector24f H_TAS;
|
||||
Vector24f Kfusion;
|
||||
float airspeed_innov_var;
|
||||
|
||||
Vector24f Hfusion_sympy;
|
||||
Vector24f Kfusion_sympy;
|
||||
|
||||
Vector24f Hfusion_matlab;
|
||||
Vector24f Kfusion_matlab;
|
||||
|
||||
const float R_TAS = sq(1.5f);
|
||||
|
||||
const bool update_wind_only = false;
|
||||
|
||||
// get latest velocity in earth frame
|
||||
const float vn = 9.0f;
|
||||
const float ve = 12.0f;
|
||||
const float vd = -1.5f;
|
||||
|
||||
// get latest wind velocity in earth frame
|
||||
const float vwn = -4.0f;
|
||||
const float vwe = 3.0f;
|
||||
|
||||
// create a symmetrical positive dfinite matrix with off diagonals between -1 and 1 and diagonals between 0 and 1
|
||||
SquareMatrix24f P;
|
||||
for (int col=0; col<=23; col++) {
|
||||
for (int row=0; row<=col; row++) {
|
||||
if (row == col) {
|
||||
P(row,col) = (float)rand();
|
||||
} else {
|
||||
P(col,row) = P(row,col) = 2.0f * ((float)rand() - 0.5f);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// First calculate observationjacobians and Kalman gains using sympy generated equations
|
||||
|
||||
{
|
||||
// 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);
|
||||
if (HK2 < 1.0f) {
|
||||
// calculation can be badly conditioned for very low airspeed values so don't fuse this time
|
||||
return 0;
|
||||
}
|
||||
const float v_tas_pred = sqrtf(HK2); // predicted airspeed
|
||||
//const float HK3 = powf(HK2, -1.0F/2.0F);
|
||||
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);
|
||||
|
||||
// Observation Jacobians
|
||||
memset(&Hfusion, 0, sizeof(Hfusion));
|
||||
Hfusion.at<4>() = HK4;
|
||||
Hfusion.at<5>() = HK5;
|
||||
Hfusion.at<6>() = HK3*vd;
|
||||
Hfusion.at<22>() = -HK4;
|
||||
Hfusion.at<23>() = -HK5;
|
||||
|
||||
if (true) {
|
||||
// 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);
|
||||
}
|
||||
|
||||
Kfusion(4) = HK12*HK16;
|
||||
Kfusion(5) = HK16*HK9;
|
||||
Kfusion(6) = HK16*HK7;
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
Kfusion(22) = HK15*HK16;
|
||||
Kfusion(23) = HK14*HK16;
|
||||
|
||||
// save output
|
||||
Hfusion_sympy(4) = Hfusion.at<4>();
|
||||
Hfusion_sympy(5) = Hfusion.at<5>();
|
||||
Hfusion_sympy(6) = Hfusion.at<6>();
|
||||
Hfusion_sympy(22) = Hfusion.at<22>();
|
||||
Hfusion_sympy(23) = Hfusion.at<23>();
|
||||
Kfusion_sympy = Kfusion;
|
||||
}
|
||||
// repeat calculation using matlab generated equations
|
||||
{
|
||||
const float v_tas_pred = sqrtf((ve - vwe) * (ve - vwe) + (vn - vwn) * (vn - vwn) + vd * vd);
|
||||
|
||||
// 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
|
||||
memset(&H_TAS, 0, sizeof(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]));
|
||||
|
||||
float SK_TAS[2];
|
||||
SK_TAS[0] = 1.0f / airspeed_innov_var;
|
||||
SK_TAS[1] = SH_TAS[1];
|
||||
|
||||
// Kalman gain
|
||||
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]);
|
||||
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]);
|
||||
|
||||
// save output;
|
||||
Hfusion_matlab = H_TAS;
|
||||
Kfusion_matlab = Kfusion;
|
||||
}
|
||||
|
||||
// find largest observation variance difference as a fraction of the matlab value
|
||||
float max_diff_fraction = 0.0f;
|
||||
int max_row;
|
||||
float max_old, max_new;
|
||||
for (int row=0; row<24; row++) {
|
||||
float diff_fraction;
|
||||
if (Hfusion_matlab(row) != 0.0f) {
|
||||
diff_fraction = fabsf(Hfusion_sympy(row) - Hfusion_matlab(row)) / fabsf(Hfusion_matlab(row));
|
||||
} else if (Hfusion_sympy(row) != 0.0f) {
|
||||
diff_fraction = fabsf(Hfusion_sympy(row) - Hfusion_matlab(row)) / fabsf(Hfusion_sympy(row));
|
||||
} else {
|
||||
diff_fraction = 0.0f;
|
||||
}
|
||||
if (Hfusion_sympy(row) - H_TAS(row) != 0.0f) {
|
||||
printf("new,old Hfusion(%i) = %e,%e\n",row,Hfusion_sympy(row),H_TAS(row));
|
||||
}
|
||||
if (diff_fraction > max_diff_fraction) {
|
||||
max_diff_fraction = diff_fraction;
|
||||
max_row = row;
|
||||
max_old = H_TAS(row);
|
||||
max_new = Hfusion_sympy(row);
|
||||
}
|
||||
}
|
||||
|
||||
if (max_diff_fraction > 1E-5f) {
|
||||
printf("Fail: Airspeed Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
|
||||
} else {
|
||||
printf("Pass: Airspeed Hfusion max diff fraction = %e\n",max_diff_fraction);
|
||||
}
|
||||
|
||||
// find largest Kalman gain difference as a fraction of the matlab value
|
||||
max_diff_fraction = 0.0f;
|
||||
for (int row=0; row<4; row++) {
|
||||
float diff_fraction;
|
||||
if (Kfusion(row) != 0.0f) {
|
||||
diff_fraction = fabsf(Kfusion_sympy(row) - Kfusion(row)) / fabsf(Kfusion(row));
|
||||
} else if (Kfusion_sympy(row) != 0.0f) {
|
||||
diff_fraction = fabsf(Kfusion_sympy(row) - Kfusion(row)) / fabsf(Kfusion_sympy(row));
|
||||
} else {
|
||||
diff_fraction = 0.0f;
|
||||
}
|
||||
// if (Kfusion_sympy(row) - Kfusion(row) != 0.0f) {
|
||||
// printf("new,old Kfusion(%i) = %e,%e\n",row,Kfusion_sympy(row),Kfusion(row));
|
||||
// }
|
||||
if (diff_fraction > max_diff_fraction) {
|
||||
max_diff_fraction = diff_fraction;
|
||||
max_row = row;
|
||||
max_old = Kfusion(row);
|
||||
max_new = Kfusion_sympy(row);
|
||||
}
|
||||
}
|
||||
|
||||
if (max_diff_fraction > 1E-5f) {
|
||||
printf("Fail: Airspeed Kfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
|
||||
} else {
|
||||
printf("Pass: Airspeed Kfusion max diff fraction = %e\n",max_diff_fraction);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
74
EKF/python/ekf_derivation/generated/tas_generated.cpp
Normal file
74
EKF/python/ekf_derivation/generated/tas_generated.cpp
Normal file
@ -0,0 +1,74 @@
|
||||
// Sub Expressions
|
||||
const float HK0 = vn - vwn;
|
||||
const float HK1 = ve - vwe;
|
||||
const float HK2 = powf(HK0, 2) + powf(HK1, 2) + powf(vd, 2);
|
||||
const float HK3 = powf(HK2, -1.0F/2.0F);
|
||||
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);
|
||||
|
||||
|
||||
// Observation Jacobians
|
||||
Hfusion.at<0>() = 0;
|
||||
Hfusion.at<1>() = 0;
|
||||
Hfusion.at<2>() = 0;
|
||||
Hfusion.at<3>() = 0;
|
||||
Hfusion.at<4>() = HK4;
|
||||
Hfusion.at<5>() = HK5;
|
||||
Hfusion.at<6>() = HK3*vd;
|
||||
Hfusion.at<7>() = 0;
|
||||
Hfusion.at<8>() = 0;
|
||||
Hfusion.at<9>() = 0;
|
||||
Hfusion.at<10>() = 0;
|
||||
Hfusion.at<11>() = 0;
|
||||
Hfusion.at<12>() = 0;
|
||||
Hfusion.at<13>() = 0;
|
||||
Hfusion.at<14>() = 0;
|
||||
Hfusion.at<15>() = 0;
|
||||
Hfusion.at<16>() = 0;
|
||||
Hfusion.at<17>() = 0;
|
||||
Hfusion.at<18>() = 0;
|
||||
Hfusion.at<19>() = 0;
|
||||
Hfusion.at<20>() = 0;
|
||||
Hfusion.at<21>() = 0;
|
||||
Hfusion.at<22>() = -HK4;
|
||||
Hfusion.at<23>() = -HK5;
|
||||
|
||||
|
||||
// Kalman gains
|
||||
Kfusion(0) = HK16*(-HK0*P(0,22) + HK0*P(0,4) - HK1*P(0,23) + HK1*P(0,5) + P(0,6)*vd);
|
||||
Kfusion(1) = HK16*(-HK0*P(1,22) + HK0*P(1,4) - HK1*P(1,23) + HK1*P(1,5) + P(1,6)*vd);
|
||||
Kfusion(2) = HK16*(-HK0*P(2,22) + HK0*P(2,4) - HK1*P(2,23) + HK1*P(2,5) + P(2,6)*vd);
|
||||
Kfusion(3) = HK16*(-HK0*P(3,22) + HK0*P(3,4) - HK1*P(3,23) + HK1*P(3,5) + P(3,6)*vd);
|
||||
Kfusion(4) = HK12*HK16;
|
||||
Kfusion(5) = HK16*HK9;
|
||||
Kfusion(6) = HK16*HK7;
|
||||
Kfusion(7) = HK16*(HK0*P(4,7) - HK0*P(7,22) + HK1*P(5,7) - HK1*P(7,23) + P(6,7)*vd);
|
||||
Kfusion(8) = HK16*(HK0*P(4,8) - HK0*P(8,22) + HK1*P(5,8) - HK1*P(8,23) + P(6,8)*vd);
|
||||
Kfusion(9) = HK16*(HK0*P(4,9) - HK0*P(9,22) + HK1*P(5,9) - HK1*P(9,23) + P(6,9)*vd);
|
||||
Kfusion(10) = HK16*(-HK0*P(10,22) + HK0*P(4,10) - HK1*P(10,23) + HK1*P(5,10) + P(6,10)*vd);
|
||||
Kfusion(11) = HK16*(-HK0*P(11,22) + HK0*P(4,11) - HK1*P(11,23) + HK1*P(5,11) + P(6,11)*vd);
|
||||
Kfusion(12) = HK16*(-HK0*P(12,22) + HK0*P(4,12) - HK1*P(12,23) + HK1*P(5,12) + P(6,12)*vd);
|
||||
Kfusion(13) = HK16*(-HK0*P(13,22) + HK0*P(4,13) - HK1*P(13,23) + HK1*P(5,13) + P(6,13)*vd);
|
||||
Kfusion(14) = HK16*(-HK0*P(14,22) + HK0*P(4,14) - HK1*P(14,23) + HK1*P(5,14) + P(6,14)*vd);
|
||||
Kfusion(15) = HK16*(-HK0*P(15,22) + HK0*P(4,15) - HK1*P(15,23) + HK1*P(5,15) + P(6,15)*vd);
|
||||
Kfusion(16) = HK16*(-HK0*P(16,22) + HK0*P(4,16) - HK1*P(16,23) + HK1*P(5,16) + P(6,16)*vd);
|
||||
Kfusion(17) = HK16*(-HK0*P(17,22) + HK0*P(4,17) - HK1*P(17,23) + HK1*P(5,17) + P(6,17)*vd);
|
||||
Kfusion(18) = HK16*(-HK0*P(18,22) + HK0*P(4,18) - HK1*P(18,23) + HK1*P(5,18) + P(6,18)*vd);
|
||||
Kfusion(19) = HK16*(-HK0*P(19,22) + HK0*P(4,19) - HK1*P(19,23) + HK1*P(5,19) + P(6,19)*vd);
|
||||
Kfusion(20) = HK16*(-HK0*P(20,22) + HK0*P(4,20) - HK1*P(20,23) + HK1*P(5,20) + P(6,20)*vd);
|
||||
Kfusion(21) = HK16*(-HK0*P(21,22) + HK0*P(4,21) - HK1*P(21,23) + HK1*P(5,21) + P(6,21)*vd);
|
||||
Kfusion(22) = HK15*HK16;
|
||||
Kfusion(23) = HK14*HK16;
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user