diff --git a/EKF/airspeed_fusion.cpp b/EKF/airspeed_fusion.cpp index c558938e9a..3ec6a8e141 100644 --- a/EKF/airspeed_fusion.cpp +++ b/EKF/airspeed_fusion.cpp @@ -52,8 +52,8 @@ void Ekf::fuseAirspeed() float vwn; // Wind speed in north direction float vwe; // Wind speed in east direction float v_tas_pred; // Predicted measurement - float EAS2TAS = 1.0f; // Where can I get this from? - float R_TAS = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * math::constrain(EAS2TAS, 0.9f, 10.0f)); // Variance for true airspeed measurement - (m/sec)^2 + 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 @@ -84,57 +84,88 @@ void Ekf::fuseAirspeed() H_TAS[5] = 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[3][3] * SH_TAS[2] + P[4][3] * SH_TAS[1] - P[22][3] * SH_TAS[2] - P[23][3] * SH_TAS[1] + P[5][3] * vd *SH_TAS[0]) + SH_TAS[1] * (P[3][4] * SH_TAS[2] + P[4][4] * SH_TAS[1] - P[22][4] * SH_TAS[2] - P[23][4] * SH_TAS[1] + P[5][4] * vd *SH_TAS[0]) - SH_TAS[2] * (P[3][22] * SH_TAS[2] + P[4][22] * SH_TAS[1] - P[22][22] * SH_TAS[2] - P[23][22] * SH_TAS[1] + P[5][22] * vd *SH_TAS[0]) - SH_TAS[1] * (P[3][23] * SH_TAS[2] + P[4][23] * SH_TAS[1] - P[22][23] * SH_TAS[2] - P[23][23] * SH_TAS[1] + P[5][23] * vd *SH_TAS[0]) + vd *SH_TAS[0] * (P[3][5] * SH_TAS[2] + P[4][5] * SH_TAS[1] - P[22][5] * SH_TAS[2] - P[23][5] * SH_TAS[1] + P[5][5] * vd *SH_TAS[0])); + float _airspeed_innov_var_temp = (R_TAS + SH_TAS[2] * (P[3][3] * SH_TAS[2] + P[4][3] * SH_TAS[1] - P[22][3] * SH_TAS[2] + - P[23][3] * SH_TAS[1] + P[5][3] * vd * SH_TAS[0]) + SH_TAS[1] * (P[3][4] * SH_TAS[2] + P[4][4] * SH_TAS[1] - P[22][4] * + SH_TAS[2] - P[23][4] * SH_TAS[1] + P[5][4] * vd * SH_TAS[0]) - SH_TAS[2] * (P[3][22] * SH_TAS[2] + P[4][22] * SH_TAS[1] + - P[22][22] * SH_TAS[2] - P[23][22] * SH_TAS[1] + P[5][22] * vd * SH_TAS[0]) - SH_TAS[1] * + (P[3][23] * SH_TAS[2] + P[4][23] * SH_TAS[1] - P[22][23] * SH_TAS[2] - P[23][23] * SH_TAS[1] + P[5][23] * vd * + SH_TAS[0]) + vd * SH_TAS[0] * (P[3][5] * SH_TAS[2] + P[4][5] * SH_TAS[1] - P[22][5] * SH_TAS[2] - P[23][5] * SH_TAS[1] + + P[5][5] * 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.bad_airspeed = false; - } else { // Reset the estimator - _fault_status.bad_airspeed = true; - initialiseCovariance(); - return; - } + SK_TAS[0] = 1.0f / _airspeed_innov_var_temp; + + } else { // Reset the estimator + initialiseCovariance(); + return; + } + SK_TAS[1] = SH_TAS[1]; - Kfusion[0] = SK_TAS[0]*(P[0][3]*SH_TAS[2] - P[0][22]*SH_TAS[2] + P[0][4]*SK_TAS[1] - P[0][23]*SK_TAS[1] + P[0][5]*vd*SH_TAS[0]); - Kfusion[1] = SK_TAS[0]*(P[1][3]*SH_TAS[2] - P[1][22]*SH_TAS[2] + P[1][4]*SK_TAS[1] - P[1][23]*SK_TAS[1] + P[1][5]*vd*SH_TAS[0]); - Kfusion[2] = SK_TAS[0]*(P[2][3]*SH_TAS[2] - P[2][22]*SH_TAS[2] + P[2][4]*SK_TAS[1] - P[2][23]*SK_TAS[1] + P[2][5]*vd*SH_TAS[0]); - Kfusion[3] = SK_TAS[0]*(P[3][3]*SH_TAS[2] - P[3][22]*SH_TAS[2] + P[3][4]*SK_TAS[1] - P[3][23]*SK_TAS[1] + P[3][5]*vd*SH_TAS[0]); - Kfusion[4] = SK_TAS[0]*(P[4][3]*SH_TAS[2] - P[4][22]*SH_TAS[2] + P[4][4]*SK_TAS[1] - P[4][23]*SK_TAS[1] + P[4][5]*vd*SH_TAS[0]); - Kfusion[5] = SK_TAS[0]*(P[5][3]*SH_TAS[2] - P[5][22]*SH_TAS[2] + P[5][4]*SK_TAS[1] - P[5][23]*SK_TAS[1] + P[5][5]*vd*SH_TAS[0]); - Kfusion[6] = SK_TAS[0]*(P[6][3]*SH_TAS[2] - P[6][22]*SH_TAS[2] + P[6][4]*SK_TAS[1] - P[6][23]*SK_TAS[1] + P[6][5]*vd*SH_TAS[0]); - Kfusion[7] = SK_TAS[0]*(P[7][3]*SH_TAS[2] - P[7][22]*SH_TAS[2] + P[7][4]*SK_TAS[1] - P[7][23]*SK_TAS[1] + P[7][5]*vd*SH_TAS[0]); - Kfusion[8] = SK_TAS[0]*(P[8][3]*SH_TAS[2] - P[8][22]*SH_TAS[2] + P[8][4]*SK_TAS[1] - P[8][23]*SK_TAS[1] + P[8][5]*vd*SH_TAS[0]); - Kfusion[9] = SK_TAS[0]*(P[9][3]*SH_TAS[2] - P[9][22]*SH_TAS[2] + P[9][4]*SK_TAS[1] - P[9][23]*SK_TAS[1] + P[9][5]*vd*SH_TAS[0]); - Kfusion[10] = SK_TAS[0]*(P[10][3]*SH_TAS[2] - P[10][22]*SH_TAS[2] + P[10][4]*SK_TAS[1] - P[10][23]*SK_TAS[1] + P[10][5]*vd*SH_TAS[0]); - Kfusion[11] = SK_TAS[0]*(P[11][3]*SH_TAS[2] - P[11][22]*SH_TAS[2] + P[11][4]*SK_TAS[1] - P[11][23]*SK_TAS[1] + P[11][5]*vd*SH_TAS[0]); - Kfusion[12] = SK_TAS[0]*(P[12][3]*SH_TAS[2] - P[12][22]*SH_TAS[2] + P[12][4]*SK_TAS[1] - P[12][23]*SK_TAS[1] + P[12][5]*vd*SH_TAS[0]); - Kfusion[13] = SK_TAS[0]*(P[13][3]*SH_TAS[2] - P[13][22]*SH_TAS[2] + P[13][4]*SK_TAS[1] - P[13][23]*SK_TAS[1] + P[13][5]*vd*SH_TAS[0]); - Kfusion[14] = SK_TAS[0]*(P[14][3]*SH_TAS[2] - P[14][22]*SH_TAS[2] + P[14][4]*SK_TAS[1] - P[14][23]*SK_TAS[1] + P[14][5]*vd*SH_TAS[0]); - Kfusion[15] = SK_TAS[0]*(P[15][3]*SH_TAS[2] - P[15][22]*SH_TAS[2] + P[15][4]*SK_TAS[1] - P[15][23]*SK_TAS[1] + P[15][5]*vd*SH_TAS[0]); - Kfusion[22] = SK_TAS[0]*(P[22][3]*SH_TAS[2] - P[22][22]*SH_TAS[2] + P[22][4]*SK_TAS[1] - P[22][23]*SK_TAS[1] + P[22][5]*vd*SH_TAS[0]); - Kfusion[23] = SK_TAS[0]*(P[23][3]*SH_TAS[2] - P[23][22]*SH_TAS[2] + P[23][4]*SK_TAS[1] - P[23][23]*SK_TAS[1] + P[23][5]*vd*SH_TAS[0]); + Kfusion[0] = SK_TAS[0] * (P[0][3] * SH_TAS[2] - P[0][22] * SH_TAS[2] + P[0][4] * SK_TAS[1] - P[0][23] * SK_TAS[1] + + P[0][5] * vd * SH_TAS[0]); + Kfusion[1] = SK_TAS[0] * (P[1][3] * SH_TAS[2] - P[1][22] * SH_TAS[2] + P[1][4] * SK_TAS[1] - P[1][23] * SK_TAS[1] + + P[1][5] * vd * SH_TAS[0]); + Kfusion[2] = SK_TAS[0] * (P[2][3] * SH_TAS[2] - P[2][22] * SH_TAS[2] + P[2][4] * SK_TAS[1] - P[2][23] * SK_TAS[1] + + P[2][5] * vd * SH_TAS[0]); + Kfusion[3] = SK_TAS[0] * (P[3][3] * SH_TAS[2] - P[3][22] * SH_TAS[2] + P[3][4] * SK_TAS[1] - P[3][23] * SK_TAS[1] + + P[3][5] * vd * SH_TAS[0]); + Kfusion[4] = SK_TAS[0] * (P[4][3] * SH_TAS[2] - P[4][22] * SH_TAS[2] + P[4][4] * SK_TAS[1] - P[4][23] * SK_TAS[1] + + P[4][5] * vd * SH_TAS[0]); + Kfusion[5] = SK_TAS[0] * (P[5][3] * SH_TAS[2] - P[5][22] * SH_TAS[2] + P[5][4] * SK_TAS[1] - P[5][23] * SK_TAS[1] + + P[5][5] * vd * SH_TAS[0]); + Kfusion[6] = SK_TAS[0] * (P[6][3] * SH_TAS[2] - P[6][22] * SH_TAS[2] + P[6][4] * SK_TAS[1] - P[6][23] * SK_TAS[1] + + P[6][5] * vd * SH_TAS[0]); + Kfusion[7] = SK_TAS[0] * (P[7][3] * SH_TAS[2] - P[7][22] * SH_TAS[2] + P[7][4] * SK_TAS[1] - P[7][23] * SK_TAS[1] + + P[7][5] * vd * SH_TAS[0]); + Kfusion[8] = SK_TAS[0] * (P[8][3] * SH_TAS[2] - P[8][22] * SH_TAS[2] + P[8][4] * SK_TAS[1] - P[8][23] * SK_TAS[1] + + P[8][5] * vd * SH_TAS[0]); + Kfusion[9] = SK_TAS[0] * (P[9][3] * SH_TAS[2] - P[9][22] * SH_TAS[2] + P[9][4] * SK_TAS[1] - P[9][23] * SK_TAS[1] + + P[9][5] * vd * SH_TAS[0]); + Kfusion[10] = SK_TAS[0] * (P[10][3] * SH_TAS[2] - P[10][22] * SH_TAS[2] + P[10][4] * SK_TAS[1] - P[10][23] * SK_TAS[1] + + P[10][5] * vd * SH_TAS[0]); + Kfusion[11] = SK_TAS[0] * (P[11][3] * SH_TAS[2] - P[11][22] * SH_TAS[2] + P[11][4] * SK_TAS[1] - P[11][23] * SK_TAS[1] + + P[11][5] * vd * SH_TAS[0]); + Kfusion[12] = SK_TAS[0] * (P[12][3] * SH_TAS[2] - P[12][22] * SH_TAS[2] + P[12][4] * SK_TAS[1] - P[12][23] * SK_TAS[1] + + P[12][5] * vd * SH_TAS[0]); + Kfusion[13] = SK_TAS[0] * (P[13][3] * SH_TAS[2] - P[13][22] * SH_TAS[2] + P[13][4] * SK_TAS[1] - P[13][23] * SK_TAS[1] + + P[13][5] * vd * SH_TAS[0]); + Kfusion[14] = SK_TAS[0] * (P[14][3] * SH_TAS[2] - P[14][22] * SH_TAS[2] + P[14][4] * SK_TAS[1] - P[14][23] * SK_TAS[1] + + P[14][5] * vd * SH_TAS[0]); + Kfusion[15] = SK_TAS[0] * (P[15][3] * SH_TAS[2] - P[15][22] * SH_TAS[2] + P[15][4] * SK_TAS[1] - P[15][23] * SK_TAS[1] + + P[15][5] * vd * SH_TAS[0]); + Kfusion[22] = SK_TAS[0] * (P[22][3] * SH_TAS[2] - P[22][22] * SH_TAS[2] + P[22][4] * SK_TAS[1] - P[22][23] * SK_TAS[1] + + P[22][5] * vd * SH_TAS[0]); + Kfusion[23] = SK_TAS[0] * (P[23][3] * SH_TAS[2] - P[23][22] * SH_TAS[2] + P[23][4] * SK_TAS[1] - P[23][23] * SK_TAS[1] + + P[23][5] * vd * SH_TAS[0]); // Only update the magnetometer states if we are airborne and using 3D mag fusion - if(_control_status.flags.mag_3D && _control_status.flags.in_air){ - Kfusion[16] = SK_TAS[0]*(P[16][3]*SH_TAS[2] - P[16][22]*SH_TAS[2] + P[16][4]*SK_TAS[1] - P[16][23]*SK_TAS[1] + P[16][5]*vd*SH_TAS[0]); - Kfusion[17] = SK_TAS[0]*(P[17][3]*SH_TAS[2] - P[17][22]*SH_TAS[2] + P[17][4]*SK_TAS[1] - P[17][23]*SK_TAS[1] + P[17][5]*vd*SH_TAS[0]); - Kfusion[18] = SK_TAS[0]*(P[18][3]*SH_TAS[2] - P[18][22]*SH_TAS[2] + P[18][4]*SK_TAS[1] - P[18][23]*SK_TAS[1] + P[18][5]*vd*SH_TAS[0]); - Kfusion[19] = SK_TAS[0]*(P[19][3]*SH_TAS[2] - P[19][22]*SH_TAS[2] + P[19][4]*SK_TAS[1] - P[19][23]*SK_TAS[1] + P[19][5]*vd*SH_TAS[0]); - Kfusion[20] = SK_TAS[0]*(P[20][3]*SH_TAS[2] - P[20][22]*SH_TAS[2] + P[20][4]*SK_TAS[1] - P[20][23]*SK_TAS[1] + P[20][5]*vd*SH_TAS[0]); - Kfusion[21] = SK_TAS[0]*(P[21][3]*SH_TAS[2] - P[21][22]*SH_TAS[2] + P[21][4]*SK_TAS[1] - P[21][23]*SK_TAS[1] + P[21][5]*vd*SH_TAS[0]); - } - else{ - for (int i = 16; i <= 21; i++) - { + if (_control_status.flags.mag_3D && _control_status.flags.in_air) { + Kfusion[16] = SK_TAS[0] * (P[16][3] * SH_TAS[2] - P[16][22] * SH_TAS[2] + P[16][4] * SK_TAS[1] - P[16][23] * SK_TAS[1] + + P[16][5] * vd * SH_TAS[0]); + Kfusion[17] = SK_TAS[0] * (P[17][3] * SH_TAS[2] - P[17][22] * SH_TAS[2] + P[17][4] * SK_TAS[1] - P[17][23] * SK_TAS[1] + + P[17][5] * vd * SH_TAS[0]); + Kfusion[18] = SK_TAS[0] * (P[18][3] * SH_TAS[2] - P[18][22] * SH_TAS[2] + P[18][4] * SK_TAS[1] - P[18][23] * SK_TAS[1] + + P[18][5] * vd * SH_TAS[0]); + Kfusion[19] = SK_TAS[0] * (P[19][3] * SH_TAS[2] - P[19][22] * SH_TAS[2] + P[19][4] * SK_TAS[1] - P[19][23] * SK_TAS[1] + + P[19][5] * vd * SH_TAS[0]); + Kfusion[20] = SK_TAS[0] * (P[20][3] * SH_TAS[2] - P[20][22] * SH_TAS[2] + P[20][4] * SK_TAS[1] - P[20][23] * SK_TAS[1] + + P[20][5] * vd * SH_TAS[0]); + Kfusion[21] = SK_TAS[0] * (P[21][3] * SH_TAS[2] - P[21][22] * SH_TAS[2] + P[21][4] * SK_TAS[1] - P[21][23] * SK_TAS[1] + + P[21][5] * vd * SH_TAS[0]); + + } else { + for (int i = 16; i <= 21; i++) { Kfusion[i] = 0.0f; } } // calculate measurement innovation - _airspeed_innov = v_tas_pred - _airspeed_sample_delayed.true_airspeed; + _airspeed_innov = v_tas_pred - + _airspeed_sample_delayed.true_airspeed; // This is TAS, maybe we should indicate that in some way // Calculate the innovation variance _airspeed_innov_var = 1.0f / SK_TAS[0]; @@ -142,42 +173,53 @@ void Ekf::fuseAirspeed() // 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 (_tas_test_ratio < 1.0f) - { - // by definition the angle error state is zero at the fusion time - _state.ang_error.setZero(); + // if the innocation consistency check fails then don't fuse the sample and indicate bad airspeed health + if (_tas_test_ratio > 1.0f) { + _airspeed_healthy = false; + return; + } - // Fuse airspeed measurement - fuse(Kfusion, _airspeed_innov); + // airspeed measurement sample has passed check so record it + _time_last_arsp_fuse = _time_last_imu; - // correct the nominal quaternion - Quaternion dq; - dq.from_axis_angle(_state.ang_error); - _state.quat_nominal = dq * _state.quat_nominal; - _state.quat_nominal.normalize(); - for (unsigned row = 0; row < _k_num_states; row++) { - for (unsigned column = 0; column < _k_num_states; column++) { // Here it will be a lot of zeros, should optimize that... - KH[row][column] = Kfusion[row] * H_TAS[column]; - } + // by definition the angle error state is zero at the fusion time + _state.ang_error.setZero(); + + // Fuse airspeed measurement + fuse(Kfusion, _airspeed_innov); //Why calculate angle error when it is always zero? + + // correct the nominal quaternion + Quaternion dq; + dq.from_axis_angle(_state.ang_error); + _state.quat_nominal = dq * _state.quat_nominal; + _state.quat_nominal.normalize(); + + // update covariance matrix via Pnew = (I - KH)P = P - KHP + float KH[_k_num_states][_k_num_states] = {}; + float KHP[_k_num_states][_k_num_states] = {}; + + for (unsigned row = 0; row < _k_num_states; row++) { + for (unsigned column = 0; column < _k_num_states; column++) { // Here it will be a lot of zeros, should optimize that... + KH[row][column] = Kfusion[row] * H_TAS[column]; } + } - for (unsigned row = 0; row < _k_num_states; row++) { - for (unsigned column = 0; column < _k_num_states; column++) { - for (unsigned i = 0; i < _k_num_states; i++) { // Check if this is correct matrix multiplication! - KHP[row][column] += KH[row][i] * P[i][column]; - } - } - } - - 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]; + for (unsigned row = 0; row < _k_num_states; row++) { + for (unsigned column = 0; column < _k_num_states; column++) { + for (unsigned i = 0; i < _k_num_states; i++) { // Check if this is correct matrix multiplication! + KHP[row][column] += KH[row][i] * P[i][column]; } } } - makeSymmetrical(); + 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]; + } + } + + makeSymmetrical(); limitCov(); } } \ No newline at end of file diff --git a/EKF/common.h b/EKF/common.h index 2e638e2eed..d8382838b2 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -113,6 +113,7 @@ struct rangeSample { struct airspeedSample { float true_airspeed; // true airspeed measurement in m/s + float eas2tas; // equivalent to true airspeed factor uint64_t time_us; // timestamp in microseconds }; diff --git a/EKF/control.cpp b/EKF/control.cpp index 8fc114a045..a6729eeb8d 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -43,6 +43,8 @@ void Ekf::controlFusionModes() { + // Store the status to enable change detection + _control_status_prev.value = _control_status.value; // Determine the vehicle status calculateVehicleStatus(); @@ -339,14 +341,13 @@ void Ekf::controlFusionModes() _control_status.flags.rng_hgt = true; } - // Placeholder for control of wind velocity states estimation - // TODO add methods for true airspeed and/or sidelsip fusion or some type of drag force measurement - if (false) { + // if the airspeed measurements have timed out for 10 seconds we declare the wind estimate to be invalid + if (_time_last_imu - _time_last_arsp_fuse > 10e6 || _time_last_arsp_fuse == 0) { _control_status.flags.wind = false; + } else { + _control_status.flags.wind = true; } - // Store the status to enable change detection - _control_status_prev.value = _control_status.value; } void Ekf::calculateVehicleStatus() diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 8869f7d213..5752395781 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -172,7 +172,7 @@ void Ekf::predictCovariance() float wind_vel_sig; // Don't continue to grow wind velocity state variances if they are becoming too large or we are not using wind velocity states as this can make the covariance matrix badly conditioned - if (_control_status.flags.wind && (P[22][22] + P[22][22]) < 1000.0f) { + if (_control_status.flags.wind && (P[22][22] + P[23][23]) < 1000.0f) { wind_vel_sig = dt * math::constrain(_params.wind_vel_p_noise, 0.0f, 1.0f); } else { @@ -641,10 +641,47 @@ void Ekf::predictCovariance() // Don't do covariance prediction on wind states unless we are using them if (_control_status.flags.wind) { - // Check if we have jsut transitioned to using wind states and set the variances accordingly - if (!_control_status_prev.flags.mag_3D) { + // Check if we have just transitioned to using wind states and set the variances accordingly + if (!_control_status_prev.flags.wind) { + // simple initialisation of wind states: calculate wind component along the forward axis + // of the plane. + + matrix::Euler euler(_output_new.quat_nominal); + float heading = euler(2); + + // ground speed component in the xy plane projected onto the directon the plane is heading to + float ground_speed_xy_nose = _output_new.vel(0) * cosf(heading) + _output_new.vel(1) * sinf(heading); + airspeedSample tmp = _airspeed_buffer.get_newest(); + float airspeed = tmp.true_airspeed; + + // check if the calculation is well conditioned: + // our airspeed measurement is at least as hight as our down velocity and the plane is moving forward + if (airspeed > fabsf(_output_new.vel(2)) && ground_speed_xy_nose > 0) { + + float ground_speed = sqrtf(_output_new.vel(0) * _output_new.vel(0) + _output_new.vel(1) * _output_new.vel(1) + _output_new.vel(2) * _output_new.vel(2)); + + // wind magnitude in the direction the plane is + float wind_magnitude = ground_speed_xy_nose - sqrtf(airspeed * airspeed - _output_new.vel(2) * _output_new.vel(2)); + + // determine direction of wind + float wind_sign = 1; + if (airspeed < ground_speed) { + // wind is in nose direction + wind_sign = 1; + } else { + wind_sign = -1; + } + + _state.wind_vel(0) = cosf(heading) * wind_magnitude * wind_sign; + _state.wind_vel(1) = sinf(heading) * wind_magnitude * wind_sign; + + } else { + // calculation is badly conditioned, just set wind states to zero + _state.wind_vel.setZero(); + } + + // initialise diagonal of covariance matrix for the wind velocity states for (uint8_t index = 22; index <= 23; index++) { - // TODO initialise wind states using ground speed and airspeed and set initial variance using sum of ground speed and airspeed variances P[index][index] = sq(5.0f); } } @@ -725,6 +762,9 @@ void Ekf::predictCovariance() // copy variances (diagonals) for (unsigned i = 0; i < _k_num_states; i++) { + if(!_control_status.flags.wind && i > 21) { + continue; + } P[i][i] = nextP[i][i]; } diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 1f8e8ea88c..4c0239a38a 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -56,6 +56,7 @@ Ekf::Ekf(): _time_last_vel_fuse(0), _time_last_hgt_fuse(0), _time_last_of_fuse(0), + _time_last_arsp_fuse(0), _last_disarmed_posD(0.0f), _airspeed_innov(0.0f), _airspeed_innov_var(0.0f), @@ -326,7 +327,7 @@ bool Ekf::update() } // TODO This is just to get the logic inside but we will only start fusion once we tested this again - if (_airspeed_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_airspeed_sample_delayed) && false) { + if (_airspeed_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_airspeed_sample_delayed)) { fuseAirspeed(); } } diff --git a/EKF/ekf.h b/EKF/ekf.h index 5fca7e4c65..e1798260a4 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -147,6 +147,7 @@ private: uint64_t _time_last_vel_fuse; // time the last fusion of velocity measurements was performed (usec) uint64_t _time_last_hgt_fuse; // time the last fusion of height measurements was performed (usec) uint64_t _time_last_of_fuse; // time the last fusion of optical flow measurements were performed (usec) + uint64_t _time_last_arsp_fuse; // time the last fusion of airspeed measurements were performed (usec) Vector2f _last_known_posNE; // last known local NE position vector (m) float _last_disarmed_posD; // vertical position recorded at arming (m) diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 876b3030e9..fb21da00dd 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -59,6 +59,7 @@ EstimatorInterface::EstimatorInterface(): _gps_origin_eph(0.0f), _gps_origin_epv(0.0f), _mag_healthy(false), + _airspeed_healthy(false), _yaw_test_ratio(0.0f), _time_last_imu(0), _time_last_gps(0), @@ -200,15 +201,16 @@ void EstimatorInterface::setBaroData(uint64_t time_usec, float *data) } } -void EstimatorInterface::setAirspeedData(uint64_t time_usec, float *data) +void EstimatorInterface::setAirspeedData(uint64_t time_usec, float *true_airspeed, float *eas2tas) { - if (!collect_airspeed(time_usec, data) || !_initialised) { + if (!collect_airspeed(time_usec, true_airspeed, eas2tas) || !_initialised) { return; } if (time_usec - _time_last_airspeed > 80000) { airspeedSample airspeed_sample_new; - airspeed_sample_new.true_airspeed = *data; + airspeed_sample_new.true_airspeed = *true_airspeed; + airspeed_sample_new.eas2tas = *eas2tas; airspeed_sample_new.time_us = time_usec - _params.airspeed_delay_ms * 1000; airspeed_sample_new.time_us -= FILTER_UPDATE_PERRIOD_MS * 1000 / 2; //typo PeRRiod _time_last_airspeed = time_usec; diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 6579c8eeeb..048eca6ce6 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -117,7 +117,7 @@ public: virtual bool collect_baro(uint64_t time_usec, float *data) { return true; } - virtual bool collect_airspeed(uint64_t time_usec, float *data) { return true; } + virtual bool collect_airspeed(uint64_t time_usec, float *true_airspeed, float *eas2tas) { return true; } virtual bool collect_range(uint64_t time_usec, float *data) { return true; } @@ -137,7 +137,7 @@ public: void setBaroData(uint64_t time_usec, float *data); // set airspeed data - void setAirspeedData(uint64_t time_usec, float *data); + void setAirspeedData(uint64_t time_usec, float *true_airspeed, float *eas2tas); // set range data void setRangeData(uint64_t time_usec, float *data); @@ -250,6 +250,7 @@ protected: struct map_projection_reference_s _pos_ref = {}; // Contains WGS-84 position latitude and longitude (radians) bool _mag_healthy; // computed by mag innovation test + float _airspeed_healthy; // computed by airspeed innovation test float _yaw_test_ratio; // yaw innovation consistency check ratio float _mag_test_ratio[3]; // magnetometer XYZ innovation consistency check ratios float _vel_pos_test_ratio[6]; // velocity and position innovation consistency check ratios