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