bool EstimatorInterface::initialise_interface(uint64_t timestamp)
{
// find the maximum time delay required to compensate for
uint16_t max_time_delay_ms = math::max(_params.mag_delay_ms,
math::max(_params.range_delay_ms,
math::max(_params.gps_delay_ms,
math::max(_params.flow_delay_ms,
math::max(_params.ev_delay_ms,
math::max(_params.airspeed_delay_ms, _params.baro_delay_ms))))));
// calculate the IMU buffer length required to accomodate the maximum delay with some allowance for jitter
_imu_buffer_length = (max_time_delay_ms / FILTER_UPDATE_PERIOD_MS) + 1;
// set the observaton buffer length to handle the minimum time of arrival between observations in combination
// with the worst case delay from current time to ekf fusion time
// allow for worst case 50% extension of the ekf fusion time horizon delay due to timing jitter
uint16_t ekf_delay_ms = max_time_delay_ms + (int)(ceil((float)max_time_delay_ms * 0.5f));
_obs_buffer_length = (ekf_delay_ms / _params.sensor_interval_min_ms) + 1;
// limit to be no longer than the IMU buffer (we can't process data faster than the EKF prediction rate)
_obs_buffer_length = math::min(_obs_buffer_length,_imu_buffer_length);
ECL_INFO("EKF IMU buffer length = %i",(int)_imu_buffer_length);
ECL_INFO("EKF observation buffer length = %i",(int)_obs_buffer_length);
if (!(_imu_buffer.allocate(_imu_buffer_length) &&
_gps_buffer.allocate(_obs_buffer_length) &&
_mag_buffer.allocate(_obs_buffer_length) &&
_baro_buffer.allocate(_obs_buffer_length) &&
_range_buffer.allocate(_obs_buffer_length) &&
_airspeed_buffer.allocate(_obs_buffer_length) &&
_flow_buffer.allocate(_obs_buffer_length) &&
_ext_vision_buffer.allocate(_obs_buffer_length) &&
_output_buffer.allocate(_imu_buffer_length))) {
ECL_ERR("EKF buffer allocation failed!");
unallocate_buffers();
return false;
}
// zero the data in the observation buffers
for (int index=0; index < _obs_buffer_length; index++) {
gpsSample gps_sample_init = {};
_gps_buffer.push(gps_sample_init);
magSample mag_sample_init = {};
_mag_buffer.push(mag_sample_init);
baroSample baro_sample_init = {};
_baro_buffer.push(baro_sample_init);
rangeSample range_sample_init = {};
_range_buffer.push(range_sample_init);
airspeedSample airspeed_sample_init = {};
_airspeed_buffer.push(airspeed_sample_init);
flowSample flow_sample_init = {};
_flow_buffer.push(flow_sample_init);
extVisionSample ext_vision_sample_init = {};
_ext_vision_buffer.push(ext_vision_sample_init);
}
// zero the data in the imu data and output observer state buffers
for (int index=0; index < _imu_buffer_length; index++) {
imuSample imu_sample_init = {};
_imu_buffer.push(imu_sample_init);
outputSample output_sample_init = {};
_output_buffer.push(output_sample_init);
}
_dt_imu_avg = 0.0f;
_imu_sample_delayed.delta_ang.setZero();
_imu_sample_delayed.delta_vel.setZero();
_imu_sample_delayed.delta_ang_dt = 0.0f;
_imu_sample_delayed.delta_vel_dt = 0.0f;
_imu_sample_delayed.time_us = timestamp;
_imu_ticks = 0;
_initialised = false;
_time_last_imu = 0;
_time_last_gps = 0;
_time_last_mag = 0;
_time_last_baro = 0;
_time_last_range = 0;
_time_last_airspeed = 0;
_time_last_optflow = 0;
memset(&_fault_status.flags, 0, sizeof(_fault_status.flags));
_time_last_ext_vision = 0;
return true;
}
void Ekf::fuseAirspeed()
{
// Initialize variables
float vn; // Velocity in north direction
float ve; // Velocity in east direction
float vd; // Velocity in downwards direction
float vwn; // Wind speed in north direction
float vwe; // Wind speed in east direction
float v_tas_pred; // Predicted measurement
float R_TAS = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * math::constrain(_airspeed_sample_delayed.eas2tas, 0.9f,
10.0f)); // Variance for true airspeed measurement - (m/sec)^2
float SH_TAS[3] = {}; // Varialbe used to optimise calculations of measurement jacobian
float H_TAS[24] = {}; // Observation Jacobian
float SK_TAS[2] = {}; // Varialbe used to optimise calculations of the Kalman gain vector
float Kfusion[24] = {}; // Kalman gain vector
// Copy required states to local variable names
vn = _state.vel(0);
ve = _state.vel(1);
vd = _state.vel(2);
vwn = _state.wind_vel(0);
vwe = _state.wind_vel(1);
// Calculate the predicted airspeed
v_tas_pred = sqrtf((ve - vwe) * (ve - vwe) + (vn - vwn) * (vn - vwn) + vd * vd);
// Perform fusion of True Airspeed measurement
if (v_tas_pred > 1.0f) {
// Calculate the observation jacobian
// intermediate variable from algebraic optimisation
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;
for (uint8_t i = 0; i < _k_num_states; i++) {
H_TAS[i] = 0.0f;
}
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];
// We don't want to update the innovation variance if the calculation is ill conditioned
float _airspeed_innov_var_temp = (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_temp >= R_TAS) { // Check for badly conditioned calculation
SK_TAS[0] = 1.0f / _airspeed_innov_var_temp;
_fault_status.flags.bad_airspeed = false;
} else { // Reset the estimator covarinace matrix
_fault_status.flags.bad_airspeed = true;
initialiseCovariance();
ECL_ERR("EKF airspeed fusion numerical error - covariance reset");
return;
}
SK_TAS[1] = SH_TAS[1];
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;
// Calculate the innovation variance
_airspeed_innov_var = 1.0f / SK_TAS[0];
// 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;
}
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
for (unsigned row = 0; row < _k_num_states; row++) {
KH[row][4] = Kfusion[row] * H_TAS[4];
KH[row][5] = Kfusion[row] * H_TAS[5];
KH[row][6] = Kfusion[row] * H_TAS[6];
KH[row][22] = Kfusion[row] * H_TAS[22];
KH[row][23] = Kfusion[row] * H_TAS[23];
}
for (unsigned row = 0; row < _k_num_states; row++) {
for (unsigned column = 0; column < _k_num_states; column++) {
float tmp = KH[row][4] * P[4][column];
tmp += KH[row][5] * P[5][column];
tmp += KH[row][6] * P[6][column];
tmp += KH[row][22] * P[22][column];
tmp += KH[row][23] * P[23][column];
KHP[row][column] = tmp;
}
}
// if the covariance correction will result in a negative variance, then
// the covariance marix is unhealthy and must be corrected
bool healthy = true;
_fault_status.flags.bad_airspeed = false;
for (int i = 0; i < _k_num_states; i++) {
if (P[i][i] < KHP[i][i]) {
// zero rows and columns
zeroRows(P,i,i);
zeroCols(P,i,i);
//flag as unhealthy
healthy = false;
// update individual measurement health status
_fault_status.flags.bad_airspeed = true;
}
}
// only apply covariance and state corrrections if healthy
if (healthy) {
// apply the covariance corrections
for (unsigned row = 0; row < _k_num_states; row++) {
for (unsigned column = 0; column < _k_num_states; column++) {
P[row][column] = P[row][column] - KHP[row][column];
}
}
// correct the covariance marix for gross errors
fixCovarianceErrors();
// apply the state corrections
fuse(Kfusion, _airspeed_innov);
}
}
}
