fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 07:03:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2 airspeed fusion:

Browse Source 
- finished logic for fusion
- fixed bug where previous control status was set in the wrong location
This commit is contained in:
Roman Bapst
2016-04-19 09:53:31 +02:00
parent 60abf07bee
commit 99fc61c27c
8 changed files with 172 additions and 83 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/airspeed_fusion.cpp
+110 -68
View File
@@ -52,8 +52,8 @@ void Ekf::fuseAirspeed()
float vwn; // Wind speed in north direction float vwn; // Wind speed in north direction
float vwe; // Wind speed in east direction float vwe; // Wind speed in east direction
float v_tas_pred; // Predicted measurement 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(_airspeed_sample_delayed.eas2tas, 0.9f,
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 10.0f)); // Variance for true airspeed measurement - (m/sec)^2
float SH_TAS[3] = {}; // Varialbe used to optimise calculations of measurement jacobian float SH_TAS[3] = {}; // Varialbe used to optimise calculations of measurement jacobian
float H_TAS[24] = {}; // Observation Jacobian float H_TAS[24] = {}; // Observation Jacobian
float SK_TAS[2] = {}; // Varialbe used to optimise calculations of the Kalman gain vector 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[5] = vd * SH_TAS[0];
H_TAS[22] = -SH_TAS[2]; H_TAS[22] = -SH_TAS[2];
H_TAS[23] = -SH_TAS[1]; H_TAS[23] = -SH_TAS[1];
// We don't want to update the innovation variance if the calculation is ill conditioned // 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 if (_airspeed_innov_var_temp >= R_TAS) { // Check for badly conditioned calculation
SK_TAS[0] = 1.0f / _airspeed_innov_var_temp; SK_TAS[0] = 1.0f / _airspeed_innov_var_temp;
_fault_status.bad_airspeed = false;
} else { // Reset the estimator } else { // Reset the estimator
_fault_status.bad_airspeed = true; initialiseCovariance();
initialiseCovariance(); return;
return; }
}
SK_TAS[1] = SH_TAS[1]; 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[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] +
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]); P[0][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[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] +
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]); P[1][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[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] +
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]); P[2][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[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] +
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]); P[3][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[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] +
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]); P[4][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[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] +
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]); P[5][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[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] +
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]); P[6][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[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] +
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]); P[7][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[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] +
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]); 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 // 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){ 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[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] +
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]); P[16][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[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] +
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]); P[17][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[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] +
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]); 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] +
else{ P[19][5] * vd * SH_TAS[0]);
for (int i = 16; i <= 21; i++) 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; Kfusion[i] = 0.0f;
} }
} }
// calculate measurement innovation // 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 // Calculate the innovation variance
_airspeed_innov_var = 1.0f / SK_TAS[0]; _airspeed_innov_var = 1.0f / SK_TAS[0];
@@ -142,42 +173,53 @@ void Ekf::fuseAirspeed()
// Compute the ratio of innovation to gate size // 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); _tas_test_ratio = sq(_airspeed_innov) / (sq(fmaxf(_params.tas_innov_gate, 1.0f)) * _airspeed_innov_var);
if (_tas_test_ratio < 1.0f) // if the innocation consistency check fails then don't fuse the sample and indicate bad airspeed health
{ if (_tas_test_ratio > 1.0f) {
// by definition the angle error state is zero at the fusion time _airspeed_healthy = false;
_state.ang_error.setZero(); return;
}
// Fuse airspeed measurement // airspeed measurement sample has passed check so record it
fuse(Kfusion, _airspeed_innov); _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++) { // by definition the angle error state is zero at the fusion time
for (unsigned column = 0; column < _k_num_states; column++) { // Here it will be a lot of zeros, should optimize that... _state.ang_error.setZero();
KH[row][column] = Kfusion[row] * H_TAS[column];
} // 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 row = 0; row < _k_num_states; row++) {
for (unsigned column = 0; column < _k_num_states; column++) { 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! 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]; 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];
} }
} }
} }
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(); limitCov();
} }
} }
EKF/common.h
+1
View File
@@ -113,6 +113,7 @@ struct rangeSample {
struct airspeedSample { struct airspeedSample {
float true_airspeed; // true airspeed measurement in m/s float true_airspeed; // true airspeed measurement in m/s
float eas2tas; // equivalent to true airspeed factor
uint64_t time_us; // timestamp in microseconds uint64_t time_us; // timestamp in microseconds
}; };
EKF/control.cpp
+6 -5
View File
@@ -43,6 +43,8 @@
void Ekf::controlFusionModes() void Ekf::controlFusionModes()
{ {
// Store the status to enable change detection
_control_status_prev.value = _control_status.value;
// Determine the vehicle status // Determine the vehicle status
calculateVehicleStatus(); calculateVehicleStatus();
@@ -339,14 +341,13 @@ void Ekf::controlFusionModes()
_control_status.flags.rng_hgt = true; _control_status.flags.rng_hgt = true;
} }
// Placeholder for control of wind velocity states estimation // if the airspeed measurements have timed out for 10 seconds we declare the wind estimate to be invalid
// TODO add methods for true airspeed and/or sidelsip fusion or some type of drag force measurement if (_time_last_imu - _time_last_arsp_fuse > 10e6 || _time_last_arsp_fuse == 0) {
if (false) {
_control_status.flags.wind = false; _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() void Ekf::calculateVehicleStatus()
EKF/covariance.cpp
+44 -4
View File
@@ -172,7 +172,7 @@ void Ekf::predictCovariance()
float wind_vel_sig; 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 // 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); wind_vel_sig = dt * math::constrain(_params.wind_vel_p_noise, 0.0f, 1.0f);
} else { } else {
@@ -641,10 +641,47 @@ void Ekf::predictCovariance()
// Don't do covariance prediction on wind states unless we are using them // Don't do covariance prediction on wind states unless we are using them
if (_control_status.flags.wind) { if (_control_status.flags.wind) {
// Check if we have jsut transitioned to using wind states and set the variances accordingly // Check if we have just transitioned to using wind states and set the variances accordingly
if (!_control_status_prev.flags.mag_3D) { if (!_control_status_prev.flags.wind) {
// simple initialisation of wind states: calculate wind component along the forward axis
// of the plane.
matrix::Euler<float> 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++) { 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); P[index][index] = sq(5.0f);
} }
} }
@@ -725,6 +762,9 @@ void Ekf::predictCovariance()
// copy variances (diagonals) // copy variances (diagonals)
for (unsigned i = 0; i < _k_num_states; i++) { for (unsigned i = 0; i < _k_num_states; i++) {
if(!_control_status.flags.wind && i > 21) {
continue;
}
P[i][i] = nextP[i][i]; P[i][i] = nextP[i][i];
} }
EKF/ekf.cpp
+2 -1
View File
@@ -56,6 +56,7 @@ Ekf::Ekf():
_time_last_vel_fuse(0), _time_last_vel_fuse(0),
_time_last_hgt_fuse(0), _time_last_hgt_fuse(0),
_time_last_of_fuse(0), _time_last_of_fuse(0),
_time_last_arsp_fuse(0),
_last_disarmed_posD(0.0f), _last_disarmed_posD(0.0f),
_airspeed_innov(0.0f), _airspeed_innov(0.0f),
_airspeed_innov_var(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 // 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(); fuseAirspeed();
} }
} }
EKF/ekf.h
+1
View File
@@ -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_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_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_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) Vector2f _last_known_posNE; // last known local NE position vector (m)
float _last_disarmed_posD; // vertical position recorded at arming (m) float _last_disarmed_posD; // vertical position recorded at arming (m)
EKF/estimator_interface.cpp
+5 -3
View File
@@ -59,6 +59,7 @@ EstimatorInterface::EstimatorInterface():
_gps_origin_eph(0.0f), _gps_origin_eph(0.0f),
_gps_origin_epv(0.0f), _gps_origin_epv(0.0f),
_mag_healthy(false), _mag_healthy(false),
_airspeed_healthy(false),
_yaw_test_ratio(0.0f), _yaw_test_ratio(0.0f),
_time_last_imu(0), _time_last_imu(0),
_time_last_gps(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; return;
} }
if (time_usec - _time_last_airspeed > 80000) { if (time_usec - _time_last_airspeed > 80000) {
airspeedSample airspeed_sample_new; 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 = time_usec - _params.airspeed_delay_ms * 1000;
airspeed_sample_new.time_us -= FILTER_UPDATE_PERRIOD_MS * 1000 / 2; //typo PeRRiod airspeed_sample_new.time_us -= FILTER_UPDATE_PERRIOD_MS * 1000 / 2; //typo PeRRiod
_time_last_airspeed = time_usec; _time_last_airspeed = time_usec;
EKF/estimator_interface.h
+3 -2
View File
@@ -117,7 +117,7 @@ public:
virtual bool collect_baro(uint64_t time_usec, float *data) { return true; } 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; } 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); void setBaroData(uint64_t time_usec, float *data);
// set airspeed 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 // set range data
void setRangeData(uint64_t time_usec, float *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) struct map_projection_reference_s _pos_ref = {}; // Contains WGS-84 position latitude and longitude (radians)
bool _mag_healthy; // computed by mag innovation test 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 _yaw_test_ratio; // yaw innovation consistency check ratio
float _mag_test_ratio[3]; // magnetometer XYZ innovation consistency check ratios float _mag_test_ratio[3]; // magnetometer XYZ innovation consistency check ratios
float _vel_pos_test_ratio[6]; // velocity and position innovation consistency check ratios float _vel_pos_test_ratio[6]; // velocity and position innovation consistency check ratios