forked from Archive/PX4-Autopilot
Add generated code for 24 state Nav Filter
This commit is contained in:
Paul Riseborough
parent
363b7d8c36
commit
af6a196187
|
|
@ -0,0 +1,50 @@
|
|||
// Auto code for fusion of true airspeed
|
||||
|
||||
// Calculate the observation jacobian
|
||||
|
||||
// intermediate variable from algebraic optimisation
|
||||
float SH_TAS[3];
|
||||
SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd)));
|
||||
SH_TAS[1] = (SH_TAS[0]*(2*ve - 2*vwe))/2;
|
||||
SH_TAS[2] = (SH_TAS[0]*(2*vn - 2*vwn))/2;
|
||||
|
||||
// observation jacobian
|
||||
float H_TAS[24];
|
||||
H_TAS[3] = SH_TAS[2];
|
||||
H_TAS[4] = SH_TAS[1];
|
||||
H_TAS[5] = vd*SH_TAS[0];
|
||||
H_TAS[22] = -SH_TAS[2];
|
||||
H_TAS[23] = -SH_TAS[1];
|
||||
|
||||
// calculate the Kalman gain matrix
|
||||
|
||||
// intermediate variables - note SK_TAS[0] is 1/(innovation variance)
|
||||
float SK_TAS[2];
|
||||
SK_TAS[0] = 1/(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]));
|
||||
SK_TAS[1] = SH_TAS[1];
|
||||
|
||||
float Kfusion[24];
|
||||
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[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]);
|
||||
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]);
|
||||
|
|
@ -0,0 +1,377 @@
|
|||
// Auto code for covariance prediction
|
||||
|
||||
// Intermediate expressions obtained from algebraic optimisation
|
||||
float SF[25];
|
||||
SF[0] = daz_b/2 + dazNoise/2 - (daz*daz_s)/2;
|
||||
SF[1] = day_b/2 + dayNoise/2 - (day*day_s)/2;
|
||||
SF[2] = dax_b/2 + daxNoise/2 - (dax*dax_s)/2;
|
||||
SF[3] = q3/2 - (q0*SF[0])/2 + (q1*SF[1])/2 - (q2*SF[2])/2;
|
||||
SF[4] = q0/2 - (q1*SF[2])/2 - (q2*SF[1])/2 + (q3*SF[0])/2;
|
||||
SF[5] = q1/2 + (q0*SF[2])/2 - (q2*SF[0])/2 - (q3*SF[1])/2;
|
||||
SF[6] = q3/2 + (q0*SF[0])/2 - (q1*SF[1])/2 - (q2*SF[2])/2;
|
||||
SF[7] = q0/2 - (q1*SF[2])/2 + (q2*SF[1])/2 - (q3*SF[0])/2;
|
||||
SF[8] = q0/2 + (q1*SF[2])/2 - (q2*SF[1])/2 - (q3*SF[0])/2;
|
||||
SF[9] = q2/2 + (q0*SF[1])/2 + (q1*SF[0])/2 + (q3*SF[2])/2;
|
||||
SF[10] = q2/2 - (q0*SF[1])/2 - (q1*SF[0])/2 + (q3*SF[2])/2;
|
||||
SF[11] = q2/2 + (q0*SF[1])/2 - (q1*SF[0])/2 - (q3*SF[2])/2;
|
||||
SF[12] = q1/2 + (q0*SF[2])/2 + (q2*SF[0])/2 + (q3*SF[1])/2;
|
||||
SF[13] = q1/2 - (q0*SF[2])/2 + (q2*SF[0])/2 - (q3*SF[1])/2;
|
||||
SF[14] = q3/2 + (q0*SF[0])/2 + (q1*SF[1])/2 + (q2*SF[2])/2;
|
||||
SF[15] = - sq(q0) - sq(q1) - sq(q2) - sq(q3);
|
||||
SF[16] = dvz_b - dvz + dvzNoise;
|
||||
SF[17] = dvx - dvxNoise;
|
||||
SF[18] = dvy - dvyNoise;
|
||||
SF[19] = sq(q2);
|
||||
SF[20] = SF[19] - sq(q0) + sq(q1) - sq(q3);
|
||||
SF[21] = SF[19] + sq(q0) - sq(q1) - sq(q3);
|
||||
SF[22] = 2*q0*q1 - 2*q2*q3;
|
||||
SF[23] = SF[19] - sq(q0) - sq(q1) + sq(q3);
|
||||
SF[24] = 2*q1*q2;
|
||||
|
||||
float SG[5];
|
||||
SG[0] = - sq(q0) - sq(q1) - sq(q2) - sq(q3);
|
||||
SG[1] = sq(q3);
|
||||
SG[2] = sq(q2);
|
||||
SG[3] = sq(q1);
|
||||
SG[4] = sq(q0);
|
||||
|
||||
float SQ[8];
|
||||
SQ[0] = - dvyNoise*(2*q0*q1 + 2*q2*q3)*(SG[1] - SG[2] + SG[3] - SG[4]) - dvzNoise*(2*q0*q1 - 2*q2*q3)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvxNoise*(2*q0*q2 - 2*q1*q3)*(2*q0*q3 + 2*q1*q2);
|
||||
SQ[1] = dvxNoise*(2*q0*q2 - 2*q1*q3)*(SG[1] + SG[2] - SG[3] - SG[4]) + dvzNoise*(2*q0*q2 + 2*q1*q3)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvyNoise*(2*q0*q1 + 2*q2*q3)*(2*q0*q3 - 2*q1*q2);
|
||||
SQ[2] = dvyNoise*(2*q0*q3 - 2*q1*q2)*(SG[1] - SG[2] + SG[3] - SG[4]) - dvxNoise*(2*q0*q3 + 2*q1*q2)*(SG[1] + SG[2] - SG[3] - SG[4]) - dvzNoise*(2*q0*q1 - 2*q2*q3)*(2*q0*q2 + 2*q1*q3);
|
||||
SQ[3] = sq(SG[0]);
|
||||
SQ[4] = 2*q2*q3;
|
||||
SQ[5] = 2*q1*q3;
|
||||
SQ[6] = 2*q1*q2;
|
||||
SQ[7] = SG[4];
|
||||
|
||||
float SPP[23];
|
||||
SPP[0] = SF[17]*(2*q0*q1 + 2*q2*q3) + SF[18]*(2*q0*q2 - 2*q1*q3);
|
||||
SPP[1] = SF[18]*(2*q0*q2 + 2*q1*q3) + SF[16]*(SF[24] - 2*q0*q3);
|
||||
SPP[2] = 2*q3*SF[8] + 2*q1*SF[11] - 2*q0*SF[14] - 2*q2*SF[13];
|
||||
SPP[3] = 2*q1*SF[7] + 2*q2*SF[6] - 2*q0*SF[12] - 2*q3*SF[10];
|
||||
SPP[4] = 2*q0*SF[6] - 2*q3*SF[7] - 2*q1*SF[10] + 2*q2*SF[12];
|
||||
SPP[5] = 2*q0*SF[8] + 2*q2*SF[11] + 2*q1*SF[13] + 2*q3*SF[14];
|
||||
SPP[6] = 2*q0*SF[7] + 2*q3*SF[6] + 2*q2*SF[10] + 2*q1*SF[12];
|
||||
SPP[7] = 2*q1*SF[3] - 2*q2*SF[4] - 2*q3*SF[5] + 2*q0*SF[9];
|
||||
SPP[8] = 2*q0*SF[5] - 2*q1*SF[4] - 2*q2*SF[3] + 2*q3*SF[9];
|
||||
SPP[9] = SF[18]*SF[20] - SF[16]*(2*q0*q1 + 2*q2*q3);
|
||||
SPP[10] = SF[17]*SF[20] + SF[16]*(2*q0*q2 - 2*q1*q3);
|
||||
SPP[11] = SF[17]*SF[21] - SF[18]*(SF[24] + 2*q0*q3);
|
||||
SPP[12] = SF[17]*SF[22] - SF[16]*(SF[24] + 2*q0*q3);
|
||||
SPP[13] = 2*q0*SF[4] + 2*q1*SF[5] + 2*q3*SF[3] + 2*q2*SF[9];
|
||||
SPP[14] = 2*q2*SF[8] - 2*q0*SF[11] - 2*q1*SF[14] + 2*q3*SF[13];
|
||||
SPP[15] = SF[18]*SF[23] + SF[17]*(SF[24] - 2*q0*q3);
|
||||
SPP[16] = daz*SF[19] + daz*sq(q0) + daz*sq(q1) + daz*sq(q3);
|
||||
SPP[17] = day*SF[19] + day*sq(q0) + day*sq(q1) + day*sq(q3);
|
||||
SPP[18] = dax*SF[19] + dax*sq(q0) + dax*sq(q1) + dax*sq(q3);
|
||||
SPP[19] = SF[16]*SF[23] - SF[17]*(2*q0*q2 + 2*q1*q3);
|
||||
SPP[20] = SF[16]*SF[21] - SF[18]*SF[22];
|
||||
SPP[21] = 2*q0*q2 + 2*q1*q3;
|
||||
SPP[22] = SF[15];
|
||||
|
||||
// Calculate uppder diagonal elements of the predicted covariance matrix
|
||||
// Use symmetry to assign value to lower diagonal
|
||||
// Note: this matrix does not include the process noise for stationary states, it only includes the effect of noise on the inertial measurements.
|
||||
// Process noise for stationary states must be added later.
|
||||
float nextP[24][24];
|
||||
nextP[0][0] = daxNoise*SQ[3] + SPP[5]*(P[0][0]*SPP[5] - P[1][0]*SPP[4] + P[2][0]*SPP[7] + P[9][0]*SPP[22] + P[12][0]*SPP[18]) - SPP[4]*(P[0][1]*SPP[5] - P[1][1]*SPP[4] + P[2][1]*SPP[7] + P[9][1]*SPP[22] + P[12][1]*SPP[18]) + SPP[7]*(P[0][2]*SPP[5] - P[1][2]*SPP[4] + P[2][2]*SPP[7] + P[9][2]*SPP[22] + P[12][2]*SPP[18]) + SPP[22]*(P[0][9]*SPP[5] - P[1][9]*SPP[4] + P[2][9]*SPP[7] + P[9][9]*SPP[22] + P[12][9]*SPP[18]) + SPP[18]*(P[0][12]*SPP[5] - P[1][12]*SPP[4] + P[2][12]*SPP[7] + P[9][12]*SPP[22] + P[12][12]*SPP[18]);
|
||||
nextP[0][1] = SPP[6]*(P[0][1]*SPP[5] - P[1][1]*SPP[4] + P[2][1]*SPP[7] + P[9][1]*SPP[22] + P[12][1]*SPP[18]) - SPP[2]*(P[0][0]*SPP[5] - P[1][0]*SPP[4] + P[2][0]*SPP[7] + P[9][0]*SPP[22] + P[12][0]*SPP[18]) - SPP[8]*(P[0][2]*SPP[5] - P[1][2]*SPP[4] + P[2][2]*SPP[7] + P[9][2]*SPP[22] + P[12][2]*SPP[18]) + SPP[22]*(P[0][10]*SPP[5] - P[1][10]*SPP[4] + P[2][10]*SPP[7] + P[9][10]*SPP[22] + P[12][10]*SPP[18]) + SPP[17]*(P[0][13]*SPP[5] - P[1][13]*SPP[4] + P[2][13]*SPP[7] + P[9][13]*SPP[22] + P[12][13]*SPP[18]);
|
||||
nextP[1][1] = dayNoise*SQ[3] - SPP[2]*(P[1][0]*SPP[6] - P[0][0]*SPP[2] - P[2][0]*SPP[8] + P[10][0]*SPP[22] + P[13][0]*SPP[17]) + SPP[6]*(P[1][1]*SPP[6] - P[0][1]*SPP[2] - P[2][1]*SPP[8] + P[10][1]*SPP[22] + P[13][1]*SPP[17]) - SPP[8]*(P[1][2]*SPP[6] - P[0][2]*SPP[2] - P[2][2]*SPP[8] + P[10][2]*SPP[22] + P[13][2]*SPP[17]) + SPP[22]*(P[1][10]*SPP[6] - P[0][10]*SPP[2] - P[2][10]*SPP[8] + P[10][10]*SPP[22] + P[13][10]*SPP[17]) + SPP[17]*(P[1][13]*SPP[6] - P[0][13]*SPP[2] - P[2][13]*SPP[8] + P[10][13]*SPP[22] + P[13][13]*SPP[17]);
|
||||
nextP[0][2] = SPP[14]*(P[0][0]*SPP[5] - P[1][0]*SPP[4] + P[2][0]*SPP[7] + P[9][0]*SPP[22] + P[12][0]*SPP[18]) - SPP[3]*(P[0][1]*SPP[5] - P[1][1]*SPP[4] + P[2][1]*SPP[7] + P[9][1]*SPP[22] + P[12][1]*SPP[18]) + SPP[13]*(P[0][2]*SPP[5] - P[1][2]*SPP[4] + P[2][2]*SPP[7] + P[9][2]*SPP[22] + P[12][2]*SPP[18]) + SPP[22]*(P[0][11]*SPP[5] - P[1][11]*SPP[4] + P[2][11]*SPP[7] + P[9][11]*SPP[22] + P[12][11]*SPP[18]) + SPP[16]*(P[0][14]*SPP[5] - P[1][14]*SPP[4] + P[2][14]*SPP[7] + P[9][14]*SPP[22] + P[12][14]*SPP[18]);
|
||||
nextP[1][2] = SPP[14]*(P[1][0]*SPP[6] - P[0][0]*SPP[2] - P[2][0]*SPP[8] + P[10][0]*SPP[22] + P[13][0]*SPP[17]) - SPP[3]*(P[1][1]*SPP[6] - P[0][1]*SPP[2] - P[2][1]*SPP[8] + P[10][1]*SPP[22] + P[13][1]*SPP[17]) + SPP[13]*(P[1][2]*SPP[6] - P[0][2]*SPP[2] - P[2][2]*SPP[8] + P[10][2]*SPP[22] + P[13][2]*SPP[17]) + SPP[22]*(P[1][11]*SPP[6] - P[0][11]*SPP[2] - P[2][11]*SPP[8] + P[10][11]*SPP[22] + P[13][11]*SPP[17]) + SPP[16]*(P[1][14]*SPP[6] - P[0][14]*SPP[2] - P[2][14]*SPP[8] + P[10][14]*SPP[22] + P[13][14]*SPP[17]);
|
||||
nextP[2][2] = dazNoise*SQ[3] - SPP[3]*(P[0][1]*SPP[14] - P[1][1]*SPP[3] + P[2][1]*SPP[13] + P[11][1]*SPP[22] + P[14][1]*SPP[16]) + SPP[14]*(P[0][0]*SPP[14] - P[1][0]*SPP[3] + P[2][0]*SPP[13] + P[11][0]*SPP[22] + P[14][0]*SPP[16]) + SPP[13]*(P[0][2]*SPP[14] - P[1][2]*SPP[3] + P[2][2]*SPP[13] + P[11][2]*SPP[22] + P[14][2]*SPP[16]) + SPP[22]*(P[0][11]*SPP[14] - P[1][11]*SPP[3] + P[2][11]*SPP[13] + P[11][11]*SPP[22] + P[14][11]*SPP[16]) + SPP[16]*(P[0][14]*SPP[14] - P[1][14]*SPP[3] + P[2][14]*SPP[13] + P[11][14]*SPP[22] + P[14][14]*SPP[16]);
|
||||
nextP[0][3] = P[0][3]*SPP[5] - P[1][3]*SPP[4] + P[2][3]*SPP[7] + P[9][3]*SPP[22] + P[12][3]*SPP[18] + SPP[1]*(P[0][0]*SPP[5] - P[1][0]*SPP[4] + P[2][0]*SPP[7] + P[9][0]*SPP[22] + P[12][0]*SPP[18]) + SPP[19]*(P[0][1]*SPP[5] - P[1][1]*SPP[4] + P[2][1]*SPP[7] + P[9][1]*SPP[22] + P[12][1]*SPP[18]) + SPP[15]*(P[0][2]*SPP[5] - P[1][2]*SPP[4] + P[2][2]*SPP[7] + P[9][2]*SPP[22] + P[12][2]*SPP[18]) - SPP[21]*(P[0][15]*SPP[5] - P[1][15]*SPP[4] + P[2][15]*SPP[7] + P[9][15]*SPP[22] + P[12][15]*SPP[18]);
|
||||
nextP[1][3] = P[1][3]*SPP[6] - P[0][3]*SPP[2] - P[2][3]*SPP[8] + P[10][3]*SPP[22] + P[13][3]*SPP[17] + SPP[1]*(P[1][0]*SPP[6] - P[0][0]*SPP[2] - P[2][0]*SPP[8] + P[10][0]*SPP[22] + P[13][0]*SPP[17]) + SPP[19]*(P[1][1]*SPP[6] - P[0][1]*SPP[2] - P[2][1]*SPP[8] + P[10][1]*SPP[22] + P[13][1]*SPP[17]) + SPP[15]*(P[1][2]*SPP[6] - P[0][2]*SPP[2] - P[2][2]*SPP[8] + P[10][2]*SPP[22] + P[13][2]*SPP[17]) - SPP[21]*(P[1][15]*SPP[6] - P[0][15]*SPP[2] - P[2][15]*SPP[8] + P[10][15]*SPP[22] + P[13][15]*SPP[17]);
|
||||
nextP[2][3] = P[0][3]*SPP[14] - P[1][3]*SPP[3] + P[2][3]*SPP[13] + P[11][3]*SPP[22] + P[14][3]*SPP[16] + SPP[1]*(P[0][0]*SPP[14] - P[1][0]*SPP[3] + P[2][0]*SPP[13] + P[11][0]*SPP[22] + P[14][0]*SPP[16]) + SPP[19]*(P[0][1]*SPP[14] - P[1][1]*SPP[3] + P[2][1]*SPP[13] + P[11][1]*SPP[22] + P[14][1]*SPP[16]) + SPP[15]*(P[0][2]*SPP[14] - P[1][2]*SPP[3] + P[2][2]*SPP[13] + P[11][2]*SPP[22] + P[14][2]*SPP[16]) - SPP[21]*(P[0][15]*SPP[14] - P[1][15]*SPP[3] + P[2][15]*SPP[13] + P[11][15]*SPP[22] + P[14][15]*SPP[16]);
|
||||
nextP[3][3] = P[3][3] + P[0][3]*SPP[1] + P[1][3]*SPP[19] + P[2][3]*SPP[15] - P[15][3]*SPP[21] + dvyNoise*sq(SQ[6] - 2*q0*q3) + dvzNoise*sq(SQ[5] + 2*q0*q2) + SPP[1]*(P[3][0] + P[0][0]*SPP[1] + P[1][0]*SPP[19] + P[2][0]*SPP[15] - P[15][0]*SPP[21]) + SPP[19]*(P[3][1] + P[0][1]*SPP[1] + P[1][1]*SPP[19] + P[2][1]*SPP[15] - P[15][1]*SPP[21]) + SPP[15]*(P[3][2] + P[0][2]*SPP[1] + P[1][2]*SPP[19] + P[2][2]*SPP[15] - P[15][2]*SPP[21]) - SPP[21]*(P[3][15] + P[0][15]*SPP[1] + P[1][15]*SPP[19] + P[2][15]*SPP[15] - P[15][15]*SPP[21]) + dvxNoise*sq(SG[1] + SG[2] - SG[3] - SQ[7]);
|
||||
nextP[0][4] = P[0][4]*SPP[5] - P[1][4]*SPP[4] + P[2][4]*SPP[7] + P[9][4]*SPP[22] + P[12][4]*SPP[18] + SF[22]*(P[0][15]*SPP[5] - P[1][15]*SPP[4] + P[2][15]*SPP[7] + P[9][15]*SPP[22] + P[12][15]*SPP[18]) + SPP[12]*(P[0][1]*SPP[5] - P[1][1]*SPP[4] + P[2][1]*SPP[7] + P[9][1]*SPP[22] + P[12][1]*SPP[18]) + SPP[20]*(P[0][0]*SPP[5] - P[1][0]*SPP[4] + P[2][0]*SPP[7] + P[9][0]*SPP[22] + P[12][0]*SPP[18]) + SPP[11]*(P[0][2]*SPP[5] - P[1][2]*SPP[4] + P[2][2]*SPP[7] + P[9][2]*SPP[22] + P[12][2]*SPP[18]);
|
||||
nextP[1][4] = P[1][4]*SPP[6] - P[0][4]*SPP[2] - P[2][4]*SPP[8] + P[10][4]*SPP[22] + P[13][4]*SPP[17] + SF[22]*(P[1][15]*SPP[6] - P[0][15]*SPP[2] - P[2][15]*SPP[8] + P[10][15]*SPP[22] + P[13][15]*SPP[17]) + SPP[12]*(P[1][1]*SPP[6] - P[0][1]*SPP[2] - P[2][1]*SPP[8] + P[10][1]*SPP[22] + P[13][1]*SPP[17]) + SPP[20]*(P[1][0]*SPP[6] - P[0][0]*SPP[2] - P[2][0]*SPP[8] + P[10][0]*SPP[22] + P[13][0]*SPP[17]) + SPP[11]*(P[1][2]*SPP[6] - P[0][2]*SPP[2] - P[2][2]*SPP[8] + P[10][2]*SPP[22] + P[13][2]*SPP[17]);
|
||||
nextP[2][4] = P[0][4]*SPP[14] - P[1][4]*SPP[3] + P[2][4]*SPP[13] + P[11][4]*SPP[22] + P[14][4]*SPP[16] + SF[22]*(P[0][15]*SPP[14] - P[1][15]*SPP[3] + P[2][15]*SPP[13] + P[11][15]*SPP[22] + P[14][15]*SPP[16]) + SPP[12]*(P[0][1]*SPP[14] - P[1][1]*SPP[3] + P[2][1]*SPP[13] + P[11][1]*SPP[22] + P[14][1]*SPP[16]) + SPP[20]*(P[0][0]*SPP[14] - P[1][0]*SPP[3] + P[2][0]*SPP[13] + P[11][0]*SPP[22] + P[14][0]*SPP[16]) + SPP[11]*(P[0][2]*SPP[14] - P[1][2]*SPP[3] + P[2][2]*SPP[13] + P[11][2]*SPP[22] + P[14][2]*SPP[16]);
|
||||
nextP[3][4] = P[3][4] + SQ[2] + P[0][4]*SPP[1] + P[1][4]*SPP[19] + P[2][4]*SPP[15] - P[15][4]*SPP[21] + SF[22]*(P[3][15] + P[0][15]*SPP[1] + P[1][15]*SPP[19] + P[2][15]*SPP[15] - P[15][15]*SPP[21]) + SPP[12]*(P[3][1] + P[0][1]*SPP[1] + P[1][1]*SPP[19] + P[2][1]*SPP[15] - P[15][1]*SPP[21]) + SPP[20]*(P[3][0] + P[0][0]*SPP[1] + P[1][0]*SPP[19] + P[2][0]*SPP[15] - P[15][0]*SPP[21]) + SPP[11]*(P[3][2] + P[0][2]*SPP[1] + P[1][2]*SPP[19] + P[2][2]*SPP[15] - P[15][2]*SPP[21]);
|
||||
nextP[4][4] = P[4][4] + P[15][4]*SF[22] + P[0][4]*SPP[20] + P[1][4]*SPP[12] + P[2][4]*SPP[11] + dvxNoise*sq(SQ[6] + 2*q0*q3) + dvzNoise*sq(SQ[4] - 2*q0*q1) + SF[22]*(P[4][15] + P[15][15]*SF[22] + P[0][15]*SPP[20] + P[1][15]*SPP[12] + P[2][15]*SPP[11]) + SPP[12]*(P[4][1] + P[15][1]*SF[22] + P[0][1]*SPP[20] + P[1][1]*SPP[12] + P[2][1]*SPP[11]) + SPP[20]*(P[4][0] + P[15][0]*SF[22] + P[0][0]*SPP[20] + P[1][0]*SPP[12] + P[2][0]*SPP[11]) + SPP[11]*(P[4][2] + P[15][2]*SF[22] + P[0][2]*SPP[20] + P[1][2]*SPP[12] + P[2][2]*SPP[11]) + dvyNoise*sq(SG[1] - SG[2] + SG[3] - SQ[7]);
|
||||
nextP[0][5] = P[0][5]*SPP[5] - P[1][5]*SPP[4] + P[2][5]*SPP[7] + P[9][5]*SPP[22] + P[12][5]*SPP[18] + SF[20]*(P[0][15]*SPP[5] - P[1][15]*SPP[4] + P[2][15]*SPP[7] + P[9][15]*SPP[22] + P[12][15]*SPP[18]) - SPP[9]*(P[0][0]*SPP[5] - P[1][0]*SPP[4] + P[2][0]*SPP[7] + P[9][0]*SPP[22] + P[12][0]*SPP[18]) + SPP[0]*(P[0][2]*SPP[5] - P[1][2]*SPP[4] + P[2][2]*SPP[7] + P[9][2]*SPP[22] + P[12][2]*SPP[18]) + SPP[10]*(P[0][1]*SPP[5] - P[1][1]*SPP[4] + P[2][1]*SPP[7] + P[9][1]*SPP[22] + P[12][1]*SPP[18]);
|
||||
nextP[1][5] = P[1][5]*SPP[6] - P[0][5]*SPP[2] - P[2][5]*SPP[8] + P[10][5]*SPP[22] + P[13][5]*SPP[17] + SF[20]*(P[1][15]*SPP[6] - P[0][15]*SPP[2] - P[2][15]*SPP[8] + P[10][15]*SPP[22] + P[13][15]*SPP[17]) - SPP[9]*(P[1][0]*SPP[6] - P[0][0]*SPP[2] - P[2][0]*SPP[8] + P[10][0]*SPP[22] + P[13][0]*SPP[17]) + SPP[0]*(P[1][2]*SPP[6] - P[0][2]*SPP[2] - P[2][2]*SPP[8] + P[10][2]*SPP[22] + P[13][2]*SPP[17]) + SPP[10]*(P[1][1]*SPP[6] - P[0][1]*SPP[2] - P[2][1]*SPP[8] + P[10][1]*SPP[22] + P[13][1]*SPP[17]);
|
||||
nextP[2][5] = P[0][5]*SPP[14] - P[1][5]*SPP[3] + P[2][5]*SPP[13] + P[11][5]*SPP[22] + P[14][5]*SPP[16] + SF[20]*(P[0][15]*SPP[14] - P[1][15]*SPP[3] + P[2][15]*SPP[13] + P[11][15]*SPP[22] + P[14][15]*SPP[16]) - SPP[9]*(P[0][0]*SPP[14] - P[1][0]*SPP[3] + P[2][0]*SPP[13] + P[11][0]*SPP[22] + P[14][0]*SPP[16]) + SPP[0]*(P[0][2]*SPP[14] - P[1][2]*SPP[3] + P[2][2]*SPP[13] + P[11][2]*SPP[22] + P[14][2]*SPP[16]) + SPP[10]*(P[0][1]*SPP[14] - P[1][1]*SPP[3] + P[2][1]*SPP[13] + P[11][1]*SPP[22] + P[14][1]*SPP[16]);
|
||||
nextP[3][5] = P[3][5] + SQ[1] + P[0][5]*SPP[1] + P[1][5]*SPP[19] + P[2][5]*SPP[15] - P[15][5]*SPP[21] + SF[20]*(P[3][15] + P[0][15]*SPP[1] + P[1][15]*SPP[19] + P[2][15]*SPP[15] - P[15][15]*SPP[21]) - SPP[9]*(P[3][0] + P[0][0]*SPP[1] + P[1][0]*SPP[19] + P[2][0]*SPP[15] - P[15][0]*SPP[21]) + SPP[0]*(P[3][2] + P[0][2]*SPP[1] + P[1][2]*SPP[19] + P[2][2]*SPP[15] - P[15][2]*SPP[21]) + SPP[10]*(P[3][1] + P[0][1]*SPP[1] + P[1][1]*SPP[19] + P[2][1]*SPP[15] - P[15][1]*SPP[21]);
|
||||
nextP[4][5] = P[4][5] + SQ[0] + P[15][5]*SF[22] + P[0][5]*SPP[20] + P[1][5]*SPP[12] + P[2][5]*SPP[11] + SF[20]*(P[4][15] + P[15][15]*SF[22] + P[0][15]*SPP[20] + P[1][15]*SPP[12] + P[2][15]*SPP[11]) - SPP[9]*(P[4][0] + P[15][0]*SF[22] + P[0][0]*SPP[20] + P[1][0]*SPP[12] + P[2][0]*SPP[11]) + SPP[0]*(P[4][2] + P[15][2]*SF[22] + P[0][2]*SPP[20] + P[1][2]*SPP[12] + P[2][2]*SPP[11]) + SPP[10]*(P[4][1] + P[15][1]*SF[22] + P[0][1]*SPP[20] + P[1][1]*SPP[12] + P[2][1]*SPP[11]);
|
||||
nextP[5][5] = P[5][5] + P[15][5]*SF[20] - P[0][5]*SPP[9] + P[1][5]*SPP[10] + P[2][5]*SPP[0] + dvxNoise*sq(SQ[5] - 2*q0*q2) + dvyNoise*sq(SQ[4] + 2*q0*q1) + SF[20]*(P[5][15] + P[15][15]*SF[20] - P[0][15]*SPP[9] + P[1][15]*SPP[10] + P[2][15]*SPP[0]) - SPP[9]*(P[5][0] + P[15][0]*SF[20] - P[0][0]*SPP[9] + P[1][0]*SPP[10] + P[2][0]*SPP[0]) + SPP[0]*(P[5][2] + P[15][2]*SF[20] - P[0][2]*SPP[9] + P[1][2]*SPP[10] + P[2][2]*SPP[0]) + SPP[10]*(P[5][1] + P[15][1]*SF[20] - P[0][1]*SPP[9] + P[1][1]*SPP[10] + P[2][1]*SPP[0]) + dvzNoise*sq(SG[1] - SG[2] - SG[3] + SQ[7]);
|
||||
nextP[0][6] = P[0][6]*SPP[5] - P[1][6]*SPP[4] + P[2][6]*SPP[7] + P[9][6]*SPP[22] + P[12][6]*SPP[18] + dt*(P[0][3]*SPP[5] - P[1][3]*SPP[4] + P[2][3]*SPP[7] + P[9][3]*SPP[22] + P[12][3]*SPP[18]);
|
||||
nextP[1][6] = P[1][6]*SPP[6] - P[0][6]*SPP[2] - P[2][6]*SPP[8] + P[10][6]*SPP[22] + P[13][6]*SPP[17] + dt*(P[1][3]*SPP[6] - P[0][3]*SPP[2] - P[2][3]*SPP[8] + P[10][3]*SPP[22] + P[13][3]*SPP[17]);
|
||||
nextP[2][6] = P[0][6]*SPP[14] - P[1][6]*SPP[3] + P[2][6]*SPP[13] + P[11][6]*SPP[22] + P[14][6]*SPP[16] + dt*(P[0][3]*SPP[14] - P[1][3]*SPP[3] + P[2][3]*SPP[13] + P[11][3]*SPP[22] + P[14][3]*SPP[16]);
|
||||
nextP[3][6] = P[3][6] + P[0][6]*SPP[1] + P[1][6]*SPP[19] + P[2][6]*SPP[15] - P[15][6]*SPP[21] + dt*(P[3][3] + P[0][3]*SPP[1] + P[1][3]*SPP[19] + P[2][3]*SPP[15] - P[15][3]*SPP[21]);
|
||||
nextP[4][6] = P[4][6] + P[15][6]*SF[22] + P[0][6]*SPP[20] + P[1][6]*SPP[12] + P[2][6]*SPP[11] + dt*(P[4][3] + P[15][3]*SF[22] + P[0][3]*SPP[20] + P[1][3]*SPP[12] + P[2][3]*SPP[11]);
|
||||
nextP[5][6] = P[5][6] + P[15][6]*SF[20] - P[0][6]*SPP[9] + P[1][6]*SPP[10] + P[2][6]*SPP[0] + dt*(P[5][3] + P[15][3]*SF[20] - P[0][3]*SPP[9] + P[1][3]*SPP[10] + P[2][3]*SPP[0]);
|
||||
nextP[6][6] = P[6][6] + P[3][6]*dt + dt*(P[6][3] + P[3][3]*dt);
|
||||
nextP[0][7] = P[0][7]*SPP[5] - P[1][7]*SPP[4] + P[2][7]*SPP[7] + P[9][7]*SPP[22] + P[12][7]*SPP[18] + dt*(P[0][4]*SPP[5] - P[1][4]*SPP[4] + P[2][4]*SPP[7] + P[9][4]*SPP[22] + P[12][4]*SPP[18]);
|
||||
nextP[1][7] = P[1][7]*SPP[6] - P[0][7]*SPP[2] - P[2][7]*SPP[8] + P[10][7]*SPP[22] + P[13][7]*SPP[17] + dt*(P[1][4]*SPP[6] - P[0][4]*SPP[2] - P[2][4]*SPP[8] + P[10][4]*SPP[22] + P[13][4]*SPP[17]);
|
||||
nextP[2][7] = P[0][7]*SPP[14] - P[1][7]*SPP[3] + P[2][7]*SPP[13] + P[11][7]*SPP[22] + P[14][7]*SPP[16] + dt*(P[0][4]*SPP[14] - P[1][4]*SPP[3] + P[2][4]*SPP[13] + P[11][4]*SPP[22] + P[14][4]*SPP[16]);
|
||||
nextP[3][7] = P[3][7] + P[0][7]*SPP[1] + P[1][7]*SPP[19] + P[2][7]*SPP[15] - P[15][7]*SPP[21] + dt*(P[3][4] + P[0][4]*SPP[1] + P[1][4]*SPP[19] + P[2][4]*SPP[15] - P[15][4]*SPP[21]);
|
||||
nextP[4][7] = P[4][7] + P[15][7]*SF[22] + P[0][7]*SPP[20] + P[1][7]*SPP[12] + P[2][7]*SPP[11] + dt*(P[4][4] + P[15][4]*SF[22] + P[0][4]*SPP[20] + P[1][4]*SPP[12] + P[2][4]*SPP[11]);
|
||||
nextP[5][7] = P[5][7] + P[15][7]*SF[20] - P[0][7]*SPP[9] + P[1][7]*SPP[10] + P[2][7]*SPP[0] + dt*(P[5][4] + P[15][4]*SF[20] - P[0][4]*SPP[9] + P[1][4]*SPP[10] + P[2][4]*SPP[0]);
|
||||
nextP[6][7] = P[6][7] + P[3][7]*dt + dt*(P[6][4] + P[3][4]*dt);
|
||||
nextP[7][7] = P[7][7] + P[4][7]*dt + dt*(P[7][4] + P[4][4]*dt);
|
||||
nextP[0][8] = P[0][8]*SPP[5] - P[1][8]*SPP[4] + P[2][8]*SPP[7] + P[9][8]*SPP[22] + P[12][8]*SPP[18] + dt*(P[0][5]*SPP[5] - P[1][5]*SPP[4] + P[2][5]*SPP[7] + P[9][5]*SPP[22] + P[12][5]*SPP[18]);
|
||||
nextP[1][8] = P[1][8]*SPP[6] - P[0][8]*SPP[2] - P[2][8]*SPP[8] + P[10][8]*SPP[22] + P[13][8]*SPP[17] + dt*(P[1][5]*SPP[6] - P[0][5]*SPP[2] - P[2][5]*SPP[8] + P[10][5]*SPP[22] + P[13][5]*SPP[17]);
|
||||
nextP[2][8] = P[0][8]*SPP[14] - P[1][8]*SPP[3] + P[2][8]*SPP[13] + P[11][8]*SPP[22] + P[14][8]*SPP[16] + dt*(P[0][5]*SPP[14] - P[1][5]*SPP[3] + P[2][5]*SPP[13] + P[11][5]*SPP[22] + P[14][5]*SPP[16]);
|
||||
nextP[3][8] = P[3][8] + P[0][8]*SPP[1] + P[1][8]*SPP[19] + P[2][8]*SPP[15] - P[15][8]*SPP[21] + dt*(P[3][5] + P[0][5]*SPP[1] + P[1][5]*SPP[19] + P[2][5]*SPP[15] - P[15][5]*SPP[21]);
|
||||
nextP[4][8] = P[4][8] + P[15][8]*SF[22] + P[0][8]*SPP[20] + P[1][8]*SPP[12] + P[2][8]*SPP[11] + dt*(P[4][5] + P[15][5]*SF[22] + P[0][5]*SPP[20] + P[1][5]*SPP[12] + P[2][5]*SPP[11]);
|
||||
nextP[5][8] = P[5][8] + P[15][8]*SF[20] - P[0][8]*SPP[9] + P[1][8]*SPP[10] + P[2][8]*SPP[0] + dt*(P[5][5] + P[15][5]*SF[20] - P[0][5]*SPP[9] + P[1][5]*SPP[10] + P[2][5]*SPP[0]);
|
||||
nextP[6][8] = P[6][8] + P[3][8]*dt + dt*(P[6][5] + P[3][5]*dt);
|
||||
nextP[7][8] = P[7][8] + P[4][8]*dt + dt*(P[7][5] + P[4][5]*dt);
|
||||
nextP[8][8] = P[8][8] + P[5][8]*dt + dt*(P[8][5] + P[5][5]*dt);
|
||||
nextP[0][9] = P[0][9]*SPP[5] - P[1][9]*SPP[4] + P[2][9]*SPP[7] + P[9][9]*SPP[22] + P[12][9]*SPP[18];
|
||||
nextP[1][9] = P[1][9]*SPP[6] - P[0][9]*SPP[2] - P[2][9]*SPP[8] + P[10][9]*SPP[22] + P[13][9]*SPP[17];
|
||||
nextP[2][9] = P[0][9]*SPP[14] - P[1][9]*SPP[3] + P[2][9]*SPP[13] + P[11][9]*SPP[22] + P[14][9]*SPP[16];
|
||||
nextP[3][9] = P[3][9] + P[0][9]*SPP[1] + P[1][9]*SPP[19] + P[2][9]*SPP[15] - P[15][9]*SPP[21];
|
||||
nextP[4][9] = P[4][9] + P[15][9]*SF[22] + P[0][9]*SPP[20] + P[1][9]*SPP[12] + P[2][9]*SPP[11];
|
||||
nextP[5][9] = P[5][9] + P[15][9]*SF[20] - P[0][9]*SPP[9] + P[1][9]*SPP[10] + P[2][9]*SPP[0];
|
||||
nextP[6][9] = P[6][9] + P[3][9]*dt;
|
||||
nextP[7][9] = P[7][9] + P[4][9]*dt;
|
||||
nextP[8][9] = P[8][9] + P[5][9]*dt;
|
||||
nextP[9][9] = P[9][9];
|
||||
nextP[0][10] = P[0][10]*SPP[5] - P[1][10]*SPP[4] + P[2][10]*SPP[7] + P[9][10]*SPP[22] + P[12][10]*SPP[18];
|
||||
nextP[1][10] = P[1][10]*SPP[6] - P[0][10]*SPP[2] - P[2][10]*SPP[8] + P[10][10]*SPP[22] + P[13][10]*SPP[17];
|
||||
nextP[2][10] = P[0][10]*SPP[14] - P[1][10]*SPP[3] + P[2][10]*SPP[13] + P[11][10]*SPP[22] + P[14][10]*SPP[16];
|
||||
nextP[3][10] = P[3][10] + P[0][10]*SPP[1] + P[1][10]*SPP[19] + P[2][10]*SPP[15] - P[15][10]*SPP[21];
|
||||
nextP[4][10] = P[4][10] + P[15][10]*SF[22] + P[0][10]*SPP[20] + P[1][10]*SPP[12] + P[2][10]*SPP[11];
|
||||
nextP[5][10] = P[5][10] + P[15][10]*SF[20] - P[0][10]*SPP[9] + P[1][10]*SPP[10] + P[2][10]*SPP[0];
|
||||
nextP[6][10] = P[6][10] + P[3][10]*dt;
|
||||
nextP[7][10] = P[7][10] + P[4][10]*dt;
|
||||
nextP[8][10] = P[8][10] + P[5][10]*dt;
|
||||
nextP[9][10] = P[9][10];
|
||||
nextP[10][10] = P[10][10];
|
||||
nextP[0][11] = P[0][11]*SPP[5] - P[1][11]*SPP[4] + P[2][11]*SPP[7] + P[9][11]*SPP[22] + P[12][11]*SPP[18];
|
||||
nextP[1][11] = P[1][11]*SPP[6] - P[0][11]*SPP[2] - P[2][11]*SPP[8] + P[10][11]*SPP[22] + P[13][11]*SPP[17];
|
||||
nextP[2][11] = P[0][11]*SPP[14] - P[1][11]*SPP[3] + P[2][11]*SPP[13] + P[11][11]*SPP[22] + P[14][11]*SPP[16];
|
||||
nextP[3][11] = P[3][11] + P[0][11]*SPP[1] + P[1][11]*SPP[19] + P[2][11]*SPP[15] - P[15][11]*SPP[21];
|
||||
nextP[4][11] = P[4][11] + P[15][11]*SF[22] + P[0][11]*SPP[20] + P[1][11]*SPP[12] + P[2][11]*SPP[11];
|
||||
nextP[5][11] = P[5][11] + P[15][11]*SF[20] - P[0][11]*SPP[9] + P[1][11]*SPP[10] + P[2][11]*SPP[0];
|
||||
nextP[6][11] = P[6][11] + P[3][11]*dt;
|
||||
nextP[7][11] = P[7][11] + P[4][11]*dt;
|
||||
nextP[8][11] = P[8][11] + P[5][11]*dt;
|
||||
nextP[9][11] = P[9][11];
|
||||
nextP[10][11] = P[10][11];
|
||||
nextP[11][11] = P[11][11];
|
||||
nextP[0][12] = P[0][12]*SPP[5] - P[1][12]*SPP[4] + P[2][12]*SPP[7] + P[9][12]*SPP[22] + P[12][12]*SPP[18];
|
||||
nextP[1][12] = P[1][12]*SPP[6] - P[0][12]*SPP[2] - P[2][12]*SPP[8] + P[10][12]*SPP[22] + P[13][12]*SPP[17];
|
||||
nextP[2][12] = P[0][12]*SPP[14] - P[1][12]*SPP[3] + P[2][12]*SPP[13] + P[11][12]*SPP[22] + P[14][12]*SPP[16];
|
||||
nextP[3][12] = P[3][12] + P[0][12]*SPP[1] + P[1][12]*SPP[19] + P[2][12]*SPP[15] - P[15][12]*SPP[21];
|
||||
nextP[4][12] = P[4][12] + P[15][12]*SF[22] + P[0][12]*SPP[20] + P[1][12]*SPP[12] + P[2][12]*SPP[11];
|
||||
nextP[5][12] = P[5][12] + P[15][12]*SF[20] - P[0][12]*SPP[9] + P[1][12]*SPP[10] + P[2][12]*SPP[0];
|
||||
nextP[6][12] = P[6][12] + P[3][12]*dt;
|
||||
nextP[7][12] = P[7][12] + P[4][12]*dt;
|
||||
nextP[8][12] = P[8][12] + P[5][12]*dt;
|
||||
nextP[9][12] = P[9][12];
|
||||
nextP[10][12] = P[10][12];
|
||||
nextP[11][12] = P[11][12];
|
||||
nextP[12][12] = P[12][12];
|
||||
nextP[0][13] = P[0][13]*SPP[5] - P[1][13]*SPP[4] + P[2][13]*SPP[7] + P[9][13]*SPP[22] + P[12][13]*SPP[18];
|
||||
nextP[1][13] = P[1][13]*SPP[6] - P[0][13]*SPP[2] - P[2][13]*SPP[8] + P[10][13]*SPP[22] + P[13][13]*SPP[17];
|
||||
nextP[2][13] = P[0][13]*SPP[14] - P[1][13]*SPP[3] + P[2][13]*SPP[13] + P[11][13]*SPP[22] + P[14][13]*SPP[16];
|
||||
nextP[3][13] = P[3][13] + P[0][13]*SPP[1] + P[1][13]*SPP[19] + P[2][13]*SPP[15] - P[15][13]*SPP[21];
|
||||
nextP[4][13] = P[4][13] + P[15][13]*SF[22] + P[0][13]*SPP[20] + P[1][13]*SPP[12] + P[2][13]*SPP[11];
|
||||
nextP[5][13] = P[5][13] + P[15][13]*SF[20] - P[0][13]*SPP[9] + P[1][13]*SPP[10] + P[2][13]*SPP[0];
|
||||
nextP[6][13] = P[6][13] + P[3][13]*dt;
|
||||
nextP[7][13] = P[7][13] + P[4][13]*dt;
|
||||
nextP[8][13] = P[8][13] + P[5][13]*dt;
|
||||
nextP[9][13] = P[9][13];
|
||||
nextP[10][13] = P[10][13];
|
||||
nextP[11][13] = P[11][13];
|
||||
nextP[12][13] = P[12][13];
|
||||
nextP[13][13] = P[13][13];
|
||||
nextP[0][14] = P[0][14]*SPP[5] - P[1][14]*SPP[4] + P[2][14]*SPP[7] + P[9][14]*SPP[22] + P[12][14]*SPP[18];
|
||||
nextP[1][14] = P[1][14]*SPP[6] - P[0][14]*SPP[2] - P[2][14]*SPP[8] + P[10][14]*SPP[22] + P[13][14]*SPP[17];
|
||||
nextP[2][14] = P[0][14]*SPP[14] - P[1][14]*SPP[3] + P[2][14]*SPP[13] + P[11][14]*SPP[22] + P[14][14]*SPP[16];
|
||||
nextP[3][14] = P[3][14] + P[0][14]*SPP[1] + P[1][14]*SPP[19] + P[2][14]*SPP[15] - P[15][14]*SPP[21];
|
||||
nextP[4][14] = P[4][14] + P[15][14]*SF[22] + P[0][14]*SPP[20] + P[1][14]*SPP[12] + P[2][14]*SPP[11];
|
||||
nextP[5][14] = P[5][14] + P[15][14]*SF[20] - P[0][14]*SPP[9] + P[1][14]*SPP[10] + P[2][14]*SPP[0];
|
||||
nextP[6][14] = P[6][14] + P[3][14]*dt;
|
||||
nextP[7][14] = P[7][14] + P[4][14]*dt;
|
||||
nextP[8][14] = P[8][14] + P[5][14]*dt;
|
||||
nextP[9][14] = P[9][14];
|
||||
nextP[10][14] = P[10][14];
|
||||
nextP[11][14] = P[11][14];
|
||||
nextP[12][14] = P[12][14];
|
||||
nextP[13][14] = P[13][14];
|
||||
nextP[14][14] = P[14][14];
|
||||
nextP[0][15] = P[0][15]*SPP[5] - P[1][15]*SPP[4] + P[2][15]*SPP[7] + P[9][15]*SPP[22] + P[12][15]*SPP[18];
|
||||
nextP[1][15] = P[1][15]*SPP[6] - P[0][15]*SPP[2] - P[2][15]*SPP[8] + P[10][15]*SPP[22] + P[13][15]*SPP[17];
|
||||
nextP[2][15] = P[0][15]*SPP[14] - P[1][15]*SPP[3] + P[2][15]*SPP[13] + P[11][15]*SPP[22] + P[14][15]*SPP[16];
|
||||
nextP[3][15] = P[3][15] + P[0][15]*SPP[1] + P[1][15]*SPP[19] + P[2][15]*SPP[15] - P[15][15]*SPP[21];
|
||||
nextP[4][15] = P[4][15] + P[15][15]*SF[22] + P[0][15]*SPP[20] + P[1][15]*SPP[12] + P[2][15]*SPP[11];
|
||||
nextP[5][15] = P[5][15] + P[15][15]*SF[20] - P[0][15]*SPP[9] + P[1][15]*SPP[10] + P[2][15]*SPP[0];
|
||||
nextP[6][15] = P[6][15] + P[3][15]*dt;
|
||||
nextP[7][15] = P[7][15] + P[4][15]*dt;
|
||||
nextP[8][15] = P[8][15] + P[5][15]*dt;
|
||||
nextP[9][15] = P[9][15];
|
||||
nextP[10][15] = P[10][15];
|
||||
nextP[11][15] = P[11][15];
|
||||
nextP[12][15] = P[12][15];
|
||||
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];
|
||||
|
|
@ -0,0 +1,15 @@
|
|||
/*
|
||||
Autocode for fusion of a magnetic declination estimate where the innovation is given by
|
||||
|
||||
innovation = atan2f(magMeasEarthFrameEast,magMeasEarthFrameNorth) - declinationAngle;
|
||||
|
||||
magMeasEarthFrameEast and magMeasEarthFrameNorth are obtained by rotating the magnetometer measurements from body frame to earth frame.
|
||||
declinationAngle is the estimated declination as that location
|
||||
|
||||
This fusion method is used to constrain the rotation of the earth field vector when there are no earth relative measurements
|
||||
(e.g. using optical flow without GPS, or when the vehicle is stationary) to provide an absolute yaw reference. In this situation the presence of yaw gyro errors
|
||||
can cause the magnetic declination of the earth field estimates to slowly rotate.
|
||||
*/
|
||||
|
||||
// Calculate intermediate variable
float t2 = 1.0f/magN;
float t4 = magE*t2;
float t3 = tanf(t4);
float t5 = t3*t3;
float t6 = t5+1.0f;
float t7 = 1.0f/(magN*magN);
float t8 = P[17][17]*t2*t6;
float t15 = P[16][17]*magE*t6*t7;
float t9 = t8-t15;
float t10 = t2*t6*t9;
float t11 = P[17][16]*t2*t6;
float t16 = P[16][16]*magE*t6*t7;
float t12 = t11-t16;
float t17 = magE*t6*t7*t12;
float t13 = R_DECL+t10-t17;
float t14 = 1.0f/t13;
float t18 = magE;
float t19 = magN;
float t21 = 1.0f/t19;
float t22 = t18*t21;
float t20 = tanf(t22);
float t23 = t20*t20;
float t24 = t23+1.0f;
// Calculate the observation Jacobian
|
||||
// Note only 2 terms are non-zero which can be used in matrix operations for calculation of Kalman gains and covariance update to significantly reduce cost
float H_MAG[24];
H_MAG[16] = -t18*1.0f/(t19*t19)*t24;
H_MAG[17] = t21*t24;
|
||||
|
|
@ -0,0 +1,146 @@
|
|||
// Auto code for fusion of XYZ magnetometer vector measurement
|
||||
// Sequential fusion of each axis is used (assumes uncorrrelated observation errors)
|
||||
|
||||
// intermediate variables from algebraic optimisation
|
||||
float SH_MAG[9];
|
||||
SH_MAG[0] = sq(q0) - sq(q1) + sq(q2) - sq(q3);
|
||||
SH_MAG[1] = sq(q0) + sq(q1) - sq(q2) - sq(q3);
|
||||
SH_MAG[2] = sq(q0) - sq(q1) - sq(q2) + sq(q3);
|
||||
SH_MAG[3] = 2*q0*q1 + 2*q2*q3;
|
||||
SH_MAG[4] = 2*q0*q3 + 2*q1*q2;
|
||||
SH_MAG[5] = 2*q0*q2 + 2*q1*q3;
|
||||
SH_MAG[6] = magE*(2*q0*q1 - 2*q2*q3);
|
||||
SH_MAG[7] = 2*q1*q3 - 2*q0*q2;
|
||||
SH_MAG[8] = 2*q0*q3;
|
||||
|
||||
// Calculate the observation jacobian for the X component
|
||||
float H_MAG[24];
|
||||
H_MAG[1] = SH_MAG[6] - magD*SH_MAG[2] - magN*SH_MAG[5];
|
||||
H_MAG[2] = magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2);
|
||||
H_MAG[16] = SH_MAG[1];
|
||||
H_MAG[17] = SH_MAG[4];
|
||||
H_MAG[18] = SH_MAG[7];
|
||||
H_MAG[19] = 1;
|
||||
|
||||
// calculate the Kalman gain matrix for the X component
|
||||
|
||||
// intermediate variables - note SK_MX[0] is 1/(innovation variance)
|
||||
float SK_MX[4];
|
||||
SK_MX[0] = 1/(P[19][19] + R_MAG - P[1][19]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][19]*SH_MAG[1] + P[17][19]*SH_MAG[4] + P[18][19]*SH_MAG[7] + P[2][19]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2)) - (magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5])*(P[19][1] - P[1][1]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][1]*SH_MAG[1] + P[17][1]*SH_MAG[4] + P[18][1]*SH_MAG[7] + P[2][1]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2))) + SH_MAG[1]*(P[19][16] - P[1][16]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][16]*SH_MAG[1] + P[17][16]*SH_MAG[4] + P[18][16]*SH_MAG[7] + P[2][16]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2))) + SH_MAG[4]*(P[19][17] - P[1][17]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][17]*SH_MAG[1] + P[17][17]*SH_MAG[4] + P[18][17]*SH_MAG[7] + P[2][17]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2))) + SH_MAG[7]*(P[19][18] - P[1][18]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][18]*SH_MAG[1] + P[17][18]*SH_MAG[4] + P[18][18]*SH_MAG[7] + P[2][18]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2))) + (magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2))*(P[19][2] - P[1][2]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][2]*SH_MAG[1] + P[17][2]*SH_MAG[4] + P[18][2]*SH_MAG[7] + P[2][2]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2))));
|
||||
SK_MX[1] = magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2);
|
||||
SK_MX[2] = magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5];
|
||||
SK_MX[3] = SH_MAG[7];
|
||||
|
||||
float Kfusion[24];
|
||||
Kfusion[0] = SK_MX[0]*(P[0][19] + P[0][16]*SH_MAG[1] + P[0][17]*SH_MAG[4] - P[0][1]*SK_MX[2] + P[0][2]*SK_MX[1] + P[0][18]*SK_MX[3]);
|
||||
Kfusion[1] = SK_MX[0]*(P[1][19] + P[1][16]*SH_MAG[1] + P[1][17]*SH_MAG[4] - P[1][1]*SK_MX[2] + P[1][2]*SK_MX[1] + P[1][18]*SK_MX[3]);
|
||||
Kfusion[2] = SK_MX[0]*(P[2][19] + P[2][16]*SH_MAG[1] + P[2][17]*SH_MAG[4] - P[2][1]*SK_MX[2] + P[2][2]*SK_MX[1] + P[2][18]*SK_MX[3]);
|
||||
Kfusion[3] = SK_MX[0]*(P[3][19] + P[3][16]*SH_MAG[1] + P[3][17]*SH_MAG[4] - P[3][1]*SK_MX[2] + P[3][2]*SK_MX[1] + P[3][18]*SK_MX[3]);
|
||||
Kfusion[4] = SK_MX[0]*(P[4][19] + P[4][16]*SH_MAG[1] + P[4][17]*SH_MAG[4] - P[4][1]*SK_MX[2] + P[4][2]*SK_MX[1] + P[4][18]*SK_MX[3]);
|
||||
Kfusion[5] = SK_MX[0]*(P[5][19] + P[5][16]*SH_MAG[1] + P[5][17]*SH_MAG[4] - P[5][1]*SK_MX[2] + P[5][2]*SK_MX[1] + P[5][18]*SK_MX[3]);
|
||||
Kfusion[6] = SK_MX[0]*(P[6][19] + P[6][16]*SH_MAG[1] + P[6][17]*SH_MAG[4] - P[6][1]*SK_MX[2] + P[6][2]*SK_MX[1] + P[6][18]*SK_MX[3]);
|
||||
Kfusion[7] = SK_MX[0]*(P[7][19] + P[7][16]*SH_MAG[1] + P[7][17]*SH_MAG[4] - P[7][1]*SK_MX[2] + P[7][2]*SK_MX[1] + P[7][18]*SK_MX[3]);
|
||||
Kfusion[8] = SK_MX[0]*(P[8][19] + P[8][16]*SH_MAG[1] + P[8][17]*SH_MAG[4] - P[8][1]*SK_MX[2] + P[8][2]*SK_MX[1] + P[8][18]*SK_MX[3]);
|
||||
Kfusion[9] = SK_MX[0]*(P[9][19] + P[9][16]*SH_MAG[1] + P[9][17]*SH_MAG[4] - P[9][1]*SK_MX[2] + P[9][2]*SK_MX[1] + P[9][18]*SK_MX[3]);
|
||||
Kfusion[10] = SK_MX[0]*(P[10][19] + P[10][16]*SH_MAG[1] + P[10][17]*SH_MAG[4] - P[10][1]*SK_MX[2] + P[10][2]*SK_MX[1] + P[10][18]*SK_MX[3]);
|
||||
Kfusion[11] = SK_MX[0]*(P[11][19] + P[11][16]*SH_MAG[1] + P[11][17]*SH_MAG[4] - P[11][1]*SK_MX[2] + P[11][2]*SK_MX[1] + P[11][18]*SK_MX[3]);
|
||||
Kfusion[12] = SK_MX[0]*(P[12][19] + P[12][16]*SH_MAG[1] + P[12][17]*SH_MAG[4] - P[12][1]*SK_MX[2] + P[12][2]*SK_MX[1] + P[12][18]*SK_MX[3]);
|
||||
Kfusion[13] = SK_MX[0]*(P[13][19] + P[13][16]*SH_MAG[1] + P[13][17]*SH_MAG[4] - P[13][1]*SK_MX[2] + P[13][2]*SK_MX[1] + P[13][18]*SK_MX[3]);
|
||||
Kfusion[14] = SK_MX[0]*(P[14][19] + P[14][16]*SH_MAG[1] + P[14][17]*SH_MAG[4] - P[14][1]*SK_MX[2] + P[14][2]*SK_MX[1] + P[14][18]*SK_MX[3]);
|
||||
Kfusion[15] = SK_MX[0]*(P[15][19] + P[15][16]*SH_MAG[1] + P[15][17]*SH_MAG[4] - P[15][1]*SK_MX[2] + P[15][2]*SK_MX[1] + P[15][18]*SK_MX[3]);
|
||||
Kfusion[16] = SK_MX[0]*(P[16][19] + P[16][16]*SH_MAG[1] + P[16][17]*SH_MAG[4] - P[16][1]*SK_MX[2] + P[16][2]*SK_MX[1] + P[16][18]*SK_MX[3]);
|
||||
Kfusion[17] = SK_MX[0]*(P[17][19] + P[17][16]*SH_MAG[1] + P[17][17]*SH_MAG[4] - P[17][1]*SK_MX[2] + P[17][2]*SK_MX[1] + P[17][18]*SK_MX[3]);
|
||||
Kfusion[18] = SK_MX[0]*(P[18][19] + P[18][16]*SH_MAG[1] + P[18][17]*SH_MAG[4] - P[18][1]*SK_MX[2] + P[18][2]*SK_MX[1] + P[18][18]*SK_MX[3]);
|
||||
Kfusion[19] = SK_MX[0]*(P[19][19] + P[19][16]*SH_MAG[1] + P[19][17]*SH_MAG[4] - P[19][1]*SK_MX[2] + P[19][2]*SK_MX[1] + P[19][18]*SK_MX[3]);
|
||||
Kfusion[20] = SK_MX[0]*(P[20][19] + P[20][16]*SH_MAG[1] + P[20][17]*SH_MAG[4] - P[20][1]*SK_MX[2] + P[20][2]*SK_MX[1] + P[20][18]*SK_MX[3]);
|
||||
Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][16]*SH_MAG[1] + P[21][17]*SH_MAG[4] - P[21][1]*SK_MX[2] + P[21][2]*SK_MX[1] + P[21][18]*SK_MX[3]);
|
||||
Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][16]*SH_MAG[1] + P[22][17]*SH_MAG[4] - P[22][1]*SK_MX[2] + P[22][2]*SK_MX[1] + P[22][18]*SK_MX[3]);
|
||||
Kfusion[23] = SK_MX[0]*(P[23][19] + P[23][16]*SH_MAG[1] + P[23][17]*SH_MAG[4] - P[23][1]*SK_MX[2] + P[23][2]*SK_MX[1] + P[23][18]*SK_MX[3]);
|
||||
|
||||
// Calculate the observation jacobian for the Y component
|
||||
float H_MAG[24];
|
||||
H_MAG[0] = magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5];
|
||||
H_MAG[2] = - magE*SH_MAG[4] - magD*SH_MAG[7] - magN*SH_MAG[1];
|
||||
H_MAG[16] = 2*q1*q2 - SH_MAG[8];
|
||||
H_MAG[17] = SH_MAG[0];
|
||||
H_MAG[18] = SH_MAG[3];
|
||||
H_MAG[20] = 1;
|
||||
|
||||
// calculate the Kalman gain matrix for the Y component
|
||||
|
||||
// intermediate variables - note SK_MY[0] is 1/(innovation variance)
|
||||
float SK_MY[4];
|
||||
SK_MY[0] = 1/(P[20][20] + R_MAG + P[0][20]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][20]*SH_MAG[0] + P[18][20]*SH_MAG[3] - (SH_MAG[8] - 2*q1*q2)*(P[20][16] + P[0][16]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][16]*SH_MAG[0] + P[18][16]*SH_MAG[3] - P[2][16]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][16]*(SH_MAG[8] - 2*q1*q2)) - P[2][20]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) + (magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5])*(P[20][0] + P[0][0]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][0]*SH_MAG[0] + P[18][0]*SH_MAG[3] - P[2][0]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][0]*(SH_MAG[8] - 2*q1*q2)) + SH_MAG[0]*(P[20][17] + P[0][17]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][17]*SH_MAG[0] + P[18][17]*SH_MAG[3] - P[2][17]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][17]*(SH_MAG[8] - 2*q1*q2)) + SH_MAG[3]*(P[20][18] + P[0][18]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][18]*SH_MAG[0] + P[18][18]*SH_MAG[3] - P[2][18]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][18]*(SH_MAG[8] - 2*q1*q2)) - P[16][20]*(SH_MAG[8] - 2*q1*q2) - (magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1])*(P[20][2] + P[0][2]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][2]*SH_MAG[0] + P[18][2]*SH_MAG[3] - P[2][2]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][2]*(SH_MAG[8] - 2*q1*q2)));
|
||||
SK_MY[1] = magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1];
|
||||
SK_MY[2] = magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5];
|
||||
SK_MY[3] = SH_MAG[8] - 2*q1*q2;
|
||||
|
||||
float Kfusion[24];
|
||||
Kfusion[0] = SK_MY[0]*(P[0][20] + P[0][17]*SH_MAG[0] + P[0][18]*SH_MAG[3] + P[0][0]*SK_MY[2] - P[0][2]*SK_MY[1] - P[0][16]*SK_MY[3]);
|
||||
Kfusion[1] = SK_MY[0]*(P[1][20] + P[1][17]*SH_MAG[0] + P[1][18]*SH_MAG[3] + P[1][0]*SK_MY[2] - P[1][2]*SK_MY[1] - P[1][16]*SK_MY[3]);
|
||||
Kfusion[2] = SK_MY[0]*(P[2][20] + P[2][17]*SH_MAG[0] + P[2][18]*SH_MAG[3] + P[2][0]*SK_MY[2] - P[2][2]*SK_MY[1] - P[2][16]*SK_MY[3]);
|
||||
Kfusion[3] = SK_MY[0]*(P[3][20] + P[3][17]*SH_MAG[0] + P[3][18]*SH_MAG[3] + P[3][0]*SK_MY[2] - P[3][2]*SK_MY[1] - P[3][16]*SK_MY[3]);
|
||||
Kfusion[4] = SK_MY[0]*(P[4][20] + P[4][17]*SH_MAG[0] + P[4][18]*SH_MAG[3] + P[4][0]*SK_MY[2] - P[4][2]*SK_MY[1] - P[4][16]*SK_MY[3]);
|
||||
Kfusion[5] = SK_MY[0]*(P[5][20] + P[5][17]*SH_MAG[0] + P[5][18]*SH_MAG[3] + P[5][0]*SK_MY[2] - P[5][2]*SK_MY[1] - P[5][16]*SK_MY[3]);
|
||||
Kfusion[6] = SK_MY[0]*(P[6][20] + P[6][17]*SH_MAG[0] + P[6][18]*SH_MAG[3] + P[6][0]*SK_MY[2] - P[6][2]*SK_MY[1] - P[6][16]*SK_MY[3]);
|
||||
Kfusion[7] = SK_MY[0]*(P[7][20] + P[7][17]*SH_MAG[0] + P[7][18]*SH_MAG[3] + P[7][0]*SK_MY[2] - P[7][2]*SK_MY[1] - P[7][16]*SK_MY[3]);
|
||||
Kfusion[8] = SK_MY[0]*(P[8][20] + P[8][17]*SH_MAG[0] + P[8][18]*SH_MAG[3] + P[8][0]*SK_MY[2] - P[8][2]*SK_MY[1] - P[8][16]*SK_MY[3]);
|
||||
Kfusion[9] = SK_MY[0]*(P[9][20] + P[9][17]*SH_MAG[0] + P[9][18]*SH_MAG[3] + P[9][0]*SK_MY[2] - P[9][2]*SK_MY[1] - P[9][16]*SK_MY[3]);
|
||||
Kfusion[10] = SK_MY[0]*(P[10][20] + P[10][17]*SH_MAG[0] + P[10][18]*SH_MAG[3] + P[10][0]*SK_MY[2] - P[10][2]*SK_MY[1] - P[10][16]*SK_MY[3]);
|
||||
Kfusion[11] = SK_MY[0]*(P[11][20] + P[11][17]*SH_MAG[0] + P[11][18]*SH_MAG[3] + P[11][0]*SK_MY[2] - P[11][2]*SK_MY[1] - P[11][16]*SK_MY[3]);
|
||||
Kfusion[12] = SK_MY[0]*(P[12][20] + P[12][17]*SH_MAG[0] + P[12][18]*SH_MAG[3] + P[12][0]*SK_MY[2] - P[12][2]*SK_MY[1] - P[12][16]*SK_MY[3]);
|
||||
Kfusion[13] = SK_MY[0]*(P[13][20] + P[13][17]*SH_MAG[0] + P[13][18]*SH_MAG[3] + P[13][0]*SK_MY[2] - P[13][2]*SK_MY[1] - P[13][16]*SK_MY[3]);
|
||||
Kfusion[14] = SK_MY[0]*(P[14][20] + P[14][17]*SH_MAG[0] + P[14][18]*SH_MAG[3] + P[14][0]*SK_MY[2] - P[14][2]*SK_MY[1] - P[14][16]*SK_MY[3]);
|
||||
Kfusion[15] = SK_MY[0]*(P[15][20] + P[15][17]*SH_MAG[0] + P[15][18]*SH_MAG[3] + P[15][0]*SK_MY[2] - P[15][2]*SK_MY[1] - P[15][16]*SK_MY[3]);
|
||||
Kfusion[16] = SK_MY[0]*(P[16][20] + P[16][17]*SH_MAG[0] + P[16][18]*SH_MAG[3] + P[16][0]*SK_MY[2] - P[16][2]*SK_MY[1] - P[16][16]*SK_MY[3]);
|
||||
Kfusion[17] = SK_MY[0]*(P[17][20] + P[17][17]*SH_MAG[0] + P[17][18]*SH_MAG[3] + P[17][0]*SK_MY[2] - P[17][2]*SK_MY[1] - P[17][16]*SK_MY[3]);
|
||||
Kfusion[18] = SK_MY[0]*(P[18][20] + P[18][17]*SH_MAG[0] + P[18][18]*SH_MAG[3] + P[18][0]*SK_MY[2] - P[18][2]*SK_MY[1] - P[18][16]*SK_MY[3]);
|
||||
Kfusion[19] = SK_MY[0]*(P[19][20] + P[19][17]*SH_MAG[0] + P[19][18]*SH_MAG[3] + P[19][0]*SK_MY[2] - P[19][2]*SK_MY[1] - P[19][16]*SK_MY[3]);
|
||||
Kfusion[20] = SK_MY[0]*(P[20][20] + P[20][17]*SH_MAG[0] + P[20][18]*SH_MAG[3] + P[20][0]*SK_MY[2] - P[20][2]*SK_MY[1] - P[20][16]*SK_MY[3]);
|
||||
Kfusion[21] = SK_MY[0]*(P[21][20] + P[21][17]*SH_MAG[0] + P[21][18]*SH_MAG[3] + P[21][0]*SK_MY[2] - P[21][2]*SK_MY[1] - P[21][16]*SK_MY[3]);
|
||||
Kfusion[22] = SK_MY[0]*(P[22][20] + P[22][17]*SH_MAG[0] + P[22][18]*SH_MAG[3] + P[22][0]*SK_MY[2] - P[22][2]*SK_MY[1] - P[22][16]*SK_MY[3]);
|
||||
Kfusion[23] = SK_MY[0]*(P[23][20] + P[23][17]*SH_MAG[0] + P[23][18]*SH_MAG[3] + P[23][0]*SK_MY[2] - P[23][2]*SK_MY[1] - P[23][16]*SK_MY[3]);
|
||||
|
||||
// Calculate the observation jacobian for the Z component
|
||||
float H_MAG[24];
|
||||
H_MAG[0] = magN*(SH_MAG[8] - 2*q1*q2) - magD*SH_MAG[3] - magE*SH_MAG[0];
|
||||
H_MAG[1] = magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1];
|
||||
H_MAG[16] = SH_MAG[5];
|
||||
H_MAG[17] = 2*q2*q3 - 2*q0*q1;
|
||||
H_MAG[18] = SH_MAG[2];
|
||||
H_MAG[21] = 1;
|
||||
|
||||
// calculate the Kalman gain matrix for the Z component
|
||||
|
||||
// intermediate variables - note SK_MZ[0] is 1/(innovation variance)
|
||||
float SK_MZ[4][1];
|
||||
SK_MZ[0] = 1/(P[21][21] + R_MAG + P[16][21]*SH_MAG[5] + P[18][21]*SH_MAG[2] - (2*q0*q1 - 2*q2*q3)*(P[21][17] + P[16][17]*SH_MAG[5] + P[18][17]*SH_MAG[2] - P[0][17]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2)) + P[1][17]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][17]*(2*q0*q1 - 2*q2*q3)) - P[0][21]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2)) + P[1][21]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) + SH_MAG[5]*(P[21][16] + P[16][16]*SH_MAG[5] + P[18][16]*SH_MAG[2] - P[0][16]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2)) + P[1][16]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][16]*(2*q0*q1 - 2*q2*q3)) + SH_MAG[2]*(P[21][18] + P[16][18]*SH_MAG[5] + P[18][18]*SH_MAG[2] - P[0][18]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2)) + P[1][18]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][18]*(2*q0*q1 - 2*q2*q3)) - (magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2))*(P[21][0] + P[16][0]*SH_MAG[5] + P[18][0]*SH_MAG[2] - P[0][0]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2)) + P[1][0]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][0]*(2*q0*q1 - 2*q2*q3)) - P[17][21]*(2*q0*q1 - 2*q2*q3) + (magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1])*(P[21][1] + P[16][1]*SH_MAG[5] + P[18][1]*SH_MAG[2] - P[0][1]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2)) + P[1][1]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][1]*(2*q0*q1 - 2*q2*q3)));
|
||||
SK_MZ[1] = magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2);
|
||||
SK_MZ[2] = magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1];
|
||||
SK_MZ[3] = 2*q0*q1 - 2*q2*q3;
|
||||
|
||||
float Kfusion[24];
|
||||
Kfusion[0] = SK_MZ[0]*(P[0][21] + P[0][18]*SH_MAG[2] + P[0][16]*SH_MAG[5] - P[0][0]*SK_MZ[1] + P[0][1]*SK_MZ[2] - P[0][17]*SK_MZ[3]);
|
||||
Kfusion[1] = SK_MZ[0]*(P[1][21] + P[1][18]*SH_MAG[2] + P[1][16]*SH_MAG[5] - P[1][0]*SK_MZ[1] + P[1][1]*SK_MZ[2] - P[1][17]*SK_MZ[3]);
|
||||
Kfusion[2] = SK_MZ[0]*(P[2][21] + P[2][18]*SH_MAG[2] + P[2][16]*SH_MAG[5] - P[2][0]*SK_MZ[1] + P[2][1]*SK_MZ[2] - P[2][17]*SK_MZ[3]);
|
||||
Kfusion[3] = SK_MZ[0]*(P[3][21] + P[3][18]*SH_MAG[2] + P[3][16]*SH_MAG[5] - P[3][0]*SK_MZ[1] + P[3][1]*SK_MZ[2] - P[3][17]*SK_MZ[3]);
|
||||
Kfusion[4] = SK_MZ[0]*(P[4][21] + P[4][18]*SH_MAG[2] + P[4][16]*SH_MAG[5] - P[4][0]*SK_MZ[1] + P[4][1]*SK_MZ[2] - P[4][17]*SK_MZ[3]);
|
||||
Kfusion[5] = SK_MZ[0]*(P[5][21] + P[5][18]*SH_MAG[2] + P[5][16]*SH_MAG[5] - P[5][0]*SK_MZ[1] + P[5][1]*SK_MZ[2] - P[5][17]*SK_MZ[3]);
|
||||
Kfusion[6] = SK_MZ[0]*(P[6][21] + P[6][18]*SH_MAG[2] + P[6][16]*SH_MAG[5] - P[6][0]*SK_MZ[1] + P[6][1]*SK_MZ[2] - P[6][17]*SK_MZ[3]);
|
||||
Kfusion[7] = SK_MZ[0]*(P[7][21] + P[7][18]*SH_MAG[2] + P[7][16]*SH_MAG[5] - P[7][0]*SK_MZ[1] + P[7][1]*SK_MZ[2] - P[7][17]*SK_MZ[3]);
|
||||
Kfusion[8] = SK_MZ[0]*(P[8][21] + P[8][18]*SH_MAG[2] + P[8][16]*SH_MAG[5] - P[8][0]*SK_MZ[1] + P[8][1]*SK_MZ[2] - P[8][17]*SK_MZ[3]);
|
||||
Kfusion[9] = SK_MZ[0]*(P[9][21] + P[9][18]*SH_MAG[2] + P[9][16]*SH_MAG[5] - P[9][0]*SK_MZ[1] + P[9][1]*SK_MZ[2] - P[9][17]*SK_MZ[3]);
|
||||
Kfusion[10] = SK_MZ[0]*(P[10][21] + P[10][18]*SH_MAG[2] + P[10][16]*SH_MAG[5] - P[10][0]*SK_MZ[1] + P[10][1]*SK_MZ[2] - P[10][17]*SK_MZ[3]);
|
||||
Kfusion[11] = SK_MZ[0]*(P[11][21] + P[11][18]*SH_MAG[2] + P[11][16]*SH_MAG[5] - P[11][0]*SK_MZ[1] + P[11][1]*SK_MZ[2] - P[11][17]*SK_MZ[3]);
|
||||
Kfusion[12] = SK_MZ[0]*(P[12][21] + P[12][18]*SH_MAG[2] + P[12][16]*SH_MAG[5] - P[12][0]*SK_MZ[1] + P[12][1]*SK_MZ[2] - P[12][17]*SK_MZ[3]);
|
||||
Kfusion[13] = SK_MZ[0]*(P[13][21] + P[13][18]*SH_MAG[2] + P[13][16]*SH_MAG[5] - P[13][0]*SK_MZ[1] + P[13][1]*SK_MZ[2] - P[13][17]*SK_MZ[3]);
|
||||
Kfusion[14] = SK_MZ[0]*(P[14][21] + P[14][18]*SH_MAG[2] + P[14][16]*SH_MAG[5] - P[14][0]*SK_MZ[1] + P[14][1]*SK_MZ[2] - P[14][17]*SK_MZ[3]);
|
||||
Kfusion[15] = SK_MZ[0]*(P[15][21] + P[15][18]*SH_MAG[2] + P[15][16]*SH_MAG[5] - P[15][0]*SK_MZ[1] + P[15][1]*SK_MZ[2] - P[15][17]*SK_MZ[3]);
|
||||
Kfusion[16] = SK_MZ[0]*(P[16][21] + P[16][18]*SH_MAG[2] + P[16][16]*SH_MAG[5] - P[16][0]*SK_MZ[1] + P[16][1]*SK_MZ[2] - P[16][17]*SK_MZ[3]);
|
||||
Kfusion[17] = SK_MZ[0]*(P[17][21] + P[17][18]*SH_MAG[2] + P[17][16]*SH_MAG[5] - P[17][0]*SK_MZ[1] + P[17][1]*SK_MZ[2] - P[17][17]*SK_MZ[3]);
|
||||
Kfusion[18] = SK_MZ[0]*(P[18][21] + P[18][18]*SH_MAG[2] + P[18][16]*SH_MAG[5] - P[18][0]*SK_MZ[1] + P[18][1]*SK_MZ[2] - P[18][17]*SK_MZ[3]);
|
||||
Kfusion[19] = SK_MZ[0]*(P[19][21] + P[19][18]*SH_MAG[2] + P[19][16]*SH_MAG[5] - P[19][0]*SK_MZ[1] + P[19][1]*SK_MZ[2] - P[19][17]*SK_MZ[3]);
|
||||
Kfusion[20] = SK_MZ[0]*(P[20][21] + P[20][18]*SH_MAG[2] + P[20][16]*SH_MAG[5] - P[20][0]*SK_MZ[1] + P[20][1]*SK_MZ[2] - P[20][17]*SK_MZ[3]);
|
||||
Kfusion[21] = SK_MZ[0]*(P[21][21] + P[21][18]*SH_MAG[2] + P[21][16]*SH_MAG[5] - P[21][0]*SK_MZ[1] + P[21][1]*SK_MZ[2] - P[21][17]*SK_MZ[3]);
|
||||
Kfusion[22] = SK_MZ[0]*(P[22][21] + P[22][18]*SH_MAG[2] + P[22][16]*SH_MAG[5] - P[22][0]*SK_MZ[1] + P[22][1]*SK_MZ[2] - P[22][17]*SK_MZ[3]);
|
||||
Kfusion[23] = SK_MZ[0]*(P[23][21] + P[23][18]*SH_MAG[2] + P[23][16]*SH_MAG[5] - P[23][0]*SK_MZ[1] + P[23][1]*SK_MZ[2] - P[23][17]*SK_MZ[3]);
|
||||
|
|
@ -0,0 +1,98 @@
|
|||
// Auto code for fusion of line of sight rate massurements from a optical flow camera aligned with the Z body axis
|
||||
// Observations are body modtion compensated optica flow rates about the X and Y body axis
|
||||
// Sequential fusion is used (observation errors about each axis are assumed to be uncorrelated)
|
||||
|
||||
// intermediate variable from algebraic optimisation
|
||||
float SH_LOS[7];
|
||||
SH_LOS[0] = sq(q0) - sq(q1) - sq(q2) + sq(q3);
|
||||
SH_LOS[1] = vn*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) - vd*(2*q0*q2 - 2*q1*q3) + ve*(2*q0*q3 + 2*q1*q2);
|
||||
SH_LOS[2] = ve*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + vd*(2*q0*q1 + 2*q2*q3) - vn*(2*q0*q3 - 2*q1*q2);
|
||||
SH_LOS[3] = 1/(pd - ptd);
|
||||
SH_LOS[4] = vd*SH_LOS[0] - ve*(2*q0*q1 - 2*q2*q3) + vn*(2*q0*q2 + 2*q1*q3);
|
||||
SH_LOS[5] = 2*q0*q2 - 2*q1*q3;
|
||||
SH_LOS[6] = 2*q0*q1 + 2*q2*q3;
|
||||
|
||||
// Calculate the observation jacobians for the LOS rate about the X body axis
|
||||
float H_LOS[24];
|
||||
H_LOS[0] = SH_LOS[2]*SH_LOS[3]*SH_LOS[6] - SH_LOS[0]*SH_LOS[3]*SH_LOS[4];
|
||||
H_LOS[1] = SH_LOS[2]*SH_LOS[3]*SH_LOS[5];
|
||||
H_LOS[2] = SH_LOS[0]*SH_LOS[1]*SH_LOS[3];
|
||||
H_LOS[3] = SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2);
|
||||
H_LOS[4] = -SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3));
|
||||
H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*SH_LOS[6];
|
||||
H_LOS[8] = SH_LOS[0]*SH_LOS[2]*sq(SH_LOS[3]);
|
||||
|
||||
// Calculate the observation jacobians for the LOS rate about the Y body axis
|
||||
float H_LOS[24];
|
||||
H_LOS[0] = -SH_LOS[1]*SH_LOS[3]*SH_LOS[6];
|
||||
H_LOS[1] = - SH_LOS[0]*SH_LOS[3]*SH_LOS[4] - SH_LOS[1]*SH_LOS[3]*SH_LOS[5];
|
||||
H_LOS[2] = SH_LOS[0]*SH_LOS[2]*SH_LOS[3];
|
||||
H_LOS[3] = SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3));
|
||||
H_LOS[4] = SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2);
|
||||
H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*SH_LOS[5];
|
||||
H_LOS[8] = -SH_LOS[0]*SH_LOS[1]*sq(SH_LOS[3]);
|
||||
|
||||
|
||||
// Intermediate variables used to calculate the Kalman gain matrices
|
||||
float SK_LOS[5];
|
||||
// this is 1/(innovation variance) for the X axis measurement
|
||||
SK_LOS[0] = 1/(R_LOS - (SH_LOS[0]*SH_LOS[3]*SH_LOS[4] - SH_LOS[2]*SH_LOS[3]*SH_LOS[6])*(P[8][0]*SH_LOS[0]*SH_LOS[2]*sq(SH_LOS[3]) - P[0][0]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] - SH_LOS[2]*SH_LOS[3]*SH_LOS[6]) + P[3][0]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[1][0]*SH_LOS[2]*SH_LOS[3]*SH_LOS[5] + P[2][0]*SH_LOS[0]*SH_LOS[1]*SH_LOS[3] - P[5][0]*SH_LOS[0]*SH_LOS[3]*SH_LOS[6] - P[4][0]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) + SH_LOS[2]*SH_LOS[3]*SH_LOS[5]*(P[8][1]*SH_LOS[0]*SH_LOS[2]*sq(SH_LOS[3]) - P[0][1]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] - SH_LOS[2]*SH_LOS[3]*SH_LOS[6]) + P[3][1]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[1][1]*SH_LOS[2]*SH_LOS[3]*SH_LOS[5] + P[2][1]*SH_LOS[0]*SH_LOS[1]*SH_LOS[3] - P[5][1]*SH_LOS[0]*SH_LOS[3]*SH_LOS[6] - P[4][1]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) + SH_LOS[0]*SH_LOS[1]*SH_LOS[3]*(P[8][2]*SH_LOS[0]*SH_LOS[2]*sq(SH_LOS[3]) - P[0][2]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] - SH_LOS[2]*SH_LOS[3]*SH_LOS[6]) + P[3][2]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[1][2]*SH_LOS[2]*SH_LOS[3]*SH_LOS[5] + P[2][2]*SH_LOS[0]*SH_LOS[1]*SH_LOS[3] - P[5][2]*SH_LOS[0]*SH_LOS[3]*SH_LOS[6] - P[4][2]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) - SH_LOS[0]*SH_LOS[3]*SH_LOS[6]*(P[8][5]*SH_LOS[0]*SH_LOS[2]*sq(SH_LOS[3]) - P[0][5]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] - SH_LOS[2]*SH_LOS[3]*SH_LOS[6]) + P[3][5]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[1][5]*SH_LOS[2]*SH_LOS[3]*SH_LOS[5] + P[2][5]*SH_LOS[0]*SH_LOS[1]*SH_LOS[3] - P[5][5]*SH_LOS[0]*SH_LOS[3]*SH_LOS[6] - P[4][5]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) - SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))*(P[8][4]*SH_LOS[0]*SH_LOS[2]*sq(SH_LOS[3]) - P[0][4]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] - SH_LOS[2]*SH_LOS[3]*SH_LOS[6]) + P[3][4]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[1][4]*SH_LOS[2]*SH_LOS[3]*SH_LOS[5] + P[2][4]*SH_LOS[0]*SH_LOS[1]*SH_LOS[3] - P[5][4]*SH_LOS[0]*SH_LOS[3]*SH_LOS[6] - P[4][4]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) + SH_LOS[0]*SH_LOS[2]*sq(SH_LOS[3])*(P[8][8]*SH_LOS[0]*SH_LOS[2]*sq(SH_LOS[3]) - P[0][8]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] - SH_LOS[2]*SH_LOS[3]*SH_LOS[6]) + P[3][8]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[1][8]*SH_LOS[2]*SH_LOS[3]*SH_LOS[5] + P[2][8]*SH_LOS[0]*SH_LOS[1]*SH_LOS[3] - P[5][8]*SH_LOS[0]*SH_LOS[3]*SH_LOS[6] - P[4][8]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) + SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2)*(P[8][3]*SH_LOS[0]*SH_LOS[2]*sq(SH_LOS[3]) - P[0][3]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] - SH_LOS[2]*SH_LOS[3]*SH_LOS[6]) + P[3][3]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[1][3]*SH_LOS[2]*SH_LOS[3]*SH_LOS[5] + P[2][3]*SH_LOS[0]*SH_LOS[1]*SH_LOS[3] - P[5][3]*SH_LOS[0]*SH_LOS[3]*SH_LOS[6] - P[4][3]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))));
|
||||
// this is 1/(innovation variance) for the Y axis measurement
|
||||
SK_LOS[1] = 1/(R_LOS + (SH_LOS[0]*SH_LOS[3]*SH_LOS[4] + SH_LOS[1]*SH_LOS[3]*SH_LOS[5])*(P[1][1]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] + SH_LOS[1]*SH_LOS[3]*SH_LOS[5]) + P[8][1]*SH_LOS[0]*SH_LOS[1]*sq(SH_LOS[3]) - P[4][1]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) + P[0][1]*SH_LOS[1]*SH_LOS[3]*SH_LOS[6] - P[2][1]*SH_LOS[0]*SH_LOS[2]*SH_LOS[3] + P[5][1]*SH_LOS[0]*SH_LOS[3]*SH_LOS[5] - P[3][1]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) + SH_LOS[1]*SH_LOS[3]*SH_LOS[6]*(P[1][0]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] + SH_LOS[1]*SH_LOS[3]*SH_LOS[5]) + P[8][0]*SH_LOS[0]*SH_LOS[1]*sq(SH_LOS[3]) - P[4][0]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) + P[0][0]*SH_LOS[1]*SH_LOS[3]*SH_LOS[6] - P[2][0]*SH_LOS[0]*SH_LOS[2]*SH_LOS[3] + P[5][0]*SH_LOS[0]*SH_LOS[3]*SH_LOS[5] - P[3][0]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) - SH_LOS[0]*SH_LOS[2]*SH_LOS[3]*(P[1][2]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] + SH_LOS[1]*SH_LOS[3]*SH_LOS[5]) + P[8][2]*SH_LOS[0]*SH_LOS[1]*sq(SH_LOS[3]) - P[4][2]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) + P[0][2]*SH_LOS[1]*SH_LOS[3]*SH_LOS[6] - P[2][2]*SH_LOS[0]*SH_LOS[2]*SH_LOS[3] + P[5][2]*SH_LOS[0]*SH_LOS[3]*SH_LOS[5] - P[3][2]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) + SH_LOS[0]*SH_LOS[3]*SH_LOS[5]*(P[1][5]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] + SH_LOS[1]*SH_LOS[3]*SH_LOS[5]) + P[8][5]*SH_LOS[0]*SH_LOS[1]*sq(SH_LOS[3]) - P[4][5]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) + P[0][5]*SH_LOS[1]*SH_LOS[3]*SH_LOS[6] - P[2][5]*SH_LOS[0]*SH_LOS[2]*SH_LOS[3] + P[5][5]*SH_LOS[0]*SH_LOS[3]*SH_LOS[5] - P[3][5]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) - SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))*(P[1][3]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] + SH_LOS[1]*SH_LOS[3]*SH_LOS[5]) + P[8][3]*SH_LOS[0]*SH_LOS[1]*sq(SH_LOS[3]) - P[4][3]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) + P[0][3]*SH_LOS[1]*SH_LOS[3]*SH_LOS[6] - P[2][3]*SH_LOS[0]*SH_LOS[2]*SH_LOS[3] + P[5][3]*SH_LOS[0]*SH_LOS[3]*SH_LOS[5] - P[3][3]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) + SH_LOS[0]*SH_LOS[1]*sq(SH_LOS[3])*(P[1][8]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] + SH_LOS[1]*SH_LOS[3]*SH_LOS[5]) + P[8][8]*SH_LOS[0]*SH_LOS[1]*sq(SH_LOS[3]) - P[4][8]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) + P[0][8]*SH_LOS[1]*SH_LOS[3]*SH_LOS[6] - P[2][8]*SH_LOS[0]*SH_LOS[2]*SH_LOS[3] + P[5][8]*SH_LOS[0]*SH_LOS[3]*SH_LOS[5] - P[3][8]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) - SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2)*(P[1][4]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] + SH_LOS[1]*SH_LOS[3]*SH_LOS[5]) + P[8][4]*SH_LOS[0]*SH_LOS[1]*sq(SH_LOS[3]) - P[4][4]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) + P[0][4]*SH_LOS[1]*SH_LOS[3]*SH_LOS[6] - P[2][4]*SH_LOS[0]*SH_LOS[2]*SH_LOS[3] + P[5][4]*SH_LOS[0]*SH_LOS[3]*SH_LOS[5] - P[3][4]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))));
|
||||
SK_LOS[2] = sq(q0) + sq(q1) - sq(q2) - sq(q3);
|
||||
SK_LOS[3] = sq(q0) - sq(q1) + sq(q2) - sq(q3);
|
||||
SK_LOS[4] = SH_LOS[3];
|
||||
|
||||
// Calculate the Kalman gain matrix for the X axis measurement
|
||||
float Kfusion[24];
|
||||
Kfusion[0] = SK_LOS[0]*(P[0][8]*SH_LOS[0]*SH_LOS[2]*sq(SK_LOS[4]) - P[0][0]*(SH_LOS[0]*SH_LOS[4]*SK_LOS[4] - SH_LOS[2]*SH_LOS[6]*SK_LOS[4]) + P[0][3]*SH_LOS[0]*SK_LOS[4]*(2*q0*q3 - 2*q1*q2) + P[0][2]*SH_LOS[0]*SH_LOS[1]*SK_LOS[4] + P[0][1]*SH_LOS[2]*SH_LOS[5]*SK_LOS[4] - P[0][5]*SH_LOS[0]*SH_LOS[6]*SK_LOS[4] - P[0][4]*SH_LOS[0]*SK_LOS[3]*SK_LOS[4]);
|
||||
Kfusion[1] = SK_LOS[0]*(P[1][8]*SH_LOS[0]*SH_LOS[2]*sq(SK_LOS[4]) - P[1][0]*(SH_LOS[0]*SH_LOS[4]*SK_LOS[4] - SH_LOS[2]*SH_LOS[6]*SK_LOS[4]) + P[1][3]*SH_LOS[0]*SK_LOS[4]*(2*q0*q3 - 2*q1*q2) + P[1][2]*SH_LOS[0]*SH_LOS[1]*SK_LOS[4] + P[1][1]*SH_LOS[2]*SH_LOS[5]*SK_LOS[4] - P[1][5]*SH_LOS[0]*SH_LOS[6]*SK_LOS[4] - P[1][4]*SH_LOS[0]*SK_LOS[3]*SK_LOS[4]);
|
||||
Kfusion[2] = SK_LOS[0]*(P[2][8]*SH_LOS[0]*SH_LOS[2]*sq(SK_LOS[4]) - P[2][0]*(SH_LOS[0]*SH_LOS[4]*SK_LOS[4] - SH_LOS[2]*SH_LOS[6]*SK_LOS[4]) + P[2][3]*SH_LOS[0]*SK_LOS[4]*(2*q0*q3 - 2*q1*q2) + P[2][2]*SH_LOS[0]*SH_LOS[1]*SK_LOS[4] + P[2][1]*SH_LOS[2]*SH_LOS[5]*SK_LOS[4] - P[2][5]*SH_LOS[0]*SH_LOS[6]*SK_LOS[4] - P[2][4]*SH_LOS[0]*SK_LOS[3]*SK_LOS[4]);
|
||||
Kfusion[3] = SK_LOS[0]*(P[3][8]*SH_LOS[0]*SH_LOS[2]*sq(SK_LOS[4]) - P[3][0]*(SH_LOS[0]*SH_LOS[4]*SK_LOS[4] - SH_LOS[2]*SH_LOS[6]*SK_LOS[4]) + P[3][3]*SH_LOS[0]*SK_LOS[4]*(2*q0*q3 - 2*q1*q2) + P[3][2]*SH_LOS[0]*SH_LOS[1]*SK_LOS[4] + P[3][1]*SH_LOS[2]*SH_LOS[5]*SK_LOS[4] - P[3][5]*SH_LOS[0]*SH_LOS[6]*SK_LOS[4] - P[3][4]*SH_LOS[0]*SK_LOS[3]*SK_LOS[4]);
|
||||
Kfusion[4] = SK_LOS[0]*(P[4][8]*SH_LOS[0]*SH_LOS[2]*sq(SK_LOS[4]) - P[4][0]*(SH_LOS[0]*SH_LOS[4]*SK_LOS[4] - SH_LOS[2]*SH_LOS[6]*SK_LOS[4]) + P[4][3]*SH_LOS[0]*SK_LOS[4]*(2*q0*q3 - 2*q1*q2) + P[4][2]*SH_LOS[0]*SH_LOS[1]*SK_LOS[4] + P[4][1]*SH_LOS[2]*SH_LOS[5]*SK_LOS[4] - P[4][5]*SH_LOS[0]*SH_LOS[6]*SK_LOS[4] - P[4][4]*SH_LOS[0]*SK_LOS[3]*SK_LOS[4]);
|
||||
Kfusion[5] = SK_LOS[0]*(P[5][8]*SH_LOS[0]*SH_LOS[2]*sq(SK_LOS[4]) - P[5][0]*(SH_LOS[0]*SH_LOS[4]*SK_LOS[4] - SH_LOS[2]*SH_LOS[6]*SK_LOS[4]) + P[5][3]*SH_LOS[0]*SK_LOS[4]*(2*q0*q3 - 2*q1*q2) + P[5][2]*SH_LOS[0]*SH_LOS[1]*SK_LOS[4] + P[5][1]*SH_LOS[2]*SH_LOS[5]*SK_LOS[4] - P[5][5]*SH_LOS[0]*SH_LOS[6]*SK_LOS[4] - P[5][4]*SH_LOS[0]*SK_LOS[3]*SK_LOS[4]);
|
||||
Kfusion[6] = SK_LOS[0]*(P[6][8]*SH_LOS[0]*SH_LOS[2]*sq(SK_LOS[4]) - P[6][0]*(SH_LOS[0]*SH_LOS[4]*SK_LOS[4] - SH_LOS[2]*SH_LOS[6]*SK_LOS[4]) + P[6][3]*SH_LOS[0]*SK_LOS[4]*(2*q0*q3 - 2*q1*q2) + P[6][2]*SH_LOS[0]*SH_LOS[1]*SK_LOS[4] + P[6][1]*SH_LOS[2]*SH_LOS[5]*SK_LOS[4] - P[6][5]*SH_LOS[0]*SH_LOS[6]*SK_LOS[4] - P[6][4]*SH_LOS[0]*SK_LOS[3]*SK_LOS[4]);
|
||||
Kfusion[7] = SK_LOS[0]*(P[7][8]*SH_LOS[0]*SH_LOS[2]*sq(SK_LOS[4]) - P[7][0]*(SH_LOS[0]*SH_LOS[4]*SK_LOS[4] - SH_LOS[2]*SH_LOS[6]*SK_LOS[4]) + P[7][3]*SH_LOS[0]*SK_LOS[4]*(2*q0*q3 - 2*q1*q2) + P[7][2]*SH_LOS[0]*SH_LOS[1]*SK_LOS[4] + P[7][1]*SH_LOS[2]*SH_LOS[5]*SK_LOS[4] - P[7][5]*SH_LOS[0]*SH_LOS[6]*SK_LOS[4] - P[7][4]*SH_LOS[0]*SK_LOS[3]*SK_LOS[4]);
|
||||
Kfusion[8] = SK_LOS[0]*(P[8][8]*SH_LOS[0]*SH_LOS[2]*sq(SK_LOS[4]) - P[8][0]*(SH_LOS[0]*SH_LOS[4]*SK_LOS[4] - SH_LOS[2]*SH_LOS[6]*SK_LOS[4]) + P[8][3]*SH_LOS[0]*SK_LOS[4]*(2*q0*q3 - 2*q1*q2) + P[8][2]*SH_LOS[0]*SH_LOS[1]*SK_LOS[4] + P[8][1]*SH_LOS[2]*SH_LOS[5]*SK_LOS[4] - P[8][5]*SH_LOS[0]*SH_LOS[6]*SK_LOS[4] - P[8][4]*SH_LOS[0]*SK_LOS[3]*SK_LOS[4]);
|
||||
Kfusion[9] = SK_LOS[0]*(P[9][8]*SH_LOS[0]*SH_LOS[2]*sq(SK_LOS[4]) - P[9][0]*(SH_LOS[0]*SH_LOS[4]*SK_LOS[4] - SH_LOS[2]*SH_LOS[6]*SK_LOS[4]) + P[9][3]*SH_LOS[0]*SK_LOS[4]*(2*q0*q3 - 2*q1*q2) + P[9][2]*SH_LOS[0]*SH_LOS[1]*SK_LOS[4] + P[9][1]*SH_LOS[2]*SH_LOS[5]*SK_LOS[4] - P[9][5]*SH_LOS[0]*SH_LOS[6]*SK_LOS[4] - P[9][4]*SH_LOS[0]*SK_LOS[3]*SK_LOS[4]);
|
||||
Kfusion[10] = SK_LOS[0]*(P[10][8]*SH_LOS[0]*SH_LOS[2]*sq(SK_LOS[4]) - P[10][0]*(SH_LOS[0]*SH_LOS[4]*SK_LOS[4] - SH_LOS[2]*SH_LOS[6]*SK_LOS[4]) + P[10][3]*SH_LOS[0]*SK_LOS[4]*(2*q0*q3 - 2*q1*q2) + P[10][2]*SH_LOS[0]*SH_LOS[1]*SK_LOS[4] + P[10][1]*SH_LOS[2]*SH_LOS[5]*SK_LOS[4] - P[10][5]*SH_LOS[0]*SH_LOS[6]*SK_LOS[4] - P[10][4]*SH_LOS[0]*SK_LOS[3]*SK_LOS[4]);
|
||||
Kfusion[11] = SK_LOS[0]*(P[11][8]*SH_LOS[0]*SH_LOS[2]*sq(SK_LOS[4]) - P[11][0]*(SH_LOS[0]*SH_LOS[4]*SK_LOS[4] - SH_LOS[2]*SH_LOS[6]*SK_LOS[4]) + P[11][3]*SH_LOS[0]*SK_LOS[4]*(2*q0*q3 - 2*q1*q2) + P[11][2]*SH_LOS[0]*SH_LOS[1]*SK_LOS[4] + P[11][1]*SH_LOS[2]*SH_LOS[5]*SK_LOS[4] - P[11][5]*SH_LOS[0]*SH_LOS[6]*SK_LOS[4] - P[11][4]*SH_LOS[0]*SK_LOS[3]*SK_LOS[4]);
|
||||
Kfusion[12] = SK_LOS[0]*(P[12][8]*SH_LOS[0]*SH_LOS[2]*sq(SK_LOS[4]) - P[12][0]*(SH_LOS[0]*SH_LOS[4]*SK_LOS[4] - SH_LOS[2]*SH_LOS[6]*SK_LOS[4]) + P[12][3]*SH_LOS[0]*SK_LOS[4]*(2*q0*q3 - 2*q1*q2) + P[12][2]*SH_LOS[0]*SH_LOS[1]*SK_LOS[4] + P[12][1]*SH_LOS[2]*SH_LOS[5]*SK_LOS[4] - P[12][5]*SH_LOS[0]*SH_LOS[6]*SK_LOS[4] - P[12][4]*SH_LOS[0]*SK_LOS[3]*SK_LOS[4]);
|
||||
Kfusion[13] = SK_LOS[0]*(P[13][8]*SH_LOS[0]*SH_LOS[2]*sq(SK_LOS[4]) - P[13][0]*(SH_LOS[0]*SH_LOS[4]*SK_LOS[4] - SH_LOS[2]*SH_LOS[6]*SK_LOS[4]) + P[13][3]*SH_LOS[0]*SK_LOS[4]*(2*q0*q3 - 2*q1*q2) + P[13][2]*SH_LOS[0]*SH_LOS[1]*SK_LOS[4] + P[13][1]*SH_LOS[2]*SH_LOS[5]*SK_LOS[4] - P[13][5]*SH_LOS[0]*SH_LOS[6]*SK_LOS[4] - P[13][4]*SH_LOS[0]*SK_LOS[3]*SK_LOS[4]);
|
||||
Kfusion[14] = SK_LOS[0]*(P[14][8]*SH_LOS[0]*SH_LOS[2]*sq(SK_LOS[4]) - P[14][0]*(SH_LOS[0]*SH_LOS[4]*SK_LOS[4] - SH_LOS[2]*SH_LOS[6]*SK_LOS[4]) + P[14][3]*SH_LOS[0]*SK_LOS[4]*(2*q0*q3 - 2*q1*q2) + P[14][2]*SH_LOS[0]*SH_LOS[1]*SK_LOS[4] + P[14][1]*SH_LOS[2]*SH_LOS[5]*SK_LOS[4] - P[14][5]*SH_LOS[0]*SH_LOS[6]*SK_LOS[4] - P[14][4]*SH_LOS[0]*SK_LOS[3]*SK_LOS[4]);
|
||||
Kfusion[15] = SK_LOS[0]*(P[15][8]*SH_LOS[0]*SH_LOS[2]*sq(SK_LOS[4]) - P[15][0]*(SH_LOS[0]*SH_LOS[4]*SK_LOS[4] - SH_LOS[2]*SH_LOS[6]*SK_LOS[4]) + P[15][3]*SH_LOS[0]*SK_LOS[4]*(2*q0*q3 - 2*q1*q2) + P[15][2]*SH_LOS[0]*SH_LOS[1]*SK_LOS[4] + P[15][1]*SH_LOS[2]*SH_LOS[5]*SK_LOS[4] - P[15][5]*SH_LOS[0]*SH_LOS[6]*SK_LOS[4] - P[15][4]*SH_LOS[0]*SK_LOS[3]*SK_LOS[4]);
|
||||
Kfusion[16] = SK_LOS[0]*(P[16][8]*SH_LOS[0]*SH_LOS[2]*sq(SK_LOS[4]) - P[16][0]*(SH_LOS[0]*SH_LOS[4]*SK_LOS[4] - SH_LOS[2]*SH_LOS[6]*SK_LOS[4]) + P[16][3]*SH_LOS[0]*SK_LOS[4]*(2*q0*q3 - 2*q1*q2) + P[16][2]*SH_LOS[0]*SH_LOS[1]*SK_LOS[4] + P[16][1]*SH_LOS[2]*SH_LOS[5]*SK_LOS[4] - P[16][5]*SH_LOS[0]*SH_LOS[6]*SK_LOS[4] - P[16][4]*SH_LOS[0]*SK_LOS[3]*SK_LOS[4]);
|
||||
Kfusion[17] = SK_LOS[0]*(P[17][8]*SH_LOS[0]*SH_LOS[2]*sq(SK_LOS[4]) - P[17][0]*(SH_LOS[0]*SH_LOS[4]*SK_LOS[4] - SH_LOS[2]*SH_LOS[6]*SK_LOS[4]) + P[17][3]*SH_LOS[0]*SK_LOS[4]*(2*q0*q3 - 2*q1*q2) + P[17][2]*SH_LOS[0]*SH_LOS[1]*SK_LOS[4] + P[17][1]*SH_LOS[2]*SH_LOS[5]*SK_LOS[4] - P[17][5]*SH_LOS[0]*SH_LOS[6]*SK_LOS[4] - P[17][4]*SH_LOS[0]*SK_LOS[3]*SK_LOS[4]);
|
||||
Kfusion[18] = SK_LOS[0]*(P[18][8]*SH_LOS[0]*SH_LOS[2]*sq(SK_LOS[4]) - P[18][0]*(SH_LOS[0]*SH_LOS[4]*SK_LOS[4] - SH_LOS[2]*SH_LOS[6]*SK_LOS[4]) + P[18][3]*SH_LOS[0]*SK_LOS[4]*(2*q0*q3 - 2*q1*q2) + P[18][2]*SH_LOS[0]*SH_LOS[1]*SK_LOS[4] + P[18][1]*SH_LOS[2]*SH_LOS[5]*SK_LOS[4] - P[18][5]*SH_LOS[0]*SH_LOS[6]*SK_LOS[4] - P[18][4]*SH_LOS[0]*SK_LOS[3]*SK_LOS[4]);
|
||||
Kfusion[19] = SK_LOS[0]*(P[19][8]*SH_LOS[0]*SH_LOS[2]*sq(SK_LOS[4]) - P[19][0]*(SH_LOS[0]*SH_LOS[4]*SK_LOS[4] - SH_LOS[2]*SH_LOS[6]*SK_LOS[4]) + P[19][3]*SH_LOS[0]*SK_LOS[4]*(2*q0*q3 - 2*q1*q2) + P[19][2]*SH_LOS[0]*SH_LOS[1]*SK_LOS[4] + P[19][1]*SH_LOS[2]*SH_LOS[5]*SK_LOS[4] - P[19][5]*SH_LOS[0]*SH_LOS[6]*SK_LOS[4] - P[19][4]*SH_LOS[0]*SK_LOS[3]*SK_LOS[4]);
|
||||
Kfusion[20] = SK_LOS[0]*(P[20][8]*SH_LOS[0]*SH_LOS[2]*sq(SK_LOS[4]) - P[20][0]*(SH_LOS[0]*SH_LOS[4]*SK_LOS[4] - SH_LOS[2]*SH_LOS[6]*SK_LOS[4]) + P[20][3]*SH_LOS[0]*SK_LOS[4]*(2*q0*q3 - 2*q1*q2) + P[20][2]*SH_LOS[0]*SH_LOS[1]*SK_LOS[4] + P[20][1]*SH_LOS[2]*SH_LOS[5]*SK_LOS[4] - P[20][5]*SH_LOS[0]*SH_LOS[6]*SK_LOS[4] - P[20][4]*SH_LOS[0]*SK_LOS[3]*SK_LOS[4]);
|
||||
Kfusion[21] = SK_LOS[0]*(P[21][8]*SH_LOS[0]*SH_LOS[2]*sq(SK_LOS[4]) - P[21][0]*(SH_LOS[0]*SH_LOS[4]*SK_LOS[4] - SH_LOS[2]*SH_LOS[6]*SK_LOS[4]) + P[21][3]*SH_LOS[0]*SK_LOS[4]*(2*q0*q3 - 2*q1*q2) + P[21][2]*SH_LOS[0]*SH_LOS[1]*SK_LOS[4] + P[21][1]*SH_LOS[2]*SH_LOS[5]*SK_LOS[4] - P[21][5]*SH_LOS[0]*SH_LOS[6]*SK_LOS[4] - P[21][4]*SH_LOS[0]*SK_LOS[3]*SK_LOS[4]);
|
||||
Kfusion[22] = SK_LOS[0]*(P[22][8]*SH_LOS[0]*SH_LOS[2]*sq(SK_LOS[4]) - P[22][0]*(SH_LOS[0]*SH_LOS[4]*SK_LOS[4] - SH_LOS[2]*SH_LOS[6]*SK_LOS[4]) + P[22][3]*SH_LOS[0]*SK_LOS[4]*(2*q0*q3 - 2*q1*q2) + P[22][2]*SH_LOS[0]*SH_LOS[1]*SK_LOS[4] + P[22][1]*SH_LOS[2]*SH_LOS[5]*SK_LOS[4] - P[22][5]*SH_LOS[0]*SH_LOS[6]*SK_LOS[4] - P[22][4]*SH_LOS[0]*SK_LOS[3]*SK_LOS[4]);
|
||||
Kfusion[23] = SK_LOS[0]*(P[23][8]*SH_LOS[0]*SH_LOS[2]*sq(SK_LOS[4]) - P[23][0]*(SH_LOS[0]*SH_LOS[4]*SK_LOS[4] - SH_LOS[2]*SH_LOS[6]*SK_LOS[4]) + P[23][3]*SH_LOS[0]*SK_LOS[4]*(2*q0*q3 - 2*q1*q2) + P[23][2]*SH_LOS[0]*SH_LOS[1]*SK_LOS[4] + P[23][1]*SH_LOS[2]*SH_LOS[5]*SK_LOS[4] - P[23][5]*SH_LOS[0]*SH_LOS[6]*SK_LOS[4] - P[23][4]*SH_LOS[0]*SK_LOS[3]*SK_LOS[4]);
|
||||
|
||||
// Calculate the Kalman gain matrix for the Y axis measurement
|
||||
float Kfusion[24];
|
||||
Kfusion[0] = -SK_LOS[1]*(P[0][1]*(SH_LOS[0]*SH_LOS[4]*SK_LOS[4] + SH_LOS[1]*SH_LOS[5]*SK_LOS[4]) + P[0][8]*SH_LOS[0]*SH_LOS[1]*sq(SK_LOS[4]) - P[0][4]*SH_LOS[0]*SK_LOS[4]*(2*q0*q3 + 2*q1*q2) - P[0][2]*SH_LOS[0]*SH_LOS[2]*SK_LOS[4] + P[0][0]*SH_LOS[1]*SH_LOS[6]*SK_LOS[4] + P[0][5]*SH_LOS[0]*SH_LOS[5]*SK_LOS[4] - P[0][3]*SH_LOS[0]*SK_LOS[2]*SK_LOS[4]);
|
||||
Kfusion[1] = -SK_LOS[1]*(P[1][1]*(SH_LOS[0]*SH_LOS[4]*SK_LOS[4] + SH_LOS[1]*SH_LOS[5]*SK_LOS[4]) + P[1][8]*SH_LOS[0]*SH_LOS[1]*sq(SK_LOS[4]) - P[1][4]*SH_LOS[0]*SK_LOS[4]*(2*q0*q3 + 2*q1*q2) - P[1][2]*SH_LOS[0]*SH_LOS[2]*SK_LOS[4] + P[1][0]*SH_LOS[1]*SH_LOS[6]*SK_LOS[4] + P[1][5]*SH_LOS[0]*SH_LOS[5]*SK_LOS[4] - P[1][3]*SH_LOS[0]*SK_LOS[2]*SK_LOS[4]);
|
||||
Kfusion[2] = -SK_LOS[1]*(P[2][1]*(SH_LOS[0]*SH_LOS[4]*SK_LOS[4] + SH_LOS[1]*SH_LOS[5]*SK_LOS[4]) + P[2][8]*SH_LOS[0]*SH_LOS[1]*sq(SK_LOS[4]) - P[2][4]*SH_LOS[0]*SK_LOS[4]*(2*q0*q3 + 2*q1*q2) - P[2][2]*SH_LOS[0]*SH_LOS[2]*SK_LOS[4] + P[2][0]*SH_LOS[1]*SH_LOS[6]*SK_LOS[4] + P[2][5]*SH_LOS[0]*SH_LOS[5]*SK_LOS[4] - P[2][3]*SH_LOS[0]*SK_LOS[2]*SK_LOS[4]);
|
||||
Kfusion[3] = -SK_LOS[1]*(P[3][1]*(SH_LOS[0]*SH_LOS[4]*SK_LOS[4] + SH_LOS[1]*SH_LOS[5]*SK_LOS[4]) + P[3][8]*SH_LOS[0]*SH_LOS[1]*sq(SK_LOS[4]) - P[3][4]*SH_LOS[0]*SK_LOS[4]*(2*q0*q3 + 2*q1*q2) - P[3][2]*SH_LOS[0]*SH_LOS[2]*SK_LOS[4] + P[3][0]*SH_LOS[1]*SH_LOS[6]*SK_LOS[4] + P[3][5]*SH_LOS[0]*SH_LOS[5]*SK_LOS[4] - P[3][3]*SH_LOS[0]*SK_LOS[2]*SK_LOS[4]);
|
||||
Kfusion[4] = -SK_LOS[1]*(P[4][1]*(SH_LOS[0]*SH_LOS[4]*SK_LOS[4] + SH_LOS[1]*SH_LOS[5]*SK_LOS[4]) + P[4][8]*SH_LOS[0]*SH_LOS[1]*sq(SK_LOS[4]) - P[4][4]*SH_LOS[0]*SK_LOS[4]*(2*q0*q3 + 2*q1*q2) - P[4][2]*SH_LOS[0]*SH_LOS[2]*SK_LOS[4] + P[4][0]*SH_LOS[1]*SH_LOS[6]*SK_LOS[4] + P[4][5]*SH_LOS[0]*SH_LOS[5]*SK_LOS[4] - P[4][3]*SH_LOS[0]*SK_LOS[2]*SK_LOS[4]);
|
||||
Kfusion[5] = -SK_LOS[1]*(P[5][1]*(SH_LOS[0]*SH_LOS[4]*SK_LOS[4] + SH_LOS[1]*SH_LOS[5]*SK_LOS[4]) + P[5][8]*SH_LOS[0]*SH_LOS[1]*sq(SK_LOS[4]) - P[5][4]*SH_LOS[0]*SK_LOS[4]*(2*q0*q3 + 2*q1*q2) - P[5][2]*SH_LOS[0]*SH_LOS[2]*SK_LOS[4] + P[5][0]*SH_LOS[1]*SH_LOS[6]*SK_LOS[4] + P[5][5]*SH_LOS[0]*SH_LOS[5]*SK_LOS[4] - P[5][3]*SH_LOS[0]*SK_LOS[2]*SK_LOS[4]);
|
||||
Kfusion[6] = -SK_LOS[1]*(P[6][1]*(SH_LOS[0]*SH_LOS[4]*SK_LOS[4] + SH_LOS[1]*SH_LOS[5]*SK_LOS[4]) + P[6][8]*SH_LOS[0]*SH_LOS[1]*sq(SK_LOS[4]) - P[6][4]*SH_LOS[0]*SK_LOS[4]*(2*q0*q3 + 2*q1*q2) - P[6][2]*SH_LOS[0]*SH_LOS[2]*SK_LOS[4] + P[6][0]*SH_LOS[1]*SH_LOS[6]*SK_LOS[4] + P[6][5]*SH_LOS[0]*SH_LOS[5]*SK_LOS[4] - P[6][3]*SH_LOS[0]*SK_LOS[2]*SK_LOS[4]);
|
||||
Kfusion[7] = -SK_LOS[1]*(P[7][1]*(SH_LOS[0]*SH_LOS[4]*SK_LOS[4] + SH_LOS[1]*SH_LOS[5]*SK_LOS[4]) + P[7][8]*SH_LOS[0]*SH_LOS[1]*sq(SK_LOS[4]) - P[7][4]*SH_LOS[0]*SK_LOS[4]*(2*q0*q3 + 2*q1*q2) - P[7][2]*SH_LOS[0]*SH_LOS[2]*SK_LOS[4] + P[7][0]*SH_LOS[1]*SH_LOS[6]*SK_LOS[4] + P[7][5]*SH_LOS[0]*SH_LOS[5]*SK_LOS[4] - P[7][3]*SH_LOS[0]*SK_LOS[2]*SK_LOS[4]);
|
||||
Kfusion[8] = -SK_LOS[1]*(P[8][1]*(SH_LOS[0]*SH_LOS[4]*SK_LOS[4] + SH_LOS[1]*SH_LOS[5]*SK_LOS[4]) + P[8][8]*SH_LOS[0]*SH_LOS[1]*sq(SK_LOS[4]) - P[8][4]*SH_LOS[0]*SK_LOS[4]*(2*q0*q3 + 2*q1*q2) - P[8][2]*SH_LOS[0]*SH_LOS[2]*SK_LOS[4] + P[8][0]*SH_LOS[1]*SH_LOS[6]*SK_LOS[4] + P[8][5]*SH_LOS[0]*SH_LOS[5]*SK_LOS[4] - P[8][3]*SH_LOS[0]*SK_LOS[2]*SK_LOS[4]);
|
||||
Kfusion[9] = -SK_LOS[1]*(P[9][1]*(SH_LOS[0]*SH_LOS[4]*SK_LOS[4] + SH_LOS[1]*SH_LOS[5]*SK_LOS[4]) + P[9][8]*SH_LOS[0]*SH_LOS[1]*sq(SK_LOS[4]) - P[9][4]*SH_LOS[0]*SK_LOS[4]*(2*q0*q3 + 2*q1*q2) - P[9][2]*SH_LOS[0]*SH_LOS[2]*SK_LOS[4] + P[9][0]*SH_LOS[1]*SH_LOS[6]*SK_LOS[4] + P[9][5]*SH_LOS[0]*SH_LOS[5]*SK_LOS[4] - P[9][3]*SH_LOS[0]*SK_LOS[2]*SK_LOS[4]);
|
||||
Kfusion[10] = -SK_LOS[1]*(P[10][1]*(SH_LOS[0]*SH_LOS[4]*SK_LOS[4] + SH_LOS[1]*SH_LOS[5]*SK_LOS[4]) + P[10][8]*SH_LOS[0]*SH_LOS[1]*sq(SK_LOS[4]) - P[10][4]*SH_LOS[0]*SK_LOS[4]*(2*q0*q3 + 2*q1*q2) - P[10][2]*SH_LOS[0]*SH_LOS[2]*SK_LOS[4] + P[10][0]*SH_LOS[1]*SH_LOS[6]*SK_LOS[4] + P[10][5]*SH_LOS[0]*SH_LOS[5]*SK_LOS[4] - P[10][3]*SH_LOS[0]*SK_LOS[2]*SK_LOS[4]);
|
||||
Kfusion[11] = -SK_LOS[1]*(P[11][1]*(SH_LOS[0]*SH_LOS[4]*SK_LOS[4] + SH_LOS[1]*SH_LOS[5]*SK_LOS[4]) + P[11][8]*SH_LOS[0]*SH_LOS[1]*sq(SK_LOS[4]) - P[11][4]*SH_LOS[0]*SK_LOS[4]*(2*q0*q3 + 2*q1*q2) - P[11][2]*SH_LOS[0]*SH_LOS[2]*SK_LOS[4] + P[11][0]*SH_LOS[1]*SH_LOS[6]*SK_LOS[4] + P[11][5]*SH_LOS[0]*SH_LOS[5]*SK_LOS[4] - P[11][3]*SH_LOS[0]*SK_LOS[2]*SK_LOS[4]);
|
||||
Kfusion[12] = -SK_LOS[1]*(P[12][1]*(SH_LOS[0]*SH_LOS[4]*SK_LOS[4] + SH_LOS[1]*SH_LOS[5]*SK_LOS[4]) + P[12][8]*SH_LOS[0]*SH_LOS[1]*sq(SK_LOS[4]) - P[12][4]*SH_LOS[0]*SK_LOS[4]*(2*q0*q3 + 2*q1*q2) - P[12][2]*SH_LOS[0]*SH_LOS[2]*SK_LOS[4] + P[12][0]*SH_LOS[1]*SH_LOS[6]*SK_LOS[4] + P[12][5]*SH_LOS[0]*SH_LOS[5]*SK_LOS[4] - P[12][3]*SH_LOS[0]*SK_LOS[2]*SK_LOS[4]);
|
||||
Kfusion[13] = -SK_LOS[1]*(P[13][1]*(SH_LOS[0]*SH_LOS[4]*SK_LOS[4] + SH_LOS[1]*SH_LOS[5]*SK_LOS[4]) + P[13][8]*SH_LOS[0]*SH_LOS[1]*sq(SK_LOS[4]) - P[13][4]*SH_LOS[0]*SK_LOS[4]*(2*q0*q3 + 2*q1*q2) - P[13][2]*SH_LOS[0]*SH_LOS[2]*SK_LOS[4] + P[13][0]*SH_LOS[1]*SH_LOS[6]*SK_LOS[4] + P[13][5]*SH_LOS[0]*SH_LOS[5]*SK_LOS[4] - P[13][3]*SH_LOS[0]*SK_LOS[2]*SK_LOS[4]);
|
||||
Kfusion[14] = -SK_LOS[1]*(P[14][1]*(SH_LOS[0]*SH_LOS[4]*SK_LOS[4] + SH_LOS[1]*SH_LOS[5]*SK_LOS[4]) + P[14][8]*SH_LOS[0]*SH_LOS[1]*sq(SK_LOS[4]) - P[14][4]*SH_LOS[0]*SK_LOS[4]*(2*q0*q3 + 2*q1*q2) - P[14][2]*SH_LOS[0]*SH_LOS[2]*SK_LOS[4] + P[14][0]*SH_LOS[1]*SH_LOS[6]*SK_LOS[4] + P[14][5]*SH_LOS[0]*SH_LOS[5]*SK_LOS[4] - P[14][3]*SH_LOS[0]*SK_LOS[2]*SK_LOS[4]);
|
||||
Kfusion[15] = -SK_LOS[1]*(P[15][1]*(SH_LOS[0]*SH_LOS[4]*SK_LOS[4] + SH_LOS[1]*SH_LOS[5]*SK_LOS[4]) + P[15][8]*SH_LOS[0]*SH_LOS[1]*sq(SK_LOS[4]) - P[15][4]*SH_LOS[0]*SK_LOS[4]*(2*q0*q3 + 2*q1*q2) - P[15][2]*SH_LOS[0]*SH_LOS[2]*SK_LOS[4] + P[15][0]*SH_LOS[1]*SH_LOS[6]*SK_LOS[4] + P[15][5]*SH_LOS[0]*SH_LOS[5]*SK_LOS[4] - P[15][3]*SH_LOS[0]*SK_LOS[2]*SK_LOS[4]);
|
||||
Kfusion[16] = -SK_LOS[1]*(P[16][1]*(SH_LOS[0]*SH_LOS[4]*SK_LOS[4] + SH_LOS[1]*SH_LOS[5]*SK_LOS[4]) + P[16][8]*SH_LOS[0]*SH_LOS[1]*sq(SK_LOS[4]) - P[16][4]*SH_LOS[0]*SK_LOS[4]*(2*q0*q3 + 2*q1*q2) - P[16][2]*SH_LOS[0]*SH_LOS[2]*SK_LOS[4] + P[16][0]*SH_LOS[1]*SH_LOS[6]*SK_LOS[4] + P[16][5]*SH_LOS[0]*SH_LOS[5]*SK_LOS[4] - P[16][3]*SH_LOS[0]*SK_LOS[2]*SK_LOS[4]);
|
||||
Kfusion[17] = -SK_LOS[1]*(P[17][1]*(SH_LOS[0]*SH_LOS[4]*SK_LOS[4] + SH_LOS[1]*SH_LOS[5]*SK_LOS[4]) + P[17][8]*SH_LOS[0]*SH_LOS[1]*sq(SK_LOS[4]) - P[17][4]*SH_LOS[0]*SK_LOS[4]*(2*q0*q3 + 2*q1*q2) - P[17][2]*SH_LOS[0]*SH_LOS[2]*SK_LOS[4] + P[17][0]*SH_LOS[1]*SH_LOS[6]*SK_LOS[4] + P[17][5]*SH_LOS[0]*SH_LOS[5]*SK_LOS[4] - P[17][3]*SH_LOS[0]*SK_LOS[2]*SK_LOS[4]);
|
||||
Kfusion[18] = -SK_LOS[1]*(P[18][1]*(SH_LOS[0]*SH_LOS[4]*SK_LOS[4] + SH_LOS[1]*SH_LOS[5]*SK_LOS[4]) + P[18][8]*SH_LOS[0]*SH_LOS[1]*sq(SK_LOS[4]) - P[18][4]*SH_LOS[0]*SK_LOS[4]*(2*q0*q3 + 2*q1*q2) - P[18][2]*SH_LOS[0]*SH_LOS[2]*SK_LOS[4] + P[18][0]*SH_LOS[1]*SH_LOS[6]*SK_LOS[4] + P[18][5]*SH_LOS[0]*SH_LOS[5]*SK_LOS[4] - P[18][3]*SH_LOS[0]*SK_LOS[2]*SK_LOS[4]);
|
||||
Kfusion[19] = -SK_LOS[1]*(P[19][1]*(SH_LOS[0]*SH_LOS[4]*SK_LOS[4] + SH_LOS[1]*SH_LOS[5]*SK_LOS[4]) + P[19][8]*SH_LOS[0]*SH_LOS[1]*sq(SK_LOS[4]) - P[19][4]*SH_LOS[0]*SK_LOS[4]*(2*q0*q3 + 2*q1*q2) - P[19][2]*SH_LOS[0]*SH_LOS[2]*SK_LOS[4] + P[19][0]*SH_LOS[1]*SH_LOS[6]*SK_LOS[4] + P[19][5]*SH_LOS[0]*SH_LOS[5]*SK_LOS[4] - P[19][3]*SH_LOS[0]*SK_LOS[2]*SK_LOS[4]);
|
||||
Kfusion[20] = -SK_LOS[1]*(P[20][1]*(SH_LOS[0]*SH_LOS[4]*SK_LOS[4] + SH_LOS[1]*SH_LOS[5]*SK_LOS[4]) + P[20][8]*SH_LOS[0]*SH_LOS[1]*sq(SK_LOS[4]) - P[20][4]*SH_LOS[0]*SK_LOS[4]*(2*q0*q3 + 2*q1*q2) - P[20][2]*SH_LOS[0]*SH_LOS[2]*SK_LOS[4] + P[20][0]*SH_LOS[1]*SH_LOS[6]*SK_LOS[4] + P[20][5]*SH_LOS[0]*SH_LOS[5]*SK_LOS[4] - P[20][3]*SH_LOS[0]*SK_LOS[2]*SK_LOS[4]);
|
||||
Kfusion[21] = -SK_LOS[1]*(P[21][1]*(SH_LOS[0]*SH_LOS[4]*SK_LOS[4] + SH_LOS[1]*SH_LOS[5]*SK_LOS[4]) + P[21][8]*SH_LOS[0]*SH_LOS[1]*sq(SK_LOS[4]) - P[21][4]*SH_LOS[0]*SK_LOS[4]*(2*q0*q3 + 2*q1*q2) - P[21][2]*SH_LOS[0]*SH_LOS[2]*SK_LOS[4] + P[21][0]*SH_LOS[1]*SH_LOS[6]*SK_LOS[4] + P[21][5]*SH_LOS[0]*SH_LOS[5]*SK_LOS[4] - P[21][3]*SH_LOS[0]*SK_LOS[2]*SK_LOS[4]);
|
||||
Kfusion[22] = -SK_LOS[1]*(P[22][1]*(SH_LOS[0]*SH_LOS[4]*SK_LOS[4] + SH_LOS[1]*SH_LOS[5]*SK_LOS[4]) + P[22][8]*SH_LOS[0]*SH_LOS[1]*sq(SK_LOS[4]) - P[22][4]*SH_LOS[0]*SK_LOS[4]*(2*q0*q3 + 2*q1*q2) - P[22][2]*SH_LOS[0]*SH_LOS[2]*SK_LOS[4] + P[22][0]*SH_LOS[1]*SH_LOS[6]*SK_LOS[4] + P[22][5]*SH_LOS[0]*SH_LOS[5]*SK_LOS[4] - P[22][3]*SH_LOS[0]*SK_LOS[2]*SK_LOS[4]);
|
||||
Kfusion[23] = -SK_LOS[1]*(P[23][1]*(SH_LOS[0]*SH_LOS[4]*SK_LOS[4] + SH_LOS[1]*SH_LOS[5]*SK_LOS[4]) + P[23][8]*SH_LOS[0]*SH_LOS[1]*sq(SK_LOS[4]) - P[23][4]*SH_LOS[0]*SK_LOS[4]*(2*q0*q3 + 2*q1*q2) - P[23][2]*SH_LOS[0]*SH_LOS[2]*SK_LOS[4] + P[23][0]*SH_LOS[1]*SH_LOS[6]*SK_LOS[4] + P[23][5]*SH_LOS[0]*SH_LOS[5]*SK_LOS[4] - P[23][3]*SH_LOS[0]*SK_LOS[2]*SK_LOS[4]);
|
||||
|
|
@ -0,0 +1,63 @@
|
|||
// Auto code for fusion of symthetic sideslip
|
||||
|
||||
// Calculate the observation jacobian
|
||||
|
||||
// intermediate variable from algebraic optimisation
|
||||
float SH_BETA[10];
|
||||
SH_BETA[0] = (vn - vwn)*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) - vd*(2*q0*q2 - 2*q1*q3) + (ve - vwe)*(2*q0*q3 + 2*q1*q2);
|
||||
SH_BETA[1] = (ve - vwe)*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + vd*(2*q0*q1 + 2*q2*q3) - (vn - vwn)*(2*q0*q3 - 2*q1*q2);
|
||||
SH_BETA[2] = vd*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) - (ve - vwe)*(2*q0*q1 - 2*q2*q3) + (vn - vwn)*(2*q0*q2 + 2*q1*q3);
|
||||
SH_BETA[3] = 1/sq(SH_BETA[0]);
|
||||
SH_BETA[4] = (sq(q0) - sq(q1) + sq(q2) - sq(q3))/SH_BETA[0];
|
||||
SH_BETA[5] = sq(q0) + sq(q1) - sq(q2) - sq(q3);
|
||||
SH_BETA[6] = 1/SH_BETA[0];
|
||||
SH_BETA[7] = 2*q0*q3;
|
||||
SH_BETA[8] = SH_BETA[7] + 2*q1*q2;
|
||||
SH_BETA[9] = SH_BETA[7] - 2*q1*q2;
|
||||
|
||||
float H_BETA[24];
|
||||
H_BETA[0] = SH_BETA[2]*SH_BETA[6];
|
||||
H_BETA[1] = SH_BETA[1]*SH_BETA[2]*SH_BETA[3];
|
||||
H_BETA[2] = - sq(SH_BETA[1])*SH_BETA[3] - 1;
|
||||
H_BETA[3] = - SH_BETA[6]*SH_BETA[9] - SH_BETA[1]*SH_BETA[3]*SH_BETA[5];
|
||||
H_BETA[4] = SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8];
|
||||
H_BETA[5] = SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3);
|
||||
H_BETA[22] = SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5];
|
||||
H_BETA[23] = SH_BETA[1]*SH_BETA[3]*SH_BETA[8] - SH_BETA[4];
|
||||
|
||||
// calculate the Kalman gain matrix
|
||||
|
||||
// intermediate variables - note SK_BETA[0] is 1/(innovation variance)
|
||||
float SK_BETA[6];
|
||||
SK_BETA[0] = 1/(R_BETA + (SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8])*(P[22][4]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][4]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][4]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][4]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][4]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][4]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][4]*SH_BETA[2]*SH_BETA[6] + P[1][4]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) - (SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8])*(P[22][23]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][23]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][23]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][23]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][23]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][23]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][23]*SH_BETA[2]*SH_BETA[6] + P[1][23]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) - (SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5])*(P[22][3]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][3]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][3]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][3]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][3]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][3]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][3]*SH_BETA[2]*SH_BETA[6] + P[1][3]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) + (SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5])*(P[22][22]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][22]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][22]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][22]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][22]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][22]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][22]*SH_BETA[2]*SH_BETA[6] + P[1][22]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) - (sq(SH_BETA[1])*SH_BETA[3] + 1)*(P[22][2]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][2]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][2]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][2]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][2]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][2]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][2]*SH_BETA[2]*SH_BETA[6] + P[1][2]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) + (SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3))*(P[22][5]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][5]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][5]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][5]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][5]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][5]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][5]*SH_BETA[2]*SH_BETA[6] + P[1][5]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) + SH_BETA[2]*SH_BETA[6]*(P[22][0]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][0]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][0]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][0]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][0]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][0]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][0]*SH_BETA[2]*SH_BETA[6] + P[1][0]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) + SH_BETA[1]*SH_BETA[2]*SH_BETA[3]*(P[22][1]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][1]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][1]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][1]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][1]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][1]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][1]*SH_BETA[2]*SH_BETA[6] + P[1][1]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]));
|
||||
SK_BETA[1] = SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3);
|
||||
SK_BETA[2] = SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5];
|
||||
SK_BETA[3] = SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8];
|
||||
SK_BETA[4] = sq(SH_BETA[1])*SH_BETA[3] + 1;
|
||||
SK_BETA[5] = SH_BETA[2];
|
||||
|
||||
float Kfusion[24];
|
||||
Kfusion[0] = SK_BETA[0]*(P[0][5]*SK_BETA[1] - P[0][2]*SK_BETA[4] - P[0][3]*SK_BETA[2] + P[0][4]*SK_BETA[3] + P[0][22]*SK_BETA[2] - P[0][23]*SK_BETA[3] + P[0][0]*SH_BETA[6]*SK_BETA[5] + P[0][1]*SH_BETA[1]*SH_BETA[3]*SK_BETA[5]);
|
||||
Kfusion[1] = SK_BETA[0]*(P[1][5]*SK_BETA[1] - P[1][2]*SK_BETA[4] - P[1][3]*SK_BETA[2] + P[1][4]*SK_BETA[3] + P[1][22]*SK_BETA[2] - P[1][23]*SK_BETA[3] + P[1][0]*SH_BETA[6]*SK_BETA[5] + P[1][1]*SH_BETA[1]*SH_BETA[3]*SK_BETA[5]);
|
||||
Kfusion[2] = SK_BETA[0]*(P[2][5]*SK_BETA[1] - P[2][2]*SK_BETA[4] - P[2][3]*SK_BETA[2] + P[2][4]*SK_BETA[3] + P[2][22]*SK_BETA[2] - P[2][23]*SK_BETA[3] + P[2][0]*SH_BETA[6]*SK_BETA[5] + P[2][1]*SH_BETA[1]*SH_BETA[3]*SK_BETA[5]);
|
||||
Kfusion[3] = SK_BETA[0]*(P[3][5]*SK_BETA[1] - P[3][2]*SK_BETA[4] - P[3][3]*SK_BETA[2] + P[3][4]*SK_BETA[3] + P[3][22]*SK_BETA[2] - P[3][23]*SK_BETA[3] + P[3][0]*SH_BETA[6]*SK_BETA[5] + P[3][1]*SH_BETA[1]*SH_BETA[3]*SK_BETA[5]);
|
||||
Kfusion[4] = SK_BETA[0]*(P[4][5]*SK_BETA[1] - P[4][2]*SK_BETA[4] - P[4][3]*SK_BETA[2] + P[4][4]*SK_BETA[3] + P[4][22]*SK_BETA[2] - P[4][23]*SK_BETA[3] + P[4][0]*SH_BETA[6]*SK_BETA[5] + P[4][1]*SH_BETA[1]*SH_BETA[3]*SK_BETA[5]);
|
||||
Kfusion[5] = SK_BETA[0]*(P[5][5]*SK_BETA[1] - P[5][2]*SK_BETA[4] - P[5][3]*SK_BETA[2] + P[5][4]*SK_BETA[3] + P[5][22]*SK_BETA[2] - P[5][23]*SK_BETA[3] + P[5][0]*SH_BETA[6]*SK_BETA[5] + P[5][1]*SH_BETA[1]*SH_BETA[3]*SK_BETA[5]);
|
||||
Kfusion[6] = SK_BETA[0]*(P[6][5]*SK_BETA[1] - P[6][2]*SK_BETA[4] - P[6][3]*SK_BETA[2] + P[6][4]*SK_BETA[3] + P[6][22]*SK_BETA[2] - P[6][23]*SK_BETA[3] + P[6][0]*SH_BETA[6]*SK_BETA[5] + P[6][1]*SH_BETA[1]*SH_BETA[3]*SK_BETA[5]);
|
||||
Kfusion[7] = SK_BETA[0]*(P[7][5]*SK_BETA[1] - P[7][2]*SK_BETA[4] - P[7][3]*SK_BETA[2] + P[7][4]*SK_BETA[3] + P[7][22]*SK_BETA[2] - P[7][23]*SK_BETA[3] + P[7][0]*SH_BETA[6]*SK_BETA[5] + P[7][1]*SH_BETA[1]*SH_BETA[3]*SK_BETA[5]);
|
||||
Kfusion[8] = SK_BETA[0]*(P[8][5]*SK_BETA[1] - P[8][2]*SK_BETA[4] - P[8][3]*SK_BETA[2] + P[8][4]*SK_BETA[3] + P[8][22]*SK_BETA[2] - P[8][23]*SK_BETA[3] + P[8][0]*SH_BETA[6]*SK_BETA[5] + P[8][1]*SH_BETA[1]*SH_BETA[3]*SK_BETA[5]);
|
||||
Kfusion[9] = SK_BETA[0]*(P[9][5]*SK_BETA[1] - P[9][2]*SK_BETA[4] - P[9][3]*SK_BETA[2] + P[9][4]*SK_BETA[3] + P[9][22]*SK_BETA[2] - P[9][23]*SK_BETA[3] + P[9][0]*SH_BETA[6]*SK_BETA[5] + P[9][1]*SH_BETA[1]*SH_BETA[3]*SK_BETA[5]);
|
||||
Kfusion[10] = SK_BETA[0]*(P[10][5]*SK_BETA[1] - P[10][2]*SK_BETA[4] - P[10][3]*SK_BETA[2] + P[10][4]*SK_BETA[3] + P[10][22]*SK_BETA[2] - P[10][23]*SK_BETA[3] + P[10][0]*SH_BETA[6]*SK_BETA[5] + P[10][1]*SH_BETA[1]*SH_BETA[3]*SK_BETA[5]);
|
||||
Kfusion[11] = SK_BETA[0]*(P[11][5]*SK_BETA[1] - P[11][2]*SK_BETA[4] - P[11][3]*SK_BETA[2] + P[11][4]*SK_BETA[3] + P[11][22]*SK_BETA[2] - P[11][23]*SK_BETA[3] + P[11][0]*SH_BETA[6]*SK_BETA[5] + P[11][1]*SH_BETA[1]*SH_BETA[3]*SK_BETA[5]);
|
||||
Kfusion[12] = SK_BETA[0]*(P[12][5]*SK_BETA[1] - P[12][2]*SK_BETA[4] - P[12][3]*SK_BETA[2] + P[12][4]*SK_BETA[3] + P[12][22]*SK_BETA[2] - P[12][23]*SK_BETA[3] + P[12][0]*SH_BETA[6]*SK_BETA[5] + P[12][1]*SH_BETA[1]*SH_BETA[3]*SK_BETA[5]);
|
||||
Kfusion[13] = SK_BETA[0]*(P[13][5]*SK_BETA[1] - P[13][2]*SK_BETA[4] - P[13][3]*SK_BETA[2] + P[13][4]*SK_BETA[3] + P[13][22]*SK_BETA[2] - P[13][23]*SK_BETA[3] + P[13][0]*SH_BETA[6]*SK_BETA[5] + P[13][1]*SH_BETA[1]*SH_BETA[3]*SK_BETA[5]);
|
||||
Kfusion[14] = SK_BETA[0]*(P[14][5]*SK_BETA[1] - P[14][2]*SK_BETA[4] - P[14][3]*SK_BETA[2] + P[14][4]*SK_BETA[3] + P[14][22]*SK_BETA[2] - P[14][23]*SK_BETA[3] + P[14][0]*SH_BETA[6]*SK_BETA[5] + P[14][1]*SH_BETA[1]*SH_BETA[3]*SK_BETA[5]);
|
||||
Kfusion[15] = SK_BETA[0]*(P[15][5]*SK_BETA[1] - P[15][2]*SK_BETA[4] - P[15][3]*SK_BETA[2] + P[15][4]*SK_BETA[3] + P[15][22]*SK_BETA[2] - P[15][23]*SK_BETA[3] + P[15][0]*SH_BETA[6]*SK_BETA[5] + P[15][1]*SH_BETA[1]*SH_BETA[3]*SK_BETA[5]);
|
||||
Kfusion[16] = SK_BETA[0]*(P[16][5]*SK_BETA[1] - P[16][2]*SK_BETA[4] - P[16][3]*SK_BETA[2] + P[16][4]*SK_BETA[3] + P[16][22]*SK_BETA[2] - P[16][23]*SK_BETA[3] + P[16][0]*SH_BETA[6]*SK_BETA[5] + P[16][1]*SH_BETA[1]*SH_BETA[3]*SK_BETA[5]);
|
||||
Kfusion[17] = SK_BETA[0]*(P[17][5]*SK_BETA[1] - P[17][2]*SK_BETA[4] - P[17][3]*SK_BETA[2] + P[17][4]*SK_BETA[3] + P[17][22]*SK_BETA[2] - P[17][23]*SK_BETA[3] + P[17][0]*SH_BETA[6]*SK_BETA[5] + P[17][1]*SH_BETA[1]*SH_BETA[3]*SK_BETA[5]);
|
||||
Kfusion[18] = SK_BETA[0]*(P[18][5]*SK_BETA[1] - P[18][2]*SK_BETA[4] - P[18][3]*SK_BETA[2] + P[18][4]*SK_BETA[3] + P[18][22]*SK_BETA[2] - P[18][23]*SK_BETA[3] + P[18][0]*SH_BETA[6]*SK_BETA[5] + P[18][1]*SH_BETA[1]*SH_BETA[3]*SK_BETA[5]);
|
||||
Kfusion[19] = SK_BETA[0]*(P[19][5]*SK_BETA[1] - P[19][2]*SK_BETA[4] - P[19][3]*SK_BETA[2] + P[19][4]*SK_BETA[3] + P[19][22]*SK_BETA[2] - P[19][23]*SK_BETA[3] + P[19][0]*SH_BETA[6]*SK_BETA[5] + P[19][1]*SH_BETA[1]*SH_BETA[3]*SK_BETA[5]);
|
||||
Kfusion[20] = SK_BETA[0]*(P[20][5]*SK_BETA[1] - P[20][2]*SK_BETA[4] - P[20][3]*SK_BETA[2] + P[20][4]*SK_BETA[3] + P[20][22]*SK_BETA[2] - P[20][23]*SK_BETA[3] + P[20][0]*SH_BETA[6]*SK_BETA[5] + P[20][1]*SH_BETA[1]*SH_BETA[3]*SK_BETA[5]);
|
||||
Kfusion[21] = SK_BETA[0]*(P[21][5]*SK_BETA[1] - P[21][2]*SK_BETA[4] - P[21][3]*SK_BETA[2] + P[21][4]*SK_BETA[3] + P[21][22]*SK_BETA[2] - P[21][23]*SK_BETA[3] + P[21][0]*SH_BETA[6]*SK_BETA[5] + P[21][1]*SH_BETA[1]*SH_BETA[3]*SK_BETA[5]);
|
||||
Kfusion[22] = SK_BETA[0]*(P[22][5]*SK_BETA[1] - P[22][2]*SK_BETA[4] - P[22][3]*SK_BETA[2] + P[22][4]*SK_BETA[3] + P[22][22]*SK_BETA[2] - P[22][23]*SK_BETA[3] + P[22][0]*SH_BETA[6]*SK_BETA[5] + P[22][1]*SH_BETA[1]*SH_BETA[3]*SK_BETA[5]);
|
||||
Kfusion[23] = SK_BETA[0]*(P[23][5]*SK_BETA[1] - P[23][2]*SK_BETA[4] - P[23][3]*SK_BETA[2] + P[23][4]*SK_BETA[3] + P[23][22]*SK_BETA[2] - P[23][23]*SK_BETA[3] + P[23][0]*SH_BETA[6]*SK_BETA[5] + P[23][1]*SH_BETA[1]*SH_BETA[3]*SK_BETA[5]);
|
||||
|
|
@ -0,0 +1,47 @@
|
|||
/*
|
||||
Autocode for fusion of a yaw error measurement where the innovation is given by:
|
||||
|
||||
innovation = atan2f(magMeasEarthFrameEast,magMeasEarthFrameNorth) - declinationAngle;
|
||||
|
||||
magMeasEarthFrameEast and magMeasEarthFrameNorth are obtained by rotating the magnetometer measurements from body frame to eath frame.
|
||||
declinationAngle is the estimated declination as that location
|
||||
*/
|
||||
|
||||
// intermediate variables
|
||||
float t2 = q0*q0;
|
||||
float t3 = q1*q1;
|
||||
float t4 = q2*q2;
|
||||
float t5 = q3*q3;
|
||||
float t6 = q0*q2*2.0f;
|
||||
float t7 = q1*q3*2.0f;
|
||||
float t8 = t6+t7;
|
||||
float t9 = q0*q3*2.0f;
|
||||
float t13 = q1*q2*2.0f;
|
||||
float t10 = t9-t13;
|
||||
float t11 = t2+t3-t4-t5;
|
||||
float t12 = magX*t11;
|
||||
float t14 = magZ*t8;
|
||||
float t19 = magY*t10;
|
||||
float t15 = t12+t14-t19;
|
||||
float t16 = t2-t3+t4-t5;
|
||||
float t17 = q0*q1*2.0f;
|
||||
float t24 = q2*q3*2.0f;
|
||||
float t18 = t17-t24;
|
||||
float t20 = 1.0f/t15;
|
||||
float t21 = magY*t16;
|
||||
float t22 = t9+t13;
|
||||
float t23 = magX*t22;
|
||||
float t28 = magZ*t18;
|
||||
float t25 = t21+t23-t28;
|
||||
float t29 = t20*t25;
|
||||
float t26 = tan(t29);
|
||||
float t27 = 1.0f/(t15*t15);
|
||||
float t30 = t26*t26;
|
||||
float t31 = t30+1.0f;
|
||||
|
||||
// Calculate the observation jacobian for the innovation derivative wrt the attitude error states only
|
||||
// Use the reduced order to optimise the calculation of the Kalman gain matrix and covariance update
|
||||
float H_MAG[3];
|
||||
H_MAG[0] = -t31*(t20*(magZ*t16+magY*t18)+t25*t27*(magY*t8+magZ*t10));
|
||||
H_MAG[1] = t31*(t20*(magX*t18+magZ*t22)+t25*t27*(magX*t8-magZ*t11));
|
||||
H_MAG[2] = t31*(t20*(magX*t16-magY*t22)+t25*t27*(magX*t10+magY*t11));
|
||||
Loading…
Reference in New Issue