forked from Archive/PX4-Autopilot
EKF: Set covariances to zero for un-used states
This commit is contained in:
Paul Riseborough
parent
294aca8609
commit
ffe1d30864
|
|
@ -520,170 +520,200 @@ void Ekf::predictCovariance()
|
|||
nextP[13][15] = P[13][15];
|
||||
nextP[14][15] = P[14][15];
|
||||
nextP[15][15] = P[15][15];
|
||||
nextP[0][16] = P[0][16] * SPP[5] - P[1][16] * SPP[4] + P[2][16] * SPP[7] + P[9][16] * SPP[22] + P[12][16] * SPP[18];
|
||||
nextP[1][16] = P[1][16] * SPP[6] - P[0][16] * SPP[2] - P[2][16] * SPP[8] + P[10][16] * SPP[22] + P[13][16] * SPP[17];
|
||||
nextP[2][16] = P[0][16] * SPP[14] - P[1][16] * SPP[3] + P[2][16] * SPP[13] + P[11][16] * SPP[22] + P[14][16] * SPP[16];
|
||||
nextP[3][16] = P[3][16] + P[0][16] * SPP[1] + P[1][16] * SPP[19] + P[2][16] * SPP[15] - P[15][16] * SPP[21];
|
||||
nextP[4][16] = P[4][16] + P[15][16] * SF[22] + P[0][16] * SPP[20] + P[1][16] * SPP[12] + P[2][16] * SPP[11];
|
||||
nextP[5][16] = P[5][16] + P[15][16] * SF[20] - P[0][16] * SPP[9] + P[1][16] * SPP[10] + P[2][16] * SPP[0];
|
||||
nextP[6][16] = P[6][16] + P[3][16] * dt;
|
||||
nextP[7][16] = P[7][16] + P[4][16] * dt;
|
||||
nextP[8][16] = P[8][16] + P[5][16] * dt;
|
||||
nextP[9][16] = P[9][16];
|
||||
nextP[10][16] = P[10][16];
|
||||
nextP[11][16] = P[11][16];
|
||||
nextP[12][16] = P[12][16];
|
||||
nextP[13][16] = P[13][16];
|
||||
nextP[14][16] = P[14][16];
|
||||
nextP[15][16] = P[15][16];
|
||||
nextP[16][16] = P[16][16];
|
||||
nextP[0][17] = P[0][17] * SPP[5] - P[1][17] * SPP[4] + P[2][17] * SPP[7] + P[9][17] * SPP[22] + P[12][17] * SPP[18];
|
||||
nextP[1][17] = P[1][17] * SPP[6] - P[0][17] * SPP[2] - P[2][17] * SPP[8] + P[10][17] * SPP[22] + P[13][17] * SPP[17];
|
||||
nextP[2][17] = P[0][17] * SPP[14] - P[1][17] * SPP[3] + P[2][17] * SPP[13] + P[11][17] * SPP[22] + P[14][17] * SPP[16];
|
||||
nextP[3][17] = P[3][17] + P[0][17] * SPP[1] + P[1][17] * SPP[19] + P[2][17] * SPP[15] - P[15][17] * SPP[21];
|
||||
nextP[4][17] = P[4][17] + P[15][17] * SF[22] + P[0][17] * SPP[20] + P[1][17] * SPP[12] + P[2][17] * SPP[11];
|
||||
nextP[5][17] = P[5][17] + P[15][17] * SF[20] - P[0][17] * SPP[9] + P[1][17] * SPP[10] + P[2][17] * SPP[0];
|
||||
nextP[6][17] = P[6][17] + P[3][17] * dt;
|
||||
nextP[7][17] = P[7][17] + P[4][17] * dt;
|
||||
nextP[8][17] = P[8][17] + P[5][17] * dt;
|
||||
nextP[9][17] = P[9][17];
|
||||
nextP[10][17] = P[10][17];
|
||||
nextP[11][17] = P[11][17];
|
||||
nextP[12][17] = P[12][17];
|
||||
nextP[13][17] = P[13][17];
|
||||
nextP[14][17] = P[14][17];
|
||||
nextP[15][17] = P[15][17];
|
||||
nextP[16][17] = P[16][17];
|
||||
nextP[17][17] = P[17][17];
|
||||
nextP[0][18] = P[0][18] * SPP[5] - P[1][18] * SPP[4] + P[2][18] * SPP[7] + P[9][18] * SPP[22] + P[12][18] * SPP[18];
|
||||
nextP[1][18] = P[1][18] * SPP[6] - P[0][18] * SPP[2] - P[2][18] * SPP[8] + P[10][18] * SPP[22] + P[13][18] * SPP[17];
|
||||
nextP[2][18] = P[0][18] * SPP[14] - P[1][18] * SPP[3] + P[2][18] * SPP[13] + P[11][18] * SPP[22] + P[14][18] * SPP[16];
|
||||
nextP[3][18] = P[3][18] + P[0][18] * SPP[1] + P[1][18] * SPP[19] + P[2][18] * SPP[15] - P[15][18] * SPP[21];
|
||||
nextP[4][18] = P[4][18] + P[15][18] * SF[22] + P[0][18] * SPP[20] + P[1][18] * SPP[12] + P[2][18] * SPP[11];
|
||||
nextP[5][18] = P[5][18] + P[15][18] * SF[20] - P[0][18] * SPP[9] + P[1][18] * SPP[10] + P[2][18] * SPP[0];
|
||||
nextP[6][18] = P[6][18] + P[3][18] * dt;
|
||||
nextP[7][18] = P[7][18] + P[4][18] * dt;
|
||||
nextP[8][18] = P[8][18] + P[5][18] * dt;
|
||||
nextP[9][18] = P[9][18];
|
||||
nextP[10][18] = P[10][18];
|
||||
nextP[11][18] = P[11][18];
|
||||
nextP[12][18] = P[12][18];
|
||||
nextP[13][18] = P[13][18];
|
||||
nextP[14][18] = P[14][18];
|
||||
nextP[15][18] = P[15][18];
|
||||
nextP[16][18] = P[16][18];
|
||||
nextP[17][18] = P[17][18];
|
||||
nextP[18][18] = P[18][18];
|
||||
nextP[0][19] = P[0][19] * SPP[5] - P[1][19] * SPP[4] + P[2][19] * SPP[7] + P[9][19] * SPP[22] + P[12][19] * SPP[18];
|
||||
nextP[1][19] = P[1][19] * SPP[6] - P[0][19] * SPP[2] - P[2][19] * SPP[8] + P[10][19] * SPP[22] + P[13][19] * SPP[17];
|
||||
nextP[2][19] = P[0][19] * SPP[14] - P[1][19] * SPP[3] + P[2][19] * SPP[13] + P[11][19] * SPP[22] + P[14][19] * SPP[16];
|
||||
nextP[3][19] = P[3][19] + P[0][19] * SPP[1] + P[1][19] * SPP[19] + P[2][19] * SPP[15] - P[15][19] * SPP[21];
|
||||
nextP[4][19] = P[4][19] + P[15][19] * SF[22] + P[0][19] * SPP[20] + P[1][19] * SPP[12] + P[2][19] * SPP[11];
|
||||
nextP[5][19] = P[5][19] + P[15][19] * SF[20] - P[0][19] * SPP[9] + P[1][19] * SPP[10] + P[2][19] * SPP[0];
|
||||
nextP[6][19] = P[6][19] + P[3][19] * dt;
|
||||
nextP[7][19] = P[7][19] + P[4][19] * dt;
|
||||
nextP[8][19] = P[8][19] + P[5][19] * dt;
|
||||
nextP[9][19] = P[9][19];
|
||||
nextP[10][19] = P[10][19];
|
||||
nextP[11][19] = P[11][19];
|
||||
nextP[12][19] = P[12][19];
|
||||
nextP[13][19] = P[13][19];
|
||||
nextP[14][19] = P[14][19];
|
||||
nextP[15][19] = P[15][19];
|
||||
nextP[16][19] = P[16][19];
|
||||
nextP[17][19] = P[17][19];
|
||||
nextP[18][19] = P[18][19];
|
||||
nextP[19][19] = P[19][19];
|
||||
nextP[0][20] = P[0][20] * SPP[5] - P[1][20] * SPP[4] + P[2][20] * SPP[7] + P[9][20] * SPP[22] + P[12][20] * SPP[18];
|
||||
nextP[1][20] = P[1][20] * SPP[6] - P[0][20] * SPP[2] - P[2][20] * SPP[8] + P[10][20] * SPP[22] + P[13][20] * SPP[17];
|
||||
nextP[2][20] = P[0][20] * SPP[14] - P[1][20] * SPP[3] + P[2][20] * SPP[13] + P[11][20] * SPP[22] + P[14][20] * SPP[16];
|
||||
nextP[3][20] = P[3][20] + P[0][20] * SPP[1] + P[1][20] * SPP[19] + P[2][20] * SPP[15] - P[15][20] * SPP[21];
|
||||
nextP[4][20] = P[4][20] + P[15][20] * SF[22] + P[0][20] * SPP[20] + P[1][20] * SPP[12] + P[2][20] * SPP[11];
|
||||
nextP[5][20] = P[5][20] + P[15][20] * SF[20] - P[0][20] * SPP[9] + P[1][20] * SPP[10] + P[2][20] * SPP[0];
|
||||
nextP[6][20] = P[6][20] + P[3][20] * dt;
|
||||
nextP[7][20] = P[7][20] + P[4][20] * dt;
|
||||
nextP[8][20] = P[8][20] + P[5][20] * dt;
|
||||
nextP[9][20] = P[9][20];
|
||||
nextP[10][20] = P[10][20];
|
||||
nextP[11][20] = P[11][20];
|
||||
nextP[12][20] = P[12][20];
|
||||
nextP[13][20] = P[13][20];
|
||||
nextP[14][20] = P[14][20];
|
||||
nextP[15][20] = P[15][20];
|
||||
nextP[16][20] = P[16][20];
|
||||
nextP[17][20] = P[17][20];
|
||||
nextP[18][20] = P[18][20];
|
||||
nextP[19][20] = P[19][20];
|
||||
nextP[20][20] = P[20][20];
|
||||
nextP[0][21] = P[0][21] * SPP[5] - P[1][21] * SPP[4] + P[2][21] * SPP[7] + P[9][21] * SPP[22] + P[12][21] * SPP[18];
|
||||
nextP[1][21] = P[1][21] * SPP[6] - P[0][21] * SPP[2] - P[2][21] * SPP[8] + P[10][21] * SPP[22] + P[13][21] * SPP[17];
|
||||
nextP[2][21] = P[0][21] * SPP[14] - P[1][21] * SPP[3] + P[2][21] * SPP[13] + P[11][21] * SPP[22] + P[14][21] * SPP[16];
|
||||
nextP[3][21] = P[3][21] + P[0][21] * SPP[1] + P[1][21] * SPP[19] + P[2][21] * SPP[15] - P[15][21] * SPP[21];
|
||||
nextP[4][21] = P[4][21] + P[15][21] * SF[22] + P[0][21] * SPP[20] + P[1][21] * SPP[12] + P[2][21] * SPP[11];
|
||||
nextP[5][21] = P[5][21] + P[15][21] * SF[20] - P[0][21] * SPP[9] + P[1][21] * SPP[10] + P[2][21] * SPP[0];
|
||||
nextP[6][21] = P[6][21] + P[3][21] * dt;
|
||||
nextP[7][21] = P[7][21] + P[4][21] * dt;
|
||||
nextP[8][21] = P[8][21] + P[5][21] * dt;
|
||||
nextP[9][21] = P[9][21];
|
||||
nextP[10][21] = P[10][21];
|
||||
nextP[11][21] = P[11][21];
|
||||
nextP[12][21] = P[12][21];
|
||||
nextP[13][21] = P[13][21];
|
||||
nextP[14][21] = P[14][21];
|
||||
nextP[15][21] = P[15][21];
|
||||
nextP[16][21] = P[16][21];
|
||||
nextP[17][21] = P[17][21];
|
||||
nextP[18][21] = P[18][21];
|
||||
nextP[19][21] = P[19][21];
|
||||
nextP[20][21] = P[20][21];
|
||||
nextP[21][21] = P[21][21];
|
||||
nextP[0][22] = P[0][22] * SPP[5] - P[1][22] * SPP[4] + P[2][22] * SPP[7] + P[9][22] * SPP[22] + P[12][22] * SPP[18];
|
||||
nextP[1][22] = P[1][22] * SPP[6] - P[0][22] * SPP[2] - P[2][22] * SPP[8] + P[10][22] * SPP[22] + P[13][22] * SPP[17];
|
||||
nextP[2][22] = P[0][22] * SPP[14] - P[1][22] * SPP[3] + P[2][22] * SPP[13] + P[11][22] * SPP[22] + P[14][22] * SPP[16];
|
||||
nextP[3][22] = P[3][22] + P[0][22] * SPP[1] + P[1][22] * SPP[19] + P[2][22] * SPP[15] - P[15][22] * SPP[21];
|
||||
nextP[4][22] = P[4][22] + P[15][22] * SF[22] + P[0][22] * SPP[20] + P[1][22] * SPP[12] + P[2][22] * SPP[11];
|
||||
nextP[5][22] = P[5][22] + P[15][22] * SF[20] - P[0][22] * SPP[9] + P[1][22] * SPP[10] + P[2][22] * SPP[0];
|
||||
nextP[6][22] = P[6][22] + P[3][22] * dt;
|
||||
nextP[7][22] = P[7][22] + P[4][22] * dt;
|
||||
nextP[8][22] = P[8][22] + P[5][22] * dt;
|
||||
nextP[9][22] = P[9][22];
|
||||
nextP[10][22] = P[10][22];
|
||||
nextP[11][22] = P[11][22];
|
||||
nextP[12][22] = P[12][22];
|
||||
nextP[13][22] = P[13][22];
|
||||
nextP[14][22] = P[14][22];
|
||||
nextP[15][22] = P[15][22];
|
||||
nextP[16][22] = P[16][22];
|
||||
nextP[17][22] = P[17][22];
|
||||
nextP[18][22] = P[18][22];
|
||||
nextP[19][22] = P[19][22];
|
||||
nextP[20][22] = P[20][22];
|
||||
nextP[21][22] = P[21][22];
|
||||
nextP[22][22] = P[22][22];
|
||||
nextP[0][23] = P[0][23] * SPP[5] - P[1][23] * SPP[4] + P[2][23] * SPP[7] + P[9][23] * SPP[22] + P[12][23] * SPP[18];
|
||||
nextP[1][23] = P[1][23] * SPP[6] - P[0][23] * SPP[2] - P[2][23] * SPP[8] + P[10][23] * SPP[22] + P[13][23] * SPP[17];
|
||||
nextP[2][23] = P[0][23] * SPP[14] - P[1][23] * SPP[3] + P[2][23] * SPP[13] + P[11][23] * SPP[22] + P[14][23] * SPP[16];
|
||||
nextP[3][23] = P[3][23] + P[0][23] * SPP[1] + P[1][23] * SPP[19] + P[2][23] * SPP[15] - P[15][23] * SPP[21];
|
||||
nextP[4][23] = P[4][23] + P[15][23] * SF[22] + P[0][23] * SPP[20] + P[1][23] * SPP[12] + P[2][23] * SPP[11];
|
||||
nextP[5][23] = P[5][23] + P[15][23] * SF[20] - P[0][23] * SPP[9] + P[1][23] * SPP[10] + P[2][23] * SPP[0];
|
||||
nextP[6][23] = P[6][23] + P[3][23] * dt;
|
||||
nextP[7][23] = P[7][23] + P[4][23] * dt;
|
||||
nextP[8][23] = P[8][23] + P[5][23] * dt;
|
||||
nextP[9][23] = P[9][23];
|
||||
nextP[10][23] = P[10][23];
|
||||
nextP[11][23] = P[11][23];
|
||||
nextP[12][23] = P[12][23];
|
||||
nextP[13][23] = P[13][23];
|
||||
nextP[14][23] = P[14][23];
|
||||
nextP[15][23] = P[15][23];
|
||||
nextP[16][23] = P[16][23];
|
||||
nextP[17][23] = P[17][23];
|
||||
nextP[18][23] = P[18][23];
|
||||
nextP[19][23] = P[19][23];
|
||||
nextP[20][23] = P[20][23];
|
||||
nextP[21][23] = P[21][23];
|
||||
nextP[22][23] = P[22][23];
|
||||
nextP[23][23] = P[23][23];
|
||||
|
||||
// Don't do covariance prediction on magnetic field states unless we are using 3-axis fusion
|
||||
if (_control_status.flags.mag_3D) {
|
||||
// Check if we have just transitioned into 3-axis fusion and set the state variances
|
||||
if (!_control_status_prev.flags.mag_3D) {
|
||||
for (uint8_t index = 16; index <= 21; index++) {
|
||||
P[index][index] = sq(fmaxf(_params.mag_noise, 0.001f));
|
||||
}
|
||||
}
|
||||
|
||||
nextP[0][16] = P[0][16] * SPP[5] - P[1][16] * SPP[4] + P[2][16] * SPP[7] + P[9][16] * SPP[22] + P[12][16] * SPP[18];
|
||||
nextP[1][16] = P[1][16] * SPP[6] - P[0][16] * SPP[2] - P[2][16] * SPP[8] + P[10][16] * SPP[22] + P[13][16] * SPP[17];
|
||||
nextP[2][16] = P[0][16] * SPP[14] - P[1][16] * SPP[3] + P[2][16] * SPP[13] + P[11][16] * SPP[22] + P[14][16] * SPP[16];
|
||||
nextP[3][16] = P[3][16] + P[0][16] * SPP[1] + P[1][16] * SPP[19] + P[2][16] * SPP[15] - P[15][16] * SPP[21];
|
||||
nextP[4][16] = P[4][16] + P[15][16] * SF[22] + P[0][16] * SPP[20] + P[1][16] * SPP[12] + P[2][16] * SPP[11];
|
||||
nextP[5][16] = P[5][16] + P[15][16] * SF[20] - P[0][16] * SPP[9] + P[1][16] * SPP[10] + P[2][16] * SPP[0];
|
||||
nextP[6][16] = P[6][16] + P[3][16] * dt;
|
||||
nextP[7][16] = P[7][16] + P[4][16] * dt;
|
||||
nextP[8][16] = P[8][16] + P[5][16] * dt;
|
||||
nextP[9][16] = P[9][16];
|
||||
nextP[10][16] = P[10][16];
|
||||
nextP[11][16] = P[11][16];
|
||||
nextP[12][16] = P[12][16];
|
||||
nextP[13][16] = P[13][16];
|
||||
nextP[14][16] = P[14][16];
|
||||
nextP[15][16] = P[15][16];
|
||||
nextP[16][16] = P[16][16];
|
||||
|
||||
nextP[0][17] = P[0][17] * SPP[5] - P[1][17] * SPP[4] + P[2][17] * SPP[7] + P[9][17] * SPP[22] + P[12][17] * SPP[18];
|
||||
nextP[1][17] = P[1][17] * SPP[6] - P[0][17] * SPP[2] - P[2][17] * SPP[8] + P[10][17] * SPP[22] + P[13][17] * SPP[17];
|
||||
nextP[2][17] = P[0][17] * SPP[14] - P[1][17] * SPP[3] + P[2][17] * SPP[13] + P[11][17] * SPP[22] + P[14][17] * SPP[16];
|
||||
nextP[3][17] = P[3][17] + P[0][17] * SPP[1] + P[1][17] * SPP[19] + P[2][17] * SPP[15] - P[15][17] * SPP[21];
|
||||
nextP[4][17] = P[4][17] + P[15][17] * SF[22] + P[0][17] * SPP[20] + P[1][17] * SPP[12] + P[2][17] * SPP[11];
|
||||
nextP[5][17] = P[5][17] + P[15][17] * SF[20] - P[0][17] * SPP[9] + P[1][17] * SPP[10] + P[2][17] * SPP[0];
|
||||
nextP[6][17] = P[6][17] + P[3][17] * dt;
|
||||
nextP[7][17] = P[7][17] + P[4][17] * dt;
|
||||
nextP[8][17] = P[8][17] + P[5][17] * dt;
|
||||
nextP[9][17] = P[9][17];
|
||||
nextP[10][17] = P[10][17];
|
||||
nextP[11][17] = P[11][17];
|
||||
nextP[12][17] = P[12][17];
|
||||
nextP[13][17] = P[13][17];
|
||||
nextP[14][17] = P[14][17];
|
||||
nextP[15][17] = P[15][17];
|
||||
nextP[16][17] = P[16][17];
|
||||
nextP[17][17] = P[17][17];
|
||||
|
||||
nextP[0][18] = P[0][18] * SPP[5] - P[1][18] * SPP[4] + P[2][18] * SPP[7] + P[9][18] * SPP[22] + P[12][18] * SPP[18];
|
||||
nextP[1][18] = P[1][18] * SPP[6] - P[0][18] * SPP[2] - P[2][18] * SPP[8] + P[10][18] * SPP[22] + P[13][18] * SPP[17];
|
||||
nextP[2][18] = P[0][18] * SPP[14] - P[1][18] * SPP[3] + P[2][18] * SPP[13] + P[11][18] * SPP[22] + P[14][18] * SPP[16];
|
||||
nextP[3][18] = P[3][18] + P[0][18] * SPP[1] + P[1][18] * SPP[19] + P[2][18] * SPP[15] - P[15][18] * SPP[21];
|
||||
nextP[4][18] = P[4][18] + P[15][18] * SF[22] + P[0][18] * SPP[20] + P[1][18] * SPP[12] + P[2][18] * SPP[11];
|
||||
nextP[5][18] = P[5][18] + P[15][18] * SF[20] - P[0][18] * SPP[9] + P[1][18] * SPP[10] + P[2][18] * SPP[0];
|
||||
nextP[6][18] = P[6][18] + P[3][18] * dt;
|
||||
nextP[7][18] = P[7][18] + P[4][18] * dt;
|
||||
nextP[8][18] = P[8][18] + P[5][18] * dt;
|
||||
nextP[9][18] = P[9][18];
|
||||
nextP[10][18] = P[10][18];
|
||||
nextP[11][18] = P[11][18];
|
||||
nextP[12][18] = P[12][18];
|
||||
nextP[13][18] = P[13][18];
|
||||
nextP[14][18] = P[14][18];
|
||||
nextP[15][18] = P[15][18];
|
||||
nextP[16][18] = P[16][18];
|
||||
nextP[17][18] = P[17][18];
|
||||
nextP[18][18] = P[18][18];
|
||||
|
||||
nextP[0][19] = P[0][19] * SPP[5] - P[1][19] * SPP[4] + P[2][19] * SPP[7] + P[9][19] * SPP[22] + P[12][19] * SPP[18];
|
||||
nextP[1][19] = P[1][19] * SPP[6] - P[0][19] * SPP[2] - P[2][19] * SPP[8] + P[10][19] * SPP[22] + P[13][19] * SPP[17];
|
||||
nextP[2][19] = P[0][19] * SPP[14] - P[1][19] * SPP[3] + P[2][19] * SPP[13] + P[11][19] * SPP[22] + P[14][19] * SPP[16];
|
||||
nextP[3][19] = P[3][19] + P[0][19] * SPP[1] + P[1][19] * SPP[19] + P[2][19] * SPP[15] - P[15][19] * SPP[21];
|
||||
nextP[4][19] = P[4][19] + P[15][19] * SF[22] + P[0][19] * SPP[20] + P[1][19] * SPP[12] + P[2][19] * SPP[11];
|
||||
nextP[5][19] = P[5][19] + P[15][19] * SF[20] - P[0][19] * SPP[9] + P[1][19] * SPP[10] + P[2][19] * SPP[0];
|
||||
nextP[6][19] = P[6][19] + P[3][19] * dt;
|
||||
nextP[7][19] = P[7][19] + P[4][19] * dt;
|
||||
nextP[8][19] = P[8][19] + P[5][19] * dt;
|
||||
nextP[9][19] = P[9][19];
|
||||
nextP[10][19] = P[10][19];
|
||||
nextP[11][19] = P[11][19];
|
||||
nextP[12][19] = P[12][19];
|
||||
nextP[13][19] = P[13][19];
|
||||
nextP[14][19] = P[14][19];
|
||||
nextP[15][19] = P[15][19];
|
||||
nextP[16][19] = P[16][19];
|
||||
nextP[17][19] = P[17][19];
|
||||
nextP[18][19] = P[18][19];
|
||||
nextP[19][19] = P[19][19];
|
||||
|
||||
nextP[0][20] = P[0][20] * SPP[5] - P[1][20] * SPP[4] + P[2][20] * SPP[7] + P[9][20] * SPP[22] + P[12][20] * SPP[18];
|
||||
nextP[1][20] = P[1][20] * SPP[6] - P[0][20] * SPP[2] - P[2][20] * SPP[8] + P[10][20] * SPP[22] + P[13][20] * SPP[17];
|
||||
nextP[2][20] = P[0][20] * SPP[14] - P[1][20] * SPP[3] + P[2][20] * SPP[13] + P[11][20] * SPP[22] + P[14][20] * SPP[16];
|
||||
nextP[3][20] = P[3][20] + P[0][20] * SPP[1] + P[1][20] * SPP[19] + P[2][20] * SPP[15] - P[15][20] * SPP[21];
|
||||
nextP[4][20] = P[4][20] + P[15][20] * SF[22] + P[0][20] * SPP[20] + P[1][20] * SPP[12] + P[2][20] * SPP[11];
|
||||
nextP[5][20] = P[5][20] + P[15][20] * SF[20] - P[0][20] * SPP[9] + P[1][20] * SPP[10] + P[2][20] * SPP[0];
|
||||
nextP[6][20] = P[6][20] + P[3][20] * dt;
|
||||
nextP[7][20] = P[7][20] + P[4][20] * dt;
|
||||
nextP[8][20] = P[8][20] + P[5][20] * dt;
|
||||
nextP[9][20] = P[9][20];
|
||||
nextP[10][20] = P[10][20];
|
||||
nextP[11][20] = P[11][20];
|
||||
nextP[12][20] = P[12][20];
|
||||
nextP[13][20] = P[13][20];
|
||||
nextP[14][20] = P[14][20];
|
||||
nextP[15][20] = P[15][20];
|
||||
nextP[16][20] = P[16][20];
|
||||
nextP[17][20] = P[17][20];
|
||||
nextP[18][20] = P[18][20];
|
||||
nextP[19][20] = P[19][20];
|
||||
nextP[20][20] = P[20][20];
|
||||
|
||||
nextP[0][21] = P[0][21] * SPP[5] - P[1][21] * SPP[4] + P[2][21] * SPP[7] + P[9][21] * SPP[22] + P[12][21] * SPP[18];
|
||||
nextP[1][21] = P[1][21] * SPP[6] - P[0][21] * SPP[2] - P[2][21] * SPP[8] + P[10][21] * SPP[22] + P[13][21] * SPP[17];
|
||||
nextP[2][21] = P[0][21] * SPP[14] - P[1][21] * SPP[3] + P[2][21] * SPP[13] + P[11][21] * SPP[22] + P[14][21] * SPP[16];
|
||||
nextP[3][21] = P[3][21] + P[0][21] * SPP[1] + P[1][21] * SPP[19] + P[2][21] * SPP[15] - P[15][21] * SPP[21];
|
||||
nextP[4][21] = P[4][21] + P[15][21] * SF[22] + P[0][21] * SPP[20] + P[1][21] * SPP[12] + P[2][21] * SPP[11];
|
||||
nextP[5][21] = P[5][21] + P[15][21] * SF[20] - P[0][21] * SPP[9] + P[1][21] * SPP[10] + P[2][21] * SPP[0];
|
||||
nextP[6][21] = P[6][21] + P[3][21] * dt;
|
||||
nextP[7][21] = P[7][21] + P[4][21] * dt;
|
||||
nextP[8][21] = P[8][21] + P[5][21] * dt;
|
||||
nextP[9][21] = P[9][21];
|
||||
nextP[10][21] = P[10][21];
|
||||
nextP[11][21] = P[11][21];
|
||||
nextP[12][21] = P[12][21];
|
||||
nextP[13][21] = P[13][21];
|
||||
nextP[14][21] = P[14][21];
|
||||
nextP[15][21] = P[15][21];
|
||||
nextP[16][21] = P[16][21];
|
||||
nextP[17][21] = P[17][21];
|
||||
nextP[18][21] = P[18][21];
|
||||
nextP[19][21] = P[19][21];
|
||||
nextP[20][21] = P[20][21];
|
||||
nextP[21][21] = P[21][21];
|
||||
}
|
||||
|
||||
// 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) {
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
nextP[0][22] = P[0][22] * SPP[5] - P[1][22] * SPP[4] + P[2][22] * SPP[7] + P[9][22] * SPP[22] + P[12][22] * SPP[18];
|
||||
nextP[1][22] = P[1][22] * SPP[6] - P[0][22] * SPP[2] - P[2][22] * SPP[8] + P[10][22] * SPP[22] + P[13][22] * SPP[17];
|
||||
nextP[2][22] = P[0][22] * SPP[14] - P[1][22] * SPP[3] + P[2][22] * SPP[13] + P[11][22] * SPP[22] + P[14][22] * SPP[16];
|
||||
nextP[3][22] = P[3][22] + P[0][22] * SPP[1] + P[1][22] * SPP[19] + P[2][22] * SPP[15] - P[15][22] * SPP[21];
|
||||
nextP[4][22] = P[4][22] + P[15][22] * SF[22] + P[0][22] * SPP[20] + P[1][22] * SPP[12] + P[2][22] * SPP[11];
|
||||
nextP[5][22] = P[5][22] + P[15][22] * SF[20] - P[0][22] * SPP[9] + P[1][22] * SPP[10] + P[2][22] * SPP[0];
|
||||
nextP[6][22] = P[6][22] + P[3][22] * dt;
|
||||
nextP[7][22] = P[7][22] + P[4][22] * dt;
|
||||
nextP[8][22] = P[8][22] + P[5][22] * dt;
|
||||
nextP[9][22] = P[9][22];
|
||||
nextP[10][22] = P[10][22];
|
||||
nextP[11][22] = P[11][22];
|
||||
nextP[12][22] = P[12][22];
|
||||
nextP[13][22] = P[13][22];
|
||||
nextP[14][22] = P[14][22];
|
||||
nextP[15][22] = P[15][22];
|
||||
nextP[16][22] = P[16][22];
|
||||
nextP[17][22] = P[17][22];
|
||||
nextP[18][22] = P[18][22];
|
||||
nextP[19][22] = P[19][22];
|
||||
nextP[20][22] = P[20][22];
|
||||
nextP[21][22] = P[21][22];
|
||||
nextP[22][22] = P[22][22];
|
||||
|
||||
nextP[0][23] = P[0][23] * SPP[5] - P[1][23] * SPP[4] + P[2][23] * SPP[7] + P[9][23] * SPP[22] + P[12][23] * SPP[18];
|
||||
nextP[1][23] = P[1][23] * SPP[6] - P[0][23] * SPP[2] - P[2][23] * SPP[8] + P[10][23] * SPP[22] + P[13][23] * SPP[17];
|
||||
nextP[2][23] = P[0][23] * SPP[14] - P[1][23] * SPP[3] + P[2][23] * SPP[13] + P[11][23] * SPP[22] + P[14][23] * SPP[16];
|
||||
nextP[3][23] = P[3][23] + P[0][23] * SPP[1] + P[1][23] * SPP[19] + P[2][23] * SPP[15] - P[15][23] * SPP[21];
|
||||
nextP[4][23] = P[4][23] + P[15][23] * SF[22] + P[0][23] * SPP[20] + P[1][23] * SPP[12] + P[2][23] * SPP[11];
|
||||
nextP[5][23] = P[5][23] + P[15][23] * SF[20] - P[0][23] * SPP[9] + P[1][23] * SPP[10] + P[2][23] * SPP[0];
|
||||
nextP[6][23] = P[6][23] + P[3][23] * dt;
|
||||
nextP[7][23] = P[7][23] + P[4][23] * dt;
|
||||
nextP[8][23] = P[8][23] + P[5][23] * dt;
|
||||
nextP[9][23] = P[9][23];
|
||||
nextP[10][23] = P[10][23];
|
||||
nextP[11][23] = P[11][23];
|
||||
nextP[12][23] = P[12][23];
|
||||
nextP[13][23] = P[13][23];
|
||||
nextP[14][23] = P[14][23];
|
||||
nextP[15][23] = P[15][23];
|
||||
nextP[16][23] = P[16][23];
|
||||
nextP[17][23] = P[17][23];
|
||||
nextP[18][23] = P[18][23];
|
||||
nextP[19][23] = P[19][23];
|
||||
nextP[20][23] = P[20][23];
|
||||
nextP[21][23] = P[21][23];
|
||||
nextP[22][23] = P[22][23];
|
||||
nextP[23][23] = P[23][23];
|
||||
|
||||
}
|
||||
|
||||
// add process noise
|
||||
for (unsigned i = 0; i < _k_num_states; i++) {
|
||||
|
|
|
|||
Loading…
Reference in New Issue