forked from Archive/PX4-Autopilot
Removing Matlab derivation
This commit is contained in:
Kamil Ritz
Paul Riseborough
parent
18f334f4db
commit
78ce46f007
|
|
@ -1,7 +1,7 @@
|
|||
The EKF uses a range of techniques acquired from several years of experience including an original method to handle the delayed time horizon problem.
|
||||
A list of references I have found useful has been included.
|
||||
|
||||
- The Matlab script used to derive the autocoded expressions in the EKF can be found here: https://github.com/PX4/ecl/blob/master/EKF/matlab/scripts/Inertial%20Nav%20EKF/GenerateNavFilterEquations.m
|
||||
- The python script used to derive the autogenerated expressions in the EKF can be found here: https://github.com/PX4/ecl/blob/master/EKF/python/ekf_derivation/main.py
|
||||
|
||||
- A working Matlab model of the filter capable of replaying flight data can be found here: https://github.com/PX4/ecl/tree/master/EKF/matlab/EKF_replay
|
||||
|
||||
|
|
|
|||
|
|
@ -1,50 +0,0 @@
|
|||
// Auto code for fusion of true airspeed
|
||||
|
||||
// Calculate the observation jacobian
|
||||
|
||||
// intermediate variable from algebraic optimisation
|
||||
float SH_TAS[3];
|
||||
SH_TAS[0] = 1.0f/v_tas_pred;
|
||||
SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2.0f*vwe))*0.5f;
|
||||
SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2.0f*vwn))*0.5f;
|
||||
|
||||
// observation jacobian
|
||||
float H_TAS[24];
|
||||
H_TAS[4] = SH_TAS[2];
|
||||
H_TAS[5] = SH_TAS[1];
|
||||
H_TAS[6] = vd*SH_TAS[0];
|
||||
H_TAS[22] = -SH_TAS[2];
|
||||
H_TAS[23] = -SH_TAS[1];
|
||||
|
||||
// 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[4][4]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[22][4]*SH_TAS[2] - P[23][4]*SH_TAS[1] + P[6][4]*vd*SH_TAS[0]) + SH_TAS[1]*(P[4][5]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[22][5]*SH_TAS[2] - P[23][5]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]) - SH_TAS[2]*(P[4][22]*SH_TAS[2] + P[5][22]*SH_TAS[1] - P[22][22]*SH_TAS[2] - P[23][22]*SH_TAS[1] + P[6][22]*vd*SH_TAS[0]) - SH_TAS[1]*(P[4][23]*SH_TAS[2] + P[5][23]*SH_TAS[1] - P[22][23]*SH_TAS[2] - P[23][23]*SH_TAS[1] + P[6][23]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[4][6]*SH_TAS[2] + P[5][6]*SH_TAS[1] - P[22][6]*SH_TAS[2] - P[23][6]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0]));
|
||||
SK_TAS[1] = SH_TAS[1];
|
||||
|
||||
float Kfusion[24];
|
||||
Kfusion[0] = SK_TAS[0]*(P[0][4]*SH_TAS[2] - P[0][22]*SH_TAS[2] + P[0][5]*SK_TAS[1] - P[0][23]*SK_TAS[1] + P[0][6]*vd*SH_TAS[0]);
|
||||
Kfusion[1] = SK_TAS[0]*(P[1][4]*SH_TAS[2] - P[1][22]*SH_TAS[2] + P[1][5]*SK_TAS[1] - P[1][23]*SK_TAS[1] + P[1][6]*vd*SH_TAS[0]);
|
||||
Kfusion[2] = SK_TAS[0]*(P[2][4]*SH_TAS[2] - P[2][22]*SH_TAS[2] + P[2][5]*SK_TAS[1] - P[2][23]*SK_TAS[1] + P[2][6]*vd*SH_TAS[0]);
|
||||
Kfusion[3] = SK_TAS[0]*(P[3][4]*SH_TAS[2] - P[3][22]*SH_TAS[2] + P[3][5]*SK_TAS[1] - P[3][23]*SK_TAS[1] + P[3][6]*vd*SH_TAS[0]);
|
||||
Kfusion[4] = SK_TAS[0]*(P[4][4]*SH_TAS[2] - P[4][22]*SH_TAS[2] + P[4][5]*SK_TAS[1] - P[4][23]*SK_TAS[1] + P[4][6]*vd*SH_TAS[0]);
|
||||
Kfusion[5] = SK_TAS[0]*(P[5][4]*SH_TAS[2] - P[5][22]*SH_TAS[2] + P[5][5]*SK_TAS[1] - P[5][23]*SK_TAS[1] + P[5][6]*vd*SH_TAS[0]);
|
||||
Kfusion[6] = SK_TAS[0]*(P[6][4]*SH_TAS[2] - P[6][22]*SH_TAS[2] + P[6][5]*SK_TAS[1] - P[6][23]*SK_TAS[1] + P[6][6]*vd*SH_TAS[0]);
|
||||
Kfusion[7] = SK_TAS[0]*(P[7][4]*SH_TAS[2] - P[7][22]*SH_TAS[2] + P[7][5]*SK_TAS[1] - P[7][23]*SK_TAS[1] + P[7][6]*vd*SH_TAS[0]);
|
||||
Kfusion[8] = SK_TAS[0]*(P[8][4]*SH_TAS[2] - P[8][22]*SH_TAS[2] + P[8][5]*SK_TAS[1] - P[8][23]*SK_TAS[1] + P[8][6]*vd*SH_TAS[0]);
|
||||
Kfusion[9] = SK_TAS[0]*(P[9][4]*SH_TAS[2] - P[9][22]*SH_TAS[2] + P[9][5]*SK_TAS[1] - P[9][23]*SK_TAS[1] + P[9][6]*vd*SH_TAS[0]);
|
||||
Kfusion[10] = SK_TAS[0]*(P[10][4]*SH_TAS[2] - P[10][22]*SH_TAS[2] + P[10][5]*SK_TAS[1] - P[10][23]*SK_TAS[1] + P[10][6]*vd*SH_TAS[0]);
|
||||
Kfusion[11] = SK_TAS[0]*(P[11][4]*SH_TAS[2] - P[11][22]*SH_TAS[2] + P[11][5]*SK_TAS[1] - P[11][23]*SK_TAS[1] + P[11][6]*vd*SH_TAS[0]);
|
||||
Kfusion[12] = SK_TAS[0]*(P[12][4]*SH_TAS[2] - P[12][22]*SH_TAS[2] + P[12][5]*SK_TAS[1] - P[12][23]*SK_TAS[1] + P[12][6]*vd*SH_TAS[0]);
|
||||
Kfusion[13] = SK_TAS[0]*(P[13][4]*SH_TAS[2] - P[13][22]*SH_TAS[2] + P[13][5]*SK_TAS[1] - P[13][23]*SK_TAS[1] + P[13][6]*vd*SH_TAS[0]);
|
||||
Kfusion[14] = SK_TAS[0]*(P[14][4]*SH_TAS[2] - P[14][22]*SH_TAS[2] + P[14][5]*SK_TAS[1] - P[14][23]*SK_TAS[1] + P[14][6]*vd*SH_TAS[0]);
|
||||
Kfusion[15] = SK_TAS[0]*(P[15][4]*SH_TAS[2] - P[15][22]*SH_TAS[2] + P[15][5]*SK_TAS[1] - P[15][23]*SK_TAS[1] + P[15][6]*vd*SH_TAS[0]);
|
||||
Kfusion[16] = SK_TAS[0]*(P[16][4]*SH_TAS[2] - P[16][22]*SH_TAS[2] + P[16][5]*SK_TAS[1] - P[16][23]*SK_TAS[1] + P[16][6]*vd*SH_TAS[0]);
|
||||
Kfusion[17] = SK_TAS[0]*(P[17][4]*SH_TAS[2] - P[17][22]*SH_TAS[2] + P[17][5]*SK_TAS[1] - P[17][23]*SK_TAS[1] + P[17][6]*vd*SH_TAS[0]);
|
||||
Kfusion[18] = SK_TAS[0]*(P[18][4]*SH_TAS[2] - P[18][22]*SH_TAS[2] + P[18][5]*SK_TAS[1] - P[18][23]*SK_TAS[1] + P[18][6]*vd*SH_TAS[0]);
|
||||
Kfusion[19] = SK_TAS[0]*(P[19][4]*SH_TAS[2] - P[19][22]*SH_TAS[2] + P[19][5]*SK_TAS[1] - P[19][23]*SK_TAS[1] + P[19][6]*vd*SH_TAS[0]);
|
||||
Kfusion[20] = SK_TAS[0]*(P[20][4]*SH_TAS[2] - P[20][22]*SH_TAS[2] + P[20][5]*SK_TAS[1] - P[20][23]*SK_TAS[1] + P[20][6]*vd*SH_TAS[0]);
|
||||
Kfusion[21] = SK_TAS[0]*(P[21][4]*SH_TAS[2] - P[21][22]*SH_TAS[2] + P[21][5]*SK_TAS[1] - P[21][23]*SK_TAS[1] + P[21][6]*vd*SH_TAS[0]);
|
||||
Kfusion[22] = SK_TAS[0]*(P[22][4]*SH_TAS[2] - P[22][22]*SH_TAS[2] + P[22][5]*SK_TAS[1] - P[22][23]*SK_TAS[1] + P[22][6]*vd*SH_TAS[0]);
|
||||
Kfusion[23] = SK_TAS[0]*(P[23][4]*SH_TAS[2] - P[23][22]*SH_TAS[2] + P[23][5]*SK_TAS[1] - P[23][23]*SK_TAS[1] + P[23][6]*vd*SH_TAS[0]);
|
||||
|
|
@ -1,367 +0,0 @@
|
|||
// Auto code for covariance prediction
|
||||
|
||||
// Intermediate expressions obtained from algebraic optimisation
|
||||
float SF[21] = {};
|
||||
SF[0] = dvz - dvz_b;
|
||||
SF[1] = dvy - dvy_b;
|
||||
SF[2] = dvx - dvx_b;
|
||||
SF[3] = 2.0f*q1*SF[2] + 2.0f*q2*SF[1] + 2.0f*q3*SF[0];
|
||||
SF[4] = 2.0f*q0*SF[1] - 2.0f*q1*SF[0] + 2.0f*q3*SF[2];
|
||||
SF[5] = 2.0f*q0*SF[2] + 2.0f*q2*SF[0] - 2.0f*q3*SF[1];
|
||||
SF[6] = day*0.5f - day_b*0.5f;
|
||||
SF[7] = daz*0.5f - daz_b*0.5f;
|
||||
SF[8] = dax*0.5f - dax_b*0.5f;
|
||||
SF[9] = dax_b*0.5f - dax*0.5f;
|
||||
SF[10] = daz_b*0.5f - daz*0.5f;
|
||||
SF[11] = day_b*0.5f - day*0.5f;
|
||||
SF[12] = 2.0f*q1*SF[1];
|
||||
SF[13] = 2.0f*q0*SF[0];
|
||||
SF[14] = q1*0.5f;
|
||||
SF[15] = q2*0.5f;
|
||||
SF[16] = q3*0.5f;
|
||||
SF[17] = sq(q3);
|
||||
SF[18] = sq(q2);
|
||||
SF[19] = sq(q1);
|
||||
SF[20] = sq(q0);
|
||||
|
||||
float SG[8] = {};
|
||||
SG[0] = q0*0.5f;
|
||||
SG[1] = sq(q3);
|
||||
SG[2] = sq(q2);
|
||||
SG[3] = sq(q1);
|
||||
SG[4] = sq(q0);
|
||||
SG[5] = 2.0f*q2*q3;
|
||||
SG[6] = 2.0f*q1*q3;
|
||||
SG[7] = 2.0f*q1*q2;
|
||||
|
||||
float SQ[11] = {};
|
||||
SQ[0] = dvzVar*(SG[5] - 2.0f*q0*q1)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvyVar*(SG[5] + 2.0f*q0*q1)*(SG[1] - SG[2] + SG[3] - SG[4]) + dvxVar*(SG[6] - 2.0f*q0*q2)*(SG[7] + 2.0f*q0*q3);
|
||||
SQ[1] = dvzVar*(SG[6] + 2.0f*q0*q2)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvxVar*(SG[6] - 2.0f*q0*q2)*(SG[1] + SG[2] - SG[3] - SG[4]) + dvyVar*(SG[5] + 2.0f*q0*q1)*(SG[7] - 2.0f*q0*q3);
|
||||
SQ[2] = dvzVar*(SG[5] - 2.0f*q0*q1)*(SG[6] + 2.0f*q0*q2) - dvyVar*(SG[7] - 2.0f*q0*q3)*(SG[1] - SG[2] + SG[3] - SG[4]) - dvxVar*(SG[7] + 2.0f*q0*q3)*(SG[1] + SG[2] - SG[3] - SG[4]);
|
||||
SQ[3] = (dayVar*q1*SG[0])*0.5f - (dazVar*q1*SG[0])*0.5f - (daxVar*q2*q3)*0.25f;
|
||||
SQ[4] = (dazVar*q2*SG[0])*0.5f - (daxVar*q2*SG[0])*0.5f - (dayVar*q1*q3)*0.25f;
|
||||
SQ[5] = (daxVar*q3*SG[0])*0.5f - (dayVar*q3*SG[0])*0.5f - (dazVar*q1*q2)*0.25f;
|
||||
SQ[6] = (daxVar*q1*q2)*0.25f - (dazVar*q3*SG[0])*0.5f - (dayVar*q1*q2)*0.25f;
|
||||
SQ[7] = (dazVar*q1*q3)*0.25f - (daxVar*q1*q3)*0.25f - (dayVar*q2*SG[0])*0.5f;
|
||||
SQ[8] = (dayVar*q2*q3)*0.25f - (daxVar*q1*SG[0])*0.5f - (dazVar*q2*q3)*0.25f;
|
||||
SQ[9] = sq(SG[0]);
|
||||
SQ[10] = sq(q1);
|
||||
|
||||
float SPP[11] = {};
|
||||
SPP[0] = SF[12] + SF[13] - 2.0f*q2*SF[2];
|
||||
SPP[1] = SF[17] - SF[18] - SF[19] + SF[20];
|
||||
SPP[2] = SF[17] - SF[18] + SF[19] - SF[20];
|
||||
SPP[3] = SF[17] + SF[18] - SF[19] - SF[20];
|
||||
SPP[4] = 2.0f*q0*q2 - 2.0f*q1*q3;
|
||||
SPP[5] = 2.0f*q0*q1 - 2.0f*q2*q3;
|
||||
SPP[6] = 2.0f*q0*q3 - 2.0f*q1*q2;
|
||||
SPP[7] = 2.0f*q0*q1 + 2.0f*q2*q3;
|
||||
SPP[8] = 2.0f*q0*q3 + 2.0f*q1*q2;
|
||||
SPP[9] = 2.0f*q0*q2 + 2.0f*q1*q3;
|
||||
SPP[10] = SF[16];
|
||||
|
||||
// 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] = P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10] + (daxVar*SQ[10])*0.25f + SF[9]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) + SF[11]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SF[10]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) + SF[14]*(P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10]) + SF[15]*(P[0][11] + P[1][11]*SF[9] + P[2][11]*SF[11] + P[3][11]*SF[10] + P[10][11]*SF[14] + P[11][11]*SF[15] + P[12][11]*SPP[10]) + SPP[10]*(P[0][12] + P[1][12]*SF[9] + P[2][12]*SF[11] + P[3][12]*SF[10] + P[10][12]*SF[14] + P[11][12]*SF[15] + P[12][12]*SPP[10]) + (dayVar*sq(q2))*0.25f + (dazVar*sq(q3))*0.25f;
|
||||
nextP[0][1] = P[0][1] + SQ[8] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10] + SF[8]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SF[7]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SF[11]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) - SF[15]*(P[0][12] + P[1][12]*SF[9] + P[2][12]*SF[11] + P[3][12]*SF[10] + P[10][12]*SF[14] + P[11][12]*SF[15] + P[12][12]*SPP[10]) + SPP[10]*(P[0][11] + P[1][11]*SF[9] + P[2][11]*SF[11] + P[3][11]*SF[10] + P[10][11]*SF[14] + P[11][11]*SF[15] + P[12][11]*SPP[10]) - (q0*(P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10]))*0.5f;
|
||||
nextP[1][1] = P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] + daxVar*SQ[9] - (P[10][1]*q0)*0.5f + SF[8]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SF[7]*(P[1][2] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2) + SF[11]*(P[1][3] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2) - SF[15]*(P[1][12] + P[0][12]*SF[8] + P[2][12]*SF[7] + P[3][12]*SF[11] - P[12][12]*SF[15] + P[11][12]*SPP[10] - (P[10][12]*q0)/2) + SPP[10]*(P[1][11] + P[0][11]*SF[8] + P[2][11]*SF[7] + P[3][11]*SF[11] - P[12][11]*SF[15] + P[11][11]*SPP[10] - (P[10][11]*q0)/2) + (dayVar*sq(q3))*0.25f + (dazVar*sq(q2))*0.25f - (q0*(P[1][10] + P[0][10]*SF[8] + P[2][10]*SF[7] + P[3][10]*SF[11] - P[12][10]*SF[15] + P[11][10]*SPP[10] - (P[10][10]*q0)/2))*0.5f;
|
||||
nextP[0][2] = P[0][2] + SQ[7] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10] + SF[6]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SF[10]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) + SF[8]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) + SF[14]*(P[0][12] + P[1][12]*SF[9] + P[2][12]*SF[11] + P[3][12]*SF[10] + P[10][12]*SF[14] + P[11][12]*SF[15] + P[12][12]*SPP[10]) - SPP[10]*(P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10]) - (q0*(P[0][11] + P[1][11]*SF[9] + P[2][11]*SF[11] + P[3][11]*SF[10] + P[10][11]*SF[14] + P[11][11]*SF[15] + P[12][11]*SPP[10]))*0.5f;
|
||||
nextP[1][2] = P[1][2] + SQ[5] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)*0.5f + SF[6]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SF[10]*(P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] - (P[10][1]*q0)/2) + SF[8]*(P[1][3] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2) + SF[14]*(P[1][12] + P[0][12]*SF[8] + P[2][12]*SF[7] + P[3][12]*SF[11] - P[12][12]*SF[15] + P[11][12]*SPP[10] - (P[10][12]*q0)/2) - SPP[10]*(P[1][10] + P[0][10]*SF[8] + P[2][10]*SF[7] + P[3][10]*SF[11] - P[12][10]*SF[15] + P[11][10]*SPP[10] - (P[10][10]*q0)/2) - (q0*(P[1][11] + P[0][11]*SF[8] + P[2][11]*SF[7] + P[3][11]*SF[11] - P[12][11]*SF[15] + P[11][11]*SPP[10] - (P[10][11]*q0)/2))*0.5f;
|
||||
nextP[2][2] = P[2][2] + P[0][2]*SF[6] + P[1][2]*SF[10] + P[3][2]*SF[8] + P[12][2]*SF[14] - P[10][2]*SPP[10] + dayVar*SQ[9] + (dazVar*SQ[10])*0.25f - (P[11][2]*q0)*0.5f + SF[6]*(P[2][0] + P[0][0]*SF[6] + P[1][0]*SF[10] + P[3][0]*SF[8] + P[12][0]*SF[14] - P[10][0]*SPP[10] - (P[11][0]*q0)/2) + SF[10]*(P[2][1] + P[0][1]*SF[6] + P[1][1]*SF[10] + P[3][1]*SF[8] + P[12][1]*SF[14] - P[10][1]*SPP[10] - (P[11][1]*q0)/2) + SF[8]*(P[2][3] + P[0][3]*SF[6] + P[1][3]*SF[10] + P[3][3]*SF[8] + P[12][3]*SF[14] - P[10][3]*SPP[10] - (P[11][3]*q0)/2) + SF[14]*(P[2][12] + P[0][12]*SF[6] + P[1][12]*SF[10] + P[3][12]*SF[8] + P[12][12]*SF[14] - P[10][12]*SPP[10] - (P[11][12]*q0)/2) - SPP[10]*(P[2][10] + P[0][10]*SF[6] + P[1][10]*SF[10] + P[3][10]*SF[8] + P[12][10]*SF[14] - P[10][10]*SPP[10] - (P[11][10]*q0)/2) + (daxVar*sq(q3))*0.25f - (q0*(P[2][11] + P[0][11]*SF[6] + P[1][11]*SF[10] + P[3][11]*SF[8] + P[12][11]*SF[14] - P[10][11]*SPP[10] - (P[11][11]*q0)/2))*0.5f;
|
||||
nextP[0][3] = P[0][3] + SQ[6] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10] + SF[7]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SF[6]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) + SF[9]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SF[15]*(P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10]) - SF[14]*(P[0][11] + P[1][11]*SF[9] + P[2][11]*SF[11] + P[3][11]*SF[10] + P[10][11]*SF[14] + P[11][11]*SF[15] + P[12][11]*SPP[10]) - (q0*(P[0][12] + P[1][12]*SF[9] + P[2][12]*SF[11] + P[3][12]*SF[10] + P[10][12]*SF[14] + P[11][12]*SF[15] + P[12][12]*SPP[10]))*0.5f;
|
||||
nextP[1][3] = P[1][3] + SQ[4] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)*0.5f + SF[7]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SF[6]*(P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] - (P[10][1]*q0)/2) + SF[9]*(P[1][2] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2) + SF[15]*(P[1][10] + P[0][10]*SF[8] + P[2][10]*SF[7] + P[3][10]*SF[11] - P[12][10]*SF[15] + P[11][10]*SPP[10] - (P[10][10]*q0)/2) - SF[14]*(P[1][11] + P[0][11]*SF[8] + P[2][11]*SF[7] + P[3][11]*SF[11] - P[12][11]*SF[15] + P[11][11]*SPP[10] - (P[10][11]*q0)/2) - (q0*(P[1][12] + P[0][12]*SF[8] + P[2][12]*SF[7] + P[3][12]*SF[11] - P[12][12]*SF[15] + P[11][12]*SPP[10] - (P[10][12]*q0)/2))*0.5f;
|
||||
nextP[2][3] = P[2][3] + SQ[3] + P[0][3]*SF[6] + P[1][3]*SF[10] + P[3][3]*SF[8] + P[12][3]*SF[14] - P[10][3]*SPP[10] - (P[11][3]*q0)*0.5f + SF[7]*(P[2][0] + P[0][0]*SF[6] + P[1][0]*SF[10] + P[3][0]*SF[8] + P[12][0]*SF[14] - P[10][0]*SPP[10] - (P[11][0]*q0)/2) + SF[6]*(P[2][1] + P[0][1]*SF[6] + P[1][1]*SF[10] + P[3][1]*SF[8] + P[12][1]*SF[14] - P[10][1]*SPP[10] - (P[11][1]*q0)/2) + SF[9]*(P[2][2] + P[0][2]*SF[6] + P[1][2]*SF[10] + P[3][2]*SF[8] + P[12][2]*SF[14] - P[10][2]*SPP[10] - (P[11][2]*q0)/2) + SF[15]*(P[2][10] + P[0][10]*SF[6] + P[1][10]*SF[10] + P[3][10]*SF[8] + P[12][10]*SF[14] - P[10][10]*SPP[10] - (P[11][10]*q0)/2) - SF[14]*(P[2][11] + P[0][11]*SF[6] + P[1][11]*SF[10] + P[3][11]*SF[8] + P[12][11]*SF[14] - P[10][11]*SPP[10] - (P[11][11]*q0)/2) - (q0*(P[2][12] + P[0][12]*SF[6] + P[1][12]*SF[10] + P[3][12]*SF[8] + P[12][12]*SF[14] - P[10][12]*SPP[10] - (P[11][12]*q0)/2))*0.5f;
|
||||
nextP[3][3] = P[3][3] + P[0][3]*SF[7] + P[1][3]*SF[6] + P[2][3]*SF[9] + P[10][3]*SF[15] - P[11][3]*SF[14] + (dayVar*SQ[10])*0.25f + dazVar*SQ[9] - (P[12][3]*q0)*0.5f + SF[7]*(P[3][0] + P[0][0]*SF[7] + P[1][0]*SF[6] + P[2][0]*SF[9] + P[10][0]*SF[15] - P[11][0]*SF[14] - (P[12][0]*q0)/2) + SF[6]*(P[3][1] + P[0][1]*SF[7] + P[1][1]*SF[6] + P[2][1]*SF[9] + P[10][1]*SF[15] - P[11][1]*SF[14] - (P[12][1]*q0)/2) + SF[9]*(P[3][2] + P[0][2]*SF[7] + P[1][2]*SF[6] + P[2][2]*SF[9] + P[10][2]*SF[15] - P[11][2]*SF[14] - (P[12][2]*q0)/2) + SF[15]*(P[3][10] + P[0][10]*SF[7] + P[1][10]*SF[6] + P[2][10]*SF[9] + P[10][10]*SF[15] - P[11][10]*SF[14] - (P[12][10]*q0)/2) - SF[14]*(P[3][11] + P[0][11]*SF[7] + P[1][11]*SF[6] + P[2][11]*SF[9] + P[10][11]*SF[15] - P[11][11]*SF[14] - (P[12][11]*q0)/2) + (daxVar*sq(q2))*0.25f - (q0*(P[3][12] + P[0][12]*SF[7] + P[1][12]*SF[6] + P[2][12]*SF[9] + P[10][12]*SF[15] - P[11][12]*SF[14] - (P[12][12]*q0)/2))*0.5f;
|
||||
nextP[0][4] = P[0][4] + P[1][4]*SF[9] + P[2][4]*SF[11] + P[3][4]*SF[10] + P[10][4]*SF[14] + P[11][4]*SF[15] + P[12][4]*SPP[10] + SF[5]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SF[3]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) - SF[4]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) + SPP[0]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SPP[3]*(P[0][13] + P[1][13]*SF[9] + P[2][13]*SF[11] + P[3][13]*SF[10] + P[10][13]*SF[14] + P[11][13]*SF[15] + P[12][13]*SPP[10]) + SPP[6]*(P[0][14] + P[1][14]*SF[9] + P[2][14]*SF[11] + P[3][14]*SF[10] + P[10][14]*SF[14] + P[11][14]*SF[15] + P[12][14]*SPP[10]) - SPP[9]*(P[0][15] + P[1][15]*SF[9] + P[2][15]*SF[11] + P[3][15]*SF[10] + P[10][15]*SF[14] + P[11][15]*SF[15] + P[12][15]*SPP[10]);
|
||||
nextP[1][4] = P[1][4] + P[0][4]*SF[8] + P[2][4]*SF[7] + P[3][4]*SF[11] - P[12][4]*SF[15] + P[11][4]*SPP[10] - (P[10][4]*q0)*0.5f + SF[5]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SF[3]*(P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] - (P[10][1]*q0)/2) - SF[4]*(P[1][3] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2) + SPP[0]*(P[1][2] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2) + SPP[3]*(P[1][13] + P[0][13]*SF[8] + P[2][13]*SF[7] + P[3][13]*SF[11] - P[12][13]*SF[15] + P[11][13]*SPP[10] - (P[10][13]*q0)/2) + SPP[6]*(P[1][14] + P[0][14]*SF[8] + P[2][14]*SF[7] + P[3][14]*SF[11] - P[12][14]*SF[15] + P[11][14]*SPP[10] - (P[10][14]*q0)/2) - SPP[9]*(P[1][15] + P[0][15]*SF[8] + P[2][15]*SF[7] + P[3][15]*SF[11] - P[12][15]*SF[15] + P[11][15]*SPP[10] - (P[10][15]*q0)/2);
|
||||
nextP[2][4] = P[2][4] + P[0][4]*SF[6] + P[1][4]*SF[10] + P[3][4]*SF[8] + P[12][4]*SF[14] - P[10][4]*SPP[10] - (P[11][4]*q0)*0.5f + SF[5]*(P[2][0] + P[0][0]*SF[6] + P[1][0]*SF[10] + P[3][0]*SF[8] + P[12][0]*SF[14] - P[10][0]*SPP[10] - (P[11][0]*q0)/2) + SF[3]*(P[2][1] + P[0][1]*SF[6] + P[1][1]*SF[10] + P[3][1]*SF[8] + P[12][1]*SF[14] - P[10][1]*SPP[10] - (P[11][1]*q0)/2) - SF[4]*(P[2][3] + P[0][3]*SF[6] + P[1][3]*SF[10] + P[3][3]*SF[8] + P[12][3]*SF[14] - P[10][3]*SPP[10] - (P[11][3]*q0)/2) + SPP[0]*(P[2][2] + P[0][2]*SF[6] + P[1][2]*SF[10] + P[3][2]*SF[8] + P[12][2]*SF[14] - P[10][2]*SPP[10] - (P[11][2]*q0)/2) + SPP[3]*(P[2][13] + P[0][13]*SF[6] + P[1][13]*SF[10] + P[3][13]*SF[8] + P[12][13]*SF[14] - P[10][13]*SPP[10] - (P[11][13]*q0)/2) + SPP[6]*(P[2][14] + P[0][14]*SF[6] + P[1][14]*SF[10] + P[3][14]*SF[8] + P[12][14]*SF[14] - P[10][14]*SPP[10] - (P[11][14]*q0)/2) - SPP[9]*(P[2][15] + P[0][15]*SF[6] + P[1][15]*SF[10] + P[3][15]*SF[8] + P[12][15]*SF[14] - P[10][15]*SPP[10] - (P[11][15]*q0)/2);
|
||||
nextP[3][4] = P[3][4] + P[0][4]*SF[7] + P[1][4]*SF[6] + P[2][4]*SF[9] + P[10][4]*SF[15] - P[11][4]*SF[14] - (P[12][4]*q0)*0.5f + SF[5]*(P[3][0] + P[0][0]*SF[7] + P[1][0]*SF[6] + P[2][0]*SF[9] + P[10][0]*SF[15] - P[11][0]*SF[14] - (P[12][0]*q0)/2) + SF[3]*(P[3][1] + P[0][1]*SF[7] + P[1][1]*SF[6] + P[2][1]*SF[9] + P[10][1]*SF[15] - P[11][1]*SF[14] - (P[12][1]*q0)/2) - SF[4]*(P[3][3] + P[0][3]*SF[7] + P[1][3]*SF[6] + P[2][3]*SF[9] + P[10][3]*SF[15] - P[11][3]*SF[14] - (P[12][3]*q0)/2) + SPP[0]*(P[3][2] + P[0][2]*SF[7] + P[1][2]*SF[6] + P[2][2]*SF[9] + P[10][2]*SF[15] - P[11][2]*SF[14] - (P[12][2]*q0)/2) + SPP[3]*(P[3][13] + P[0][13]*SF[7] + P[1][13]*SF[6] + P[2][13]*SF[9] + P[10][13]*SF[15] - P[11][13]*SF[14] - (P[12][13]*q0)/2) + SPP[6]*(P[3][14] + P[0][14]*SF[7] + P[1][14]*SF[6] + P[2][14]*SF[9] + P[10][14]*SF[15] - P[11][14]*SF[14] - (P[12][14]*q0)/2) - SPP[9]*(P[3][15] + P[0][15]*SF[7] + P[1][15]*SF[6] + P[2][15]*SF[9] + P[10][15]*SF[15] - P[11][15]*SF[14] - (P[12][15]*q0)/2);
|
||||
nextP[4][4] = P[4][4] + P[0][4]*SF[5] + P[1][4]*SF[3] - P[3][4]*SF[4] + P[2][4]*SPP[0] + P[13][4]*SPP[3] + P[14][4]*SPP[6] - P[15][4]*SPP[9] + dvyVar*sq(SG[7] - 2.0f*q0*q3) + dvzVar*sq(SG[6] + 2.0f*q0*q2) + SF[5]*(P[4][0] + P[0][0]*SF[5] + P[1][0]*SF[3] - P[3][0]*SF[4] + P[2][0]*SPP[0] + P[13][0]*SPP[3] + P[14][0]*SPP[6] - P[15][0]*SPP[9]) + SF[3]*(P[4][1] + P[0][1]*SF[5] + P[1][1]*SF[3] - P[3][1]*SF[4] + P[2][1]*SPP[0] + P[13][1]*SPP[3] + P[14][1]*SPP[6] - P[15][1]*SPP[9]) - SF[4]*(P[4][3] + P[0][3]*SF[5] + P[1][3]*SF[3] - P[3][3]*SF[4] + P[2][3]*SPP[0] + P[13][3]*SPP[3] + P[14][3]*SPP[6] - P[15][3]*SPP[9]) + SPP[0]*(P[4][2] + P[0][2]*SF[5] + P[1][2]*SF[3] - P[3][2]*SF[4] + P[2][2]*SPP[0] + P[13][2]*SPP[3] + P[14][2]*SPP[6] - P[15][2]*SPP[9]) + SPP[3]*(P[4][13] + P[0][13]*SF[5] + P[1][13]*SF[3] - P[3][13]*SF[4] + P[2][13]*SPP[0] + P[13][13]*SPP[3] + P[14][13]*SPP[6] - P[15][13]*SPP[9]) + SPP[6]*(P[4][14] + P[0][14]*SF[5] + P[1][14]*SF[3] - P[3][14]*SF[4] + P[2][14]*SPP[0] + P[13][14]*SPP[3] + P[14][14]*SPP[6] - P[15][14]*SPP[9]) - SPP[9]*(P[4][15] + P[0][15]*SF[5] + P[1][15]*SF[3] - P[3][15]*SF[4] + P[2][15]*SPP[0] + P[13][15]*SPP[3] + P[14][15]*SPP[6] - P[15][15]*SPP[9]) + dvxVar*sq(SG[1] + SG[2] - SG[3] - SG[4]);
|
||||
nextP[0][5] = P[0][5] + P[1][5]*SF[9] + P[2][5]*SF[11] + P[3][5]*SF[10] + P[10][5]*SF[14] + P[11][5]*SF[15] + P[12][5]*SPP[10] + SF[4]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SF[3]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SF[5]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) - SPP[0]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) - SPP[8]*(P[0][13] + P[1][13]*SF[9] + P[2][13]*SF[11] + P[3][13]*SF[10] + P[10][13]*SF[14] + P[11][13]*SF[15] + P[12][13]*SPP[10]) + SPP[2]*(P[0][14] + P[1][14]*SF[9] + P[2][14]*SF[11] + P[3][14]*SF[10] + P[10][14]*SF[14] + P[11][14]*SF[15] + P[12][14]*SPP[10]) + SPP[5]*(P[0][15] + P[1][15]*SF[9] + P[2][15]*SF[11] + P[3][15]*SF[10] + P[10][15]*SF[14] + P[11][15]*SF[15] + P[12][15]*SPP[10]);
|
||||
nextP[1][5] = P[1][5] + P[0][5]*SF[8] + P[2][5]*SF[7] + P[3][5]*SF[11] - P[12][5]*SF[15] + P[11][5]*SPP[10] - (P[10][5]*q0)*0.5f + SF[4]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SF[3]*(P[1][2] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2) + SF[5]*(P[1][3] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2) - SPP[0]*(P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] - (P[10][1]*q0)/2) - SPP[8]*(P[1][13] + P[0][13]*SF[8] + P[2][13]*SF[7] + P[3][13]*SF[11] - P[12][13]*SF[15] + P[11][13]*SPP[10] - (P[10][13]*q0)/2) + SPP[2]*(P[1][14] + P[0][14]*SF[8] + P[2][14]*SF[7] + P[3][14]*SF[11] - P[12][14]*SF[15] + P[11][14]*SPP[10] - (P[10][14]*q0)/2) + SPP[5]*(P[1][15] + P[0][15]*SF[8] + P[2][15]*SF[7] + P[3][15]*SF[11] - P[12][15]*SF[15] + P[11][15]*SPP[10] - (P[10][15]*q0)/2);
|
||||
nextP[2][5] = P[2][5] + P[0][5]*SF[6] + P[1][5]*SF[10] + P[3][5]*SF[8] + P[12][5]*SF[14] - P[10][5]*SPP[10] - (P[11][5]*q0)*0.5f + SF[4]*(P[2][0] + P[0][0]*SF[6] + P[1][0]*SF[10] + P[3][0]*SF[8] + P[12][0]*SF[14] - P[10][0]*SPP[10] - (P[11][0]*q0)/2) + SF[3]*(P[2][2] + P[0][2]*SF[6] + P[1][2]*SF[10] + P[3][2]*SF[8] + P[12][2]*SF[14] - P[10][2]*SPP[10] - (P[11][2]*q0)/2) + SF[5]*(P[2][3] + P[0][3]*SF[6] + P[1][3]*SF[10] + P[3][3]*SF[8] + P[12][3]*SF[14] - P[10][3]*SPP[10] - (P[11][3]*q0)/2) - SPP[0]*(P[2][1] + P[0][1]*SF[6] + P[1][1]*SF[10] + P[3][1]*SF[8] + P[12][1]*SF[14] - P[10][1]*SPP[10] - (P[11][1]*q0)/2) - SPP[8]*(P[2][13] + P[0][13]*SF[6] + P[1][13]*SF[10] + P[3][13]*SF[8] + P[12][13]*SF[14] - P[10][13]*SPP[10] - (P[11][13]*q0)/2) + SPP[2]*(P[2][14] + P[0][14]*SF[6] + P[1][14]*SF[10] + P[3][14]*SF[8] + P[12][14]*SF[14] - P[10][14]*SPP[10] - (P[11][14]*q0)/2) + SPP[5]*(P[2][15] + P[0][15]*SF[6] + P[1][15]*SF[10] + P[3][15]*SF[8] + P[12][15]*SF[14] - P[10][15]*SPP[10] - (P[11][15]*q0)/2);
|
||||
nextP[3][5] = P[3][5] + P[0][5]*SF[7] + P[1][5]*SF[6] + P[2][5]*SF[9] + P[10][5]*SF[15] - P[11][5]*SF[14] - (P[12][5]*q0)*0.5f + SF[4]*(P[3][0] + P[0][0]*SF[7] + P[1][0]*SF[6] + P[2][0]*SF[9] + P[10][0]*SF[15] - P[11][0]*SF[14] - (P[12][0]*q0)/2) + SF[3]*(P[3][2] + P[0][2]*SF[7] + P[1][2]*SF[6] + P[2][2]*SF[9] + P[10][2]*SF[15] - P[11][2]*SF[14] - (P[12][2]*q0)/2) + SF[5]*(P[3][3] + P[0][3]*SF[7] + P[1][3]*SF[6] + P[2][3]*SF[9] + P[10][3]*SF[15] - P[11][3]*SF[14] - (P[12][3]*q0)/2) - SPP[0]*(P[3][1] + P[0][1]*SF[7] + P[1][1]*SF[6] + P[2][1]*SF[9] + P[10][1]*SF[15] - P[11][1]*SF[14] - (P[12][1]*q0)/2) - SPP[8]*(P[3][13] + P[0][13]*SF[7] + P[1][13]*SF[6] + P[2][13]*SF[9] + P[10][13]*SF[15] - P[11][13]*SF[14] - (P[12][13]*q0)/2) + SPP[2]*(P[3][14] + P[0][14]*SF[7] + P[1][14]*SF[6] + P[2][14]*SF[9] + P[10][14]*SF[15] - P[11][14]*SF[14] - (P[12][14]*q0)/2) + SPP[5]*(P[3][15] + P[0][15]*SF[7] + P[1][15]*SF[6] + P[2][15]*SF[9] + P[10][15]*SF[15] - P[11][15]*SF[14] - (P[12][15]*q0)/2);
|
||||
nextP[4][5] = P[4][5] + SQ[2] + P[0][5]*SF[5] + P[1][5]*SF[3] - P[3][5]*SF[4] + P[2][5]*SPP[0] + P[13][5]*SPP[3] + P[14][5]*SPP[6] - P[15][5]*SPP[9] + SF[4]*(P[4][0] + P[0][0]*SF[5] + P[1][0]*SF[3] - P[3][0]*SF[4] + P[2][0]*SPP[0] + P[13][0]*SPP[3] + P[14][0]*SPP[6] - P[15][0]*SPP[9]) + SF[3]*(P[4][2] + P[0][2]*SF[5] + P[1][2]*SF[3] - P[3][2]*SF[4] + P[2][2]*SPP[0] + P[13][2]*SPP[3] + P[14][2]*SPP[6] - P[15][2]*SPP[9]) + SF[5]*(P[4][3] + P[0][3]*SF[5] + P[1][3]*SF[3] - P[3][3]*SF[4] + P[2][3]*SPP[0] + P[13][3]*SPP[3] + P[14][3]*SPP[6] - P[15][3]*SPP[9]) - SPP[0]*(P[4][1] + P[0][1]*SF[5] + P[1][1]*SF[3] - P[3][1]*SF[4] + P[2][1]*SPP[0] + P[13][1]*SPP[3] + P[14][1]*SPP[6] - P[15][1]*SPP[9]) - SPP[8]*(P[4][13] + P[0][13]*SF[5] + P[1][13]*SF[3] - P[3][13]*SF[4] + P[2][13]*SPP[0] + P[13][13]*SPP[3] + P[14][13]*SPP[6] - P[15][13]*SPP[9]) + SPP[2]*(P[4][14] + P[0][14]*SF[5] + P[1][14]*SF[3] - P[3][14]*SF[4] + P[2][14]*SPP[0] + P[13][14]*SPP[3] + P[14][14]*SPP[6] - P[15][14]*SPP[9]) + SPP[5]*(P[4][15] + P[0][15]*SF[5] + P[1][15]*SF[3] - P[3][15]*SF[4] + P[2][15]*SPP[0] + P[13][15]*SPP[3] + P[14][15]*SPP[6] - P[15][15]*SPP[9]);
|
||||
nextP[5][5] = P[5][5] + P[0][5]*SF[4] + P[2][5]*SF[3] + P[3][5]*SF[5] - P[1][5]*SPP[0] - P[13][5]*SPP[8] + P[14][5]*SPP[2] + P[15][5]*SPP[5] + dvxVar*sq(SG[7] + 2.0f*q0*q3) + dvzVar*sq(SG[5] - 2.0f*q0*q1) + SF[4]*(P[5][0] + P[0][0]*SF[4] + P[2][0]*SF[3] + P[3][0]*SF[5] - P[1][0]*SPP[0] - P[13][0]*SPP[8] + P[14][0]*SPP[2] + P[15][0]*SPP[5]) + SF[3]*(P[5][2] + P[0][2]*SF[4] + P[2][2]*SF[3] + P[3][2]*SF[5] - P[1][2]*SPP[0] - P[13][2]*SPP[8] + P[14][2]*SPP[2] + P[15][2]*SPP[5]) + SF[5]*(P[5][3] + P[0][3]*SF[4] + P[2][3]*SF[3] + P[3][3]*SF[5] - P[1][3]*SPP[0] - P[13][3]*SPP[8] + P[14][3]*SPP[2] + P[15][3]*SPP[5]) - SPP[0]*(P[5][1] + P[0][1]*SF[4] + P[2][1]*SF[3] + P[3][1]*SF[5] - P[1][1]*SPP[0] - P[13][1]*SPP[8] + P[14][1]*SPP[2] + P[15][1]*SPP[5]) - SPP[8]*(P[5][13] + P[0][13]*SF[4] + P[2][13]*SF[3] + P[3][13]*SF[5] - P[1][13]*SPP[0] - P[13][13]*SPP[8] + P[14][13]*SPP[2] + P[15][13]*SPP[5]) + SPP[2]*(P[5][14] + P[0][14]*SF[4] + P[2][14]*SF[3] + P[3][14]*SF[5] - P[1][14]*SPP[0] - P[13][14]*SPP[8] + P[14][14]*SPP[2] + P[15][14]*SPP[5]) + SPP[5]*(P[5][15] + P[0][15]*SF[4] + P[2][15]*SF[3] + P[3][15]*SF[5] - P[1][15]*SPP[0] - P[13][15]*SPP[8] + P[14][15]*SPP[2] + P[15][15]*SPP[5]) + dvyVar*sq(SG[1] - SG[2] + SG[3] - SG[4]);
|
||||
nextP[0][6] = P[0][6] + P[1][6]*SF[9] + P[2][6]*SF[11] + P[3][6]*SF[10] + P[10][6]*SF[14] + P[11][6]*SF[15] + P[12][6]*SPP[10] + SF[4]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) - SF[5]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SF[3]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) + SPP[0]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SPP[4]*(P[0][13] + P[1][13]*SF[9] + P[2][13]*SF[11] + P[3][13]*SF[10] + P[10][13]*SF[14] + P[11][13]*SF[15] + P[12][13]*SPP[10]) - SPP[7]*(P[0][14] + P[1][14]*SF[9] + P[2][14]*SF[11] + P[3][14]*SF[10] + P[10][14]*SF[14] + P[11][14]*SF[15] + P[12][14]*SPP[10]) - SPP[1]*(P[0][15] + P[1][15]*SF[9] + P[2][15]*SF[11] + P[3][15]*SF[10] + P[10][15]*SF[14] + P[11][15]*SF[15] + P[12][15]*SPP[10]);
|
||||
nextP[1][6] = P[1][6] + P[0][6]*SF[8] + P[2][6]*SF[7] + P[3][6]*SF[11] - P[12][6]*SF[15] + P[11][6]*SPP[10] - (P[10][6]*q0)*0.5f + SF[4]*(P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] - (P[10][1]*q0)/2) - SF[5]*(P[1][2] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2) + SF[3]*(P[1][3] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2) + SPP[0]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SPP[4]*(P[1][13] + P[0][13]*SF[8] + P[2][13]*SF[7] + P[3][13]*SF[11] - P[12][13]*SF[15] + P[11][13]*SPP[10] - (P[10][13]*q0)/2) - SPP[7]*(P[1][14] + P[0][14]*SF[8] + P[2][14]*SF[7] + P[3][14]*SF[11] - P[12][14]*SF[15] + P[11][14]*SPP[10] - (P[10][14]*q0)/2) - SPP[1]*(P[1][15] + P[0][15]*SF[8] + P[2][15]*SF[7] + P[3][15]*SF[11] - P[12][15]*SF[15] + P[11][15]*SPP[10] - (P[10][15]*q0)/2);
|
||||
nextP[2][6] = P[2][6] + P[0][6]*SF[6] + P[1][6]*SF[10] + P[3][6]*SF[8] + P[12][6]*SF[14] - P[10][6]*SPP[10] - (P[11][6]*q0)*0.5f + SF[4]*(P[2][1] + P[0][1]*SF[6] + P[1][1]*SF[10] + P[3][1]*SF[8] + P[12][1]*SF[14] - P[10][1]*SPP[10] - (P[11][1]*q0)/2) - SF[5]*(P[2][2] + P[0][2]*SF[6] + P[1][2]*SF[10] + P[3][2]*SF[8] + P[12][2]*SF[14] - P[10][2]*SPP[10] - (P[11][2]*q0)/2) + SF[3]*(P[2][3] + P[0][3]*SF[6] + P[1][3]*SF[10] + P[3][3]*SF[8] + P[12][3]*SF[14] - P[10][3]*SPP[10] - (P[11][3]*q0)/2) + SPP[0]*(P[2][0] + P[0][0]*SF[6] + P[1][0]*SF[10] + P[3][0]*SF[8] + P[12][0]*SF[14] - P[10][0]*SPP[10] - (P[11][0]*q0)/2) + SPP[4]*(P[2][13] + P[0][13]*SF[6] + P[1][13]*SF[10] + P[3][13]*SF[8] + P[12][13]*SF[14] - P[10][13]*SPP[10] - (P[11][13]*q0)/2) - SPP[7]*(P[2][14] + P[0][14]*SF[6] + P[1][14]*SF[10] + P[3][14]*SF[8] + P[12][14]*SF[14] - P[10][14]*SPP[10] - (P[11][14]*q0)/2) - SPP[1]*(P[2][15] + P[0][15]*SF[6] + P[1][15]*SF[10] + P[3][15]*SF[8] + P[12][15]*SF[14] - P[10][15]*SPP[10] - (P[11][15]*q0)/2);
|
||||
nextP[3][6] = P[3][6] + P[0][6]*SF[7] + P[1][6]*SF[6] + P[2][6]*SF[9] + P[10][6]*SF[15] - P[11][6]*SF[14] - (P[12][6]*q0)*0.5f + SF[4]*(P[3][1] + P[0][1]*SF[7] + P[1][1]*SF[6] + P[2][1]*SF[9] + P[10][1]*SF[15] - P[11][1]*SF[14] - (P[12][1]*q0)/2) - SF[5]*(P[3][2] + P[0][2]*SF[7] + P[1][2]*SF[6] + P[2][2]*SF[9] + P[10][2]*SF[15] - P[11][2]*SF[14] - (P[12][2]*q0)/2) + SF[3]*(P[3][3] + P[0][3]*SF[7] + P[1][3]*SF[6] + P[2][3]*SF[9] + P[10][3]*SF[15] - P[11][3]*SF[14] - (P[12][3]*q0)/2) + SPP[0]*(P[3][0] + P[0][0]*SF[7] + P[1][0]*SF[6] + P[2][0]*SF[9] + P[10][0]*SF[15] - P[11][0]*SF[14] - (P[12][0]*q0)/2) + SPP[4]*(P[3][13] + P[0][13]*SF[7] + P[1][13]*SF[6] + P[2][13]*SF[9] + P[10][13]*SF[15] - P[11][13]*SF[14] - (P[12][13]*q0)/2) - SPP[7]*(P[3][14] + P[0][14]*SF[7] + P[1][14]*SF[6] + P[2][14]*SF[9] + P[10][14]*SF[15] - P[11][14]*SF[14] - (P[12][14]*q0)/2) - SPP[1]*(P[3][15] + P[0][15]*SF[7] + P[1][15]*SF[6] + P[2][15]*SF[9] + P[10][15]*SF[15] - P[11][15]*SF[14] - (P[12][15]*q0)/2);
|
||||
nextP[4][6] = P[4][6] + SQ[1] + P[0][6]*SF[5] + P[1][6]*SF[3] - P[3][6]*SF[4] + P[2][6]*SPP[0] + P[13][6]*SPP[3] + P[14][6]*SPP[6] - P[15][6]*SPP[9] + SF[4]*(P[4][1] + P[0][1]*SF[5] + P[1][1]*SF[3] - P[3][1]*SF[4] + P[2][1]*SPP[0] + P[13][1]*SPP[3] + P[14][1]*SPP[6] - P[15][1]*SPP[9]) - SF[5]*(P[4][2] + P[0][2]*SF[5] + P[1][2]*SF[3] - P[3][2]*SF[4] + P[2][2]*SPP[0] + P[13][2]*SPP[3] + P[14][2]*SPP[6] - P[15][2]*SPP[9]) + SF[3]*(P[4][3] + P[0][3]*SF[5] + P[1][3]*SF[3] - P[3][3]*SF[4] + P[2][3]*SPP[0] + P[13][3]*SPP[3] + P[14][3]*SPP[6] - P[15][3]*SPP[9]) + SPP[0]*(P[4][0] + P[0][0]*SF[5] + P[1][0]*SF[3] - P[3][0]*SF[4] + P[2][0]*SPP[0] + P[13][0]*SPP[3] + P[14][0]*SPP[6] - P[15][0]*SPP[9]) + SPP[4]*(P[4][13] + P[0][13]*SF[5] + P[1][13]*SF[3] - P[3][13]*SF[4] + P[2][13]*SPP[0] + P[13][13]*SPP[3] + P[14][13]*SPP[6] - P[15][13]*SPP[9]) - SPP[7]*(P[4][14] + P[0][14]*SF[5] + P[1][14]*SF[3] - P[3][14]*SF[4] + P[2][14]*SPP[0] + P[13][14]*SPP[3] + P[14][14]*SPP[6] - P[15][14]*SPP[9]) - SPP[1]*(P[4][15] + P[0][15]*SF[5] + P[1][15]*SF[3] - P[3][15]*SF[4] + P[2][15]*SPP[0] + P[13][15]*SPP[3] + P[14][15]*SPP[6] - P[15][15]*SPP[9]);
|
||||
nextP[5][6] = P[5][6] + SQ[0] + P[0][6]*SF[4] + P[2][6]*SF[3] + P[3][6]*SF[5] - P[1][6]*SPP[0] - P[13][6]*SPP[8] + P[14][6]*SPP[2] + P[15][6]*SPP[5] + SF[4]*(P[5][1] + P[0][1]*SF[4] + P[2][1]*SF[3] + P[3][1]*SF[5] - P[1][1]*SPP[0] - P[13][1]*SPP[8] + P[14][1]*SPP[2] + P[15][1]*SPP[5]) - SF[5]*(P[5][2] + P[0][2]*SF[4] + P[2][2]*SF[3] + P[3][2]*SF[5] - P[1][2]*SPP[0] - P[13][2]*SPP[8] + P[14][2]*SPP[2] + P[15][2]*SPP[5]) + SF[3]*(P[5][3] + P[0][3]*SF[4] + P[2][3]*SF[3] + P[3][3]*SF[5] - P[1][3]*SPP[0] - P[13][3]*SPP[8] + P[14][3]*SPP[2] + P[15][3]*SPP[5]) + SPP[0]*(P[5][0] + P[0][0]*SF[4] + P[2][0]*SF[3] + P[3][0]*SF[5] - P[1][0]*SPP[0] - P[13][0]*SPP[8] + P[14][0]*SPP[2] + P[15][0]*SPP[5]) + SPP[4]*(P[5][13] + P[0][13]*SF[4] + P[2][13]*SF[3] + P[3][13]*SF[5] - P[1][13]*SPP[0] - P[13][13]*SPP[8] + P[14][13]*SPP[2] + P[15][13]*SPP[5]) - SPP[7]*(P[5][14] + P[0][14]*SF[4] + P[2][14]*SF[3] + P[3][14]*SF[5] - P[1][14]*SPP[0] - P[13][14]*SPP[8] + P[14][14]*SPP[2] + P[15][14]*SPP[5]) - SPP[1]*(P[5][15] + P[0][15]*SF[4] + P[2][15]*SF[3] + P[3][15]*SF[5] - P[1][15]*SPP[0] - P[13][15]*SPP[8] + P[14][15]*SPP[2] + P[15][15]*SPP[5]);
|
||||
nextP[6][6] = P[6][6] + P[1][6]*SF[4] - P[2][6]*SF[5] + P[3][6]*SF[3] + P[0][6]*SPP[0] + P[13][6]*SPP[4] - P[14][6]*SPP[7] - P[15][6]*SPP[1] + dvxVar*sq(SG[6] - 2.0f*q0*q2) + dvyVar*sq(SG[5] + 2.0f*q0*q1) + SF[4]*(P[6][1] + P[1][1]*SF[4] - P[2][1]*SF[5] + P[3][1]*SF[3] + P[0][1]*SPP[0] + P[13][1]*SPP[4] - P[14][1]*SPP[7] - P[15][1]*SPP[1]) - SF[5]*(P[6][2] + P[1][2]*SF[4] - P[2][2]*SF[5] + P[3][2]*SF[3] + P[0][2]*SPP[0] + P[13][2]*SPP[4] - P[14][2]*SPP[7] - P[15][2]*SPP[1]) + SF[3]*(P[6][3] + P[1][3]*SF[4] - P[2][3]*SF[5] + P[3][3]*SF[3] + P[0][3]*SPP[0] + P[13][3]*SPP[4] - P[14][3]*SPP[7] - P[15][3]*SPP[1]) + SPP[0]*(P[6][0] + P[1][0]*SF[4] - P[2][0]*SF[5] + P[3][0]*SF[3] + P[0][0]*SPP[0] + P[13][0]*SPP[4] - P[14][0]*SPP[7] - P[15][0]*SPP[1]) + SPP[4]*(P[6][13] + P[1][13]*SF[4] - P[2][13]*SF[5] + P[3][13]*SF[3] + P[0][13]*SPP[0] + P[13][13]*SPP[4] - P[14][13]*SPP[7] - P[15][13]*SPP[1]) - SPP[7]*(P[6][14] + P[1][14]*SF[4] - P[2][14]*SF[5] + P[3][14]*SF[3] + P[0][14]*SPP[0] + P[13][14]*SPP[4] - P[14][14]*SPP[7] - P[15][14]*SPP[1]) - SPP[1]*(P[6][15] + P[1][15]*SF[4] - P[2][15]*SF[5] + P[3][15]*SF[3] + P[0][15]*SPP[0] + P[13][15]*SPP[4] - P[14][15]*SPP[7] - P[15][15]*SPP[1]) + dvzVar*sq(SG[1] - SG[2] - SG[3] + SG[4]);
|
||||
nextP[0][7] = P[0][7] + P[1][7]*SF[9] + P[2][7]*SF[11] + P[3][7]*SF[10] + P[10][7]*SF[14] + P[11][7]*SF[15] + P[12][7]*SPP[10] + dt*(P[0][4] + P[1][4]*SF[9] + P[2][4]*SF[11] + P[3][4]*SF[10] + P[10][4]*SF[14] + P[11][4]*SF[15] + P[12][4]*SPP[10]);
|
||||
nextP[1][7] = P[1][7] + P[0][7]*SF[8] + P[2][7]*SF[7] + P[3][7]*SF[11] - P[12][7]*SF[15] + P[11][7]*SPP[10] - (P[10][7]*q0)*0.5f + dt*(P[1][4] + P[0][4]*SF[8] + P[2][4]*SF[7] + P[3][4]*SF[11] - P[12][4]*SF[15] + P[11][4]*SPP[10] - (P[10][4]*q0)/2);
|
||||
nextP[2][7] = P[2][7] + P[0][7]*SF[6] + P[1][7]*SF[10] + P[3][7]*SF[8] + P[12][7]*SF[14] - P[10][7]*SPP[10] - (P[11][7]*q0)*0.5f + dt*(P[2][4] + P[0][4]*SF[6] + P[1][4]*SF[10] + P[3][4]*SF[8] + P[12][4]*SF[14] - P[10][4]*SPP[10] - (P[11][4]*q0)/2);
|
||||
nextP[3][7] = P[3][7] + P[0][7]*SF[7] + P[1][7]*SF[6] + P[2][7]*SF[9] + P[10][7]*SF[15] - P[11][7]*SF[14] - (P[12][7]*q0)*0.5f + dt*(P[3][4] + P[0][4]*SF[7] + P[1][4]*SF[6] + P[2][4]*SF[9] + P[10][4]*SF[15] - P[11][4]*SF[14] - (P[12][4]*q0)/2);
|
||||
nextP[4][7] = P[4][7] + P[0][7]*SF[5] + P[1][7]*SF[3] - P[3][7]*SF[4] + P[2][7]*SPP[0] + P[13][7]*SPP[3] + P[14][7]*SPP[6] - P[15][7]*SPP[9] + dt*(P[4][4] + P[0][4]*SF[5] + P[1][4]*SF[3] - P[3][4]*SF[4] + P[2][4]*SPP[0] + P[13][4]*SPP[3] + P[14][4]*SPP[6] - P[15][4]*SPP[9]);
|
||||
nextP[5][7] = P[5][7] + P[0][7]*SF[4] + P[2][7]*SF[3] + P[3][7]*SF[5] - P[1][7]*SPP[0] - P[13][7]*SPP[8] + P[14][7]*SPP[2] + P[15][7]*SPP[5] + dt*(P[5][4] + P[0][4]*SF[4] + P[2][4]*SF[3] + P[3][4]*SF[5] - P[1][4]*SPP[0] - P[13][4]*SPP[8] + P[14][4]*SPP[2] + P[15][4]*SPP[5]);
|
||||
nextP[6][7] = P[6][7] + P[1][7]*SF[4] - P[2][7]*SF[5] + P[3][7]*SF[3] + P[0][7]*SPP[0] + P[13][7]*SPP[4] - P[14][7]*SPP[7] - P[15][7]*SPP[1] + dt*(P[6][4] + P[1][4]*SF[4] - P[2][4]*SF[5] + P[3][4]*SF[3] + P[0][4]*SPP[0] + P[13][4]*SPP[4] - P[14][4]*SPP[7] - P[15][4]*SPP[1]);
|
||||
nextP[7][7] = P[7][7] + P[4][7]*dt + dt*(P[7][4] + P[4][4]*dt);
|
||||
nextP[0][8] = P[0][8] + P[1][8]*SF[9] + P[2][8]*SF[11] + P[3][8]*SF[10] + P[10][8]*SF[14] + P[11][8]*SF[15] + P[12][8]*SPP[10] + dt*(P[0][5] + P[1][5]*SF[9] + P[2][5]*SF[11] + P[3][5]*SF[10] + P[10][5]*SF[14] + P[11][5]*SF[15] + P[12][5]*SPP[10]);
|
||||
nextP[1][8] = P[1][8] + P[0][8]*SF[8] + P[2][8]*SF[7] + P[3][8]*SF[11] - P[12][8]*SF[15] + P[11][8]*SPP[10] - (P[10][8]*q0)*0.5f + dt*(P[1][5] + P[0][5]*SF[8] + P[2][5]*SF[7] + P[3][5]*SF[11] - P[12][5]*SF[15] + P[11][5]*SPP[10] - (P[10][5]*q0)/2);
|
||||
nextP[2][8] = P[2][8] + P[0][8]*SF[6] + P[1][8]*SF[10] + P[3][8]*SF[8] + P[12][8]*SF[14] - P[10][8]*SPP[10] - (P[11][8]*q0)*0.5f + dt*(P[2][5] + P[0][5]*SF[6] + P[1][5]*SF[10] + P[3][5]*SF[8] + P[12][5]*SF[14] - P[10][5]*SPP[10] - (P[11][5]*q0)/2);
|
||||
nextP[3][8] = P[3][8] + P[0][8]*SF[7] + P[1][8]*SF[6] + P[2][8]*SF[9] + P[10][8]*SF[15] - P[11][8]*SF[14] - (P[12][8]*q0)*0.5f + dt*(P[3][5] + P[0][5]*SF[7] + P[1][5]*SF[6] + P[2][5]*SF[9] + P[10][5]*SF[15] - P[11][5]*SF[14] - (P[12][5]*q0)/2);
|
||||
nextP[4][8] = P[4][8] + P[0][8]*SF[5] + P[1][8]*SF[3] - P[3][8]*SF[4] + P[2][8]*SPP[0] + P[13][8]*SPP[3] + P[14][8]*SPP[6] - P[15][8]*SPP[9] + dt*(P[4][5] + P[0][5]*SF[5] + P[1][5]*SF[3] - P[3][5]*SF[4] + P[2][5]*SPP[0] + P[13][5]*SPP[3] + P[14][5]*SPP[6] - P[15][5]*SPP[9]);
|
||||
nextP[5][8] = P[5][8] + P[0][8]*SF[4] + P[2][8]*SF[3] + P[3][8]*SF[5] - P[1][8]*SPP[0] - P[13][8]*SPP[8] + P[14][8]*SPP[2] + P[15][8]*SPP[5] + dt*(P[5][5] + P[0][5]*SF[4] + P[2][5]*SF[3] + P[3][5]*SF[5] - P[1][5]*SPP[0] - P[13][5]*SPP[8] + P[14][5]*SPP[2] + P[15][5]*SPP[5]);
|
||||
nextP[6][8] = P[6][8] + P[1][8]*SF[4] - P[2][8]*SF[5] + P[3][8]*SF[3] + P[0][8]*SPP[0] + P[13][8]*SPP[4] - P[14][8]*SPP[7] - P[15][8]*SPP[1] + dt*(P[6][5] + P[1][5]*SF[4] - P[2][5]*SF[5] + P[3][5]*SF[3] + P[0][5]*SPP[0] + P[13][5]*SPP[4] - P[14][5]*SPP[7] - P[15][5]*SPP[1]);
|
||||
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] + P[1][9]*SF[9] + P[2][9]*SF[11] + P[3][9]*SF[10] + P[10][9]*SF[14] + P[11][9]*SF[15] + P[12][9]*SPP[10] + dt*(P[0][6] + P[1][6]*SF[9] + P[2][6]*SF[11] + P[3][6]*SF[10] + P[10][6]*SF[14] + P[11][6]*SF[15] + P[12][6]*SPP[10]);
|
||||
nextP[1][9] = P[1][9] + P[0][9]*SF[8] + P[2][9]*SF[7] + P[3][9]*SF[11] - P[12][9]*SF[15] + P[11][9]*SPP[10] - (P[10][9]*q0)*0.5f + dt*(P[1][6] + P[0][6]*SF[8] + P[2][6]*SF[7] + P[3][6]*SF[11] - P[12][6]*SF[15] + P[11][6]*SPP[10] - (P[10][6]*q0)/2);
|
||||
nextP[2][9] = P[2][9] + P[0][9]*SF[6] + P[1][9]*SF[10] + P[3][9]*SF[8] + P[12][9]*SF[14] - P[10][9]*SPP[10] - (P[11][9]*q0)*0.5f + dt*(P[2][6] + P[0][6]*SF[6] + P[1][6]*SF[10] + P[3][6]*SF[8] + P[12][6]*SF[14] - P[10][6]*SPP[10] - (P[11][6]*q0)/2);
|
||||
nextP[3][9] = P[3][9] + P[0][9]*SF[7] + P[1][9]*SF[6] + P[2][9]*SF[9] + P[10][9]*SF[15] - P[11][9]*SF[14] - (P[12][9]*q0)*0.5f + dt*(P[3][6] + P[0][6]*SF[7] + P[1][6]*SF[6] + P[2][6]*SF[9] + P[10][6]*SF[15] - P[11][6]*SF[14] - (P[12][6]*q0)/2);
|
||||
nextP[4][9] = P[4][9] + P[0][9]*SF[5] + P[1][9]*SF[3] - P[3][9]*SF[4] + P[2][9]*SPP[0] + P[13][9]*SPP[3] + P[14][9]*SPP[6] - P[15][9]*SPP[9] + dt*(P[4][6] + P[0][6]*SF[5] + P[1][6]*SF[3] - P[3][6]*SF[4] + P[2][6]*SPP[0] + P[13][6]*SPP[3] + P[14][6]*SPP[6] - P[15][6]*SPP[9]);
|
||||
nextP[5][9] = P[5][9] + P[0][9]*SF[4] + P[2][9]*SF[3] + P[3][9]*SF[5] - P[1][9]*SPP[0] - P[13][9]*SPP[8] + P[14][9]*SPP[2] + P[15][9]*SPP[5] + dt*(P[5][6] + P[0][6]*SF[4] + P[2][6]*SF[3] + P[3][6]*SF[5] - P[1][6]*SPP[0] - P[13][6]*SPP[8] + P[14][6]*SPP[2] + P[15][6]*SPP[5]);
|
||||
nextP[6][9] = P[6][9] + P[1][9]*SF[4] - P[2][9]*SF[5] + P[3][9]*SF[3] + P[0][9]*SPP[0] + P[13][9]*SPP[4] - P[14][9]*SPP[7] - P[15][9]*SPP[1] + dt*(P[6][6] + P[1][6]*SF[4] - P[2][6]*SF[5] + P[3][6]*SF[3] + P[0][6]*SPP[0] + P[13][6]*SPP[4] - P[14][6]*SPP[7] - P[15][6]*SPP[1]);
|
||||
nextP[7][9] = P[7][9] + P[4][9]*dt + dt*(P[7][6] + P[4][6]*dt);
|
||||
nextP[8][9] = P[8][9] + P[5][9]*dt + dt*(P[8][6] + P[5][6]*dt);
|
||||
nextP[9][9] = P[9][9] + P[6][9]*dt + dt*(P[9][6] + P[6][6]*dt);
|
||||
nextP[0][10] = P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10];
|
||||
nextP[1][10] = P[1][10] + P[0][10]*SF[8] + P[2][10]*SF[7] + P[3][10]*SF[11] - P[12][10]*SF[15] + P[11][10]*SPP[10] - (P[10][10]*q0)*0.5f;
|
||||
nextP[2][10] = P[2][10] + P[0][10]*SF[6] + P[1][10]*SF[10] + P[3][10]*SF[8] + P[12][10]*SF[14] - P[10][10]*SPP[10] - (P[11][10]*q0)*0.5f;
|
||||
nextP[3][10] = P[3][10] + P[0][10]*SF[7] + P[1][10]*SF[6] + P[2][10]*SF[9] + P[10][10]*SF[15] - P[11][10]*SF[14] - (P[12][10]*q0)*0.5f;
|
||||
nextP[4][10] = P[4][10] + P[0][10]*SF[5] + P[1][10]*SF[3] - P[3][10]*SF[4] + P[2][10]*SPP[0] + P[13][10]*SPP[3] + P[14][10]*SPP[6] - P[15][10]*SPP[9];
|
||||
nextP[5][10] = P[5][10] + P[0][10]*SF[4] + P[2][10]*SF[3] + P[3][10]*SF[5] - P[1][10]*SPP[0] - P[13][10]*SPP[8] + P[14][10]*SPP[2] + P[15][10]*SPP[5];
|
||||
nextP[6][10] = P[6][10] + P[1][10]*SF[4] - P[2][10]*SF[5] + P[3][10]*SF[3] + P[0][10]*SPP[0] + P[13][10]*SPP[4] - P[14][10]*SPP[7] - P[15][10]*SPP[1];
|
||||
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] + P[6][10]*dt;
|
||||
nextP[10][10] = P[10][10];
|
||||
nextP[0][11] = P[0][11] + P[1][11]*SF[9] + P[2][11]*SF[11] + P[3][11]*SF[10] + P[10][11]*SF[14] + P[11][11]*SF[15] + P[12][11]*SPP[10];
|
||||
nextP[1][11] = P[1][11] + P[0][11]*SF[8] + P[2][11]*SF[7] + P[3][11]*SF[11] - P[12][11]*SF[15] + P[11][11]*SPP[10] - (P[10][11]*q0)*0.5f;
|
||||
nextP[2][11] = P[2][11] + P[0][11]*SF[6] + P[1][11]*SF[10] + P[3][11]*SF[8] + P[12][11]*SF[14] - P[10][11]*SPP[10] - (P[11][11]*q0)*0.5f;
|
||||
nextP[3][11] = P[3][11] + P[0][11]*SF[7] + P[1][11]*SF[6] + P[2][11]*SF[9] + P[10][11]*SF[15] - P[11][11]*SF[14] - (P[12][11]*q0)*0.5f;
|
||||
nextP[4][11] = P[4][11] + P[0][11]*SF[5] + P[1][11]*SF[3] - P[3][11]*SF[4] + P[2][11]*SPP[0] + P[13][11]*SPP[3] + P[14][11]*SPP[6] - P[15][11]*SPP[9];
|
||||
nextP[5][11] = P[5][11] + P[0][11]*SF[4] + P[2][11]*SF[3] + P[3][11]*SF[5] - P[1][11]*SPP[0] - P[13][11]*SPP[8] + P[14][11]*SPP[2] + P[15][11]*SPP[5];
|
||||
nextP[6][11] = P[6][11] + P[1][11]*SF[4] - P[2][11]*SF[5] + P[3][11]*SF[3] + P[0][11]*SPP[0] + P[13][11]*SPP[4] - P[14][11]*SPP[7] - P[15][11]*SPP[1];
|
||||
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] + P[6][11]*dt;
|
||||
nextP[10][11] = P[10][11];
|
||||
nextP[11][11] = P[11][11];
|
||||
nextP[0][12] = P[0][12] + P[1][12]*SF[9] + P[2][12]*SF[11] + P[3][12]*SF[10] + P[10][12]*SF[14] + P[11][12]*SF[15] + P[12][12]*SPP[10];
|
||||
nextP[1][12] = P[1][12] + P[0][12]*SF[8] + P[2][12]*SF[7] + P[3][12]*SF[11] - P[12][12]*SF[15] + P[11][12]*SPP[10] - (P[10][12]*q0)*0.5f;
|
||||
nextP[2][12] = P[2][12] + P[0][12]*SF[6] + P[1][12]*SF[10] + P[3][12]*SF[8] + P[12][12]*SF[14] - P[10][12]*SPP[10] - (P[11][12]*q0)*0.5f;
|
||||
nextP[3][12] = P[3][12] + P[0][12]*SF[7] + P[1][12]*SF[6] + P[2][12]*SF[9] + P[10][12]*SF[15] - P[11][12]*SF[14] - (P[12][12]*q0)*0.5f;
|
||||
nextP[4][12] = P[4][12] + P[0][12]*SF[5] + P[1][12]*SF[3] - P[3][12]*SF[4] + P[2][12]*SPP[0] + P[13][12]*SPP[3] + P[14][12]*SPP[6] - P[15][12]*SPP[9];
|
||||
nextP[5][12] = P[5][12] + P[0][12]*SF[4] + P[2][12]*SF[3] + P[3][12]*SF[5] - P[1][12]*SPP[0] - P[13][12]*SPP[8] + P[14][12]*SPP[2] + P[15][12]*SPP[5];
|
||||
nextP[6][12] = P[6][12] + P[1][12]*SF[4] - P[2][12]*SF[5] + P[3][12]*SF[3] + P[0][12]*SPP[0] + P[13][12]*SPP[4] - P[14][12]*SPP[7] - P[15][12]*SPP[1];
|
||||
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] + P[6][12]*dt;
|
||||
nextP[10][12] = P[10][12];
|
||||
nextP[11][12] = P[11][12];
|
||||
nextP[12][12] = P[12][12];
|
||||
nextP[0][13] = P[0][13] + P[1][13]*SF[9] + P[2][13]*SF[11] + P[3][13]*SF[10] + P[10][13]*SF[14] + P[11][13]*SF[15] + P[12][13]*SPP[10];
|
||||
nextP[1][13] = P[1][13] + P[0][13]*SF[8] + P[2][13]*SF[7] + P[3][13]*SF[11] - P[12][13]*SF[15] + P[11][13]*SPP[10] - (P[10][13]*q0)*0.5f;
|
||||
nextP[2][13] = P[2][13] + P[0][13]*SF[6] + P[1][13]*SF[10] + P[3][13]*SF[8] + P[12][13]*SF[14] - P[10][13]*SPP[10] - (P[11][13]*q0)*0.5f;
|
||||
nextP[3][13] = P[3][13] + P[0][13]*SF[7] + P[1][13]*SF[6] + P[2][13]*SF[9] + P[10][13]*SF[15] - P[11][13]*SF[14] - (P[12][13]*q0)*0.5f;
|
||||
nextP[4][13] = P[4][13] + P[0][13]*SF[5] + P[1][13]*SF[3] - P[3][13]*SF[4] + P[2][13]*SPP[0] + P[13][13]*SPP[3] + P[14][13]*SPP[6] - P[15][13]*SPP[9];
|
||||
nextP[5][13] = P[5][13] + P[0][13]*SF[4] + P[2][13]*SF[3] + P[3][13]*SF[5] - P[1][13]*SPP[0] - P[13][13]*SPP[8] + P[14][13]*SPP[2] + P[15][13]*SPP[5];
|
||||
nextP[6][13] = P[6][13] + P[1][13]*SF[4] - P[2][13]*SF[5] + P[3][13]*SF[3] + P[0][13]*SPP[0] + P[13][13]*SPP[4] - P[14][13]*SPP[7] - P[15][13]*SPP[1];
|
||||
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] + P[6][13]*dt;
|
||||
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] + P[1][14]*SF[9] + P[2][14]*SF[11] + P[3][14]*SF[10] + P[10][14]*SF[14] + P[11][14]*SF[15] + P[12][14]*SPP[10];
|
||||
nextP[1][14] = P[1][14] + P[0][14]*SF[8] + P[2][14]*SF[7] + P[3][14]*SF[11] - P[12][14]*SF[15] + P[11][14]*SPP[10] - (P[10][14]*q0)*0.5f;
|
||||
nextP[2][14] = P[2][14] + P[0][14]*SF[6] + P[1][14]*SF[10] + P[3][14]*SF[8] + P[12][14]*SF[14] - P[10][14]*SPP[10] - (P[11][14]*q0)*0.5f;
|
||||
nextP[3][14] = P[3][14] + P[0][14]*SF[7] + P[1][14]*SF[6] + P[2][14]*SF[9] + P[10][14]*SF[15] - P[11][14]*SF[14] - (P[12][14]*q0)*0.5f;
|
||||
nextP[4][14] = P[4][14] + P[0][14]*SF[5] + P[1][14]*SF[3] - P[3][14]*SF[4] + P[2][14]*SPP[0] + P[13][14]*SPP[3] + P[14][14]*SPP[6] - P[15][14]*SPP[9];
|
||||
nextP[5][14] = P[5][14] + P[0][14]*SF[4] + P[2][14]*SF[3] + P[3][14]*SF[5] - P[1][14]*SPP[0] - P[13][14]*SPP[8] + P[14][14]*SPP[2] + P[15][14]*SPP[5];
|
||||
nextP[6][14] = P[6][14] + P[1][14]*SF[4] - P[2][14]*SF[5] + P[3][14]*SF[3] + P[0][14]*SPP[0] + P[13][14]*SPP[4] - P[14][14]*SPP[7] - P[15][14]*SPP[1];
|
||||
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] + P[6][14]*dt;
|
||||
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] + P[1][15]*SF[9] + P[2][15]*SF[11] + P[3][15]*SF[10] + P[10][15]*SF[14] + P[11][15]*SF[15] + P[12][15]*SPP[10];
|
||||
nextP[1][15] = P[1][15] + P[0][15]*SF[8] + P[2][15]*SF[7] + P[3][15]*SF[11] - P[12][15]*SF[15] + P[11][15]*SPP[10] - (P[10][15]*q0)*0.5f;
|
||||
nextP[2][15] = P[2][15] + P[0][15]*SF[6] + P[1][15]*SF[10] + P[3][15]*SF[8] + P[12][15]*SF[14] - P[10][15]*SPP[10] - (P[11][15]*q0)*0.5f;
|
||||
nextP[3][15] = P[3][15] + P[0][15]*SF[7] + P[1][15]*SF[6] + P[2][15]*SF[9] + P[10][15]*SF[15] - P[11][15]*SF[14] - (P[12][15]*q0)*0.5f;
|
||||
nextP[4][15] = P[4][15] + P[0][15]*SF[5] + P[1][15]*SF[3] - P[3][15]*SF[4] + P[2][15]*SPP[0] + P[13][15]*SPP[3] + P[14][15]*SPP[6] - P[15][15]*SPP[9];
|
||||
nextP[5][15] = P[5][15] + P[0][15]*SF[4] + P[2][15]*SF[3] + P[3][15]*SF[5] - P[1][15]*SPP[0] - P[13][15]*SPP[8] + P[14][15]*SPP[2] + P[15][15]*SPP[5];
|
||||
nextP[6][15] = P[6][15] + P[1][15]*SF[4] - P[2][15]*SF[5] + P[3][15]*SF[3] + P[0][15]*SPP[0] + P[13][15]*SPP[4] - P[14][15]*SPP[7] - P[15][15]*SPP[1];
|
||||
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] + P[6][15]*dt;
|
||||
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] + P[1][16]*SF[9] + P[2][16]*SF[11] + P[3][16]*SF[10] + P[10][16]*SF[14] + P[11][16]*SF[15] + P[12][16]*SPP[10];
|
||||
nextP[1][16] = P[1][16] + P[0][16]*SF[8] + P[2][16]*SF[7] + P[3][16]*SF[11] - P[12][16]*SF[15] + P[11][16]*SPP[10] - (P[10][16]*q0)*0.5f;
|
||||
nextP[2][16] = P[2][16] + P[0][16]*SF[6] + P[1][16]*SF[10] + P[3][16]*SF[8] + P[12][16]*SF[14] - P[10][16]*SPP[10] - (P[11][16]*q0)*0.5f;
|
||||
nextP[3][16] = P[3][16] + P[0][16]*SF[7] + P[1][16]*SF[6] + P[2][16]*SF[9] + P[10][16]*SF[15] - P[11][16]*SF[14] - (P[12][16]*q0)*0.5f;
|
||||
nextP[4][16] = P[4][16] + P[0][16]*SF[5] + P[1][16]*SF[3] - P[3][16]*SF[4] + P[2][16]*SPP[0] + P[13][16]*SPP[3] + P[14][16]*SPP[6] - P[15][16]*SPP[9];
|
||||
nextP[5][16] = P[5][16] + P[0][16]*SF[4] + P[2][16]*SF[3] + P[3][16]*SF[5] - P[1][16]*SPP[0] - P[13][16]*SPP[8] + P[14][16]*SPP[2] + P[15][16]*SPP[5];
|
||||
nextP[6][16] = P[6][16] + P[1][16]*SF[4] - P[2][16]*SF[5] + P[3][16]*SF[3] + P[0][16]*SPP[0] + P[13][16]*SPP[4] - P[14][16]*SPP[7] - P[15][16]*SPP[1];
|
||||
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] + P[6][16]*dt;
|
||||
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] + P[1][17]*SF[9] + P[2][17]*SF[11] + P[3][17]*SF[10] + P[10][17]*SF[14] + P[11][17]*SF[15] + P[12][17]*SPP[10];
|
||||
nextP[1][17] = P[1][17] + P[0][17]*SF[8] + P[2][17]*SF[7] + P[3][17]*SF[11] - P[12][17]*SF[15] + P[11][17]*SPP[10] - (P[10][17]*q0)*0.5f;
|
||||
nextP[2][17] = P[2][17] + P[0][17]*SF[6] + P[1][17]*SF[10] + P[3][17]*SF[8] + P[12][17]*SF[14] - P[10][17]*SPP[10] - (P[11][17]*q0)*0.5f;
|
||||
nextP[3][17] = P[3][17] + P[0][17]*SF[7] + P[1][17]*SF[6] + P[2][17]*SF[9] + P[10][17]*SF[15] - P[11][17]*SF[14] - (P[12][17]*q0)*0.5f;
|
||||
nextP[4][17] = P[4][17] + P[0][17]*SF[5] + P[1][17]*SF[3] - P[3][17]*SF[4] + P[2][17]*SPP[0] + P[13][17]*SPP[3] + P[14][17]*SPP[6] - P[15][17]*SPP[9];
|
||||
nextP[5][17] = P[5][17] + P[0][17]*SF[4] + P[2][17]*SF[3] + P[3][17]*SF[5] - P[1][17]*SPP[0] - P[13][17]*SPP[8] + P[14][17]*SPP[2] + P[15][17]*SPP[5];
|
||||
nextP[6][17] = P[6][17] + P[1][17]*SF[4] - P[2][17]*SF[5] + P[3][17]*SF[3] + P[0][17]*SPP[0] + P[13][17]*SPP[4] - P[14][17]*SPP[7] - P[15][17]*SPP[1];
|
||||
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] + P[6][17]*dt;
|
||||
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] + P[1][18]*SF[9] + P[2][18]*SF[11] + P[3][18]*SF[10] + P[10][18]*SF[14] + P[11][18]*SF[15] + P[12][18]*SPP[10];
|
||||
nextP[1][18] = P[1][18] + P[0][18]*SF[8] + P[2][18]*SF[7] + P[3][18]*SF[11] - P[12][18]*SF[15] + P[11][18]*SPP[10] - (P[10][18]*q0)*0.5f;
|
||||
nextP[2][18] = P[2][18] + P[0][18]*SF[6] + P[1][18]*SF[10] + P[3][18]*SF[8] + P[12][18]*SF[14] - P[10][18]*SPP[10] - (P[11][18]*q0)*0.5f;
|
||||
nextP[3][18] = P[3][18] + P[0][18]*SF[7] + P[1][18]*SF[6] + P[2][18]*SF[9] + P[10][18]*SF[15] - P[11][18]*SF[14] - (P[12][18]*q0)*0.5f;
|
||||
nextP[4][18] = P[4][18] + P[0][18]*SF[5] + P[1][18]*SF[3] - P[3][18]*SF[4] + P[2][18]*SPP[0] + P[13][18]*SPP[3] + P[14][18]*SPP[6] - P[15][18]*SPP[9];
|
||||
nextP[5][18] = P[5][18] + P[0][18]*SF[4] + P[2][18]*SF[3] + P[3][18]*SF[5] - P[1][18]*SPP[0] - P[13][18]*SPP[8] + P[14][18]*SPP[2] + P[15][18]*SPP[5];
|
||||
nextP[6][18] = P[6][18] + P[1][18]*SF[4] - P[2][18]*SF[5] + P[3][18]*SF[3] + P[0][18]*SPP[0] + P[13][18]*SPP[4] - P[14][18]*SPP[7] - P[15][18]*SPP[1];
|
||||
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] + P[6][18]*dt;
|
||||
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] + P[1][19]*SF[9] + P[2][19]*SF[11] + P[3][19]*SF[10] + P[10][19]*SF[14] + P[11][19]*SF[15] + P[12][19]*SPP[10];
|
||||
nextP[1][19] = P[1][19] + P[0][19]*SF[8] + P[2][19]*SF[7] + P[3][19]*SF[11] - P[12][19]*SF[15] + P[11][19]*SPP[10] - (P[10][19]*q0)*0.5f;
|
||||
nextP[2][19] = P[2][19] + P[0][19]*SF[6] + P[1][19]*SF[10] + P[3][19]*SF[8] + P[12][19]*SF[14] - P[10][19]*SPP[10] - (P[11][19]*q0)*0.5f;
|
||||
nextP[3][19] = P[3][19] + P[0][19]*SF[7] + P[1][19]*SF[6] + P[2][19]*SF[9] + P[10][19]*SF[15] - P[11][19]*SF[14] - (P[12][19]*q0)*0.5f;
|
||||
nextP[4][19] = P[4][19] + P[0][19]*SF[5] + P[1][19]*SF[3] - P[3][19]*SF[4] + P[2][19]*SPP[0] + P[13][19]*SPP[3] + P[14][19]*SPP[6] - P[15][19]*SPP[9];
|
||||
nextP[5][19] = P[5][19] + P[0][19]*SF[4] + P[2][19]*SF[3] + P[3][19]*SF[5] - P[1][19]*SPP[0] - P[13][19]*SPP[8] + P[14][19]*SPP[2] + P[15][19]*SPP[5];
|
||||
nextP[6][19] = P[6][19] + P[1][19]*SF[4] - P[2][19]*SF[5] + P[3][19]*SF[3] + P[0][19]*SPP[0] + P[13][19]*SPP[4] - P[14][19]*SPP[7] - P[15][19]*SPP[1];
|
||||
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] + P[6][19]*dt;
|
||||
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] + P[1][20]*SF[9] + P[2][20]*SF[11] + P[3][20]*SF[10] + P[10][20]*SF[14] + P[11][20]*SF[15] + P[12][20]*SPP[10];
|
||||
nextP[1][20] = P[1][20] + P[0][20]*SF[8] + P[2][20]*SF[7] + P[3][20]*SF[11] - P[12][20]*SF[15] + P[11][20]*SPP[10] - (P[10][20]*q0)*0.5f;
|
||||
nextP[2][20] = P[2][20] + P[0][20]*SF[6] + P[1][20]*SF[10] + P[3][20]*SF[8] + P[12][20]*SF[14] - P[10][20]*SPP[10] - (P[11][20]*q0)*0.5f;
|
||||
nextP[3][20] = P[3][20] + P[0][20]*SF[7] + P[1][20]*SF[6] + P[2][20]*SF[9] + P[10][20]*SF[15] - P[11][20]*SF[14] - (P[12][20]*q0)*0.5f;
|
||||
nextP[4][20] = P[4][20] + P[0][20]*SF[5] + P[1][20]*SF[3] - P[3][20]*SF[4] + P[2][20]*SPP[0] + P[13][20]*SPP[3] + P[14][20]*SPP[6] - P[15][20]*SPP[9];
|
||||
nextP[5][20] = P[5][20] + P[0][20]*SF[4] + P[2][20]*SF[3] + P[3][20]*SF[5] - P[1][20]*SPP[0] - P[13][20]*SPP[8] + P[14][20]*SPP[2] + P[15][20]*SPP[5];
|
||||
nextP[6][20] = P[6][20] + P[1][20]*SF[4] - P[2][20]*SF[5] + P[3][20]*SF[3] + P[0][20]*SPP[0] + P[13][20]*SPP[4] - P[14][20]*SPP[7] - P[15][20]*SPP[1];
|
||||
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] + P[6][20]*dt;
|
||||
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] + P[1][21]*SF[9] + P[2][21]*SF[11] + P[3][21]*SF[10] + P[10][21]*SF[14] + P[11][21]*SF[15] + P[12][21]*SPP[10];
|
||||
nextP[1][21] = P[1][21] + P[0][21]*SF[8] + P[2][21]*SF[7] + P[3][21]*SF[11] - P[12][21]*SF[15] + P[11][21]*SPP[10] - (P[10][21]*q0)*0.5f;
|
||||
nextP[2][21] = P[2][21] + P[0][21]*SF[6] + P[1][21]*SF[10] + P[3][21]*SF[8] + P[12][21]*SF[14] - P[10][21]*SPP[10] - (P[11][21]*q0)*0.5f;
|
||||
nextP[3][21] = P[3][21] + P[0][21]*SF[7] + P[1][21]*SF[6] + P[2][21]*SF[9] + P[10][21]*SF[15] - P[11][21]*SF[14] - (P[12][21]*q0)*0.5f;
|
||||
nextP[4][21] = P[4][21] + P[0][21]*SF[5] + P[1][21]*SF[3] - P[3][21]*SF[4] + P[2][21]*SPP[0] + P[13][21]*SPP[3] + P[14][21]*SPP[6] - P[15][21]*SPP[9];
|
||||
nextP[5][21] = P[5][21] + P[0][21]*SF[4] + P[2][21]*SF[3] + P[3][21]*SF[5] - P[1][21]*SPP[0] - P[13][21]*SPP[8] + P[14][21]*SPP[2] + P[15][21]*SPP[5];
|
||||
nextP[6][21] = P[6][21] + P[1][21]*SF[4] - P[2][21]*SF[5] + P[3][21]*SF[3] + P[0][21]*SPP[0] + P[13][21]*SPP[4] - P[14][21]*SPP[7] - P[15][21]*SPP[1];
|
||||
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] + P[6][21]*dt;
|
||||
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] + P[1][22]*SF[9] + P[2][22]*SF[11] + P[3][22]*SF[10] + P[10][22]*SF[14] + P[11][22]*SF[15] + P[12][22]*SPP[10];
|
||||
nextP[1][22] = P[1][22] + P[0][22]*SF[8] + P[2][22]*SF[7] + P[3][22]*SF[11] - P[12][22]*SF[15] + P[11][22]*SPP[10] - (P[10][22]*q0)*0.5f;
|
||||
nextP[2][22] = P[2][22] + P[0][22]*SF[6] + P[1][22]*SF[10] + P[3][22]*SF[8] + P[12][22]*SF[14] - P[10][22]*SPP[10] - (P[11][22]*q0)*0.5f;
|
||||
nextP[3][22] = P[3][22] + P[0][22]*SF[7] + P[1][22]*SF[6] + P[2][22]*SF[9] + P[10][22]*SF[15] - P[11][22]*SF[14] - (P[12][22]*q0)*0.5f;
|
||||
nextP[4][22] = P[4][22] + P[0][22]*SF[5] + P[1][22]*SF[3] - P[3][22]*SF[4] + P[2][22]*SPP[0] + P[13][22]*SPP[3] + P[14][22]*SPP[6] - P[15][22]*SPP[9];
|
||||
nextP[5][22] = P[5][22] + P[0][22]*SF[4] + P[2][22]*SF[3] + P[3][22]*SF[5] - P[1][22]*SPP[0] - P[13][22]*SPP[8] + P[14][22]*SPP[2] + P[15][22]*SPP[5];
|
||||
nextP[6][22] = P[6][22] + P[1][22]*SF[4] - P[2][22]*SF[5] + P[3][22]*SF[3] + P[0][22]*SPP[0] + P[13][22]*SPP[4] - P[14][22]*SPP[7] - P[15][22]*SPP[1];
|
||||
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] + P[6][22]*dt;
|
||||
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] + P[1][23]*SF[9] + P[2][23]*SF[11] + P[3][23]*SF[10] + P[10][23]*SF[14] + P[11][23]*SF[15] + P[12][23]*SPP[10];
|
||||
nextP[1][23] = P[1][23] + P[0][23]*SF[8] + P[2][23]*SF[7] + P[3][23]*SF[11] - P[12][23]*SF[15] + P[11][23]*SPP[10] - (P[10][23]*q0)*0.5f;
|
||||
nextP[2][23] = P[2][23] + P[0][23]*SF[6] + P[1][23]*SF[10] + P[3][23]*SF[8] + P[12][23]*SF[14] - P[10][23]*SPP[10] - (P[11][23]*q0)*0.5f;
|
||||
nextP[3][23] = P[3][23] + P[0][23]*SF[7] + P[1][23]*SF[6] + P[2][23]*SF[9] + P[10][23]*SF[15] - P[11][23]*SF[14] - (P[12][23]*q0)*0.5f;
|
||||
nextP[4][23] = P[4][23] + P[0][23]*SF[5] + P[1][23]*SF[3] - P[3][23]*SF[4] + P[2][23]*SPP[0] + P[13][23]*SPP[3] + P[14][23]*SPP[6] - P[15][23]*SPP[9];
|
||||
nextP[5][23] = P[5][23] + P[0][23]*SF[4] + P[2][23]*SF[3] + P[3][23]*SF[5] - P[1][23]*SPP[0] - P[13][23]*SPP[8] + P[14][23]*SPP[2] + P[15][23]*SPP[5];
|
||||
nextP[6][23] = P[6][23] + P[1][23]*SF[4] - P[2][23]*SF[5] + P[3][23]*SF[3] + P[0][23]*SPP[0] + P[13][23]*SPP[4] - P[14][23]*SPP[7] - P[15][23]*SPP[1];
|
||||
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] + P[6][23]*dt;
|
||||
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];
|
||||
|
|
@ -1,67 +0,0 @@
|
|||
/*
|
||||
Autocode for fusion of a magnetic declination estimate where the innovation is given by
|
||||
|
||||
innovation = atanf(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.
|
||||
|
||||
Divide by zero protection and protection against a badly conditioned covariance matrix must be included.
|
||||
*/
|
||||
|
||||
// Calculate intermediate variable
|
||||
float t2 = magE*magE;
|
||||
float t3 = magN*magN;
|
||||
float t4 = t2+t3;
|
||||
float t5 = P[16][16]*t2;
|
||||
float t6 = P[17][17]*t3;
|
||||
float t7 = t2*t2;
|
||||
float t8 = R_DECL*t7;
|
||||
float t9 = t3*t3;
|
||||
float t10 = R_DECL*t9;
|
||||
float t11 = R_DECL*t2*t3*2.0f;
|
||||
float t14 = P[16][17]*magE*magN;
|
||||
float t15 = P[17][16]*magE*magN;
|
||||
float t12 = t5+t6+t8+t10+t11-t14-t15;
|
||||
float t13 = 1.0f / t12;
|
||||
float t16 = magE;
|
||||
float t17 = magN;
|
||||
float t18 = t16*t16;
|
||||
float t19 = t17*t17;
|
||||
float t20 = t18+t19;
|
||||
float t21 = 1.0f/t20;
|
||||
|
||||
// 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
|
||||
H_DECL[16] = -t16*t21;
|
||||
H_DECL[17] = t17*t21;
|
||||
|
||||
// Calculate the Kalman gains
|
||||
Kfusion[0] = -t4*t13*(P[0][16]*magE-P[0][17]*magN);
|
||||
Kfusion[1] = -t4*t13*(P[1][16]*magE-P[1][17]*magN);
|
||||
Kfusion[2] = -t4*t13*(P[2][16]*magE-P[2][17]*magN);
|
||||
Kfusion[3] = -t4*t13*(P[3][16]*magE-P[3][17]*magN);
|
||||
Kfusion[4] = -t4*t13*(P[4][16]*magE-P[4][17]*magN);
|
||||
Kfusion[5] = -t4*t13*(P[5][16]*magE-P[5][17]*magN);
|
||||
Kfusion[6] = -t4*t13*(P[6][16]*magE-P[6][17]*magN);
|
||||
Kfusion[7] = -t4*t13*(P[7][16]*magE-P[7][17]*magN);
|
||||
Kfusion[8] = -t4*t13*(P[8][16]*magE-P[8][17]*magN);
|
||||
Kfusion[9] = -t4*t13*(P[9][16]*magE-P[9][17]*magN);
|
||||
Kfusion[10] = -t4*t13*(P[10][16]*magE-P[10][17]*magN);
|
||||
Kfusion[11] = -t4*t13*(P[11][16]*magE-P[11][17]*magN);
|
||||
Kfusion[12] = -t4*t13*(P[12][16]*magE-P[12][17]*magN);
|
||||
Kfusion[13] = -t4*t13*(P[13][16]*magE-P[13][17]*magN);
|
||||
Kfusion[14] = -t4*t13*(P[14][16]*magE-P[14][17]*magN);
|
||||
Kfusion[15] = -t4*t13*(P[15][16]*magE-P[15][17]*magN);
|
||||
Kfusion[16] = -t4*t13*(P[16][16]*magE-P[16][17]*magN);
|
||||
Kfusion[17] = -t4*t13*(P[17][16]*magE-P[17][17]*magN);
|
||||
Kfusion[18] = -t4*t13*(P[18][16]*magE-P[18][17]*magN);
|
||||
Kfusion[19] = -t4*t13*(P[19][16]*magE-P[19][17]*magN);
|
||||
Kfusion[20] = -t4*t13*(P[20][16]*magE-P[20][17]*magN);
|
||||
Kfusion[21] = -t4*t13*(P[21][16]*magE-P[21][17]*magN);
|
||||
Kfusion[22] = -t4*t13*(P[22][16]*magE-P[22][17]*magN);
|
||||
Kfusion[23] = -t4*t13*(P[23][16]*magE-P[23][17]*magN);
|
||||
|
|
@ -1,142 +0,0 @@
|
|||
// Auto code for fusion of XYZ magnetometer vector measurement
|
||||
// Sequential fusion of each axis is used (assumes uncorrrelated observation errors)
|
||||
|
||||
// common expressions for XYZ axis observation jacobians
|
||||
SH_MAG[0] = 2.0f*magD*q3 + 2.0f*magE*q2 + 2.0f*magN*q1;
|
||||
SH_MAG[1] = 2.0f*magD*q0 - 2.0f*magE*q1 + 2.0f*magN*q2;
|
||||
SH_MAG[2] = 2.0f*magD*q1 + 2.0f*magE*q0 - 2.0f*magN*q3;
|
||||
SH_MAG[3] = sq(q3);
|
||||
SH_MAG[4] = sq(q2);
|
||||
SH_MAG[5] = sq(q1);
|
||||
SH_MAG[6] = sq(q0);
|
||||
SH_MAG[7] = 2.0f*magN*q0;
|
||||
SH_MAG[8] = 2.0f*magE*q3;
|
||||
|
||||
// X axis observation jacobians
|
||||
H_MAG[0] = SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2;
|
||||
H_MAG[1] = SH_MAG[0];
|
||||
H_MAG[2] = -SH_MAG[1];
|
||||
H_MAG[3] = SH_MAG[2];
|
||||
H_MAG[16] = SH_MAG[5] - SH_MAG[4] - SH_MAG[3] + SH_MAG[6];
|
||||
H_MAG[17] = 2.0f*q0*q3 + 2.0f*q1*q2;
|
||||
H_MAG[18] = 2.0f*q1*q3 - 2.0f*q0*q2;
|
||||
H_MAG[19] = 1.0f;
|
||||
|
||||
// common expressions for X axis Kalman gains
|
||||
SK_MX[0] = 1.0f/(P[19][19] + R_MAG + P[1][19]*SH_MAG[0] - P[2][19]*SH_MAG[1] + P[3][19]*SH_MAG[2] - P[16][19]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + (2*q0*q3 + 2.0f*q1*q2)*(P[19][17] + P[1][17]*SH_MAG[0] - P[2][17]*SH_MAG[1] + P[3][17]*SH_MAG[2] - P[16][17]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][17]*(2*q0*q3 + 2.0f*q1*q2) - P[18][17]*(2*q0*q2 - 2.0f*q1*q3) + P[0][17]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (2*q0*q2 - 2.0f*q1*q3)*(P[19][18] + P[1][18]*SH_MAG[0] - P[2][18]*SH_MAG[1] + P[3][18]*SH_MAG[2] - P[16][18]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][18]*(2*q0*q3 + 2.0f*q1*q2) - P[18][18]*(2*q0*q2 - 2.0f*q1*q3) + P[0][18]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)*(P[19][0] + P[1][0]*SH_MAG[0] - P[2][0]*SH_MAG[1] + P[3][0]*SH_MAG[2] - P[16][0]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][0]*(2*q0*q3 + 2.0f*q1*q2) - P[18][0]*(2*q0*q2 - 2.0f*q1*q3) + P[0][0]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P[17][19]*(2*q0*q3 + 2.0f*q1*q2) - P[18][19]*(2*q0*q2 - 2.0f*q1*q3) + SH_MAG[0]*(P[19][1] + P[1][1]*SH_MAG[0] - P[2][1]*SH_MAG[1] + P[3][1]*SH_MAG[2] - P[16][1]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][1]*(2*q0*q3 + 2.0f*q1*q2) - P[18][1]*(2*q0*q2 - 2.0f*q1*q3) + P[0][1]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - SH_MAG[1]*(P[19][2] + P[1][2]*SH_MAG[0] - P[2][2]*SH_MAG[1] + P[3][2]*SH_MAG[2] - P[16][2]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][2]*(2*q0*q3 + 2.0f*q1*q2) - P[18][2]*(2*q0*q2 - 2.0f*q1*q3) + P[0][2]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + SH_MAG[2]*(P[19][3] + P[1][3]*SH_MAG[0] - P[2][3]*SH_MAG[1] + P[3][3]*SH_MAG[2] - P[16][3]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][3]*(2*q0*q3 + 2.0f*q1*q2) - P[18][3]*(2*q0*q2 - 2.0f*q1*q3) + P[0][3]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P[19][16] + P[1][16]*SH_MAG[0] - P[2][16]*SH_MAG[1] + P[3][16]*SH_MAG[2] - P[16][16]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][16]*(2*q0*q3 + 2.0f*q1*q2) - P[18][16]*(2*q0*q2 - 2.0f*q1*q3) + P[0][16]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P[0][19]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2));
|
||||
SK_MX[1] = SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6];
|
||||
SK_MX[2] = SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2;
|
||||
SK_MX[3] = 2.0f*q0*q2 - 2.0f*q1*q3;
|
||||
SK_MX[4] = 2.0f*q0*q3 + 2.0f*q1*q2;
|
||||
|
||||
// X axis Kalman gains
|
||||
Kfusion[0] = SK_MX[0]*(P[0][19] + P[0][1]*SH_MAG[0] - P[0][2]*SH_MAG[1] + P[0][3]*SH_MAG[2] + P[0][0]*SK_MX[2] - P[0][16]*SK_MX[1] + P[0][17]*SK_MX[4] - P[0][18]*SK_MX[3]);
|
||||
Kfusion[1] = SK_MX[0]*(P[1][19] + P[1][1]*SH_MAG[0] - P[1][2]*SH_MAG[1] + P[1][3]*SH_MAG[2] + P[1][0]*SK_MX[2] - P[1][16]*SK_MX[1] + P[1][17]*SK_MX[4] - P[1][18]*SK_MX[3]);
|
||||
Kfusion[2] = SK_MX[0]*(P[2][19] + P[2][1]*SH_MAG[0] - P[2][2]*SH_MAG[1] + P[2][3]*SH_MAG[2] + P[2][0]*SK_MX[2] - P[2][16]*SK_MX[1] + P[2][17]*SK_MX[4] - P[2][18]*SK_MX[3]);
|
||||
Kfusion[3] = SK_MX[0]*(P[3][19] + P[3][1]*SH_MAG[0] - P[3][2]*SH_MAG[1] + P[3][3]*SH_MAG[2] + P[3][0]*SK_MX[2] - P[3][16]*SK_MX[1] + P[3][17]*SK_MX[4] - P[3][18]*SK_MX[3]);
|
||||
Kfusion[4] = SK_MX[0]*(P[4][19] + P[4][1]*SH_MAG[0] - P[4][2]*SH_MAG[1] + P[4][3]*SH_MAG[2] + P[4][0]*SK_MX[2] - P[4][16]*SK_MX[1] + P[4][17]*SK_MX[4] - P[4][18]*SK_MX[3]);
|
||||
Kfusion[5] = SK_MX[0]*(P[5][19] + P[5][1]*SH_MAG[0] - P[5][2]*SH_MAG[1] + P[5][3]*SH_MAG[2] + P[5][0]*SK_MX[2] - P[5][16]*SK_MX[1] + P[5][17]*SK_MX[4] - P[5][18]*SK_MX[3]);
|
||||
Kfusion[6] = SK_MX[0]*(P[6][19] + P[6][1]*SH_MAG[0] - P[6][2]*SH_MAG[1] + P[6][3]*SH_MAG[2] + P[6][0]*SK_MX[2] - P[6][16]*SK_MX[1] + P[6][17]*SK_MX[4] - P[6][18]*SK_MX[3]);
|
||||
Kfusion[7] = SK_MX[0]*(P[7][19] + P[7][1]*SH_MAG[0] - P[7][2]*SH_MAG[1] + P[7][3]*SH_MAG[2] + P[7][0]*SK_MX[2] - P[7][16]*SK_MX[1] + P[7][17]*SK_MX[4] - P[7][18]*SK_MX[3]);
|
||||
Kfusion[8] = SK_MX[0]*(P[8][19] + P[8][1]*SH_MAG[0] - P[8][2]*SH_MAG[1] + P[8][3]*SH_MAG[2] + P[8][0]*SK_MX[2] - P[8][16]*SK_MX[1] + P[8][17]*SK_MX[4] - P[8][18]*SK_MX[3]);
|
||||
Kfusion[9] = SK_MX[0]*(P[9][19] + P[9][1]*SH_MAG[0] - P[9][2]*SH_MAG[1] + P[9][3]*SH_MAG[2] + P[9][0]*SK_MX[2] - P[9][16]*SK_MX[1] + P[9][17]*SK_MX[4] - P[9][18]*SK_MX[3]);
|
||||
Kfusion[10] = SK_MX[0]*(P[10][19] + P[10][1]*SH_MAG[0] - P[10][2]*SH_MAG[1] + P[10][3]*SH_MAG[2] + P[10][0]*SK_MX[2] - P[10][16]*SK_MX[1] + P[10][17]*SK_MX[4] - P[10][18]*SK_MX[3]);
|
||||
Kfusion[11] = SK_MX[0]*(P[11][19] + P[11][1]*SH_MAG[0] - P[11][2]*SH_MAG[1] + P[11][3]*SH_MAG[2] + P[11][0]*SK_MX[2] - P[11][16]*SK_MX[1] + P[11][17]*SK_MX[4] - P[11][18]*SK_MX[3]);
|
||||
Kfusion[12] = SK_MX[0]*(P[12][19] + P[12][1]*SH_MAG[0] - P[12][2]*SH_MAG[1] + P[12][3]*SH_MAG[2] + P[12][0]*SK_MX[2] - P[12][16]*SK_MX[1] + P[12][17]*SK_MX[4] - P[12][18]*SK_MX[3]);
|
||||
Kfusion[13] = SK_MX[0]*(P[13][19] + P[13][1]*SH_MAG[0] - P[13][2]*SH_MAG[1] + P[13][3]*SH_MAG[2] + P[13][0]*SK_MX[2] - P[13][16]*SK_MX[1] + P[13][17]*SK_MX[4] - P[13][18]*SK_MX[3]);
|
||||
Kfusion[14] = SK_MX[0]*(P[14][19] + P[14][1]*SH_MAG[0] - P[14][2]*SH_MAG[1] + P[14][3]*SH_MAG[2] + P[14][0]*SK_MX[2] - P[14][16]*SK_MX[1] + P[14][17]*SK_MX[4] - P[14][18]*SK_MX[3]);
|
||||
Kfusion[15] = SK_MX[0]*(P[15][19] + P[15][1]*SH_MAG[0] - P[15][2]*SH_MAG[1] + P[15][3]*SH_MAG[2] + P[15][0]*SK_MX[2] - P[15][16]*SK_MX[1] + P[15][17]*SK_MX[4] - P[15][18]*SK_MX[3]);
|
||||
Kfusion[16] = SK_MX[0]*(P[16][19] + P[16][1]*SH_MAG[0] - P[16][2]*SH_MAG[1] + P[16][3]*SH_MAG[2] + P[16][0]*SK_MX[2] - P[16][16]*SK_MX[1] + P[16][17]*SK_MX[4] - P[16][18]*SK_MX[3]);
|
||||
Kfusion[17] = SK_MX[0]*(P[17][19] + P[17][1]*SH_MAG[0] - P[17][2]*SH_MAG[1] + P[17][3]*SH_MAG[2] + P[17][0]*SK_MX[2] - P[17][16]*SK_MX[1] + P[17][17]*SK_MX[4] - P[17][18]*SK_MX[3]);
|
||||
Kfusion[18] = SK_MX[0]*(P[18][19] + P[18][1]*SH_MAG[0] - P[18][2]*SH_MAG[1] + P[18][3]*SH_MAG[2] + P[18][0]*SK_MX[2] - P[18][16]*SK_MX[1] + P[18][17]*SK_MX[4] - P[18][18]*SK_MX[3]);
|
||||
Kfusion[19] = SK_MX[0]*(P[19][19] + P[19][1]*SH_MAG[0] - P[19][2]*SH_MAG[1] + P[19][3]*SH_MAG[2] + P[19][0]*SK_MX[2] - P[19][16]*SK_MX[1] + P[19][17]*SK_MX[4] - P[19][18]*SK_MX[3]);
|
||||
Kfusion[20] = SK_MX[0]*(P[20][19] + P[20][1]*SH_MAG[0] - P[20][2]*SH_MAG[1] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[2] - P[20][16]*SK_MX[1] + P[20][17]*SK_MX[4] - P[20][18]*SK_MX[3]);
|
||||
Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][1]*SH_MAG[0] - P[21][2]*SH_MAG[1] + P[21][3]*SH_MAG[2] + P[21][0]*SK_MX[2] - P[21][16]*SK_MX[1] + P[21][17]*SK_MX[4] - P[21][18]*SK_MX[3]);
|
||||
Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] - P[22][2]*SH_MAG[1] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[4] - P[22][18]*SK_MX[3]);
|
||||
Kfusion[23] = SK_MX[0]*(P[23][19] + P[23][1]*SH_MAG[0] - P[23][2]*SH_MAG[1] + P[23][3]*SH_MAG[2] + P[23][0]*SK_MX[2] - P[23][16]*SK_MX[1] + P[23][17]*SK_MX[4] - P[23][18]*SK_MX[3]);
|
||||
|
||||
// Y axis observation Jacobians
|
||||
H_MAG[0] = SH_MAG[2];
|
||||
H_MAG[1] = SH_MAG[1];
|
||||
H_MAG[2] = SH_MAG[0];
|
||||
H_MAG[3] = 2.0f*magD*q2 - SH_MAG[8] - SH_MAG[7];
|
||||
H_MAG[16] = 2.0f*q1*q2 - 2.0f*q0*q3;
|
||||
H_MAG[17] = SH_MAG[4] - SH_MAG[3] - SH_MAG[5] + SH_MAG[6];
|
||||
H_MAG[18] = 2.0f*q0*q1 + 2.0f*q2*q3;
|
||||
H_MAG[20] = 1;
|
||||
|
||||
// Common expressions for Y axis Kalamn gains
|
||||
SK_MY[0] = 1.0f/(P[20][20] + R_MAG + P[0][20]*SH_MAG[2] + P[1][20]*SH_MAG[1] + P[2][20]*SH_MAG[0] - P[17][20]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - (2*q0*q3 - 2.0f*q1*q2)*(P[20][16] + P[0][16]*SH_MAG[2] + P[1][16]*SH_MAG[1] + P[2][16]*SH_MAG[0] - P[17][16]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][16]*(2*q0*q3 - 2.0f*q1*q2) + P[18][16]*(2*q0*q1 + 2.0f*q2*q3) - P[3][16]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + (2*q0*q1 + 2.0f*q2*q3)*(P[20][18] + P[0][18]*SH_MAG[2] + P[1][18]*SH_MAG[1] + P[2][18]*SH_MAG[0] - P[17][18]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][18]*(2*q0*q3 - 2.0f*q1*q2) + P[18][18]*(2*q0*q1 + 2.0f*q2*q3) - P[3][18]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)*(P[20][3] + P[0][3]*SH_MAG[2] + P[1][3]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[17][3]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][3]*(2*q0*q3 - 2.0f*q1*q2) + P[18][3]*(2*q0*q1 + 2.0f*q2*q3) - P[3][3]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - P[16][20]*(2*q0*q3 - 2.0f*q1*q2) + P[18][20]*(2*q0*q1 + 2.0f*q2*q3) + SH_MAG[2]*(P[20][0] + P[0][0]*SH_MAG[2] + P[1][0]*SH_MAG[1] + P[2][0]*SH_MAG[0] - P[17][0]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][0]*(2*q0*q3 - 2.0f*q1*q2) + P[18][0]*(2*q0*q1 + 2.0f*q2*q3) - P[3][0]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + SH_MAG[1]*(P[20][1] + P[0][1]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[2][1]*SH_MAG[0] - P[17][1]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][1]*(2*q0*q3 - 2.0f*q1*q2) + P[18][1]*(2*q0*q1 + 2.0f*q2*q3) - P[3][1]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + SH_MAG[0]*(P[20][2] + P[0][2]*SH_MAG[2] + P[1][2]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[17][2]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][2]*(2*q0*q3 - 2.0f*q1*q2) + P[18][2]*(2*q0*q1 + 2.0f*q2*q3) - P[3][2]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6])*(P[20][17] + P[0][17]*SH_MAG[2] + P[1][17]*SH_MAG[1] + P[2][17]*SH_MAG[0] - P[17][17]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][17]*(2*q0*q3 - 2.0f*q1*q2) + P[18][17]*(2*q0*q1 + 2.0f*q2*q3) - P[3][17]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - P[3][20]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2));
|
||||
SK_MY[1] = SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6];
|
||||
SK_MY[2] = SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2;
|
||||
SK_MY[3] = 2.0f*q0*q3 - 2.0f*q1*q2;
|
||||
SK_MY[4] = 2.0f*q0*q1 + 2.0f*q2*q3;
|
||||
|
||||
// Y axis Kalmna gains
|
||||
Kfusion[0] = SK_MY[0]*(P[0][20] + P[0][0]*SH_MAG[2] + P[0][1]*SH_MAG[1] + P[0][2]*SH_MAG[0] - P[0][3]*SK_MY[2] - P[0][17]*SK_MY[1] - P[0][16]*SK_MY[3] + P[0][18]*SK_MY[4]);
|
||||
Kfusion[1] = SK_MY[0]*(P[1][20] + P[1][0]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[1][2]*SH_MAG[0] - P[1][3]*SK_MY[2] - P[1][17]*SK_MY[1] - P[1][16]*SK_MY[3] + P[1][18]*SK_MY[4]);
|
||||
Kfusion[2] = SK_MY[0]*(P[2][20] + P[2][0]*SH_MAG[2] + P[2][1]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[2][3]*SK_MY[2] - P[2][17]*SK_MY[1] - P[2][16]*SK_MY[3] + P[2][18]*SK_MY[4]);
|
||||
Kfusion[3] = SK_MY[0]*(P[3][20] + P[3][0]*SH_MAG[2] + P[3][1]*SH_MAG[1] + P[3][2]*SH_MAG[0] - P[3][3]*SK_MY[2] - P[3][17]*SK_MY[1] - P[3][16]*SK_MY[3] + P[3][18]*SK_MY[4]);
|
||||
Kfusion[4] = SK_MY[0]*(P[4][20] + P[4][0]*SH_MAG[2] + P[4][1]*SH_MAG[1] + P[4][2]*SH_MAG[0] - P[4][3]*SK_MY[2] - P[4][17]*SK_MY[1] - P[4][16]*SK_MY[3] + P[4][18]*SK_MY[4]);
|
||||
Kfusion[5] = SK_MY[0]*(P[5][20] + P[5][0]*SH_MAG[2] + P[5][1]*SH_MAG[1] + P[5][2]*SH_MAG[0] - P[5][3]*SK_MY[2] - P[5][17]*SK_MY[1] - P[5][16]*SK_MY[3] + P[5][18]*SK_MY[4]);
|
||||
Kfusion[6] = SK_MY[0]*(P[6][20] + P[6][0]*SH_MAG[2] + P[6][1]*SH_MAG[1] + P[6][2]*SH_MAG[0] - P[6][3]*SK_MY[2] - P[6][17]*SK_MY[1] - P[6][16]*SK_MY[3] + P[6][18]*SK_MY[4]);
|
||||
Kfusion[7] = SK_MY[0]*(P[7][20] + P[7][0]*SH_MAG[2] + P[7][1]*SH_MAG[1] + P[7][2]*SH_MAG[0] - P[7][3]*SK_MY[2] - P[7][17]*SK_MY[1] - P[7][16]*SK_MY[3] + P[7][18]*SK_MY[4]);
|
||||
Kfusion[8] = SK_MY[0]*(P[8][20] + P[8][0]*SH_MAG[2] + P[8][1]*SH_MAG[1] + P[8][2]*SH_MAG[0] - P[8][3]*SK_MY[2] - P[8][17]*SK_MY[1] - P[8][16]*SK_MY[3] + P[8][18]*SK_MY[4]);
|
||||
Kfusion[9] = SK_MY[0]*(P[9][20] + P[9][0]*SH_MAG[2] + P[9][1]*SH_MAG[1] + P[9][2]*SH_MAG[0] - P[9][3]*SK_MY[2] - P[9][17]*SK_MY[1] - P[9][16]*SK_MY[3] + P[9][18]*SK_MY[4]);
|
||||
Kfusion[10] = SK_MY[0]*(P[10][20] + P[10][0]*SH_MAG[2] + P[10][1]*SH_MAG[1] + P[10][2]*SH_MAG[0] - P[10][3]*SK_MY[2] - P[10][17]*SK_MY[1] - P[10][16]*SK_MY[3] + P[10][18]*SK_MY[4]);
|
||||
Kfusion[11] = SK_MY[0]*(P[11][20] + P[11][0]*SH_MAG[2] + P[11][1]*SH_MAG[1] + P[11][2]*SH_MAG[0] - P[11][3]*SK_MY[2] - P[11][17]*SK_MY[1] - P[11][16]*SK_MY[3] + P[11][18]*SK_MY[4]);
|
||||
Kfusion[12] = SK_MY[0]*(P[12][20] + P[12][0]*SH_MAG[2] + P[12][1]*SH_MAG[1] + P[12][2]*SH_MAG[0] - P[12][3]*SK_MY[2] - P[12][17]*SK_MY[1] - P[12][16]*SK_MY[3] + P[12][18]*SK_MY[4]);
|
||||
Kfusion[13] = SK_MY[0]*(P[13][20] + P[13][0]*SH_MAG[2] + P[13][1]*SH_MAG[1] + P[13][2]*SH_MAG[0] - P[13][3]*SK_MY[2] - P[13][17]*SK_MY[1] - P[13][16]*SK_MY[3] + P[13][18]*SK_MY[4]);
|
||||
Kfusion[14] = SK_MY[0]*(P[14][20] + P[14][0]*SH_MAG[2] + P[14][1]*SH_MAG[1] + P[14][2]*SH_MAG[0] - P[14][3]*SK_MY[2] - P[14][17]*SK_MY[1] - P[14][16]*SK_MY[3] + P[14][18]*SK_MY[4]);
|
||||
Kfusion[15] = SK_MY[0]*(P[15][20] + P[15][0]*SH_MAG[2] + P[15][1]*SH_MAG[1] + P[15][2]*SH_MAG[0] - P[15][3]*SK_MY[2] - P[15][17]*SK_MY[1] - P[15][16]*SK_MY[3] + P[15][18]*SK_MY[4]);
|
||||
Kfusion[16] = SK_MY[0]*(P[16][20] + P[16][0]*SH_MAG[2] + P[16][1]*SH_MAG[1] + P[16][2]*SH_MAG[0] - P[16][3]*SK_MY[2] - P[16][17]*SK_MY[1] - P[16][16]*SK_MY[3] + P[16][18]*SK_MY[4]);
|
||||
Kfusion[17] = SK_MY[0]*(P[17][20] + P[17][0]*SH_MAG[2] + P[17][1]*SH_MAG[1] + P[17][2]*SH_MAG[0] - P[17][3]*SK_MY[2] - P[17][17]*SK_MY[1] - P[17][16]*SK_MY[3] + P[17][18]*SK_MY[4]);
|
||||
Kfusion[18] = SK_MY[0]*(P[18][20] + P[18][0]*SH_MAG[2] + P[18][1]*SH_MAG[1] + P[18][2]*SH_MAG[0] - P[18][3]*SK_MY[2] - P[18][17]*SK_MY[1] - P[18][16]*SK_MY[3] + P[18][18]*SK_MY[4]);
|
||||
Kfusion[19] = SK_MY[0]*(P[19][20] + P[19][0]*SH_MAG[2] + P[19][1]*SH_MAG[1] + P[19][2]*SH_MAG[0] - P[19][3]*SK_MY[2] - P[19][17]*SK_MY[1] - P[19][16]*SK_MY[3] + P[19][18]*SK_MY[4]);
|
||||
Kfusion[20] = SK_MY[0]*(P[20][20] + P[20][0]*SH_MAG[2] + P[20][1]*SH_MAG[1] + P[20][2]*SH_MAG[0] - P[20][3]*SK_MY[2] - P[20][17]*SK_MY[1] - P[20][16]*SK_MY[3] + P[20][18]*SK_MY[4]);
|
||||
Kfusion[21] = SK_MY[0]*(P[21][20] + P[21][0]*SH_MAG[2] + P[21][1]*SH_MAG[1] + P[21][2]*SH_MAG[0] - P[21][3]*SK_MY[2] - P[21][17]*SK_MY[1] - P[21][16]*SK_MY[3] + P[21][18]*SK_MY[4]);
|
||||
Kfusion[22] = SK_MY[0]*(P[22][20] + P[22][0]*SH_MAG[2] + P[22][1]*SH_MAG[1] + P[22][2]*SH_MAG[0] - P[22][3]*SK_MY[2] - P[22][17]*SK_MY[1] - P[22][16]*SK_MY[3] + P[22][18]*SK_MY[4]);
|
||||
Kfusion[23] = SK_MY[0]*(P[23][20] + P[23][0]*SH_MAG[2] + P[23][1]*SH_MAG[1] + P[23][2]*SH_MAG[0] - P[23][3]*SK_MY[2] - P[23][17]*SK_MY[1] - P[23][16]*SK_MY[3] + P[23][18]*SK_MY[4]);
|
||||
|
||||
// Z axis observation Jacobians
|
||||
H_MAG[0] = SH_MAG[1];
|
||||
H_MAG[1] = -SH_MAG[2];
|
||||
H_MAG[2] = SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2;
|
||||
H_MAG[3] = SH_MAG[0];
|
||||
H_MAG[16] = 2.0f*q0*q2 + 2.0f*q1*q3;
|
||||
H_MAG[17] = 2.0f*q2*q3 - 2.0f*q0*q1;
|
||||
H_MAG[18] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6];
|
||||
H_MAG[21] = 1;
|
||||
|
||||
// Common expressions for Z axis Kalman gains
|
||||
SK_MZ[0] = 1.0f/(P[21][21] + R_MAG + P[0][21]*SH_MAG[1] - P[1][21]*SH_MAG[2] + P[3][21]*SH_MAG[0] + P[18][21]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + (2*q0*q2 + 2.0f*q1*q3)*(P[21][16] + P[0][16]*SH_MAG[1] - P[1][16]*SH_MAG[2] + P[3][16]*SH_MAG[0] + P[18][16]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][16]*(2*q0*q2 + 2.0f*q1*q3) - P[17][16]*(2*q0*q1 - 2.0f*q2*q3) + P[2][16]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (2*q0*q1 - 2.0f*q2*q3)*(P[21][17] + P[0][17]*SH_MAG[1] - P[1][17]*SH_MAG[2] + P[3][17]*SH_MAG[0] + P[18][17]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][17]*(2*q0*q2 + 2.0f*q1*q3) - P[17][17]*(2*q0*q1 - 2.0f*q2*q3) + P[2][17]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)*(P[21][2] + P[0][2]*SH_MAG[1] - P[1][2]*SH_MAG[2] + P[3][2]*SH_MAG[0] + P[18][2]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][2]*(2*q0*q2 + 2.0f*q1*q3) - P[17][2]*(2*q0*q1 - 2.0f*q2*q3) + P[2][2]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P[16][21]*(2*q0*q2 + 2.0f*q1*q3) - P[17][21]*(2*q0*q1 - 2.0f*q2*q3) + SH_MAG[1]*(P[21][0] + P[0][0]*SH_MAG[1] - P[1][0]*SH_MAG[2] + P[3][0]*SH_MAG[0] + P[18][0]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][0]*(2*q0*q2 + 2.0f*q1*q3) - P[17][0]*(2*q0*q1 - 2.0f*q2*q3) + P[2][0]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - SH_MAG[2]*(P[21][1] + P[0][1]*SH_MAG[1] - P[1][1]*SH_MAG[2] + P[3][1]*SH_MAG[0] + P[18][1]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][1]*(2*q0*q2 + 2.0f*q1*q3) - P[17][1]*(2*q0*q1 - 2.0f*q2*q3) + P[2][1]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + SH_MAG[0]*(P[21][3] + P[0][3]*SH_MAG[1] - P[1][3]*SH_MAG[2] + P[3][3]*SH_MAG[0] + P[18][3]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][3]*(2*q0*q2 + 2.0f*q1*q3) - P[17][3]*(2*q0*q1 - 2.0f*q2*q3) + P[2][3]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6])*(P[21][18] + P[0][18]*SH_MAG[1] - P[1][18]*SH_MAG[2] + P[3][18]*SH_MAG[0] + P[18][18]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][18]*(2*q0*q2 + 2.0f*q1*q3) - P[17][18]*(2*q0*q1 - 2.0f*q2*q3) + P[2][18]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P[2][21]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2));
|
||||
SK_MZ[1] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6];
|
||||
SK_MZ[2] = SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2;
|
||||
SK_MZ[3] = 2.0f*q0*q1 - 2.0f*q2*q3;
|
||||
SK_MZ[4] = 2.0f*q0*q2 + 2.0f*q1*q3;
|
||||
|
||||
// Z axis Kalman gains
|
||||
Kfusion[0] = SK_MZ[0]*(P[0][21] + P[0][0]*SH_MAG[1] - P[0][1]*SH_MAG[2] + P[0][3]*SH_MAG[0] + P[0][2]*SK_MZ[2] + P[0][18]*SK_MZ[1] + P[0][16]*SK_MZ[4] - P[0][17]*SK_MZ[3]);
|
||||
Kfusion[1] = SK_MZ[0]*(P[1][21] + P[1][0]*SH_MAG[1] - P[1][1]*SH_MAG[2] + P[1][3]*SH_MAG[0] + P[1][2]*SK_MZ[2] + P[1][18]*SK_MZ[1] + P[1][16]*SK_MZ[4] - P[1][17]*SK_MZ[3]);
|
||||
Kfusion[2] = SK_MZ[0]*(P[2][21] + P[2][0]*SH_MAG[1] - P[2][1]*SH_MAG[2] + P[2][3]*SH_MAG[0] + P[2][2]*SK_MZ[2] + P[2][18]*SK_MZ[1] + P[2][16]*SK_MZ[4] - P[2][17]*SK_MZ[3]);
|
||||
Kfusion[3] = SK_MZ[0]*(P[3][21] + P[3][0]*SH_MAG[1] - P[3][1]*SH_MAG[2] + P[3][3]*SH_MAG[0] + P[3][2]*SK_MZ[2] + P[3][18]*SK_MZ[1] + P[3][16]*SK_MZ[4] - P[3][17]*SK_MZ[3]);
|
||||
Kfusion[4] = SK_MZ[0]*(P[4][21] + P[4][0]*SH_MAG[1] - P[4][1]*SH_MAG[2] + P[4][3]*SH_MAG[0] + P[4][2]*SK_MZ[2] + P[4][18]*SK_MZ[1] + P[4][16]*SK_MZ[4] - P[4][17]*SK_MZ[3]);
|
||||
Kfusion[5] = SK_MZ[0]*(P[5][21] + P[5][0]*SH_MAG[1] - P[5][1]*SH_MAG[2] + P[5][3]*SH_MAG[0] + P[5][2]*SK_MZ[2] + P[5][18]*SK_MZ[1] + P[5][16]*SK_MZ[4] - P[5][17]*SK_MZ[3]);
|
||||
Kfusion[6] = SK_MZ[0]*(P[6][21] + P[6][0]*SH_MAG[1] - P[6][1]*SH_MAG[2] + P[6][3]*SH_MAG[0] + P[6][2]*SK_MZ[2] + P[6][18]*SK_MZ[1] + P[6][16]*SK_MZ[4] - P[6][17]*SK_MZ[3]);
|
||||
Kfusion[7] = SK_MZ[0]*(P[7][21] + P[7][0]*SH_MAG[1] - P[7][1]*SH_MAG[2] + P[7][3]*SH_MAG[0] + P[7][2]*SK_MZ[2] + P[7][18]*SK_MZ[1] + P[7][16]*SK_MZ[4] - P[7][17]*SK_MZ[3]);
|
||||
Kfusion[8] = SK_MZ[0]*(P[8][21] + P[8][0]*SH_MAG[1] - P[8][1]*SH_MAG[2] + P[8][3]*SH_MAG[0] + P[8][2]*SK_MZ[2] + P[8][18]*SK_MZ[1] + P[8][16]*SK_MZ[4] - P[8][17]*SK_MZ[3]);
|
||||
Kfusion[9] = SK_MZ[0]*(P[9][21] + P[9][0]*SH_MAG[1] - P[9][1]*SH_MAG[2] + P[9][3]*SH_MAG[0] + P[9][2]*SK_MZ[2] + P[9][18]*SK_MZ[1] + P[9][16]*SK_MZ[4] - P[9][17]*SK_MZ[3]);
|
||||
Kfusion[10] = SK_MZ[0]*(P[10][21] + P[10][0]*SH_MAG[1] - P[10][1]*SH_MAG[2] + P[10][3]*SH_MAG[0] + P[10][2]*SK_MZ[2] + P[10][18]*SK_MZ[1] + P[10][16]*SK_MZ[4] - P[10][17]*SK_MZ[3]);
|
||||
Kfusion[11] = SK_MZ[0]*(P[11][21] + P[11][0]*SH_MAG[1] - P[11][1]*SH_MAG[2] + P[11][3]*SH_MAG[0] + P[11][2]*SK_MZ[2] + P[11][18]*SK_MZ[1] + P[11][16]*SK_MZ[4] - P[11][17]*SK_MZ[3]);
|
||||
Kfusion[12] = SK_MZ[0]*(P[12][21] + P[12][0]*SH_MAG[1] - P[12][1]*SH_MAG[2] + P[12][3]*SH_MAG[0] + P[12][2]*SK_MZ[2] + P[12][18]*SK_MZ[1] + P[12][16]*SK_MZ[4] - P[12][17]*SK_MZ[3]);
|
||||
Kfusion[13] = SK_MZ[0]*(P[13][21] + P[13][0]*SH_MAG[1] - P[13][1]*SH_MAG[2] + P[13][3]*SH_MAG[0] + P[13][2]*SK_MZ[2] + P[13][18]*SK_MZ[1] + P[13][16]*SK_MZ[4] - P[13][17]*SK_MZ[3]);
|
||||
Kfusion[14] = SK_MZ[0]*(P[14][21] + P[14][0]*SH_MAG[1] - P[14][1]*SH_MAG[2] + P[14][3]*SH_MAG[0] + P[14][2]*SK_MZ[2] + P[14][18]*SK_MZ[1] + P[14][16]*SK_MZ[4] - P[14][17]*SK_MZ[3]);
|
||||
Kfusion[15] = SK_MZ[0]*(P[15][21] + P[15][0]*SH_MAG[1] - P[15][1]*SH_MAG[2] + P[15][3]*SH_MAG[0] + P[15][2]*SK_MZ[2] + P[15][18]*SK_MZ[1] + P[15][16]*SK_MZ[4] - P[15][17]*SK_MZ[3]);
|
||||
Kfusion[16] = SK_MZ[0]*(P[16][21] + P[16][0]*SH_MAG[1] - P[16][1]*SH_MAG[2] + P[16][3]*SH_MAG[0] + P[16][2]*SK_MZ[2] + P[16][18]*SK_MZ[1] + P[16][16]*SK_MZ[4] - P[16][17]*SK_MZ[3]);
|
||||
Kfusion[17] = SK_MZ[0]*(P[17][21] + P[17][0]*SH_MAG[1] - P[17][1]*SH_MAG[2] + P[17][3]*SH_MAG[0] + P[17][2]*SK_MZ[2] + P[17][18]*SK_MZ[1] + P[17][16]*SK_MZ[4] - P[17][17]*SK_MZ[3]);
|
||||
Kfusion[18] = SK_MZ[0]*(P[18][21] + P[18][0]*SH_MAG[1] - P[18][1]*SH_MAG[2] + P[18][3]*SH_MAG[0] + P[18][2]*SK_MZ[2] + P[18][18]*SK_MZ[1] + P[18][16]*SK_MZ[4] - P[18][17]*SK_MZ[3]);
|
||||
Kfusion[19] = SK_MZ[0]*(P[19][21] + P[19][0]*SH_MAG[1] - P[19][1]*SH_MAG[2] + P[19][3]*SH_MAG[0] + P[19][2]*SK_MZ[2] + P[19][18]*SK_MZ[1] + P[19][16]*SK_MZ[4] - P[19][17]*SK_MZ[3]);
|
||||
Kfusion[20] = SK_MZ[0]*(P[20][21] + P[20][0]*SH_MAG[1] - P[20][1]*SH_MAG[2] + P[20][3]*SH_MAG[0] + P[20][2]*SK_MZ[2] + P[20][18]*SK_MZ[1] + P[20][16]*SK_MZ[4] - P[20][17]*SK_MZ[3]);
|
||||
Kfusion[21] = SK_MZ[0]*(P[21][21] + P[21][0]*SH_MAG[1] - P[21][1]*SH_MAG[2] + P[21][3]*SH_MAG[0] + P[21][2]*SK_MZ[2] + P[21][18]*SK_MZ[1] + P[21][16]*SK_MZ[4] - P[21][17]*SK_MZ[3]);
|
||||
Kfusion[22] = SK_MZ[0]*(P[22][21] + P[22][0]*SH_MAG[1] - P[22][1]*SH_MAG[2] + P[22][3]*SH_MAG[0] + P[22][2]*SK_MZ[2] + P[22][18]*SK_MZ[1] + P[22][16]*SK_MZ[4] - P[22][17]*SK_MZ[3]);
|
||||
Kfusion[23] = SK_MZ[0]*(P[23][21] + P[23][0]*SH_MAG[1] - P[23][1]*SH_MAG[2] + P[23][3]*SH_MAG[0] + P[23][2]*SK_MZ[2] + P[23][18]*SK_MZ[1] + P[23][16]*SK_MZ[4] - P[23][17]*SK_MZ[3]);
|
||||
|
|
@ -1,22 +0,0 @@
|
|||
NOTES FOR FUSION OF MEASUREMENTS USING AUTOCODE FRAGMENTS
|
||||
|
||||
The auto-code for the fusion of the various measurements provides in most cases the observation Jacobian and Kalman gain matrix for that observation.
|
||||
Where no Kalman gain is provided, it will need to be calculated using the usual K = P*transpose(H)/(H*P*transpose(H) + R) where:
|
||||
|
||||
K = Kalman Gain matrix
|
||||
H = observation partial derivative matrix (observaton Jacobian)
|
||||
R = observation variance
|
||||
(H*P*transpose(H) + R) is the innovation variance (always a scalar)
|
||||
|
||||
When the observation is a vector, it is always assumed that the errors in the vectors are uncorelated to each other and the observations are fused
|
||||
sequentially, so no matrix inversion is required.
|
||||
|
||||
It is important that maximum useage of the sparsity in the H matrix be taken advantage of. If a sparse math library is available it could be used,
|
||||
otherwise the matrix operations should need unrolled with inclusion of conditional statements to improve efficiency. Examples of this technique can
|
||||
be found in the existing att_pos_ekf_estimator library
|
||||
|
||||
NOTES FOR COVARIANCE PREDICTION
|
||||
|
||||
Only expression for the upper diagonal is provided. The values will need to be copied across to the lower diagnal elements assuming symmetry
|
||||
Process noise for time invariant states, eg Gyro bias, has not been included in the auto-code. The process noise variance for these states will
|
||||
need to be added in a separate operation.
|
||||
|
|
@ -1,267 +0,0 @@
|
|||
// Auto code for fusion of line of sight rate massurements from a optical flow camera aligned with the Z body axis
|
||||
// Conversion from Matlab symbolic toolbox objects to c-code made using custom scripts and auto-coder from the InertialNav repo
|
||||
// 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)
|
||||
|
||||
float H_LOS[2][24]; // Optical flow observation Jacobians
|
||||
float Kfusion[24][2]; // Optical flow Kalman gains
|
||||
|
||||
// calculate X axis observation Jacobian
|
||||
float t2 = 1.0f / range;
|
||||
H_LOS[0][0] = t2*(q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f);
|
||||
H_LOS[0][1] = t2*(q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f);
|
||||
H_LOS[0][2] = t2*(q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f);
|
||||
H_LOS[0][3] = -t2*(q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f);
|
||||
H_LOS[0][4] = -t2*(q0*q3*2.0f-q1*q2*2.0f);
|
||||
H_LOS[0][5] = t2*(q0*q0-q1*q1+q2*q2-q3*q3);
|
||||
H_LOS[0][6] = t2*(q0*q1*2.0f+q2*q3*2.0f);
|
||||
|
||||
// calculate intermediate variables for the X observaton innovatoin variance and Kalman gains
|
||||
float t3 = q1*vd*2.0f;
|
||||
float t4 = q0*ve*2.0f;
|
||||
float t11 = q3*vn*2.0f;
|
||||
float t5 = t3+t4-t11;
|
||||
float t6 = q0*q3*2.0f;
|
||||
float t29 = q1*q2*2.0f;
|
||||
float t7 = t6-t29;
|
||||
float t8 = q0*q1*2.0f;
|
||||
float t9 = q2*q3*2.0f;
|
||||
float t10 = t8+t9;
|
||||
float t12 = P[0][0]*t2*t5;
|
||||
float t13 = q0*vd*2.0f;
|
||||
float t14 = q2*vn*2.0f;
|
||||
float t28 = q1*ve*2.0f;
|
||||
float t15 = t13+t14-t28;
|
||||
float t16 = q3*vd*2.0f;
|
||||
float t17 = q2*ve*2.0f;
|
||||
float t18 = q1*vn*2.0f;
|
||||
float t19 = t16+t17+t18;
|
||||
float t20 = q3*ve*2.0f;
|
||||
float t21 = q0*vn*2.0f;
|
||||
float t30 = q2*vd*2.0f;
|
||||
float t22 = t20+t21-t30;
|
||||
float t23 = q0*q0;
|
||||
float t24 = q1*q1;
|
||||
float t25 = q2*q2;
|
||||
float t26 = q3*q3;
|
||||
float t27 = t23-t24+t25-t26;
|
||||
float t31 = P[1][1]*t2*t15;
|
||||
float t32 = P[6][0]*t2*t10;
|
||||
float t33 = P[1][0]*t2*t15;
|
||||
float t34 = P[2][0]*t2*t19;
|
||||
float t35 = P[5][0]*t2*t27;
|
||||
float t79 = P[4][0]*t2*t7;
|
||||
float t80 = P[3][0]*t2*t22;
|
||||
float t36 = t12+t32+t33+t34+t35-t79-t80;
|
||||
float t37 = t2*t5*t36;
|
||||
float t38 = P[6][1]*t2*t10;
|
||||
float t39 = P[0][1]*t2*t5;
|
||||
float t40 = P[2][1]*t2*t19;
|
||||
float t41 = P[5][1]*t2*t27;
|
||||
float t81 = P[4][1]*t2*t7;
|
||||
float t82 = P[3][1]*t2*t22;
|
||||
float t42 = t31+t38+t39+t40+t41-t81-t82;
|
||||
float t43 = t2*t15*t42;
|
||||
float t44 = P[6][2]*t2*t10;
|
||||
float t45 = P[0][2]*t2*t5;
|
||||
float t46 = P[1][2]*t2*t15;
|
||||
float t47 = P[2][2]*t2*t19;
|
||||
float t48 = P[5][2]*t2*t27;
|
||||
float t83 = P[4][2]*t2*t7;
|
||||
float t84 = P[3][2]*t2*t22;
|
||||
float t49 = t44+t45+t46+t47+t48-t83-t84;
|
||||
float t50 = t2*t19*t49;
|
||||
float t51 = P[6][3]*t2*t10;
|
||||
float t52 = P[0][3]*t2*t5;
|
||||
float t53 = P[1][3]*t2*t15;
|
||||
float t54 = P[2][3]*t2*t19;
|
||||
float t55 = P[5][3]*t2*t27;
|
||||
float t85 = P[4][3]*t2*t7;
|
||||
float t86 = P[3][3]*t2*t22;
|
||||
float t56 = t51+t52+t53+t54+t55-t85-t86;
|
||||
float t57 = P[6][5]*t2*t10;
|
||||
float t58 = P[0][5]*t2*t5;
|
||||
float t59 = P[1][5]*t2*t15;
|
||||
float t60 = P[2][5]*t2*t19;
|
||||
float t61 = P[5][5]*t2*t27;
|
||||
float t88 = P[4][5]*t2*t7;
|
||||
float t89 = P[3][5]*t2*t22;
|
||||
float t62 = t57+t58+t59+t60+t61-t88-t89;
|
||||
float t63 = t2*t27*t62;
|
||||
float t64 = P[6][4]*t2*t10;
|
||||
float t65 = P[0][4]*t2*t5;
|
||||
float t66 = P[1][4]*t2*t15;
|
||||
float t67 = P[2][4]*t2*t19;
|
||||
float t68 = P[5][4]*t2*t27;
|
||||
float t90 = P[4][4]*t2*t7;
|
||||
float t91 = P[3][4]*t2*t22;
|
||||
float t69 = t64+t65+t66+t67+t68-t90-t91;
|
||||
float t70 = P[6][6]*t2*t10;
|
||||
float t71 = P[0][6]*t2*t5;
|
||||
float t72 = P[1][6]*t2*t15;
|
||||
float t73 = P[2][6]*t2*t19;
|
||||
float t74 = P[5][6]*t2*t27;
|
||||
float t93 = P[4][6]*t2*t7;
|
||||
float t94 = P[3][6]*t2*t22;
|
||||
float t75 = t70+t71+t72+t73+t74-t93-t94;
|
||||
float t76 = t2*t10*t75;
|
||||
float t87 = t2*t22*t56;
|
||||
float t92 = t2*t7*t69;
|
||||
float t77 = R_LOS+t37+t43+t50+t63+t76-t87-t92;
|
||||
float t78 = 1.0f / t77;
|
||||
|
||||
// calculate Kalman gains for X-axis observation
|
||||
Kfusion[0][0] = t78*(t12-P[0][4]*t2*t7+P[0][1]*t2*t15+P[0][6]*t2*t10+P[0][2]*t2*t19-P[0][3]*t2*t22+P[0][5]*t2*t27);
|
||||
Kfusion[1][0] = t78*(t31+P[1][0]*t2*t5-P[1][4]*t2*t7+P[1][6]*t2*t10+P[1][2]*t2*t19-P[1][3]*t2*t22+P[1][5]*t2*t27);
|
||||
Kfusion[2][0] = t78*(t47+P[2][0]*t2*t5-P[2][4]*t2*t7+P[2][1]*t2*t15+P[2][6]*t2*t10-P[2][3]*t2*t22+P[2][5]*t2*t27);
|
||||
Kfusion[3][0] = t78*(-t86+P[3][0]*t2*t5-P[3][4]*t2*t7+P[3][1]*t2*t15+P[3][6]*t2*t10+P[3][2]*t2*t19+P[3][5]*t2*t27);
|
||||
Kfusion[4][0] = t78*(-t90+P[4][0]*t2*t5+P[4][1]*t2*t15+P[4][6]*t2*t10+P[4][2]*t2*t19-P[4][3]*t2*t22+P[4][5]*t2*t27);
|
||||
Kfusion[5][0] = t78*(t61+P[5][0]*t2*t5-P[5][4]*t2*t7+P[5][1]*t2*t15+P[5][6]*t2*t10+P[5][2]*t2*t19-P[5][3]*t2*t22);
|
||||
Kfusion[6][0] = t78*(t70+P[6][0]*t2*t5-P[6][4]*t2*t7+P[6][1]*t2*t15+P[6][2]*t2*t19-P[6][3]*t2*t22+P[6][5]*t2*t27);
|
||||
Kfusion[7][0] = t78*(P[7][0]*t2*t5-P[7][4]*t2*t7+P[7][1]*t2*t15+P[7][6]*t2*t10+P[7][2]*t2*t19-P[7][3]*t2*t22+P[7][5]*t2*t27);
|
||||
Kfusion[8][0] = t78*(P[8][0]*t2*t5-P[8][4]*t2*t7+P[8][1]*t2*t15+P[8][6]*t2*t10+P[8][2]*t2*t19-P[8][3]*t2*t22+P[8][5]*t2*t27);
|
||||
Kfusion[9][0] = t78*(P[9][0]*t2*t5-P[9][4]*t2*t7+P[9][1]*t2*t15+P[9][6]*t2*t10+P[9][2]*t2*t19-P[9][3]*t2*t22+P[9][5]*t2*t27);
|
||||
Kfusion[10][0] = t78*(P[10][0]*t2*t5-P[10][4]*t2*t7+P[10][1]*t2*t15+P[10][6]*t2*t10+P[10][2]*t2*t19-P[10][3]*t2*t22+P[10][5]*t2*t27);
|
||||
Kfusion[11][0] = t78*(P[11][0]*t2*t5-P[11][4]*t2*t7+P[11][1]*t2*t15+P[11][6]*t2*t10+P[11][2]*t2*t19-P[11][3]*t2*t22+P[11][5]*t2*t27);
|
||||
Kfusion[12][0] = t78*(P[12][0]*t2*t5-P[12][4]*t2*t7+P[12][1]*t2*t15+P[12][6]*t2*t10+P[12][2]*t2*t19-P[12][3]*t2*t22+P[12][5]*t2*t27);
|
||||
Kfusion[13][0] = t78*(P[13][0]*t2*t5-P[13][4]*t2*t7+P[13][1]*t2*t15+P[13][6]*t2*t10+P[13][2]*t2*t19-P[13][3]*t2*t22+P[13][5]*t2*t27);
|
||||
Kfusion[14][0] = t78*(P[14][0]*t2*t5-P[14][4]*t2*t7+P[14][1]*t2*t15+P[14][6]*t2*t10+P[14][2]*t2*t19-P[14][3]*t2*t22+P[14][5]*t2*t27);
|
||||
Kfusion[15][0] = t78*(P[15][0]*t2*t5-P[15][4]*t2*t7+P[15][1]*t2*t15+P[15][6]*t2*t10+P[15][2]*t2*t19-P[15][3]*t2*t22+P[15][5]*t2*t27);
|
||||
Kfusion[16][0] = t78*(P[16][0]*t2*t5-P[16][4]*t2*t7+P[16][1]*t2*t15+P[16][6]*t2*t10+P[16][2]*t2*t19-P[16][3]*t2*t22+P[16][5]*t2*t27);
|
||||
Kfusion[17][0] = t78*(P[17][0]*t2*t5-P[17][4]*t2*t7+P[17][1]*t2*t15+P[17][6]*t2*t10+P[17][2]*t2*t19-P[17][3]*t2*t22+P[17][5]*t2*t27);
|
||||
Kfusion[18][0] = t78*(P[18][0]*t2*t5-P[18][4]*t2*t7+P[18][1]*t2*t15+P[18][6]*t2*t10+P[18][2]*t2*t19-P[18][3]*t2*t22+P[18][5]*t2*t27);
|
||||
Kfusion[19][0] = t78*(P[19][0]*t2*t5-P[19][4]*t2*t7+P[19][1]*t2*t15+P[19][6]*t2*t10+P[19][2]*t2*t19-P[19][3]*t2*t22+P[19][5]*t2*t27);
|
||||
Kfusion[20][0] = t78*(P[20][0]*t2*t5-P[20][4]*t2*t7+P[20][1]*t2*t15+P[20][6]*t2*t10+P[20][2]*t2*t19-P[20][3]*t2*t22+P[20][5]*t2*t27);
|
||||
Kfusion[21][0] = t78*(P[21][0]*t2*t5-P[21][4]*t2*t7+P[21][1]*t2*t15+P[21][6]*t2*t10+P[21][2]*t2*t19-P[21][3]*t2*t22+P[21][5]*t2*t27);
|
||||
Kfusion[22][0] = t78*(P[22][0]*t2*t5-P[22][4]*t2*t7+P[22][1]*t2*t15+P[22][6]*t2*t10+P[22][2]*t2*t19-P[22][3]*t2*t22+P[22][5]*t2*t27);
|
||||
Kfusion[23][0] = t78*(P[23][0]*t2*t5-P[23][4]*t2*t7+P[23][1]*t2*t15+P[23][6]*t2*t10+P[23][2]*t2*t19-P[23][3]*t2*t22+P[23][5]*t2*t27);
|
||||
|
||||
// calculate Y axis observation Jacobian
|
||||
float t2 = 1.0f / range;
|
||||
H_LOS[1][0] = -t2*(q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f);
|
||||
H_LOS[1][1] = -t2*(q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f);
|
||||
H_LOS[1][2] = t2*(q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f);
|
||||
H_LOS[1][3] = -t2*(q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f);
|
||||
H_LOS[1][4] = -t2*(q0*q0+q1*q1-q2*q2-q3*q3);
|
||||
H_LOS[1][5] = -t2*(q0*q3*2.0f+q1*q2*2.0f);
|
||||
H_LOS[1][6] = t2*(q0*q2*2.0f-q1*q3*2.0f);
|
||||
|
||||
// calculate intermediate variables for the Y axis observaton innovation variance and Kalman gains
|
||||
float t3 = q3*ve*2.0f;
|
||||
float t4 = q0*vn*2.0f;
|
||||
float t11 = q2*vd*2.0f;
|
||||
float t5 = t3+t4-t11;
|
||||
float t6 = q0*q3*2.0f;
|
||||
float t7 = q1*q2*2.0f;
|
||||
float t8 = t6+t7;
|
||||
float t9 = q0*q2*2.0f;
|
||||
float t28 = q1*q3*2.0f;
|
||||
float t10 = t9-t28;
|
||||
float t12 = P[0][0]*t2*t5;
|
||||
float t13 = q3*vd*2.0f;
|
||||
float t14 = q2*ve*2.0f;
|
||||
float t15 = q1*vn*2.0f;
|
||||
float t16 = t13+t14+t15;
|
||||
float t17 = q0*vd*2.0f;
|
||||
float t18 = q2*vn*2.0f;
|
||||
float t29 = q1*ve*2.0f;
|
||||
float t19 = t17+t18-t29;
|
||||
float t20 = q1*vd*2.0f;
|
||||
float t21 = q0*ve*2.0f;
|
||||
float t30 = q3*vn*2.0f;
|
||||
float t22 = t20+t21-t30;
|
||||
float t23 = q0*q0;
|
||||
float t24 = q1*q1;
|
||||
float t25 = q2*q2;
|
||||
float t26 = q3*q3;
|
||||
float t27 = t23+t24-t25-t26;
|
||||
float t31 = P[1][1]*t2*t16;
|
||||
float t32 = P[5][0]*t2*t8;
|
||||
float t33 = P[1][0]*t2*t16;
|
||||
float t34 = P[3][0]*t2*t22;
|
||||
float t35 = P[4][0]*t2*t27;
|
||||
float t80 = P[6][0]*t2*t10;
|
||||
float t81 = P[2][0]*t2*t19;
|
||||
float t36 = t12+t32+t33+t34+t35-t80-t81;
|
||||
float t37 = t2*t5*t36;
|
||||
float t38 = P[5][1]*t2*t8;
|
||||
float t39 = P[0][1]*t2*t5;
|
||||
float t40 = P[3][1]*t2*t22;
|
||||
float t41 = P[4][1]*t2*t27;
|
||||
float t82 = P[6][1]*t2*t10;
|
||||
float t83 = P[2][1]*t2*t19;
|
||||
float t42 = t31+t38+t39+t40+t41-t82-t83;
|
||||
float t43 = t2*t16*t42;
|
||||
float t44 = P[5][2]*t2*t8;
|
||||
float t45 = P[0][2]*t2*t5;
|
||||
float t46 = P[1][2]*t2*t16;
|
||||
float t47 = P[3][2]*t2*t22;
|
||||
float t48 = P[4][2]*t2*t27;
|
||||
float t79 = P[2][2]*t2*t19;
|
||||
float t84 = P[6][2]*t2*t10;
|
||||
float t49 = t44+t45+t46+t47+t48-t79-t84;
|
||||
float t50 = P[5][3]*t2*t8;
|
||||
float t51 = P[0][3]*t2*t5;
|
||||
float t52 = P[1][3]*t2*t16;
|
||||
float t53 = P[3][3]*t2*t22;
|
||||
float t54 = P[4][3]*t2*t27;
|
||||
float t86 = P[6][3]*t2*t10;
|
||||
float t87 = P[2][3]*t2*t19;
|
||||
float t55 = t50+t51+t52+t53+t54-t86-t87;
|
||||
float t56 = t2*t22*t55;
|
||||
float t57 = P[5][4]*t2*t8;
|
||||
float t58 = P[0][4]*t2*t5;
|
||||
float t59 = P[1][4]*t2*t16;
|
||||
float t60 = P[3][4]*t2*t22;
|
||||
float t61 = P[4][4]*t2*t27;
|
||||
float t88 = P[6][4]*t2*t10;
|
||||
float t89 = P[2][4]*t2*t19;
|
||||
float t62 = t57+t58+t59+t60+t61-t88-t89;
|
||||
float t63 = t2*t27*t62;
|
||||
float t64 = P[5][5]*t2*t8;
|
||||
float t65 = P[0][5]*t2*t5;
|
||||
float t66 = P[1][5]*t2*t16;
|
||||
float t67 = P[3][5]*t2*t22;
|
||||
float t68 = P[4][5]*t2*t27;
|
||||
float t90 = P[6][5]*t2*t10;
|
||||
float t91 = P[2][5]*t2*t19;
|
||||
float t69 = t64+t65+t66+t67+t68-t90-t91;
|
||||
float t70 = t2*t8*t69;
|
||||
float t71 = P[5][6]*t2*t8;
|
||||
float t72 = P[0][6]*t2*t5;
|
||||
float t73 = P[1][6]*t2*t16;
|
||||
float t74 = P[3][6]*t2*t22;
|
||||
float t75 = P[4][6]*t2*t27;
|
||||
float t92 = P[6][6]*t2*t10;
|
||||
float t93 = P[2][6]*t2*t19;
|
||||
float t76 = t71+t72+t73+t74+t75-t92-t93;
|
||||
float t85 = t2*t19*t49;
|
||||
float t94 = t2*t10*t76;
|
||||
float t77 = R_LOS+t37+t43+t56+t63+t70-t85-t94;
|
||||
float t78 = 1.0f / t77;
|
||||
|
||||
// calculate Kalman gains for Y-axis observation
|
||||
Kfusion[0][1] = -t78*(t12+P[0][5]*t2*t8-P[0][6]*t2*t10+P[0][1]*t2*t16-P[0][2]*t2*t19+P[0][3]*t2*t22+P[0][4]*t2*t27);
|
||||
Kfusion[1][1] = -t78*(t31+P[1][0]*t2*t5+P[1][5]*t2*t8-P[1][6]*t2*t10-P[1][2]*t2*t19+P[1][3]*t2*t22+P[1][4]*t2*t27);
|
||||
Kfusion[2][1] = -t78*(-t79+P[2][0]*t2*t5+P[2][5]*t2*t8-P[2][6]*t2*t10+P[2][1]*t2*t16+P[2][3]*t2*t22+P[2][4]*t2*t27);
|
||||
Kfusion[3][1] = -t78*(t53+P[3][0]*t2*t5+P[3][5]*t2*t8-P[3][6]*t2*t10+P[3][1]*t2*t16-P[3][2]*t2*t19+P[3][4]*t2*t27);
|
||||
Kfusion[4][1] = -t78*(t61+P[4][0]*t2*t5+P[4][5]*t2*t8-P[4][6]*t2*t10+P[4][1]*t2*t16-P[4][2]*t2*t19+P[4][3]*t2*t22);
|
||||
Kfusion[5][1] = -t78*(t64+P[5][0]*t2*t5-P[5][6]*t2*t10+P[5][1]*t2*t16-P[5][2]*t2*t19+P[5][3]*t2*t22+P[5][4]*t2*t27);
|
||||
Kfusion[6][1] = -t78*(-t92+P[6][0]*t2*t5+P[6][5]*t2*t8+P[6][1]*t2*t16-P[6][2]*t2*t19+P[6][3]*t2*t22+P[6][4]*t2*t27);
|
||||
Kfusion[7][1] = -t78*(P[7][0]*t2*t5+P[7][5]*t2*t8-P[7][6]*t2*t10+P[7][1]*t2*t16-P[7][2]*t2*t19+P[7][3]*t2*t22+P[7][4]*t2*t27);
|
||||
Kfusion[8][1] = -t78*(P[8][0]*t2*t5+P[8][5]*t2*t8-P[8][6]*t2*t10+P[8][1]*t2*t16-P[8][2]*t2*t19+P[8][3]*t2*t22+P[8][4]*t2*t27);
|
||||
Kfusion[9][1] = -t78*(P[9][0]*t2*t5+P[9][5]*t2*t8-P[9][6]*t2*t10+P[9][1]*t2*t16-P[9][2]*t2*t19+P[9][3]*t2*t22+P[9][4]*t2*t27);
|
||||
Kfusion[10][1] = -t78*(P[10][0]*t2*t5+P[10][5]*t2*t8-P[10][6]*t2*t10+P[10][1]*t2*t16-P[10][2]*t2*t19+P[10][3]*t2*t22+P[10][4]*t2*t27);
|
||||
Kfusion[11][1] = -t78*(P[11][0]*t2*t5+P[11][5]*t2*t8-P[11][6]*t2*t10+P[11][1]*t2*t16-P[11][2]*t2*t19+P[11][3]*t2*t22+P[11][4]*t2*t27);
|
||||
Kfusion[12][1] = -t78*(P[12][0]*t2*t5+P[12][5]*t2*t8-P[12][6]*t2*t10+P[12][1]*t2*t16-P[12][2]*t2*t19+P[12][3]*t2*t22+P[12][4]*t2*t27);
|
||||
Kfusion[13][1] = -t78*(P[13][0]*t2*t5+P[13][5]*t2*t8-P[13][6]*t2*t10+P[13][1]*t2*t16-P[13][2]*t2*t19+P[13][3]*t2*t22+P[13][4]*t2*t27);
|
||||
Kfusion[14][1] = -t78*(P[14][0]*t2*t5+P[14][5]*t2*t8-P[14][6]*t2*t10+P[14][1]*t2*t16-P[14][2]*t2*t19+P[14][3]*t2*t22+P[14][4]*t2*t27);
|
||||
Kfusion[15][1] = -t78*(P[15][0]*t2*t5+P[15][5]*t2*t8-P[15][6]*t2*t10+P[15][1]*t2*t16-P[15][2]*t2*t19+P[15][3]*t2*t22+P[15][4]*t2*t27);
|
||||
Kfusion[16][1] = -t78*(P[16][0]*t2*t5+P[16][5]*t2*t8-P[16][6]*t2*t10+P[16][1]*t2*t16-P[16][2]*t2*t19+P[16][3]*t2*t22+P[16][4]*t2*t27);
|
||||
Kfusion[17][1] = -t78*(P[17][0]*t2*t5+P[17][5]*t2*t8-P[17][6]*t2*t10+P[17][1]*t2*t16-P[17][2]*t2*t19+P[17][3]*t2*t22+P[17][4]*t2*t27);
|
||||
Kfusion[18][1] = -t78*(P[18][0]*t2*t5+P[18][5]*t2*t8-P[18][6]*t2*t10+P[18][1]*t2*t16-P[18][2]*t2*t19+P[18][3]*t2*t22+P[18][4]*t2*t27);
|
||||
Kfusion[19][1] = -t78*(P[19][0]*t2*t5+P[19][5]*t2*t8-P[19][6]*t2*t10+P[19][1]*t2*t16-P[19][2]*t2*t19+P[19][3]*t2*t22+P[19][4]*t2*t27);
|
||||
Kfusion[20][1] = -t78*(P[20][0]*t2*t5+P[20][5]*t2*t8-P[20][6]*t2*t10+P[20][1]*t2*t16-P[20][2]*t2*t19+P[20][3]*t2*t22+P[20][4]*t2*t27);
|
||||
Kfusion[21][1] = -t78*(P[21][0]*t2*t5+P[21][5]*t2*t8-P[21][6]*t2*t10+P[21][1]*t2*t16-P[21][2]*t2*t19+P[21][3]*t2*t22+P[21][4]*t2*t27);
|
||||
Kfusion[22][1] = -t78*(P[22][0]*t2*t5+P[22][5]*t2*t8-P[22][6]*t2*t10+P[22][1]*t2*t16-P[22][2]*t2*t19+P[22][3]*t2*t22+P[22][4]*t2*t27);
|
||||
Kfusion[23][1] = -t78*(P[23][0]*t2*t5+P[23][5]*t2*t8-P[23][6]*t2*t10+P[23][1]*t2*t16-P[23][2]*t2*t19+P[23][3]*t2*t22+P[23][4]*t2*t27);
|
||||
File diff suppressed because one or more lines are too long
|
|
@ -1,42 +0,0 @@
|
|||
/*
|
||||
Observation jacobian for fusion of the horizontal components of magnetometer measurements.
|
||||
|
||||
innovation = atan2(magE/magN) - declination, where magN and magE are the North and East components obtained
|
||||
by rotating the measured magnetometer readings from body into earth axes.
|
||||
|
||||
This method of fusion reduces roll and pitch errors due to external field disturbances and is suitable for initial alignment and ground / indoor use
|
||||
|
||||
Divide by zero protection hs been added
|
||||
*/
|
||||
|
||||
// calculate intermediate variables for observation jacobian
|
||||
float t9 = q0*q3;
|
||||
float t10 = q1*q2;
|
||||
float t2 = t9+t10;
|
||||
float t3 = q0*q0;
|
||||
float t4 = q1*q1;
|
||||
float t5 = q2*q2;
|
||||
float t6 = q3*q3;
|
||||
float t7 = t3+t4-t5-t6;
|
||||
float t8 = t7*t7;
|
||||
if (t8 > 1e-6f) {
|
||||
t8 = 1.0f/t8;
|
||||
} else {
|
||||
return;
|
||||
}
|
||||
float t11 = t2*t2;
|
||||
float t12 = t8*t11*4.0f;
|
||||
float t13 = t12+1.0f;
|
||||
float t14;
|
||||
if (fabsf(t13) > 1e-6f) {
|
||||
t14 = 1.0f/t13;
|
||||
} else {
|
||||
return;
|
||||
}
|
||||
|
||||
// calculate observation jacobian
|
||||
float H_DECL[4] = {};
|
||||
H_YAW[0] = t8*t14*(q3*t3-q3*t4+q3*t5+q3*t6+q0*q1*q2*2.0f)*-2.0f;
|
||||
H_YAW[1] = t8*t14*(-q2*t3+q2*t4+q2*t5+q2*t6+q0*q1*q3*2.0f)*-2.0f;
|
||||
H_YAW[2] = t8*t14*(q1*t3+q1*t4+q1*t5-q1*t6+q0*q2*q3*2.0f)*2.0f;
|
||||
H_YAW[3] = t8*t14*(q0*t3+q0*t4-q0*t5+q0*t6+q1*q2*q3*2.0f)*2.0f;
|
||||
|
|
@ -1,67 +0,0 @@
|
|||
/*
|
||||
Code fragments for fusion of an Euler yaw measurement from a 321 sequence.
|
||||
*/
|
||||
|
||||
// calculate observation jacobian when we are observing the first rotation in a 321 sequence
|
||||
float t9 = q0*q3;
|
||||
float t10 = q1*q2;
|
||||
float t2 = t9+t10;
|
||||
float t3 = q0*q0;
|
||||
float t4 = q1*q1;
|
||||
float t5 = q2*q2;
|
||||
float t6 = q3*q3;
|
||||
float t7 = t3+t4-t5-t6;
|
||||
float t8 = t7*t7;
|
||||
if (t8 > 1e-6f) {
|
||||
t8 = 1.0f/t8;
|
||||
} else {
|
||||
return;
|
||||
}
|
||||
float t11 = t2*t2;
|
||||
float t12 = t8*t11*4.0f;
|
||||
float t13 = t12+1.0f;
|
||||
float t14;
|
||||
if (fabsf(t13) > 1e-6f) {
|
||||
t14 = 1.0f/t13;
|
||||
} else {
|
||||
return;
|
||||
}
|
||||
|
||||
H_YAW[0] = t8*t14*(q3*t3-q3*t4+q3*t5+q3*t6+q0*q1*q2*2.0f)*-2.0f;
|
||||
H_YAW[1] = t8*t14*(-q2*t3+q2*t4+q2*t5+q2*t6+q0*q1*q3*2.0f)*-2.0f;
|
||||
H_YAW[2] = t8*t14*(q1*t3+q1*t4+q1*t5-q1*t6+q0*q2*q3*2.0f)*2.0f;
|
||||
H_YAW[3] = t8*t14*(q0*t3+q0*t4-q0*t5+q0*t6+q1*q2*q3*2.0f)*2.0f;
|
||||
|
||||
/*
|
||||
Code fragments for fusion of an Euler yaw measurement from a 312 sequence.
|
||||
*/
|
||||
|
||||
// calculate observaton jacobian when we are observing a rotation in a 312 sequence
|
||||
float t9 = q0*q3;
|
||||
float t10 = q1*q2;
|
||||
float t2 = t9-t10;
|
||||
float t3 = q0*q0;
|
||||
float t4 = q1*q1;
|
||||
float t5 = q2*q2;
|
||||
float t6 = q3*q3;
|
||||
float t7 = t3-t4+t5-t6;
|
||||
float t8 = t7*t7;
|
||||
if (t8 > 1e-6f) {
|
||||
t8 = 1.0f/t8;
|
||||
} else {
|
||||
return;
|
||||
}
|
||||
float t11 = t2*t2;
|
||||
float t12 = t8*t11*4.0f;
|
||||
float t13 = t12+1.0f;
|
||||
float t14;
|
||||
if (fabsf(t13) > 1e-6f) {
|
||||
t14 = 1.0f/t13;
|
||||
} else {
|
||||
return;
|
||||
}
|
||||
|
||||
H_YAW[0] = t8*t14*(q3*t3+q3*t4-q3*t5+q3*t6-q0*q1*q2*2.0f)*-2.0f;
|
||||
H_YAW[1] = t8*t14*(q2*t3+q2*t4+q2*t5-q2*t6-q0*q1*q3*2.0f)*-2.0f;
|
||||
H_YAW[2] = t8*t14*(-q1*t3+q1*t4+q1*t5+q1*t6-q0*q2*q3*2.0f)*2.0f;
|
||||
H_YAW[3] = t8*t14*(q0*t3-q0*t4+q0*t5+q0*t6-q1*q2*q3*2.0f)*2.0f;
|
||||
|
|
@ -1,5 +0,0 @@
|
|||
# Generated Code
|
||||
|
||||
These directories contains code generated using the derivation script files that has been 'cleaned up'.
|
||||
|
||||
Inertia Nav EKF: contains code generated by GenerateNavFilterEquations.m
|
||||
Binary file not shown.
File diff suppressed because one or more lines are too long
|
|
@ -1,35 +0,0 @@
|
|||
float SF[9][1];
|
||||
SF[0] = 0;
|
||||
SF[1] = 0;
|
||||
SF[2] = 0;
|
||||
SF[3] = -SF[2];
|
||||
SF[4] = SF[2];
|
||||
SF[5] = 0;
|
||||
SF[6] = 0;
|
||||
SF[7] = sq(q3);
|
||||
SF[8] = sq(q2);
|
||||
float SG[3][1];
|
||||
SG[0] = sq(q0) - sq(q1) - sq(q2) + sq(q3);
|
||||
SG[1] = 2*q0*q2 - 2*q1*q3;
|
||||
SG[2] = 2*q0*q1 + 2*q2*q3;
|
||||
float SQ[4][1];
|
||||
SQ[0] = (q1*SG[0])/2 - (q0*SG[2])/2 + (q3*SG[1])/2;
|
||||
SQ[1] = (q0*SG[1])/2 - (q2*SG[0])/2 + (q3*SG[2])/2;
|
||||
SQ[2] = (q3*SG[0])/2 - (q1*SG[1])/2 + (q2*SG[2])/2;
|
||||
SQ[3] = (q0*SG[0])/2 + (q1*SG[2])/2 + (q2*SG[1])/2;
|
||||
float SPP[4][1];
|
||||
SPP[0] = SF[5] - SF[0] + SF[6];
|
||||
SPP[1] = SF[0] - SF[5];
|
||||
SPP[2] = SF[0] + SF[5] + SF[6];
|
||||
SPP[3] = SF[1];
|
||||
float nextP[4][4];
|
||||
nextP[0][0] = P[0][0] + P[2][0]*SF[3] + P[1][0]*SPP[3] + P[3][0]*SPP[1] + SF[3]*(P[0][2] + P[2][2]*SF[3] + P[1][2]*SPP[3] + P[3][2]*SPP[1]) + SPP[3]*(P[0][1] + P[2][1]*SF[3] + P[1][1]*SPP[3] + P[3][1]*SPP[1]) + SPP[1]*(P[0][3] + P[2][3]*SF[3] + P[1][3]*SPP[3] + P[3][3]*SPP[1]) + daYawVar*sq(SQ[2]);
|
||||
nextP[0][1] = P[0][1] + P[2][1]*SF[3] + P[1][1]*SPP[3] + P[3][1]*SPP[1] - (SF[0] + SF[6])*(P[0][2] + P[2][2]*SF[3] + P[1][2]*SPP[3] + P[3][2]*SPP[1]) + SF[3]*(P[0][3] + P[2][3]*SF[3] + P[1][3]*SPP[3] + P[3][3]*SPP[1]) - SPP[3]*(P[0][0] + P[2][0]*SF[3] + P[1][0]*SPP[3] + P[3][0]*SPP[1]) + daYawVar*SQ[1]*SQ[2];
|
||||
nextP[1][1] = P[1][1] + P[3][1]*SF[3] - P[0][1]*SPP[3] + SF[3]*(P[1][3] + P[3][3]*SF[3] - P[0][3]*SPP[3] - P[2][3]*(SF[0] + SF[6])) - SPP[3]*(P[1][0] + P[3][0]*SF[3] - P[0][0]*SPP[3] - P[2][0]*(SF[0] + SF[6])) + daYawVar*sq(SQ[1]) - P[2][1]*(SF[0] + SF[6]) - (SF[0] + SF[6])*(P[1][2] + P[3][2]*SF[3] - P[0][2]*SPP[3] - P[2][2]*(SF[0] + SF[6]));
|
||||
nextP[0][2] = P[0][2] + P[2][2]*SF[3] + P[1][2]*SPP[3] + P[3][2]*SPP[1] + SF[4]*(P[0][0] + P[2][0]*SF[3] + P[1][0]*SPP[3] + P[3][0]*SPP[1]) + SPP[2]*(P[0][1] + P[2][1]*SF[3] + P[1][1]*SPP[3] + P[3][1]*SPP[1]) - SPP[3]*(P[0][3] + P[2][3]*SF[3] + P[1][3]*SPP[3] + P[3][3]*SPP[1]) + daYawVar*SQ[0]*SQ[2];
|
||||
nextP[1][2] = P[1][2] + P[3][2]*SF[3] - P[0][2]*SPP[3] + SF[4]*(P[1][0] + P[3][0]*SF[3] - P[0][0]*SPP[3] - P[2][0]*(SF[0] + SF[6])) + SPP[2]*(P[1][1] + P[3][1]*SF[3] - P[0][1]*SPP[3] - P[2][1]*(SF[0] + SF[6])) - SPP[3]*(P[1][3] + P[3][3]*SF[3] - P[0][3]*SPP[3] - P[2][3]*(SF[0] + SF[6])) - P[2][2]*(SF[0] + SF[6]) + daYawVar*SQ[0]*SQ[1];
|
||||
nextP[2][2] = P[2][2] + P[0][2]*SF[4] + P[1][2]*SPP[2] - P[3][2]*SPP[3] + SF[4]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SPP[2] - P[3][0]*SPP[3]) + SPP[2]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SPP[2] - P[3][1]*SPP[3]) - SPP[3]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SPP[2] - P[3][3]*SPP[3]) + daYawVar*sq(SQ[0]);
|
||||
nextP[0][3] = P[0][3] + P[2][3]*SF[3] + P[1][3]*SPP[3] + P[3][3]*SPP[1] + SF[4]*(P[0][1] + P[2][1]*SF[3] + P[1][1]*SPP[3] + P[3][1]*SPP[1]) + SPP[0]*(P[0][0] + P[2][0]*SF[3] + P[1][0]*SPP[3] + P[3][0]*SPP[1]) + SPP[3]*(P[0][2] + P[2][2]*SF[3] + P[1][2]*SPP[3] + P[3][2]*SPP[1]) - daYawVar*SQ[2]*SQ[3];
|
||||
nextP[1][3] = P[1][3] + P[3][3]*SF[3] - P[0][3]*SPP[3] + SF[4]*(P[1][1] + P[3][1]*SF[3] - P[0][1]*SPP[3] - P[2][1]*(SF[0] + SF[6])) + SPP[0]*(P[1][0] + P[3][0]*SF[3] - P[0][0]*SPP[3] - P[2][0]*(SF[0] + SF[6])) + SPP[3]*(P[1][2] + P[3][2]*SF[3] - P[0][2]*SPP[3] - P[2][2]*(SF[0] + SF[6])) - P[2][3]*(SF[0] + SF[6]) - daYawVar*SQ[1]*SQ[3];
|
||||
nextP[2][3] = P[2][3] + P[0][3]*SF[4] + P[1][3]*SPP[2] - P[3][3]*SPP[3] + SF[4]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SPP[2] - P[3][1]*SPP[3]) + SPP[0]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SPP[2] - P[3][0]*SPP[3]) + SPP[3]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SPP[2] - P[3][2]*SPP[3]) - daYawVar*SQ[0]*SQ[3];
|
||||
nextP[3][3] = P[3][3] + P[1][3]*SF[4] + P[0][3]*SPP[0] + P[2][3]*SPP[3] + SF[4]*(P[3][1] + P[1][1]*SF[4] + P[0][1]*SPP[0] + P[2][1]*SPP[3]) + SPP[0]*(P[3][0] + P[1][0]*SF[4] + P[0][0]*SPP[0] + P[2][0]*SPP[3]) + SPP[3]*(P[3][2] + P[1][2]*SF[4] + P[0][2]*SPP[0] + P[2][2]*SPP[3]) + daYawVar*sq(SQ[3]);
|
||||
File diff suppressed because one or more lines are too long
|
|
@ -1,58 +0,0 @@
|
|||
function ConvertCToC(nStates)
|
||||
|
||||
% This function converts the nextP and P covaraince matrix expressions from
|
||||
% P[row index][col index] to P(row index,col index) syntax to enable use
|
||||
% of the matrix library type used by PX4
|
||||
|
||||
%% Define file to read in
|
||||
fileName = strcat('C_code',int2str(nStates),'.txt');
|
||||
delimiter = '';
|
||||
|
||||
%% Format string for each line of text:
|
||||
% column1: text (%s)
|
||||
% For more information, see the TEXTSCAN documentation.
|
||||
formatSpec = '%s%[^\n\r]';
|
||||
|
||||
%% Open the text file.
|
||||
fileID = fopen(fileName,'r');
|
||||
|
||||
%% Read columns of data according to format string.
|
||||
% This call is based on the structure of the file used to generate this
|
||||
% code. If an error occurs for a different file, try regenerating the code
|
||||
% from the Import Tool.
|
||||
dataArray = textscan(fileID, formatSpec, 'Delimiter', delimiter, 'ReturnOnError', false);
|
||||
|
||||
%% Close the text file.
|
||||
fclose(fileID);
|
||||
|
||||
%% Create output variable
|
||||
SymbolicOutput = [dataArray{1:end-1}];
|
||||
|
||||
%% Clear temporary variables
|
||||
clearvars filename delimiter formatSpec fileID dataArray ans;
|
||||
|
||||
%% Replace [row][col] brackets for nextP and P arrays with (row,col)
|
||||
|
||||
% replace 2-D left indexes
|
||||
for rowIndex = 1:nStates
|
||||
for colIndex = 1:nStates
|
||||
strRowIndex = int2str(rowIndex-1);
|
||||
strColIndex = int2str(colIndex-1);
|
||||
strRep = sprintf('P(%d,%d)',(rowIndex-1),(colIndex-1));
|
||||
strPat = strcat('P\[',strRowIndex,'\]\[',strColIndex,'\]');
|
||||
for lineIndex = 1:length(SymbolicOutput)
|
||||
str = char(SymbolicOutput(lineIndex));
|
||||
SymbolicOutput(lineIndex) = {regexprep(str, strPat, strRep)};
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
%% Write to file
|
||||
fileName = strcat('C_code_use_matrix_lib',int2str(nStates),'.txt');
|
||||
fid = fopen(fileName,'wt');
|
||||
for lineIndex = 1:length(SymbolicOutput)
|
||||
fprintf(fid,char(SymbolicOutput(lineIndex)));
|
||||
fprintf(fid,'\n');
|
||||
end
|
||||
fclose(fid);
|
||||
clear all;
|
||||
|
|
@ -1,213 +0,0 @@
|
|||
function ConvertToC(nStates)
|
||||
%% Define file to read in
|
||||
fileName = strcat('M_code',int2str(nStates),'.txt');
|
||||
delimiter = '';
|
||||
|
||||
%% Format string for each line of text:
|
||||
% column1: text (%s)
|
||||
% For more information, see the TEXTSCAN documentation.
|
||||
formatSpec = '%s%[^\n\r]';
|
||||
|
||||
%% Open the text file.
|
||||
fileID = fopen(fileName,'r');
|
||||
|
||||
%% Read columns of data according to format string.
|
||||
% This call is based on the structure of the file used to generate this
|
||||
% code. If an error occurs for a different file, try regenerating the code
|
||||
% from the Import Tool.
|
||||
dataArray = textscan(fileID, formatSpec, 'Delimiter', delimiter, 'ReturnOnError', false);
|
||||
|
||||
%% Close the text file.
|
||||
fclose(fileID);
|
||||
|
||||
%% Create output variable
|
||||
SymbolicOutput = [dataArray{1:end-1}];
|
||||
|
||||
%% Clear temporary variables
|
||||
clearvars filename delimiter formatSpec fileID dataArray ans;
|
||||
|
||||
%% Convert indexing and replace brackets
|
||||
|
||||
% replace 1-D indexes
|
||||
for arrayIndex = 1:99
|
||||
strIndex = int2str(arrayIndex);
|
||||
strRep = sprintf('[%d]',(arrayIndex-1));
|
||||
strPat = strcat('\(',strIndex,'\)');
|
||||
for lineIndex = 1:length(SymbolicOutput)
|
||||
str = char(SymbolicOutput(lineIndex));
|
||||
SymbolicOutput(lineIndex) = {regexprep(str, strPat, strRep)};
|
||||
end
|
||||
end
|
||||
|
||||
% replace 2-D left indexes
|
||||
for arrayIndex = 1:99
|
||||
strIndex = int2str(arrayIndex);
|
||||
strRep = sprintf('[%d,',(arrayIndex-1));
|
||||
strPat = strcat('\(',strIndex,'\,');
|
||||
for lineIndex = 1:length(SymbolicOutput)
|
||||
str = char(SymbolicOutput(lineIndex));
|
||||
SymbolicOutput(lineIndex) = {regexprep(str, strPat, strRep)};
|
||||
end
|
||||
end
|
||||
|
||||
% replace 2-D right indexes
|
||||
for arrayIndex = 1:99
|
||||
strIndex = int2str(arrayIndex);
|
||||
strRep = sprintf(',%d]',(arrayIndex-1));
|
||||
strPat = strcat('\,',strIndex,'\)');
|
||||
for lineIndex = 1:length(SymbolicOutput)
|
||||
str = char(SymbolicOutput(lineIndex));
|
||||
SymbolicOutput(lineIndex) = {regexprep(str, strPat, strRep)};
|
||||
end
|
||||
end
|
||||
|
||||
% replace commas
|
||||
for lineIndex = 1:length(SymbolicOutput)
|
||||
str = char(SymbolicOutput(lineIndex));
|
||||
SymbolicOutput(lineIndex) = {regexprep(str, '\,', '][')};
|
||||
end
|
||||
|
||||
%% Replace ^2
|
||||
|
||||
% replace where adjacent to ) parenthesis
|
||||
for lineIndex = 1:length(SymbolicOutput)
|
||||
idxsq = regexp(SymbolicOutput(lineIndex),'\)\^2');
|
||||
if ~isempty(idxsq{1})
|
||||
strIn = SymbolicOutput(lineIndex);
|
||||
idxlp = regexp(strIn,'\(');
|
||||
idxrp = regexp(strIn,'\)');
|
||||
for pwrIndex = 1:length(idxsq{1})
|
||||
counter = 1;
|
||||
index = idxsq{1}(pwrIndex);
|
||||
endIndex(pwrIndex) = index;
|
||||
while (counter > 0 && index > 0)
|
||||
index = index - 1;
|
||||
counter = counter + ~isempty(find(idxrp{1} == index, 1));
|
||||
counter = counter - ~isempty(find(idxlp{1} == index, 1));
|
||||
end
|
||||
startIndex(pwrIndex) = index;
|
||||
% strPat = strcat(strIn{1}(startIndex(pwrIndex):endIndex(pwrIndex)),'^2');
|
||||
strRep = strcat('sq',strIn{1}(startIndex(pwrIndex):endIndex(pwrIndex)));
|
||||
% cellStrPat(pwrIndex) = cellstr(strPat);
|
||||
cellStrRep(pwrIndex) = cellstr(strRep);
|
||||
end
|
||||
for pwrIndex = 1:length(idxsq{1})
|
||||
strRep = char(cellStrRep(pwrIndex));
|
||||
strIn{1}(startIndex(pwrIndex):endIndex(pwrIndex)+2) = strRep;
|
||||
end
|
||||
SymbolicOutput(lineIndex) = strIn;
|
||||
end
|
||||
end
|
||||
|
||||
% replace where adjacent to ] parenthesis
|
||||
for lineIndex = 1:length(SymbolicOutput)
|
||||
strIn = char(SymbolicOutput(lineIndex));
|
||||
[match,idxsq1,idxsq2] = regexp(strIn,'\w*\[\w*\]\^2','match','start','end');
|
||||
[idxsq3] = regexp(strIn,'\[\w*\]\^2','start');
|
||||
if ~isempty(match)
|
||||
for pwrIndex = 1:length(match)
|
||||
strVar = strIn(idxsq1(pwrIndex):idxsq3(pwrIndex)-1);
|
||||
strIndex = strIn(idxsq3(pwrIndex)+1:idxsq2(pwrIndex)-3);
|
||||
strPat = strcat(strVar,'\[',strIndex,'\]\^2');
|
||||
strRep = strcat('sq(',strVar,'[',strIndex,']',')');
|
||||
strIn = regexprep(strIn,strPat,strRep);
|
||||
end
|
||||
SymbolicOutput(lineIndex) = cellstr(strIn);
|
||||
end
|
||||
end
|
||||
|
||||
% replace where adjacent to alpanumeric characters
|
||||
for lineIndex = 1:length(SymbolicOutput)
|
||||
strIn = char(SymbolicOutput(lineIndex));
|
||||
[match,idxsq1,idxsq2] = regexp(strIn,'\w*\^2','match','start','end');
|
||||
[idxsq3] = regexp(strIn,'\^2','start');
|
||||
if ~isempty(match)
|
||||
for pwrIndex = 1:length(match)
|
||||
strVar = strIn(idxsq1(pwrIndex)+2*(pwrIndex-1):idxsq2(pwrIndex)-2+2*(pwrIndex-1));
|
||||
strPat = strcat(strVar,'\^2');
|
||||
strRep = strcat('sq(',strVar,')');
|
||||
strIn = regexprep(strIn,strPat,strRep);
|
||||
end
|
||||
SymbolicOutput(lineIndex) = cellstr(strIn);
|
||||
end
|
||||
end
|
||||
|
||||
%% Replace ^(1/2)
|
||||
|
||||
% replace where adjacent to ) parenthesis
|
||||
for lineIndex = 1:length(SymbolicOutput)
|
||||
idxsq = regexp(SymbolicOutput(lineIndex),'\)\^\(1\/2\)');
|
||||
if ~isempty(idxsq{1})
|
||||
strIn = SymbolicOutput(lineIndex);
|
||||
idxlp = regexp(strIn,'\(');
|
||||
idxrp = regexp(strIn,'\)');
|
||||
for pwrIndex = 1:length(idxsq{1})
|
||||
counter = 1;
|
||||
index = idxsq{1}(pwrIndex);
|
||||
endIndex(pwrIndex) = index;
|
||||
while (counter > 0 && index > 0)
|
||||
index = index - 1;
|
||||
counter = counter + ~isempty(find(idxrp{1} == index, 1));
|
||||
counter = counter - ~isempty(find(idxlp{1} == index, 1));
|
||||
end
|
||||
startIndex(pwrIndex) = index;
|
||||
% strPat = strcat(strIn{1}(startIndex(pwrIndex):endIndex(pwrIndex)),'^2');
|
||||
strRep = strcat('(sqrt',strIn{1}(startIndex(pwrIndex):endIndex(pwrIndex)),')');
|
||||
% cellStrPat(pwrIndex) = cellstr(strPat);
|
||||
cellStrRep(pwrIndex) = cellstr(strRep);
|
||||
end
|
||||
for pwrIndex = 1:length(idxsq{1})
|
||||
strRep = char(cellStrRep(pwrIndex));
|
||||
strIn{1}(startIndex(pwrIndex):endIndex(pwrIndex)+6) = strRep;
|
||||
end
|
||||
SymbolicOutput(lineIndex) = strIn;
|
||||
end
|
||||
end
|
||||
|
||||
%% Replace Divisions
|
||||
% Compiler looks after this type of optimisation for us
|
||||
% for lineIndex = 1:length(SymbolicOutput)
|
||||
% strIn = char(SymbolicOutput(lineIndex));
|
||||
% strIn = regexprep(strIn,'\/2','\*0\.5');
|
||||
% strIn = regexprep(strIn,'\/4','\*0\.25');
|
||||
% SymbolicOutput(lineIndex) = cellstr(strIn);
|
||||
% end
|
||||
|
||||
%% Convert declarations
|
||||
for lineIndex = 1:length(SymbolicOutput)
|
||||
str = char(SymbolicOutput(lineIndex));
|
||||
if ~isempty(regexp(str,'zeros', 'once'))
|
||||
index1 = regexp(str,' = zeros[','once')-1;
|
||||
index2 = regexp(str,' = zeros[','end','once')+1;
|
||||
index3 = regexp(str,'\]\[','once')-1;
|
||||
index4 = index3 + 3;
|
||||
index5 = max(regexp(str,'\]'))-1;
|
||||
str1 = {'float '};
|
||||
str2 = str(1:index1);
|
||||
str3 = '[';
|
||||
str4 = str(index2:index3);
|
||||
str4 = num2str(str2num(str4)+1);
|
||||
str5 = '][';
|
||||
str6 = str(index4:index5);
|
||||
str6 = num2str(str2num(str6)+1);
|
||||
str7 = '];';
|
||||
SymbolicOutput(lineIndex) = strcat(str1,str2,str3,str4,str5,str6,str7);
|
||||
end
|
||||
end
|
||||
|
||||
%% Change covariance matrix variable name to P
|
||||
for lineIndex = 1:length(SymbolicOutput)
|
||||
strIn = char(SymbolicOutput(lineIndex));
|
||||
strIn = regexprep(strIn,'OP\[','P[');
|
||||
SymbolicOutput(lineIndex) = cellstr(strIn);
|
||||
end
|
||||
|
||||
%% Write to file
|
||||
fileName = strcat('C_code',int2str(nStates),'.txt');
|
||||
fid = fopen(fileName,'wt');
|
||||
for lineIndex = 1:length(SymbolicOutput)
|
||||
fprintf(fid,char(SymbolicOutput(lineIndex)));
|
||||
fprintf(fid,'\n');
|
||||
end
|
||||
fclose(fid);
|
||||
clear all;
|
||||
|
|
@ -1,46 +0,0 @@
|
|||
function ConvertToM(nStates)
|
||||
%% Initialize variables
|
||||
fileName = strcat('SymbolicOutput',int2str(nStates),'.txt');
|
||||
delimiter = '';
|
||||
|
||||
%% Format string for each line of text:
|
||||
% column1: text (%s)
|
||||
% For more information, see the TEXTSCAN documentation.
|
||||
formatSpec = '%s%[^\n\r]';
|
||||
|
||||
%% Open the text file.
|
||||
fileID = fopen(fileName,'r');
|
||||
|
||||
%% Read columns of data according to format string.
|
||||
% This call is based on the structure of the file used to generate this
|
||||
% code. If an error occurs for a different file, try regenerating the code
|
||||
% from the Import Tool.
|
||||
dataArray = textscan(fileID, formatSpec, 'Delimiter', delimiter, 'ReturnOnError', false);
|
||||
|
||||
%% Close the text file.
|
||||
fclose(fileID);
|
||||
|
||||
%% Create output variable
|
||||
SymbolicOutput = [dataArray{1:end-1}];
|
||||
|
||||
%% Clear temporary variables
|
||||
clearvars filename delimiter formatSpec fileID dataArray ans;
|
||||
|
||||
%% replace brackets and commas
|
||||
for lineIndex = 1:length(SymbolicOutput)
|
||||
SymbolicOutput(lineIndex) = regexprep(SymbolicOutput(lineIndex), '_l_', '(');
|
||||
SymbolicOutput(lineIndex) = regexprep(SymbolicOutput(lineIndex), '_c_', ',');
|
||||
SymbolicOutput(lineIndex) = regexprep(SymbolicOutput(lineIndex), '_r_', ')');
|
||||
end
|
||||
|
||||
%% Write to file
|
||||
fileName = strcat('M_code',int2str(nStates),'.txt');
|
||||
fid = fopen(fileName,'wt');
|
||||
for lineIndex = 1:length(SymbolicOutput)
|
||||
fprintf(fid,char(SymbolicOutput(lineIndex)));
|
||||
fprintf(fid,'\n');
|
||||
end
|
||||
fclose(fid);
|
||||
clear all;
|
||||
|
||||
end
|
||||
|
|
@ -1,106 +0,0 @@
|
|||
% IMPORTANT - This script requires the Matlab symbolic toolbox
|
||||
|
||||
% Derivation of quaterion covariance prediction for a rotation about the
|
||||
% earth frame Z axis and starting at an arbitary orientation. This 4x4
|
||||
% matrix can be used to add an additional
|
||||
|
||||
% Author: Paul Riseborough
|
||||
|
||||
%% define symbolic variables and constants
|
||||
clear all;
|
||||
reset(symengine);
|
||||
syms q0 q1 q2 q3 real % quaternions defining attitude of body axes relative to local NED
|
||||
syms daYaw real % earth frame yaw delta angle - rad
|
||||
syms daYawVar real; % earth frame yaw delta angle variance - rad^2
|
||||
|
||||
%% define the state prediction equations
|
||||
|
||||
% define the quaternion rotation vector for the state estimate
|
||||
quat = [q0;q1;q2;q3];
|
||||
|
||||
% derive the truth body to nav direction cosine matrix
|
||||
Tbn = Quat2Tbn(quat);
|
||||
|
||||
% define the yaw rotation delta angle in body frame
|
||||
dAngMeas = transpose(Tbn) * [0; 0; daYaw];
|
||||
|
||||
% define the attitude update equations
|
||||
% use a first order expansion of rotation to calculate the quaternion increment
|
||||
% acceptable for propagation of covariances
|
||||
deltaQuat = [1;
|
||||
0.5*dAngMeas(1);
|
||||
0.5*dAngMeas(2);
|
||||
0.5*dAngMeas(3);
|
||||
];
|
||||
quatNew = QuatMult(quat,deltaQuat);
|
||||
|
||||
% Define the state vector & number of states
|
||||
stateVector = quat;
|
||||
nStates=numel(stateVector);
|
||||
|
||||
% Define vector of process equations
|
||||
newStateVector = quatNew;
|
||||
|
||||
% derive the state transition matrix
|
||||
F = jacobian(newStateVector, stateVector);
|
||||
% set the rotation error states to zero
|
||||
[F,SF]=OptimiseAlgebra(F,'SF');
|
||||
|
||||
% define a symbolic covariance matrix using strings to represent
|
||||
% '_l_' to represent '( '
|
||||
% '_c_' to represent ,
|
||||
% '_r_' to represent ')'
|
||||
% these can be substituted later to create executable code
|
||||
for rowIndex = 1:nStates
|
||||
for colIndex = 1:nStates
|
||||
eval(['syms OP_l_',num2str(rowIndex),'_c_',num2str(colIndex), '_r_ real']);
|
||||
eval(['P(',num2str(rowIndex),',',num2str(colIndex), ') = OP_l_',num2str(rowIndex),'_c_',num2str(colIndex),'_r_;']);
|
||||
end
|
||||
end
|
||||
|
||||
save 'StatePrediction.mat';
|
||||
|
||||
%% derive the covariance prediction equations
|
||||
% This reduces the number of floating point operations by a factor of 6 or
|
||||
% more compared to using the standard matrix operations in code
|
||||
|
||||
% Error growth in the inertial solution is assumed to be driven by 'noise' in the delta angles and
|
||||
% velocities, after bias effects have been removed.
|
||||
|
||||
% derive the control(disturbance) influence matrix from IMU noise to state
|
||||
% noise
|
||||
G = jacobian(newStateVector, daYaw);
|
||||
[G,SG]=OptimiseAlgebra(G,'SG');
|
||||
|
||||
% derive the state error matrix
|
||||
distMatrix = diag(daYawVar);
|
||||
Q = G*distMatrix*transpose(G);
|
||||
[Q,SQ]=OptimiseAlgebra(Q,'SQ');
|
||||
|
||||
% set the yaw delta angle to zero - we only needed it to determine the error
|
||||
% propagation
|
||||
SF = subs(SF, daYaw, 0);
|
||||
SG = subs(SG, daYaw, 0);
|
||||
SQ = subs(SQ, daYaw, 0);
|
||||
|
||||
% Derive the predicted covariance matrix using the standard equation
|
||||
PP = F*P*transpose(F) + Q;
|
||||
PP = subs(PP, daYaw, 0);
|
||||
|
||||
% Collect common expressions to optimise processing
|
||||
[PP,SPP]=OptimiseAlgebra(PP,'SPP');
|
||||
|
||||
save('StateAndCovariancePrediction.mat');
|
||||
clear all;
|
||||
reset(symengine);
|
||||
|
||||
%% Save output and convert to m and c code fragments
|
||||
|
||||
% load equations for predictions and updates
|
||||
load('StateAndCovariancePrediction.mat');
|
||||
|
||||
fileName = strcat('SymbolicOutput',int2str(nStates),'.mat');
|
||||
save(fileName);
|
||||
SaveScriptCode(nStates);
|
||||
ConvertToM(nStates);
|
||||
ConvertToC(nStates);
|
||||
Binary file not shown.
|
|
@ -1,23 +0,0 @@
|
|||
function quaterion = EulToQuat(Euler)
|
||||
|
||||
% Convert from a 321 Euler rotation sequence specified in radians to a
|
||||
% Quaternion
|
||||
|
||||
quaterion = zeros(4,1);
|
||||
|
||||
Euler = Euler * 0.5;
|
||||
cosPhi = cos(Euler(1));
|
||||
sinPhi = sin(Euler(1));
|
||||
cosTheta = cos(Euler(2));
|
||||
sinTheta = sin(Euler(2));
|
||||
cosPsi = cos(Euler(3));
|
||||
sinPsi = sin(Euler(3));
|
||||
|
||||
quaterion(1,1) = (cosPhi*cosTheta*cosPsi + sinPhi*sinTheta*sinPsi);
|
||||
quaterion(2,1) = (sinPhi*cosTheta*cosPsi - cosPhi*sinTheta*sinPsi);
|
||||
quaterion(3,1) = (cosPhi*sinTheta*cosPsi + sinPhi*cosTheta*sinPsi);
|
||||
quaterion(4,1) = (cosPhi*cosTheta*sinPsi - sinPhi*sinTheta*cosPsi);
|
||||
|
||||
return;
|
||||
|
||||
|
||||
|
|
@ -1,519 +0,0 @@
|
|||
% IMPORTANT - This script requires the Matlab symbolic toolbox and takes ~3 hours to run
|
||||
|
||||
% Derivation of Navigation EKF using a local NED earth Tangent Frame and
|
||||
% XYZ body fixed frame
|
||||
% Sequential fusion of velocity and position measurements
|
||||
% Fusion of true airspeed
|
||||
% Sequential fusion of magnetic flux measurements
|
||||
% 24 state architecture.
|
||||
% IMU data is assumed to arrive at a constant rate with a time step of dt
|
||||
% IMU delta angle and velocity data are used as control inputs,
|
||||
% not observations
|
||||
|
||||
% Author: Paul Riseborough
|
||||
|
||||
% State vector:
|
||||
% attitude quaternion
|
||||
% Velocity - m/sec (North, East, Down)
|
||||
% Position - m (North, East, Down)
|
||||
% Delta Angle bias - rad (X,Y,Z)
|
||||
% Delta Velocity bias - m/s (X,Y,Z)
|
||||
% Earth Magnetic Field Vector - (North, East, Down)
|
||||
% Body Magnetic Field Vector - (X,Y,Z)
|
||||
% Wind Vector - m/sec (North,East)
|
||||
|
||||
% Observations:
|
||||
% NED velocity - m/s
|
||||
% NED position - m
|
||||
% True airspeed - m/s
|
||||
% angle of sideslip - rad
|
||||
% XYZ magnetic flux
|
||||
|
||||
% Time varying parameters:
|
||||
% XYZ delta angle measurements in body axes - rad
|
||||
% XYZ delta velocity measurements in body axes - m/sec
|
||||
|
||||
|
||||
%% define symbolic variables and constants
|
||||
clear all;
|
||||
reset(symengine);
|
||||
syms dax day daz real % IMU delta angle measurements in body axes - rad
|
||||
syms dvx dvy dvz real % IMU delta velocity measurements in body axes - m/sec
|
||||
syms q0 q1 q2 q3 real % quaternions defining attitude of body axes relative to local NED
|
||||
syms vn ve vd real % NED velocity - m/sec
|
||||
syms pn pe pd real % NED position - m
|
||||
syms dax_b day_b daz_b real % delta angle bias - rad
|
||||
syms dvx_b dvy_b dvz_b real % delta velocity bias - m/sec
|
||||
syms dt real % IMU time step - sec
|
||||
syms gravity real % gravity - m/sec^2
|
||||
syms daxVar dayVar dazVar dvxVar dvyVar dvzVar real; % IMU delta angle and delta velocity measurement variances
|
||||
syms vwn vwe real; % NE wind velocity - m/sec
|
||||
syms magX magY magZ real; % XYZ body fixed magnetic field measurements - milligauss
|
||||
syms magN magE magD real; % NED earth fixed magnetic field components - milligauss
|
||||
syms R_VN R_VE R_VD real % variances for NED velocity measurements - (m/sec)^2
|
||||
syms R_PN R_PE R_PD real % variances for NED position measurements - m^2
|
||||
syms R_TAS real % variance for true airspeed measurement - (m/sec)^2
|
||||
syms R_MAG real % variance for magnetic flux measurements - milligauss^2
|
||||
syms R_BETA real % variance of sidelsip measurements rad^2
|
||||
syms R_LOS real % variance of LOS angular rate mesurements (rad/sec)^2
|
||||
syms ptd real % location of terrain in D axis
|
||||
syms decl real; % earth magnetic field declination from true north
|
||||
syms R_DECL R_YAW real; % variance of declination or yaw angle observation
|
||||
syms BCXinv BCYinv real % inverse of ballistic coefficient for wind relative movement along the x and y body axes
|
||||
syms rho real % air density (kg/m^3)
|
||||
syms R_ACC real % variance of accelerometer measurements (m/s^2)^2
|
||||
syms Kaccx Kaccy real % derivative of X and Y body specific forces wrt component of true airspeed along each axis (1/s)
|
||||
|
||||
%% define the state prediction equations
|
||||
|
||||
% define the measured Delta angle and delta velocity vectors
|
||||
dAngMeas = [dax; day; daz];
|
||||
dVelMeas = [dvx; dvy; dvz];
|
||||
|
||||
% define the IMU bias errors and scale factor
|
||||
dAngBias = [dax_b; day_b; daz_b];
|
||||
dVelBias = [dvx_b; dvy_b; dvz_b];
|
||||
|
||||
% define the quaternion rotation vector for the state estimate
|
||||
quat = [q0;q1;q2;q3];
|
||||
% derive the truth body to nav direction cosine matrix
|
||||
Tbn = Quat2Tbn(quat);
|
||||
|
||||
% define the truth delta angle
|
||||
% ignore coning compensation as these effects are negligible in terms of
|
||||
% covariance growth for our application and grade of sensor
|
||||
dAngTruth = dAngMeas - dAngBias;
|
||||
|
||||
% Define the truth delta velocity -ignore sculling and transport rate
|
||||
% corrections as these negligible are in terms of covariance growth for our
|
||||
% application and grade of sensor
|
||||
dVelTruth = dVelMeas - dVelBias;
|
||||
|
||||
% define the attitude update equations
|
||||
% use a first order expansion of rotation to calculate the quaternion increment
|
||||
% acceptable for propagation of covariances
|
||||
deltaQuat = [1;
|
||||
0.5*dAngTruth(1);
|
||||
0.5*dAngTruth(2);
|
||||
0.5*dAngTruth(3);
|
||||
];
|
||||
quatNew = QuatMult(quat,deltaQuat);
|
||||
|
||||
% define the velocity update equations
|
||||
% ignore coriolis terms for linearisation purposes
|
||||
vNew = [vn;ve;vd] + [0;0;gravity]*dt + Tbn*dVelTruth;
|
||||
|
||||
% define the position update equations
|
||||
pNew = [pn;pe;pd] + [vn;ve;vd]*dt;
|
||||
|
||||
% define the IMU error update equations
|
||||
dAngBiasNew = dAngBias;
|
||||
dVelBiasNew = dVelBias;
|
||||
|
||||
% define the wind velocity update equations
|
||||
vwnNew = vwn;
|
||||
vweNew = vwe;
|
||||
|
||||
% define the earth magnetic field update equations
|
||||
magNnew = magN;
|
||||
magEnew = magE;
|
||||
magDnew = magD;
|
||||
|
||||
% define the body magnetic field update equations
|
||||
magXnew = magX;
|
||||
magYnew = magY;
|
||||
magZnew = magZ;
|
||||
|
||||
% Define the state vector & number of states
|
||||
stateVector = [quat;vn;ve;vd;pn;pe;pd;dAngBias;dVelBias;magN;magE;magD;magX;magY;magZ;vwn;vwe];
|
||||
nStates=numel(stateVector);
|
||||
|
||||
% Define vector of process equations
|
||||
newStateVector = [quatNew;vNew;pNew;dAngBiasNew;dVelBiasNew;magNnew;magEnew;magDnew;magXnew;magYnew;magZnew;vwnNew;vweNew];
|
||||
|
||||
% derive the state transition matrix
|
||||
F = jacobian(newStateVector, stateVector);
|
||||
% set the rotation error states to zero
|
||||
[F,SF]=OptimiseAlgebra(F,'SF');
|
||||
|
||||
% define a symbolic covariance matrix using strings to represent
|
||||
% '_l_' to represent '( '
|
||||
% '_c_' to represent ,
|
||||
% '_r_' to represent ')'
|
||||
% these can be substituted later to create executable code
|
||||
for rowIndex = 1:nStates
|
||||
for colIndex = 1:nStates
|
||||
eval(['syms OP_l_',num2str(rowIndex),'_c_',num2str(colIndex), '_r_ real']);
|
||||
eval(['P(',num2str(rowIndex),',',num2str(colIndex), ') = OP_l_',num2str(rowIndex),'_c_',num2str(colIndex),'_r_;']);
|
||||
end
|
||||
end
|
||||
|
||||
save 'StatePrediction.mat';
|
||||
|
||||
%% derive the covariance prediction equations
|
||||
% This reduces the number of floating point operations by a factor of 6 or
|
||||
% more compared to using the standard matrix operations in code
|
||||
|
||||
% Error growth in the inertial solution is assumed to be driven by 'noise' in the delta angles and
|
||||
% velocities, after bias effects have been removed.
|
||||
|
||||
% derive the control(disturbance) influence matrix from IMu noise to state
|
||||
% noise
|
||||
G = jacobian(newStateVector, [dAngMeas;dVelMeas]);
|
||||
[G,SG]=OptimiseAlgebra(G,'SG');
|
||||
|
||||
% derive the state error matrix
|
||||
distMatrix = diag([daxVar dayVar dazVar dvxVar dvyVar dvzVar]);
|
||||
Q = G*distMatrix*transpose(G);
|
||||
[Q,SQ]=OptimiseAlgebra(Q,'SQ');
|
||||
|
||||
% Derive the predicted covariance matrix using the standard equation
|
||||
PP = F*P*transpose(F) + Q;
|
||||
|
||||
% Collect common expressions to optimise processing
|
||||
[PP,SPP]=OptimiseAlgebra(PP,'SPP');
|
||||
|
||||
save('StateAndCovariancePrediction.mat');
|
||||
clear all;
|
||||
reset(symengine);
|
||||
|
||||
%% derive equations for fusion of true airspeed measurements
|
||||
load('StatePrediction.mat');
|
||||
VtasPred = sqrt((vn-vwn)^2 + (ve-vwe)^2 + vd^2); % predicted measurement
|
||||
H_TAS = jacobian(VtasPred,stateVector); % measurement Jacobian
|
||||
[H_TAS,SH_TAS]=OptimiseAlgebra(H_TAS,'SH_TAS'); % optimise processing
|
||||
K_TAS = (P*transpose(H_TAS))/(H_TAS*P*transpose(H_TAS) + R_TAS);
|
||||
[K_TAS,SK_TAS]=OptimiseAlgebra(K_TAS,'SK_TAS'); % Kalman gain vector
|
||||
|
||||
% save equations and reset workspace
|
||||
save('Airspeed.mat','SH_TAS','H_TAS','SK_TAS','K_TAS');
|
||||
clear all;
|
||||
reset(symengine);
|
||||
|
||||
%% derive equations for fusion of angle of sideslip measurements
|
||||
load('StatePrediction.mat');
|
||||
|
||||
% calculate wind relative velocities in nav frame and rotate into body frame
|
||||
Vbw = transpose(Tbn)*[(vn-vwn);(ve-vwe);vd];
|
||||
% calculate predicted angle of sideslip using small angle assumption
|
||||
BetaPred = Vbw(2)/Vbw(1);
|
||||
H_BETA = jacobian(BetaPred,stateVector); % measurement Jacobian
|
||||
[H_BETA,SH_BETA]=OptimiseAlgebra(H_BETA,'SH_BETA'); % optimise processing
|
||||
K_BETA = (P*transpose(H_BETA))/(H_BETA*P*transpose(H_BETA) + R_BETA);[K_BETA,SK_BETA]=OptimiseAlgebra(K_BETA,'SK_BETA'); % Kalman gain vector
|
||||
|
||||
% save equations and reset workspace
|
||||
save('Sideslip.mat','SH_BETA','H_BETA','SK_BETA','K_BETA');
|
||||
clear all;
|
||||
reset(symengine);
|
||||
|
||||
%% derive equations for fusion of magnetic field measurement
|
||||
load('StatePrediction.mat');
|
||||
|
||||
magMeas = transpose(Tbn)*[magN;magE;magD] + [magX;magY;magZ]; % predicted measurement
|
||||
H_MAG = jacobian(magMeas,stateVector); % measurement Jacobian
|
||||
[H_MAG,SH_MAG]=OptimiseAlgebra(H_MAG,'SH_MAG');
|
||||
|
||||
K_MX = (P*transpose(H_MAG(1,:)))/(H_MAG(1,:)*P*transpose(H_MAG(1,:)) + R_MAG); % Kalman gain vector
|
||||
[K_MX,SK_MX]=OptimiseAlgebra(K_MX,'SK_MX');
|
||||
K_MY = (P*transpose(H_MAG(2,:)))/(H_MAG(2,:)*P*transpose(H_MAG(2,:)) + R_MAG); % Kalman gain vector
|
||||
[K_MY,SK_MY]=OptimiseAlgebra(K_MY,'SK_MY');
|
||||
K_MZ = (P*transpose(H_MAG(3,:)))/(H_MAG(3,:)*P*transpose(H_MAG(3,:)) + R_MAG); % Kalman gain vector
|
||||
[K_MZ,SK_MZ]=OptimiseAlgebra(K_MZ,'SK_MZ');
|
||||
|
||||
% save equations and reset workspace
|
||||
save('Magnetometer.mat','SH_MAG','H_MAG','SK_MX','K_MX','SK_MY','K_MY','SK_MZ','K_MZ');
|
||||
clear all;
|
||||
reset(symengine);
|
||||
|
||||
%% derive equations for sequential fusion of optical flow measurements
|
||||
load('StatePrediction.mat');
|
||||
|
||||
% Range is defined as distance from camera focal point to object measured
|
||||
% along sensor Z axis
|
||||
syms range real;
|
||||
|
||||
% Define rotation matrix from body to sensor frame
|
||||
syms Tbs_a_x Tbs_a_y Tbs_a_z real;
|
||||
syms Tbs_b_x Tbs_b_y Tbs_b_z real;
|
||||
syms Tbs_c_x Tbs_c_y Tbs_c_z real;
|
||||
Tbs = [ ...
|
||||
Tbs_a_x Tbs_a_y Tbs_a_z ; ...
|
||||
Tbs_b_x Tbs_b_y Tbs_b_z ; ...
|
||||
Tbs_c_x Tbs_c_y Tbs_c_z ...
|
||||
];
|
||||
|
||||
% Calculate earth relative velocity in a non-rotating sensor frame
|
||||
relVelSensor = Tbs * transpose(Tbn) * [vn;ve;vd];
|
||||
|
||||
% Divide by range to get predicted angular LOS rates relative to X and Y
|
||||
% axes. Note these are rates in a non-rotating sensor frame
|
||||
losRateSensorX = +relVelSensor(2)/range;
|
||||
losRateSensorY = -relVelSensor(1)/range;
|
||||
|
||||
save('temp1.mat','losRateSensorX','losRateSensorY');
|
||||
|
||||
clear all;
|
||||
reset(symengine);
|
||||
load('StatePrediction.mat');
|
||||
load('temp1.mat');
|
||||
|
||||
% calculate the observation Jacobian and Kalman gain for the X axis
|
||||
H_LOSX = jacobian(losRateSensorX,stateVector); % measurement Jacobian
|
||||
H_LOSX = simplify(H_LOSX);
|
||||
K_LOSX = (P*transpose(H_LOSX))/(H_LOSX*P*transpose(H_LOSX) + R_LOS); % Kalman gain vector
|
||||
K_LOSX = simplify(K_LOSX);
|
||||
save('temp2.mat','H_LOSX','K_LOSX');
|
||||
ccode([H_LOSX;transpose(K_LOSX)],'file','LOSX.c');
|
||||
fix_c_code('LOSX.c');
|
||||
|
||||
clear all;
|
||||
reset(symengine);
|
||||
load('StatePrediction.mat');
|
||||
load('temp1.mat');
|
||||
|
||||
% calculate the observation Jacobian for the Y axis
|
||||
H_LOSY = jacobian(losRateSensorY,stateVector); % measurement Jacobian
|
||||
H_LOSY = simplify(H_LOSY);
|
||||
K_LOSY = (P*transpose(H_LOSY))/(H_LOSY*P*transpose(H_LOSY) + R_LOS); % Kalman gain vector
|
||||
K_LOSY = simplify(K_LOSY);
|
||||
save('temp3.mat','H_LOSY','K_LOSY');
|
||||
ccode([H_LOSY;transpose(K_LOSY)],'file','LOSY.c');
|
||||
fix_c_code('LOSY.c');
|
||||
|
||||
% reset workspace
|
||||
clear all;
|
||||
reset(symengine);
|
||||
|
||||
%% derive equations for sequential fusion of body frame velocity measurements
|
||||
load('StatePrediction.mat');
|
||||
|
||||
% body frame velocity observations
|
||||
syms velX velY velZ real;
|
||||
|
||||
% velocity observation variance
|
||||
syms R_VEL real;
|
||||
|
||||
% calculate relative velocity in body frame
|
||||
relVelBody = transpose(Tbn)*[vn;ve;vd];
|
||||
|
||||
save('temp1.mat','relVelBody','R_VEL');
|
||||
|
||||
% calculate the observation Jacobian for the X axis
|
||||
H_VELX = jacobian(relVelBody(1),stateVector); % measurement Jacobian
|
||||
H_VELX = simplify(H_VELX);
|
||||
save('temp2.mat','H_VELX');
|
||||
ccode(H_VELX,'file','H_VELX.c');
|
||||
fix_c_code('H_VELX.c');
|
||||
|
||||
clear all;
|
||||
reset(symengine);
|
||||
load('StatePrediction.mat');
|
||||
load('temp1.mat');
|
||||
|
||||
% calculate the observation Jacobian for the Y axis
|
||||
H_VELY = jacobian(relVelBody(2),stateVector); % measurement Jacobian
|
||||
H_VELY = simplify(H_VELY);
|
||||
save('temp3.mat','H_VELY');
|
||||
ccode(H_VELY,'file','H_VELY.c');
|
||||
fix_c_code('H_VELY.c');
|
||||
|
||||
clear all;
|
||||
reset(symengine);
|
||||
load('StatePrediction.mat');
|
||||
load('temp1.mat');
|
||||
|
||||
% calculate the observation Jacobian for the Z axis
|
||||
H_VELZ = jacobian(relVelBody(3),stateVector); % measurement Jacobian
|
||||
H_VELZ = simplify(H_VELZ);
|
||||
save('temp4.mat','H_VELZ');
|
||||
ccode(H_VELZ,'file','H_VELZ.c');
|
||||
fix_c_code('H_VELZ.c');
|
||||
|
||||
clear all;
|
||||
reset(symengine);
|
||||
|
||||
% calculate Kalman gain vector for the X axis
|
||||
load('StatePrediction.mat');
|
||||
load('temp1.mat');
|
||||
load('temp2.mat');
|
||||
|
||||
K_VELX = (P*transpose(H_VELX))/(H_VELX*P*transpose(H_VELX) + R_VEL); % Kalman gain vector
|
||||
K_VELX = simplify(K_VELX);
|
||||
ccode(K_VELX,'file','K_VELX.c');
|
||||
fix_c_code('K_VELX.c');
|
||||
|
||||
clear all;
|
||||
reset(symengine);
|
||||
|
||||
% calculate Kalman gain vector for the Y axis
|
||||
load('StatePrediction.mat');
|
||||
load('temp1.mat');
|
||||
load('temp3.mat');
|
||||
|
||||
K_VELY = (P*transpose(H_VELY))/(H_VELY*P*transpose(H_VELY) + R_VEL); % Kalman gain vector
|
||||
K_VELY = simplify(K_VELY);
|
||||
ccode(K_VELY,'file','K_VELY.c');
|
||||
fix_c_code('K_VELY.c');
|
||||
|
||||
clear all;
|
||||
reset(symengine);
|
||||
|
||||
% calculate Kalman gain vector for the Z axis
|
||||
load('StatePrediction.mat');
|
||||
load('temp1.mat');
|
||||
load('temp4.mat');
|
||||
|
||||
K_VELZ = (P*transpose(H_VELZ))/(H_VELZ*P*transpose(H_VELZ) + R_VEL); % Kalman gain vector
|
||||
K_VELZ = simplify(K_VELZ);
|
||||
ccode(K_VELZ,'file','K_VELZ.c');
|
||||
fix_c_code('K_VELZ.c');
|
||||
|
||||
% reset workspace
|
||||
clear all;
|
||||
reset(symengine);
|
||||
|
||||
% calculate Kalman gains vectors for X,Y,Z to take advantage of common
|
||||
% terms
|
||||
load('StatePrediction.mat');
|
||||
load('temp1.mat');
|
||||
load('temp2.mat');
|
||||
load('temp3.mat');
|
||||
load('temp4.mat');
|
||||
K_VELX = (P*transpose(H_VELX))/(H_VELX*P*transpose(H_VELX) + R_VEL); % Kalman gain vector
|
||||
K_VELY = (P*transpose(H_VELY))/(H_VELY*P*transpose(H_VELY) + R_VEL); % Kalman gain vector
|
||||
K_VELZ = (P*transpose(H_VELZ))/(H_VELZ*P*transpose(H_VELZ) + R_VEL); % Kalman gain vector
|
||||
K_VEL = simplify([K_VELX,K_VELY,K_VELZ]);
|
||||
ccode(K_VEL,'file','K_VEL.c');
|
||||
fix_c_code('K_VEL.c');
|
||||
|
||||
|
||||
%% derive equations for fusion of 321 sequence yaw measurement
|
||||
load('StatePrediction.mat');
|
||||
|
||||
% Calculate the yaw (first rotation) angle from the 321 rotation sequence
|
||||
% Provide alternative angle that avoids singularity at +-pi/2
|
||||
angMeasA = atan(Tbn(2,1)/Tbn(1,1));
|
||||
angMeasB = pi/2 - atan(Tbn(1,1)/Tbn(2,1));
|
||||
H_YAW321 = jacobian([angMeasA;angMeasB],stateVector); % measurement Jacobian
|
||||
H_YAW321 = simplify(H_YAW321);
|
||||
ccode(H_YAW321,'file','calcH_YAW321.c');
|
||||
fix_c_code('calcH_YAW321.c');
|
||||
|
||||
% reset workspace
|
||||
clear all;
|
||||
reset(symengine);
|
||||
|
||||
%% derive equations for fusion of 312 sequence yaw measurement
|
||||
load('StatePrediction.mat');
|
||||
|
||||
% Calculate the yaw (first rotation) angle from an Euler 312 sequence
|
||||
% Provide alternative angle that avoids singularity at +-pi/2
|
||||
angMeasA = atan(-Tbn(1,2)/Tbn(2,2));
|
||||
angMeasB = pi/2 - atan(-Tbn(2,2)/Tbn(1,2));
|
||||
H_YAW312 = jacobian([angMeasA;angMeasB],stateVector); % measurement Jacobian
|
||||
H_YAW312 = simplify(H_YAW312);
|
||||
ccode(H_YAW312,'file','calcH_YAW312.c');
|
||||
fix_c_code('calcH_YAW312.c');
|
||||
|
||||
% reset workspace
|
||||
clear all;
|
||||
reset(symengine);
|
||||
|
||||
%% derive equations for fusion of dual antenna yaw measurement
|
||||
load('StatePrediction.mat');
|
||||
|
||||
syms ant_yaw real; % yaw angle of antenna array axis wrt X body axis
|
||||
|
||||
% define antenna vector in body frame
|
||||
ant_vec_bf = [cos(ant_yaw);sin(ant_yaw);0];
|
||||
|
||||
% rotate into earth frame
|
||||
ant_vec_ef = Tbn * ant_vec_bf;
|
||||
|
||||
% Calculate the yaw angle from the projection
|
||||
angMeas = atan(ant_vec_ef(2)/ant_vec_ef(1));
|
||||
|
||||
H_YAWGPS = jacobian(angMeas,stateVector); % measurement Jacobian
|
||||
H_YAWGPS = simplify(H_YAWGPS);
|
||||
ccode(H_YAWGPS,'file','calcH_YAWGPS.c');
|
||||
fix_c_code('calcH_YAWGPS.c');
|
||||
|
||||
% reset workspace
|
||||
clear all;
|
||||
reset(symengine);
|
||||
|
||||
%% derive equations for fusion of declination
|
||||
load('StatePrediction.mat');
|
||||
|
||||
% the predicted measurement is the angle wrt magnetic north of the horizontal
|
||||
% component of the measured field
|
||||
angMeas = atan(magE/magN);
|
||||
H_MAGD = jacobian(angMeas,stateVector); % measurement Jacobian
|
||||
H_MAGD = simplify(H_MAGD);
|
||||
K_MAGD = (P*transpose(H_MAGD))/(H_MAGD*P*transpose(H_MAGD) + R_DECL);
|
||||
K_MAGD = simplify(K_MAGD);
|
||||
ccode([K_MAGD,H_MAGD'],'file','calcMAGD.c');
|
||||
fix_c_code('calcMAGD.c');
|
||||
|
||||
% reset workspace
|
||||
clear all;
|
||||
reset(symengine);
|
||||
|
||||
%% derive equations for fusion of lateral body acceleration (multirotors only)
|
||||
load('StatePrediction.mat');
|
||||
|
||||
% use relationship between airspeed along the X and Y body axis and the
|
||||
% drag to predict the lateral acceleration for a multirotor vehicle type
|
||||
% where propulsion forces are generated primarily along the Z body axis
|
||||
|
||||
vrel = transpose(Tbn)*[(vn-vwn);(ve-vwe);vd]; % predicted wind relative velocity
|
||||
|
||||
% calculate drag assuming flight along axis in positive direction
|
||||
% sign change will be looked after in implementation rather than by adding
|
||||
% sign functions to symbolic derivation which genererates output with dirac
|
||||
% functions
|
||||
% accXpred = -0.5*rho*vrel(1)*vrel(1)*BCXinv; % predicted acceleration measured along X body axis
|
||||
% accYpred = -0.5*rho*vrel(2)*vrel(2)*BCYinv; % predicted acceleration measured along Y body axis
|
||||
|
||||
% Use a simple viscous drag model for the linear estimator equations
|
||||
% Use the the derivative from speed to acceleration averaged across the
|
||||
% speed range
|
||||
% The nonlinear equation will be used to calculate the predicted
|
||||
% measurement in implementation
|
||||
accXpred = -Kaccx*vrel(1); % predicted acceleration measured along X body axis
|
||||
accYpred = -Kaccy*vrel(2); % predicted acceleration measured along Y body axis
|
||||
|
||||
% Derive observation Jacobian and Kalman gain matrix for X accel fusion
|
||||
H_ACCX = jacobian(accXpred,stateVector); % measurement Jacobian
|
||||
H_ACCX = simplify(H_ACCX);
|
||||
[H_ACCX,SH_ACCX]=OptimiseAlgebra(H_ACCX,'SH_ACCX'); % optimise processing
|
||||
K_ACCX = (P*transpose(H_ACCX))/(H_ACCX*P*transpose(H_ACCX) + R_ACC);
|
||||
[K_ACCX,SK_ACCX]=OptimiseAlgebra(K_ACCX,'SK_ACCX'); % Kalman gain vector
|
||||
|
||||
% Derive observation Jacobian and Kalman gain matrix for Y accel fusion
|
||||
H_ACCY = jacobian(accYpred,stateVector); % measurement Jacobian
|
||||
H_ACCY = simplify(H_ACCY);
|
||||
[H_ACCY,SH_ACCY]=OptimiseAlgebra(H_ACCY,'SH_ACCY'); % optimise processing
|
||||
K_ACCY = (P*transpose(H_ACCY))/(H_ACCY*P*transpose(H_ACCY) + R_ACC);
|
||||
[K_ACCY,SK_ACCY]=OptimiseAlgebra(K_ACCY,'SK_ACCY'); % Kalman gain vector
|
||||
|
||||
% save equations and reset workspace
|
||||
save('Drag.mat','SH_ACCX','H_ACCX','SK_ACCX','K_ACCX','SH_ACCY','H_ACCY','SK_ACCY','K_ACCY');
|
||||
clear all;
|
||||
reset(symengine);
|
||||
|
||||
%% Save output and convert to m and c code fragments
|
||||
|
||||
% load equations for predictions and updates
|
||||
load('StateAndCovariancePrediction.mat');
|
||||
load('Airspeed.mat');
|
||||
load('Sideslip.mat');
|
||||
load('Magnetometer.mat');
|
||||
load('Drag.mat');
|
||||
|
||||
fileName = strcat('SymbolicOutput',int2str(nStates),'.mat');
|
||||
save(fileName);
|
||||
SaveScriptCode(nStates);
|
||||
ConvertToM(nStates); % convert symbolic expressions to Matlab expressions
|
||||
ConvertToC(nStates); % convert Matlab expressions to C code expressions
|
||||
ConvertCtoC(nStates); % convert covariance matrix expressions from array to matrix syntax
|
||||
|
|
@ -1,8 +0,0 @@
|
|||
t2 = 1.0/range;
|
||||
A0[0][0] = t2*(q1*vd*2.0+q0*ve*2.0-q3*vn*2.0);
|
||||
A0[0][1] = t2*(q0*vd*2.0-q1*ve*2.0+q2*vn*2.0);
|
||||
A0[0][2] = t2*(q3*vd*2.0+q2*ve*2.0+q1*vn*2.0);
|
||||
A0[0][3] = -t2*(q2*vd*-2.0+q3*ve*2.0+q0*vn*2.0);
|
||||
A0[0][4] = -t2*(q0*q3*2.0-q1*q2*2.0);
|
||||
A0[0][5] = t2*(q0*q0-q1*q1+q2*q2-q3*q3);
|
||||
A0[0][6] = t2*(q0*q1*2.0+q2*q3*2.0);
|
||||
|
|
@ -1,8 +0,0 @@
|
|||
t2 = 1.0/range;
|
||||
A0[0][0] = -t2*(q2*vd*-2.0+q3*ve*2.0+q0*vn*2.0);
|
||||
A0[0][1] = -t2*(q3*vd*2.0+q2*ve*2.0+q1*vn*2.0);
|
||||
A0[0][2] = t2*(q0*vd*2.0-q1*ve*2.0+q2*vn*2.0);
|
||||
A0[0][3] = -t2*(q1*vd*2.0+q0*ve*2.0-q3*vn*2.0);
|
||||
A0[0][4] = -t2*(q0*q0+q1*q1-q2*q2-q3*q3);
|
||||
A0[0][5] = -t2*(q0*q3*2.0+q1*q2*2.0);
|
||||
A0[0][6] = t2*(q0*q2*2.0-q1*q3*2.0);
|
||||
|
|
@ -1,7 +0,0 @@
|
|||
H_VEL[0] = q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f;
|
||||
H_VEL[1] = q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f;
|
||||
H_VEL[2] = q0*vd*-2.0f+q1*ve*2.0f-q2*vn*2.0f;
|
||||
H_VEL[3] = q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f;
|
||||
H_VEL[4] = q0*q0+q1*q1-q2*q2-q3*q3;
|
||||
H_VEL[5] = q0*q3*2.0f+q1*q2*2.0f;
|
||||
H_VEL[6] = q0*q2*-2.0f+q1*q3*2.0f;
|
||||
|
|
@ -1,7 +0,0 @@
|
|||
H_VEL[0] = q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f;
|
||||
H_VEL[1] = q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f;
|
||||
H_VEL[2] = q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f;
|
||||
H_VEL[3] = q2*vd*2.0f-q3*ve*2.0f-q0*vn*2.0f;
|
||||
H_VEL[4] = q0*q3*-2.0f+q1*q2*2.0f;
|
||||
H_VEL[5] = q0*q0-q1*q1+q2*q2-q3*q3;
|
||||
H_VEL[6] = q0*q1*2.0f+q2*q3*2.0f;
|
||||
|
|
@ -1,7 +0,0 @@
|
|||
H_VEL[0] = q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f;
|
||||
H_VEL[1] = q1*vd*-2.0f-q0*ve*2.0f+q3*vn*2.0f;
|
||||
H_VEL[2] = q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f;
|
||||
H_VEL[3] = q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f;
|
||||
H_VEL[4] = q0*q2*2.0f+q1*q3*2.0f;
|
||||
H_VEL[5] = q0*q1*-2.0f+q2*q3*2.0f;
|
||||
H_VEL[6] = q0*q0-q1*q1-q2*q2+q3*q3;
|
||||
|
|
@ -1,117 +0,0 @@
|
|||
t2 = 1.0/range;
|
||||
t3 = q1*vd*2.0;
|
||||
t4 = q0*ve*2.0;
|
||||
t11 = q3*vn*2.0;
|
||||
t5 = t3+t4-t11;
|
||||
t6 = q0*q3*2.0;
|
||||
t29 = q1*q2*2.0;
|
||||
t7 = t6-t29;
|
||||
t8 = q0*q1*2.0;
|
||||
t9 = q2*q3*2.0;
|
||||
t10 = t8+t9;
|
||||
t12 = P[0][0]*t2*t5;
|
||||
t13 = q0*vd*2.0;
|
||||
t14 = q2*vn*2.0;
|
||||
t28 = q1*ve*2.0;
|
||||
t15 = t13+t14-t28;
|
||||
t16 = q3*vd*2.0;
|
||||
t17 = q2*ve*2.0;
|
||||
t18 = q1*vn*2.0;
|
||||
t19 = t16+t17+t18;
|
||||
t20 = q3*ve*2.0;
|
||||
t21 = q0*vn*2.0;
|
||||
t30 = q2*vd*2.0;
|
||||
t22 = t20+t21-t30;
|
||||
t23 = q0*q0;
|
||||
t24 = q1*q1;
|
||||
t25 = q2*q2;
|
||||
t26 = q3*q3;
|
||||
t27 = t23-t24+t25-t26;
|
||||
t31 = P[1][1]*t2*t15;
|
||||
t32 = P[6][0]*t2*t10;
|
||||
t33 = P[1][0]*t2*t15;
|
||||
t34 = P[2][0]*t2*t19;
|
||||
t35 = P[5][0]*t2*t27;
|
||||
t79 = P[4][0]*t2*t7;
|
||||
t80 = P[3][0]*t2*t22;
|
||||
t36 = t12+t32+t33+t34+t35-t79-t80;
|
||||
t37 = t2*t5*t36;
|
||||
t38 = P[6][1]*t2*t10;
|
||||
t39 = P[0][1]*t2*t5;
|
||||
t40 = P[2][1]*t2*t19;
|
||||
t41 = P[5][1]*t2*t27;
|
||||
t81 = P[4][1]*t2*t7;
|
||||
t82 = P[3][1]*t2*t22;
|
||||
t42 = t31+t38+t39+t40+t41-t81-t82;
|
||||
t43 = t2*t15*t42;
|
||||
t44 = P[6][2]*t2*t10;
|
||||
t45 = P[0][2]*t2*t5;
|
||||
t46 = P[1][2]*t2*t15;
|
||||
t47 = P[2][2]*t2*t19;
|
||||
t48 = P[5][2]*t2*t27;
|
||||
t83 = P[4][2]*t2*t7;
|
||||
t84 = P[3][2]*t2*t22;
|
||||
t49 = t44+t45+t46+t47+t48-t83-t84;
|
||||
t50 = t2*t19*t49;
|
||||
t51 = P[6][3]*t2*t10;
|
||||
t52 = P[0][3]*t2*t5;
|
||||
t53 = P[1][3]*t2*t15;
|
||||
t54 = P[2][3]*t2*t19;
|
||||
t55 = P[5][3]*t2*t27;
|
||||
t85 = P[4][3]*t2*t7;
|
||||
t86 = P[3][3]*t2*t22;
|
||||
t56 = t51+t52+t53+t54+t55-t85-t86;
|
||||
t57 = P[6][5]*t2*t10;
|
||||
t58 = P[0][5]*t2*t5;
|
||||
t59 = P[1][5]*t2*t15;
|
||||
t60 = P[2][5]*t2*t19;
|
||||
t61 = P[5][5]*t2*t27;
|
||||
t88 = P[4][5]*t2*t7;
|
||||
t89 = P[3][5]*t2*t22;
|
||||
t62 = t57+t58+t59+t60+t61-t88-t89;
|
||||
t63 = t2*t27*t62;
|
||||
t64 = P[6][4]*t2*t10;
|
||||
t65 = P[0][4]*t2*t5;
|
||||
t66 = P[1][4]*t2*t15;
|
||||
t67 = P[2][4]*t2*t19;
|
||||
t68 = P[5][4]*t2*t27;
|
||||
t90 = P[4][4]*t2*t7;
|
||||
t91 = P[3][4]*t2*t22;
|
||||
t69 = t64+t65+t66+t67+t68-t90-t91;
|
||||
t70 = P[6][6]*t2*t10;
|
||||
t71 = P[0][6]*t2*t5;
|
||||
t72 = P[1][6]*t2*t15;
|
||||
t73 = P[2][6]*t2*t19;
|
||||
t74 = P[5][6]*t2*t27;
|
||||
t93 = P[4][6]*t2*t7;
|
||||
t94 = P[3][6]*t2*t22;
|
||||
t75 = t70+t71+t72+t73+t74-t93-t94;
|
||||
t76 = t2*t10*t75;
|
||||
t87 = t2*t22*t56;
|
||||
t92 = t2*t7*t69;
|
||||
t77 = R_LOS+t37+t43+t50+t63+t76-t87-t92;
|
||||
t78 = 1.0/t77;
|
||||
A0[0][0] = t78*(t12-P[0][4]*t2*t7+P[0][1]*t2*t15+P[0][6]*t2*t10+P[0][2]*t2*t19-P[0][3]*t2*t22+P[0][5]*t2*t27);
|
||||
A0[1][0] = t78*(t31+P[1][0]*t2*t5-P[1][4]*t2*t7+P[1][6]*t2*t10+P[1][2]*t2*t19-P[1][3]*t2*t22+P[1][5]*t2*t27);
|
||||
A0[2][0] = t78*(t47+P[2][0]*t2*t5-P[2][4]*t2*t7+P[2][1]*t2*t15+P[2][6]*t2*t10-P[2][3]*t2*t22+P[2][5]*t2*t27);
|
||||
A0[3][0] = t78*(-t86+P[3][0]*t2*t5-P[3][4]*t2*t7+P[3][1]*t2*t15+P[3][6]*t2*t10+P[3][2]*t2*t19+P[3][5]*t2*t27);
|
||||
A0[4][0] = t78*(-t90+P[4][0]*t2*t5+P[4][1]*t2*t15+P[4][6]*t2*t10+P[4][2]*t2*t19-P[4][3]*t2*t22+P[4][5]*t2*t27);
|
||||
A0[5][0] = t78*(t61+P[5][0]*t2*t5-P[5][4]*t2*t7+P[5][1]*t2*t15+P[5][6]*t2*t10+P[5][2]*t2*t19-P[5][3]*t2*t22);
|
||||
A0[6][0] = t78*(t70+P[6][0]*t2*t5-P[6][4]*t2*t7+P[6][1]*t2*t15+P[6][2]*t2*t19-P[6][3]*t2*t22+P[6][5]*t2*t27);
|
||||
A0[7][0] = t78*(P[7][0]*t2*t5-P[7][4]*t2*t7+P[7][1]*t2*t15+P[7][6]*t2*t10+P[7][2]*t2*t19-P[7][3]*t2*t22+P[7][5]*t2*t27);
|
||||
A0[8][0] = t78*(P[8][0]*t2*t5-P[8][4]*t2*t7+P[8][1]*t2*t15+P[8][6]*t2*t10+P[8][2]*t2*t19-P[8][3]*t2*t22+P[8][5]*t2*t27);
|
||||
A0[9][0] = t78*(P[9][0]*t2*t5-P[9][4]*t2*t7+P[9][1]*t2*t15+P[9][6]*t2*t10+P[9][2]*t2*t19-P[9][3]*t2*t22+P[9][5]*t2*t27);
|
||||
A0[10][0] = t78*(P[10][0]*t2*t5-P[10][4]*t2*t7+P[10][1]*t2*t15+P[10][6]*t2*t10+P[10][2]*t2*t19-P[10][3]*t2*t22+P[10][5]*t2*t27);
|
||||
A0[11][0] = t78*(P[11][0]*t2*t5-P[11][4]*t2*t7+P[11][1]*t2*t15+P[11][6]*t2*t10+P[11][2]*t2*t19-P[11][3]*t2*t22+P[11][5]*t2*t27);
|
||||
A0[12][0] = t78*(P[12][0]*t2*t5-P[12][4]*t2*t7+P[12][1]*t2*t15+P[12][6]*t2*t10+P[12][2]*t2*t19-P[12][3]*t2*t22+P[12][5]*t2*t27);
|
||||
A0[13][0] = t78*(P[13][0]*t2*t5-P[13][4]*t2*t7+P[13][1]*t2*t15+P[13][6]*t2*t10+P[13][2]*t2*t19-P[13][3]*t2*t22+P[13][5]*t2*t27);
|
||||
A0[14][0] = t78*(P[14][0]*t2*t5-P[14][4]*t2*t7+P[14][1]*t2*t15+P[14][6]*t2*t10+P[14][2]*t2*t19-P[14][3]*t2*t22+P[14][5]*t2*t27);
|
||||
A0[15][0] = t78*(P[15][0]*t2*t5-P[15][4]*t2*t7+P[15][1]*t2*t15+P[15][6]*t2*t10+P[15][2]*t2*t19-P[15][3]*t2*t22+P[15][5]*t2*t27);
|
||||
A0[16][0] = t78*(P[16][0]*t2*t5-P[16][4]*t2*t7+P[16][1]*t2*t15+P[16][6]*t2*t10+P[16][2]*t2*t19-P[16][3]*t2*t22+P[16][5]*t2*t27);
|
||||
A0[17][0] = t78*(P[17][0]*t2*t5-P[17][4]*t2*t7+P[17][1]*t2*t15+P[17][6]*t2*t10+P[17][2]*t2*t19-P[17][3]*t2*t22+P[17][5]*t2*t27);
|
||||
A0[18][0] = t78*(P[18][0]*t2*t5-P[18][4]*t2*t7+P[18][1]*t2*t15+P[18][6]*t2*t10+P[18][2]*t2*t19-P[18][3]*t2*t22+P[18][5]*t2*t27);
|
||||
A0[19][0] = t78*(P[19][0]*t2*t5-P[19][4]*t2*t7+P[19][1]*t2*t15+P[19][6]*t2*t10+P[19][2]*t2*t19-P[19][3]*t2*t22+P[19][5]*t2*t27);
|
||||
A0[20][0] = t78*(P[20][0]*t2*t5-P[20][4]*t2*t7+P[20][1]*t2*t15+P[20][6]*t2*t10+P[20][2]*t2*t19-P[20][3]*t2*t22+P[20][5]*t2*t27);
|
||||
A0[21][0] = t78*(P[21][0]*t2*t5-P[21][4]*t2*t7+P[21][1]*t2*t15+P[21][6]*t2*t10+P[21][2]*t2*t19-P[21][3]*t2*t22+P[21][5]*t2*t27);
|
||||
A0[22][0] = t78*(P[22][0]*t2*t5-P[22][4]*t2*t7+P[22][1]*t2*t15+P[22][6]*t2*t10+P[22][2]*t2*t19-P[22][3]*t2*t22+P[22][5]*t2*t27);
|
||||
A0[23][0] = t78*(P[23][0]*t2*t5-P[23][4]*t2*t7+P[23][1]*t2*t15+P[23][6]*t2*t10+P[23][2]*t2*t19-P[23][3]*t2*t22+P[23][5]*t2*t27);
|
||||
|
|
@ -1,117 +0,0 @@
|
|||
t2 = 1.0/range;
|
||||
t3 = q3*ve*2.0;
|
||||
t4 = q0*vn*2.0;
|
||||
t11 = q2*vd*2.0;
|
||||
t5 = t3+t4-t11;
|
||||
t6 = q0*q3*2.0;
|
||||
t7 = q1*q2*2.0;
|
||||
t8 = t6+t7;
|
||||
t9 = q0*q2*2.0;
|
||||
t28 = q1*q3*2.0;
|
||||
t10 = t9-t28;
|
||||
t12 = P[0][0]*t2*t5;
|
||||
t13 = q3*vd*2.0;
|
||||
t14 = q2*ve*2.0;
|
||||
t15 = q1*vn*2.0;
|
||||
t16 = t13+t14+t15;
|
||||
t17 = q0*vd*2.0;
|
||||
t18 = q2*vn*2.0;
|
||||
t29 = q1*ve*2.0;
|
||||
t19 = t17+t18-t29;
|
||||
t20 = q1*vd*2.0;
|
||||
t21 = q0*ve*2.0;
|
||||
t30 = q3*vn*2.0;
|
||||
t22 = t20+t21-t30;
|
||||
t23 = q0*q0;
|
||||
t24 = q1*q1;
|
||||
t25 = q2*q2;
|
||||
t26 = q3*q3;
|
||||
t27 = t23+t24-t25-t26;
|
||||
t31 = P[1][1]*t2*t16;
|
||||
t32 = P[5][0]*t2*t8;
|
||||
t33 = P[1][0]*t2*t16;
|
||||
t34 = P[3][0]*t2*t22;
|
||||
t35 = P[4][0]*t2*t27;
|
||||
t80 = P[6][0]*t2*t10;
|
||||
t81 = P[2][0]*t2*t19;
|
||||
t36 = t12+t32+t33+t34+t35-t80-t81;
|
||||
t37 = t2*t5*t36;
|
||||
t38 = P[5][1]*t2*t8;
|
||||
t39 = P[0][1]*t2*t5;
|
||||
t40 = P[3][1]*t2*t22;
|
||||
t41 = P[4][1]*t2*t27;
|
||||
t82 = P[6][1]*t2*t10;
|
||||
t83 = P[2][1]*t2*t19;
|
||||
t42 = t31+t38+t39+t40+t41-t82-t83;
|
||||
t43 = t2*t16*t42;
|
||||
t44 = P[5][2]*t2*t8;
|
||||
t45 = P[0][2]*t2*t5;
|
||||
t46 = P[1][2]*t2*t16;
|
||||
t47 = P[3][2]*t2*t22;
|
||||
t48 = P[4][2]*t2*t27;
|
||||
t79 = P[2][2]*t2*t19;
|
||||
t84 = P[6][2]*t2*t10;
|
||||
t49 = t44+t45+t46+t47+t48-t79-t84;
|
||||
t50 = P[5][3]*t2*t8;
|
||||
t51 = P[0][3]*t2*t5;
|
||||
t52 = P[1][3]*t2*t16;
|
||||
t53 = P[3][3]*t2*t22;
|
||||
t54 = P[4][3]*t2*t27;
|
||||
t86 = P[6][3]*t2*t10;
|
||||
t87 = P[2][3]*t2*t19;
|
||||
t55 = t50+t51+t52+t53+t54-t86-t87;
|
||||
t56 = t2*t22*t55;
|
||||
t57 = P[5][4]*t2*t8;
|
||||
t58 = P[0][4]*t2*t5;
|
||||
t59 = P[1][4]*t2*t16;
|
||||
t60 = P[3][4]*t2*t22;
|
||||
t61 = P[4][4]*t2*t27;
|
||||
t88 = P[6][4]*t2*t10;
|
||||
t89 = P[2][4]*t2*t19;
|
||||
t62 = t57+t58+t59+t60+t61-t88-t89;
|
||||
t63 = t2*t27*t62;
|
||||
t64 = P[5][5]*t2*t8;
|
||||
t65 = P[0][5]*t2*t5;
|
||||
t66 = P[1][5]*t2*t16;
|
||||
t67 = P[3][5]*t2*t22;
|
||||
t68 = P[4][5]*t2*t27;
|
||||
t90 = P[6][5]*t2*t10;
|
||||
t91 = P[2][5]*t2*t19;
|
||||
t69 = t64+t65+t66+t67+t68-t90-t91;
|
||||
t70 = t2*t8*t69;
|
||||
t71 = P[5][6]*t2*t8;
|
||||
t72 = P[0][6]*t2*t5;
|
||||
t73 = P[1][6]*t2*t16;
|
||||
t74 = P[3][6]*t2*t22;
|
||||
t75 = P[4][6]*t2*t27;
|
||||
t92 = P[6][6]*t2*t10;
|
||||
t93 = P[2][6]*t2*t19;
|
||||
t76 = t71+t72+t73+t74+t75-t92-t93;
|
||||
t85 = t2*t19*t49;
|
||||
t94 = t2*t10*t76;
|
||||
t77 = R_LOS+t37+t43+t56+t63+t70-t85-t94;
|
||||
t78 = 1.0/t77;
|
||||
A0[0][0] = -t78*(t12+P[0][5]*t2*t8-P[0][6]*t2*t10+P[0][1]*t2*t16-P[0][2]*t2*t19+P[0][3]*t2*t22+P[0][4]*t2*t27);
|
||||
A0[1][0] = -t78*(t31+P[1][0]*t2*t5+P[1][5]*t2*t8-P[1][6]*t2*t10-P[1][2]*t2*t19+P[1][3]*t2*t22+P[1][4]*t2*t27);
|
||||
A0[2][0] = -t78*(-t79+P[2][0]*t2*t5+P[2][5]*t2*t8-P[2][6]*t2*t10+P[2][1]*t2*t16+P[2][3]*t2*t22+P[2][4]*t2*t27);
|
||||
A0[3][0] = -t78*(t53+P[3][0]*t2*t5+P[3][5]*t2*t8-P[3][6]*t2*t10+P[3][1]*t2*t16-P[3][2]*t2*t19+P[3][4]*t2*t27);
|
||||
A0[4][0] = -t78*(t61+P[4][0]*t2*t5+P[4][5]*t2*t8-P[4][6]*t2*t10+P[4][1]*t2*t16-P[4][2]*t2*t19+P[4][3]*t2*t22);
|
||||
A0[5][0] = -t78*(t64+P[5][0]*t2*t5-P[5][6]*t2*t10+P[5][1]*t2*t16-P[5][2]*t2*t19+P[5][3]*t2*t22+P[5][4]*t2*t27);
|
||||
A0[6][0] = -t78*(-t92+P[6][0]*t2*t5+P[6][5]*t2*t8+P[6][1]*t2*t16-P[6][2]*t2*t19+P[6][3]*t2*t22+P[6][4]*t2*t27);
|
||||
A0[7][0] = -t78*(P[7][0]*t2*t5+P[7][5]*t2*t8-P[7][6]*t2*t10+P[7][1]*t2*t16-P[7][2]*t2*t19+P[7][3]*t2*t22+P[7][4]*t2*t27);
|
||||
A0[8][0] = -t78*(P[8][0]*t2*t5+P[8][5]*t2*t8-P[8][6]*t2*t10+P[8][1]*t2*t16-P[8][2]*t2*t19+P[8][3]*t2*t22+P[8][4]*t2*t27);
|
||||
A0[9][0] = -t78*(P[9][0]*t2*t5+P[9][5]*t2*t8-P[9][6]*t2*t10+P[9][1]*t2*t16-P[9][2]*t2*t19+P[9][3]*t2*t22+P[9][4]*t2*t27);
|
||||
A0[10][0] = -t78*(P[10][0]*t2*t5+P[10][5]*t2*t8-P[10][6]*t2*t10+P[10][1]*t2*t16-P[10][2]*t2*t19+P[10][3]*t2*t22+P[10][4]*t2*t27);
|
||||
A0[11][0] = -t78*(P[11][0]*t2*t5+P[11][5]*t2*t8-P[11][6]*t2*t10+P[11][1]*t2*t16-P[11][2]*t2*t19+P[11][3]*t2*t22+P[11][4]*t2*t27);
|
||||
A0[12][0] = -t78*(P[12][0]*t2*t5+P[12][5]*t2*t8-P[12][6]*t2*t10+P[12][1]*t2*t16-P[12][2]*t2*t19+P[12][3]*t2*t22+P[12][4]*t2*t27);
|
||||
A0[13][0] = -t78*(P[13][0]*t2*t5+P[13][5]*t2*t8-P[13][6]*t2*t10+P[13][1]*t2*t16-P[13][2]*t2*t19+P[13][3]*t2*t22+P[13][4]*t2*t27);
|
||||
A0[14][0] = -t78*(P[14][0]*t2*t5+P[14][5]*t2*t8-P[14][6]*t2*t10+P[14][1]*t2*t16-P[14][2]*t2*t19+P[14][3]*t2*t22+P[14][4]*t2*t27);
|
||||
A0[15][0] = -t78*(P[15][0]*t2*t5+P[15][5]*t2*t8-P[15][6]*t2*t10+P[15][1]*t2*t16-P[15][2]*t2*t19+P[15][3]*t2*t22+P[15][4]*t2*t27);
|
||||
A0[16][0] = -t78*(P[16][0]*t2*t5+P[16][5]*t2*t8-P[16][6]*t2*t10+P[16][1]*t2*t16-P[16][2]*t2*t19+P[16][3]*t2*t22+P[16][4]*t2*t27);
|
||||
A0[17][0] = -t78*(P[17][0]*t2*t5+P[17][5]*t2*t8-P[17][6]*t2*t10+P[17][1]*t2*t16-P[17][2]*t2*t19+P[17][3]*t2*t22+P[17][4]*t2*t27);
|
||||
A0[18][0] = -t78*(P[18][0]*t2*t5+P[18][5]*t2*t8-P[18][6]*t2*t10+P[18][1]*t2*t16-P[18][2]*t2*t19+P[18][3]*t2*t22+P[18][4]*t2*t27);
|
||||
A0[19][0] = -t78*(P[19][0]*t2*t5+P[19][5]*t2*t8-P[19][6]*t2*t10+P[19][1]*t2*t16-P[19][2]*t2*t19+P[19][3]*t2*t22+P[19][4]*t2*t27);
|
||||
A0[20][0] = -t78*(P[20][0]*t2*t5+P[20][5]*t2*t8-P[20][6]*t2*t10+P[20][1]*t2*t16-P[20][2]*t2*t19+P[20][3]*t2*t22+P[20][4]*t2*t27);
|
||||
A0[21][0] = -t78*(P[21][0]*t2*t5+P[21][5]*t2*t8-P[21][6]*t2*t10+P[21][1]*t2*t16-P[21][2]*t2*t19+P[21][3]*t2*t22+P[21][4]*t2*t27);
|
||||
A0[22][0] = -t78*(P[22][0]*t2*t5+P[22][5]*t2*t8-P[22][6]*t2*t10+P[22][1]*t2*t16-P[22][2]*t2*t19+P[22][3]*t2*t22+P[22][4]*t2*t27);
|
||||
A0[23][0] = -t78*(P[23][0]*t2*t5+P[23][5]*t2*t8-P[23][6]*t2*t10+P[23][1]*t2*t16-P[23][2]*t2*t19+P[23][3]*t2*t22+P[23][4]*t2*t27);
|
||||
|
|
@ -1,116 +0,0 @@
|
|||
float t2 = q0*q3*2.0f;
|
||||
float t3 = q1*q2*2.0f;
|
||||
float t4 = t2+t3;
|
||||
float t5 = q0*q0;
|
||||
float t6 = q1*q1;
|
||||
float t7 = q2*q2;
|
||||
float t8 = q3*q3;
|
||||
float t9 = t5+t6-t7-t8;
|
||||
float t10 = q0*q2*2.0f;
|
||||
float t25 = q1*q3*2.0f;
|
||||
float t11 = t10-t25;
|
||||
float t12 = q3*ve*2.0f;
|
||||
float t13 = q0*vn*2.0f;
|
||||
float t26 = q2*vd*2.0f;
|
||||
float t14 = t12+t13-t26;
|
||||
float t15 = q3*vd*2.0f;
|
||||
float t16 = q2*ve*2.0f;
|
||||
float t17 = q1*vn*2.0f;
|
||||
float t18 = t15+t16+t17;
|
||||
float t19 = q0*vd*2.0f;
|
||||
float t20 = q2*vn*2.0f;
|
||||
float t27 = q1*ve*2.0f;
|
||||
float t21 = t19+t20-t27;
|
||||
float t22 = q1*vd*2.0f;
|
||||
float t23 = q0*ve*2.0f;
|
||||
float t28 = q3*vn*2.0f;
|
||||
float t24 = t22+t23-t28;
|
||||
float t29 = P[0][0]*t14;
|
||||
float t30 = P[1][1]*t18;
|
||||
float t31 = P[4][5]*t9;
|
||||
float t32 = P[5][5]*t4;
|
||||
float t33 = P[0][5]*t14;
|
||||
float t34 = P[1][5]*t18;
|
||||
float t35 = P[3][5]*t24;
|
||||
float t79 = P[6][5]*t11;
|
||||
float t80 = P[2][5]*t21;
|
||||
float t36 = t31+t32+t33+t34+t35-t79-t80;
|
||||
float t37 = t4*t36;
|
||||
float t38 = P[4][6]*t9;
|
||||
float t39 = P[5][6]*t4;
|
||||
float t40 = P[0][6]*t14;
|
||||
float t41 = P[1][6]*t18;
|
||||
float t42 = P[3][6]*t24;
|
||||
float t81 = P[6][6]*t11;
|
||||
float t82 = P[2][6]*t21;
|
||||
float t43 = t38+t39+t40+t41+t42-t81-t82;
|
||||
float t44 = P[4][0]*t9;
|
||||
float t45 = P[5][0]*t4;
|
||||
float t46 = P[1][0]*t18;
|
||||
float t47 = P[3][0]*t24;
|
||||
float t84 = P[6][0]*t11;
|
||||
float t85 = P[2][0]*t21;
|
||||
float t48 = t29+t44+t45+t46+t47-t84-t85;
|
||||
float t49 = t14*t48;
|
||||
float t50 = P[4][1]*t9;
|
||||
float t51 = P[5][1]*t4;
|
||||
float t52 = P[0][1]*t14;
|
||||
float t53 = P[3][1]*t24;
|
||||
float t86 = P[6][1]*t11;
|
||||
float t87 = P[2][1]*t21;
|
||||
float t54 = t30+t50+t51+t52+t53-t86-t87;
|
||||
float t55 = t18*t54;
|
||||
float t56 = P[4][2]*t9;
|
||||
float t57 = P[5][2]*t4;
|
||||
float t58 = P[0][2]*t14;
|
||||
float t59 = P[1][2]*t18;
|
||||
float t60 = P[3][2]*t24;
|
||||
float t78 = P[2][2]*t21;
|
||||
float t88 = P[6][2]*t11;
|
||||
float t61 = t56+t57+t58+t59+t60-t78-t88;
|
||||
float t62 = P[4][3]*t9;
|
||||
float t63 = P[5][3]*t4;
|
||||
float t64 = P[0][3]*t14;
|
||||
float t65 = P[1][3]*t18;
|
||||
float t66 = P[3][3]*t24;
|
||||
float t90 = P[6][3]*t11;
|
||||
float t91 = P[2][3]*t21;
|
||||
float t67 = t62+t63+t64+t65+t66-t90-t91;
|
||||
float t68 = t24*t67;
|
||||
float t69 = P[4][4]*t9;
|
||||
float t70 = P[5][4]*t4;
|
||||
float t71 = P[0][4]*t14;
|
||||
float t72 = P[1][4]*t18;
|
||||
float t73 = P[3][4]*t24;
|
||||
float t92 = P[6][4]*t11;
|
||||
float t93 = P[2][4]*t21;
|
||||
float t74 = t69+t70+t71+t72+t73-t92-t93;
|
||||
float t75 = t9*t74;
|
||||
float t83 = t11*t43;
|
||||
float t89 = t21*t61;
|
||||
float t76 = R_VEL+t37+t49+t55+t68+t75-t83-t89;
|
||||
float t77 = 1.0f/t76;
|
||||
Kfusion[0] = t77*(t29+P[0][5]*t4+P[0][4]*t9-P[0][6]*t11+P[0][1]*t18-P[0][2]*t21+P[0][3]*t24);
|
||||
Kfusion[1] = t77*(t30+P[1][5]*t4+P[1][4]*t9+P[1][0]*t14-P[1][6]*t11-P[1][2]*t21+P[1][3]*t24);
|
||||
Kfusion[2] = t77*(-t78+P[2][5]*t4+P[2][4]*t9+P[2][0]*t14-P[2][6]*t11+P[2][1]*t18+P[2][3]*t24);
|
||||
Kfusion[3] = t77*(t66+P[3][5]*t4+P[3][4]*t9+P[3][0]*t14-P[3][6]*t11+P[3][1]*t18-P[3][2]*t21);
|
||||
Kfusion[4] = t77*(t69+P[4][5]*t4+P[4][0]*t14-P[4][6]*t11+P[4][1]*t18-P[4][2]*t21+P[4][3]*t24);
|
||||
Kfusion[5] = t77*(t32+P[5][4]*t9+P[5][0]*t14-P[5][6]*t11+P[5][1]*t18-P[5][2]*t21+P[5][3]*t24);
|
||||
Kfusion[6] = t77*(-t81+P[6][5]*t4+P[6][4]*t9+P[6][0]*t14+P[6][1]*t18-P[6][2]*t21+P[6][3]*t24);
|
||||
Kfusion[7] = t77*(P[7][5]*t4+P[7][4]*t9+P[7][0]*t14-P[7][6]*t11+P[7][1]*t18-P[7][2]*t21+P[7][3]*t24);
|
||||
Kfusion[8] = t77*(P[8][5]*t4+P[8][4]*t9+P[8][0]*t14-P[8][6]*t11+P[8][1]*t18-P[8][2]*t21+P[8][3]*t24);
|
||||
Kfusion[9] = t77*(P[9][5]*t4+P[9][4]*t9+P[9][0]*t14-P[9][6]*t11+P[9][1]*t18-P[9][2]*t21+P[9][3]*t24);
|
||||
Kfusion[10] = t77*(P[10][5]*t4+P[10][4]*t9+P[10][0]*t14-P[10][6]*t11+P[10][1]*t18-P[10][2]*t21+P[10][3]*t24);
|
||||
Kfusion[11] = t77*(P[11][5]*t4+P[11][4]*t9+P[11][0]*t14-P[11][6]*t11+P[11][1]*t18-P[11][2]*t21+P[11][3]*t24);
|
||||
Kfusion[12] = t77*(P[12][5]*t4+P[12][4]*t9+P[12][0]*t14-P[12][6]*t11+P[12][1]*t18-P[12][2]*t21+P[12][3]*t24);
|
||||
Kfusion[13] = t77*(P[13][5]*t4+P[13][4]*t9+P[13][0]*t14-P[13][6]*t11+P[13][1]*t18-P[13][2]*t21+P[13][3]*t24);
|
||||
Kfusion[14] = t77*(P[14][5]*t4+P[14][4]*t9+P[14][0]*t14-P[14][6]*t11+P[14][1]*t18-P[14][2]*t21+P[14][3]*t24);
|
||||
Kfusion[15] = t77*(P[15][5]*t4+P[15][4]*t9+P[15][0]*t14-P[15][6]*t11+P[15][1]*t18-P[15][2]*t21+P[15][3]*t24);
|
||||
Kfusion[16] = t77*(P[16][5]*t4+P[16][4]*t9+P[16][0]*t14-P[16][6]*t11+P[16][1]*t18-P[16][2]*t21+P[16][3]*t24);
|
||||
Kfusion[17] = t77*(P[17][5]*t4+P[17][4]*t9+P[17][0]*t14-P[17][6]*t11+P[17][1]*t18-P[17][2]*t21+P[17][3]*t24);
|
||||
Kfusion[18] = t77*(P[18][5]*t4+P[18][4]*t9+P[18][0]*t14-P[18][6]*t11+P[18][1]*t18-P[18][2]*t21+P[18][3]*t24);
|
||||
Kfusion[19] = t77*(P[19][5]*t4+P[19][4]*t9+P[19][0]*t14-P[19][6]*t11+P[19][1]*t18-P[19][2]*t21+P[19][3]*t24);
|
||||
Kfusion[20] = t77*(P[20][5]*t4+P[20][4]*t9+P[20][0]*t14-P[20][6]*t11+P[20][1]*t18-P[20][2]*t21+P[20][3]*t24);
|
||||
Kfusion[21] = t77*(P[21][5]*t4+P[21][4]*t9+P[21][0]*t14-P[21][6]*t11+P[21][1]*t18-P[21][2]*t21+P[21][3]*t24);
|
||||
Kfusion[22] = t77*(P[22][5]*t4+P[22][4]*t9+P[22][0]*t14-P[22][6]*t11+P[22][1]*t18-P[22][2]*t21+P[22][3]*t24);
|
||||
Kfusion[23] = t77*(P[23][5]*t4+P[23][4]*t9+P[23][0]*t14-P[23][6]*t11+P[23][1]*t18-P[23][2]*t21+P[23][3]*t24);
|
||||
|
|
@ -1,116 +0,0 @@
|
|||
float t2 = q0*q3*2.0f;
|
||||
float t9 = q1*q2*2.0f;
|
||||
float t3 = t2-t9;
|
||||
float t4 = q0*q0;
|
||||
float t5 = q1*q1;
|
||||
float t6 = q2*q2;
|
||||
float t7 = q3*q3;
|
||||
float t8 = t4-t5+t6-t7;
|
||||
float t10 = q0*q1*2.0f;
|
||||
float t11 = q2*q3*2.0f;
|
||||
float t12 = t10+t11;
|
||||
float t13 = q1*vd*2.0f;
|
||||
float t14 = q0*ve*2.0f;
|
||||
float t26 = q3*vn*2.0f;
|
||||
float t15 = t13+t14-t26;
|
||||
float t16 = q0*vd*2.0f;
|
||||
float t17 = q2*vn*2.0f;
|
||||
float t27 = q1*ve*2.0f;
|
||||
float t18 = t16+t17-t27;
|
||||
float t19 = q3*vd*2.0f;
|
||||
float t20 = q2*ve*2.0f;
|
||||
float t21 = q1*vn*2.0f;
|
||||
float t22 = t19+t20+t21;
|
||||
float t23 = q3*ve*2.0f;
|
||||
float t24 = q0*vn*2.0f;
|
||||
float t28 = q2*vd*2.0f;
|
||||
float t25 = t23+t24-t28;
|
||||
float t29 = P[0][0]*t15;
|
||||
float t30 = P[1][1]*t18;
|
||||
float t31 = P[5][4]*t8;
|
||||
float t32 = P[6][4]*t12;
|
||||
float t33 = P[0][4]*t15;
|
||||
float t34 = P[1][4]*t18;
|
||||
float t35 = P[2][4]*t22;
|
||||
float t78 = P[4][4]*t3;
|
||||
float t79 = P[3][4]*t25;
|
||||
float t36 = t31+t32+t33+t34+t35-t78-t79;
|
||||
float t37 = P[5][6]*t8;
|
||||
float t38 = P[6][6]*t12;
|
||||
float t39 = P[0][6]*t15;
|
||||
float t40 = P[1][6]*t18;
|
||||
float t41 = P[2][6]*t22;
|
||||
float t81 = P[4][6]*t3;
|
||||
float t82 = P[3][6]*t25;
|
||||
float t42 = t37+t38+t39+t40+t41-t81-t82;
|
||||
float t43 = t12*t42;
|
||||
float t44 = P[5][0]*t8;
|
||||
float t45 = P[6][0]*t12;
|
||||
float t46 = P[1][0]*t18;
|
||||
float t47 = P[2][0]*t22;
|
||||
float t83 = P[4][0]*t3;
|
||||
float t84 = P[3][0]*t25;
|
||||
float t48 = t29+t44+t45+t46+t47-t83-t84;
|
||||
float t49 = t15*t48;
|
||||
float t50 = P[5][1]*t8;
|
||||
float t51 = P[6][1]*t12;
|
||||
float t52 = P[0][1]*t15;
|
||||
float t53 = P[2][1]*t22;
|
||||
float t85 = P[4][1]*t3;
|
||||
float t86 = P[3][1]*t25;
|
||||
float t54 = t30+t50+t51+t52+t53-t85-t86;
|
||||
float t55 = t18*t54;
|
||||
float t56 = P[5][2]*t8;
|
||||
float t57 = P[6][2]*t12;
|
||||
float t58 = P[0][2]*t15;
|
||||
float t59 = P[1][2]*t18;
|
||||
float t60 = P[2][2]*t22;
|
||||
float t87 = P[4][2]*t3;
|
||||
float t88 = P[3][2]*t25;
|
||||
float t61 = t56+t57+t58+t59+t60-t87-t88;
|
||||
float t62 = t22*t61;
|
||||
float t63 = P[5][3]*t8;
|
||||
float t64 = P[6][3]*t12;
|
||||
float t65 = P[0][3]*t15;
|
||||
float t66 = P[1][3]*t18;
|
||||
float t67 = P[2][3]*t22;
|
||||
float t89 = P[4][3]*t3;
|
||||
float t90 = P[3][3]*t25;
|
||||
float t68 = t63+t64+t65+t66+t67-t89-t90;
|
||||
float t69 = P[5][5]*t8;
|
||||
float t70 = P[6][5]*t12;
|
||||
float t71 = P[0][5]*t15;
|
||||
float t72 = P[1][5]*t18;
|
||||
float t73 = P[2][5]*t22;
|
||||
float t92 = P[4][5]*t3;
|
||||
float t93 = P[3][5]*t25;
|
||||
float t74 = t69+t70+t71+t72+t73-t92-t93;
|
||||
float t75 = t8*t74;
|
||||
float t80 = t3*t36;
|
||||
float t91 = t25*t68;
|
||||
float t76 = R_VEL+t43+t49+t55+t62+t75-t80-t91;
|
||||
float t77 = 1.0f/t76;
|
||||
Kfusion[0] = t77*(t29-P[0][4]*t3+P[0][5]*t8+P[0][6]*t12+P[0][1]*t18+P[0][2]*t22-P[0][3]*t25);
|
||||
Kfusion[1] = t77*(t30-P[1][4]*t3+P[1][5]*t8+P[1][0]*t15+P[1][6]*t12+P[1][2]*t22-P[1][3]*t25);
|
||||
Kfusion[2] = t77*(t60-P[2][4]*t3+P[2][5]*t8+P[2][0]*t15+P[2][6]*t12+P[2][1]*t18-P[2][3]*t25);
|
||||
Kfusion[3] = t77*(-t90-P[3][4]*t3+P[3][5]*t8+P[3][0]*t15+P[3][6]*t12+P[3][1]*t18+P[3][2]*t22);
|
||||
Kfusion[4] = t77*(-t78+P[4][5]*t8+P[4][0]*t15+P[4][6]*t12+P[4][1]*t18+P[4][2]*t22-P[4][3]*t25);
|
||||
Kfusion[5] = t77*(t69-P[5][4]*t3+P[5][0]*t15+P[5][6]*t12+P[5][1]*t18+P[5][2]*t22-P[5][3]*t25);
|
||||
Kfusion[6] = t77*(t38-P[6][4]*t3+P[6][5]*t8+P[6][0]*t15+P[6][1]*t18+P[6][2]*t22-P[6][3]*t25);
|
||||
Kfusion[7] = t77*(-P[7][4]*t3+P[7][5]*t8+P[7][0]*t15+P[7][6]*t12+P[7][1]*t18+P[7][2]*t22-P[7][3]*t25);
|
||||
Kfusion[8] = t77*(-P[8][4]*t3+P[8][5]*t8+P[8][0]*t15+P[8][6]*t12+P[8][1]*t18+P[8][2]*t22-P[8][3]*t25);
|
||||
Kfusion[9] = t77*(-P[9][4]*t3+P[9][5]*t8+P[9][0]*t15+P[9][6]*t12+P[9][1]*t18+P[9][2]*t22-P[9][3]*t25);
|
||||
Kfusion[10] = t77*(-P[10][4]*t3+P[10][5]*t8+P[10][0]*t15+P[10][6]*t12+P[10][1]*t18+P[10][2]*t22-P[10][3]*t25);
|
||||
Kfusion[11] = t77*(-P[11][4]*t3+P[11][5]*t8+P[11][0]*t15+P[11][6]*t12+P[11][1]*t18+P[11][2]*t22-P[11][3]*t25);
|
||||
Kfusion[12] = t77*(-P[12][4]*t3+P[12][5]*t8+P[12][0]*t15+P[12][6]*t12+P[12][1]*t18+P[12][2]*t22-P[12][3]*t25);
|
||||
Kfusion[13] = t77*(-P[13][4]*t3+P[13][5]*t8+P[13][0]*t15+P[13][6]*t12+P[13][1]*t18+P[13][2]*t22-P[13][3]*t25);
|
||||
Kfusion[14] = t77*(-P[14][4]*t3+P[14][5]*t8+P[14][0]*t15+P[14][6]*t12+P[14][1]*t18+P[14][2]*t22-P[14][3]*t25);
|
||||
Kfusion[15] = t77*(-P[15][4]*t3+P[15][5]*t8+P[15][0]*t15+P[15][6]*t12+P[15][1]*t18+P[15][2]*t22-P[15][3]*t25);
|
||||
Kfusion[16] = t77*(-P[16][4]*t3+P[16][5]*t8+P[16][0]*t15+P[16][6]*t12+P[16][1]*t18+P[16][2]*t22-P[16][3]*t25);
|
||||
Kfusion[17] = t77*(-P[17][4]*t3+P[17][5]*t8+P[17][0]*t15+P[17][6]*t12+P[17][1]*t18+P[17][2]*t22-P[17][3]*t25);
|
||||
Kfusion[18] = t77*(-P[18][4]*t3+P[18][5]*t8+P[18][0]*t15+P[18][6]*t12+P[18][1]*t18+P[18][2]*t22-P[18][3]*t25);
|
||||
Kfusion[19] = t77*(-P[19][4]*t3+P[19][5]*t8+P[19][0]*t15+P[19][6]*t12+P[19][1]*t18+P[19][2]*t22-P[19][3]*t25);
|
||||
Kfusion[20] = t77*(-P[20][4]*t3+P[20][5]*t8+P[20][0]*t15+P[20][6]*t12+P[20][1]*t18+P[20][2]*t22-P[20][3]*t25);
|
||||
Kfusion[21] = t77*(-P[21][4]*t3+P[21][5]*t8+P[21][0]*t15+P[21][6]*t12+P[21][1]*t18+P[21][2]*t22-P[21][3]*t25);
|
||||
Kfusion[22] = t77*(-P[22][4]*t3+P[22][5]*t8+P[22][0]*t15+P[22][6]*t12+P[22][1]*t18+P[22][2]*t22-P[22][3]*t25);
|
||||
Kfusion[23] = t77*(-P[23][4]*t3+P[23][5]*t8+P[23][0]*t15+P[23][6]*t12+P[23][1]*t18+P[23][2]*t22-P[23][3]*t25);
|
||||
|
|
@ -1,116 +0,0 @@
|
|||
float t2 = q0*q2*2.0;
|
||||
float t3 = q1*q3*2.0;
|
||||
float t4 = t2+t3;
|
||||
float t5 = q0*q0;
|
||||
float t6 = q1*q1;
|
||||
float t7 = q2*q2;
|
||||
float t8 = q3*q3;
|
||||
float t9 = t5-t6-t7+t8;
|
||||
float t10 = q0*q1*2.0;
|
||||
float t25 = q2*q3*2.0;
|
||||
float t11 = t10-t25;
|
||||
float t12 = q0*vd*2.0;
|
||||
float t13 = q2*vn*2.0;
|
||||
float t26 = q1*ve*2.0;
|
||||
float t14 = t12+t13-t26;
|
||||
float t15 = q1*vd*2.0;
|
||||
float t16 = q0*ve*2.0;
|
||||
float t27 = q3*vn*2.0;
|
||||
float t17 = t15+t16-t27;
|
||||
float t18 = q3*ve*2.0;
|
||||
float t19 = q0*vn*2.0;
|
||||
float t28 = q2*vd*2.0;
|
||||
float t20 = t18+t19-t28;
|
||||
float t21 = q3*vd*2.0;
|
||||
float t22 = q2*ve*2.0;
|
||||
float t23 = q1*vn*2.0;
|
||||
float t24 = t21+t22+t23;
|
||||
float t29 = P[0][0]*t14;
|
||||
float t30 = P[6][4]*t9;
|
||||
float t31 = P[4][4]*t4;
|
||||
float t32 = P[0][4]*t14;
|
||||
float t33 = P[2][4]*t20;
|
||||
float t34 = P[3][4]*t24;
|
||||
float t78 = P[5][4]*t11;
|
||||
float t79 = P[1][4]*t17;
|
||||
float t35 = t30+t31+t32+t33+t34-t78-t79;
|
||||
float t36 = t4*t35;
|
||||
float t37 = P[6][5]*t9;
|
||||
float t38 = P[4][5]*t4;
|
||||
float t39 = P[0][5]*t14;
|
||||
float t40 = P[2][5]*t20;
|
||||
float t41 = P[3][5]*t24;
|
||||
float t80 = P[5][5]*t11;
|
||||
float t81 = P[1][5]*t17;
|
||||
float t42 = t37+t38+t39+t40+t41-t80-t81;
|
||||
float t43 = P[6][0]*t9;
|
||||
float t44 = P[4][0]*t4;
|
||||
float t45 = P[2][0]*t20;
|
||||
float t46 = P[3][0]*t24;
|
||||
float t83 = P[5][0]*t11;
|
||||
float t84 = P[1][0]*t17;
|
||||
float t47 = t29+t43+t44+t45+t46-t83-t84;
|
||||
float t48 = t14*t47;
|
||||
float t49 = P[6][1]*t9;
|
||||
float t50 = P[4][1]*t4;
|
||||
float t51 = P[0][1]*t14;
|
||||
float t52 = P[2][1]*t20;
|
||||
float t53 = P[3][1]*t24;
|
||||
float t85 = P[5][1]*t11;
|
||||
float t86 = P[1][1]*t17;
|
||||
float t54 = t49+t50+t51+t52+t53-t85-t86;
|
||||
float t55 = P[6][2]*t9;
|
||||
float t56 = P[4][2]*t4;
|
||||
float t57 = P[0][2]*t14;
|
||||
float t58 = P[2][2]*t20;
|
||||
float t59 = P[3][2]*t24;
|
||||
float t88 = P[5][2]*t11;
|
||||
float t89 = P[1][2]*t17;
|
||||
float t60 = t55+t56+t57+t58+t59-t88-t89;
|
||||
float t61 = t20*t60;
|
||||
float t62 = P[6][3]*t9;
|
||||
float t63 = P[4][3]*t4;
|
||||
float t64 = P[0][3]*t14;
|
||||
float t65 = P[2][3]*t20;
|
||||
float t66 = P[3][3]*t24;
|
||||
float t90 = P[5][3]*t11;
|
||||
float t91 = P[1][3]*t17;
|
||||
float t67 = t62+t63+t64+t65+t66-t90-t91;
|
||||
float t68 = t24*t67;
|
||||
float t69 = P[6][6]*t9;
|
||||
float t70 = P[4][6]*t4;
|
||||
float t71 = P[0][6]*t14;
|
||||
float t72 = P[2][6]*t20;
|
||||
float t73 = P[3][6]*t24;
|
||||
float t92 = P[5][6]*t11;
|
||||
float t93 = P[1][6]*t17;
|
||||
float t74 = t69+t70+t71+t72+t73-t92-t93;
|
||||
float t75 = t9*t74;
|
||||
float t82 = t11*t42;
|
||||
float t87 = t17*t54;
|
||||
float t76 = R_VEL+t36+t48+t61+t68+t75-t82-t87;
|
||||
float t77 = 1.0f/t76;
|
||||
Kfusion[0] = t77*(t29+P[0][4]*t4+P[0][6]*t9-P[0][5]*t11-P[0][1]*t17+P[0][2]*t20+P[0][3]*t24);
|
||||
Kfusion[1] = t77*(P[1][4]*t4+P[1][0]*t14+P[1][6]*t9-P[1][5]*t11-P[1][1]*t17+P[1][2]*t20+P[1][3]*t24);
|
||||
Kfusion[2] = t77*(t58+P[2][4]*t4+P[2][0]*t14+P[2][6]*t9-P[2][5]*t11-P[2][1]*t17+P[2][3]*t24);
|
||||
Kfusion[3] = t77*(t66+P[3][4]*t4+P[3][0]*t14+P[3][6]*t9-P[3][5]*t11-P[3][1]*t17+P[3][2]*t20);
|
||||
Kfusion[4] = t77*(t31+P[4][0]*t14+P[4][6]*t9-P[4][5]*t11-P[4][1]*t17+P[4][2]*t20+P[4][3]*t24);
|
||||
Kfusion[5] = t77*(-t80+P[5][4]*t4+P[5][0]*t14+P[5][6]*t9-P[5][1]*t17+P[5][2]*t20+P[5][3]*t24);
|
||||
Kfusion[6] = t77*(t69+P[6][4]*t4+P[6][0]*t14-P[6][5]*t11-P[6][1]*t17+P[6][2]*t20+P[6][3]*t24);
|
||||
Kfusion[7] = t77*(P[7][4]*t4+P[7][0]*t14+P[7][6]*t9-P[7][5]*t11-P[7][1]*t17+P[7][2]*t20+P[7][3]*t24);
|
||||
Kfusion[8] = t77*(P[8][4]*t4+P[8][0]*t14+P[8][6]*t9-P[8][5]*t11-P[8][1]*t17+P[8][2]*t20+P[8][3]*t24);
|
||||
Kfusion[9] = t77*(P[9][4]*t4+P[9][0]*t14+P[9][6]*t9-P[9][5]*t11-P[9][1]*t17+P[9][2]*t20+P[9][3]*t24);
|
||||
Kfusion[10] = t77*(P[10][4]*t4+P[10][0]*t14+P[10][6]*t9-P[10][5]*t11-P[10][1]*t17+P[10][2]*t20+P[10][3]*t24);
|
||||
Kfusion[11] = t77*(P[11][4]*t4+P[11][0]*t14+P[11][6]*t9-P[11][5]*t11-P[11][1]*t17+P[11][2]*t20+P[11][3]*t24);
|
||||
Kfusion[12] = t77*(P[12][4]*t4+P[12][0]*t14+P[12][6]*t9-P[12][5]*t11-P[12][1]*t17+P[12][2]*t20+P[12][3]*t24);
|
||||
Kfusion[13] = t77*(P[13][4]*t4+P[13][0]*t14+P[13][6]*t9-P[13][5]*t11-P[13][1]*t17+P[13][2]*t20+P[13][3]*t24);
|
||||
Kfusion[14] = t77*(P[14][4]*t4+P[14][0]*t14+P[14][6]*t9-P[14][5]*t11-P[14][1]*t17+P[14][2]*t20+P[14][3]*t24);
|
||||
Kfusion[15] = t77*(P[15][4]*t4+P[15][0]*t14+P[15][6]*t9-P[15][5]*t11-P[15][1]*t17+P[15][2]*t20+P[15][3]*t24);
|
||||
Kfusion[16] = t77*(P[16][4]*t4+P[16][0]*t14+P[16][6]*t9-P[16][5]*t11-P[16][1]*t17+P[16][2]*t20+P[16][3]*t24);
|
||||
Kfusion[17] = t77*(P[17][4]*t4+P[17][0]*t14+P[17][6]*t9-P[17][5]*t11-P[17][1]*t17+P[17][2]*t20+P[17][3]*t24);
|
||||
Kfusion[18] = t77*(P[18][4]*t4+P[18][0]*t14+P[18][6]*t9-P[18][5]*t11-P[18][1]*t17+P[18][2]*t20+P[18][3]*t24);
|
||||
Kfusion[19] = t77*(P[19][4]*t4+P[19][0]*t14+P[19][6]*t9-P[19][5]*t11-P[19][1]*t17+P[19][2]*t20+P[19][3]*t24);
|
||||
Kfusion[20] = t77*(P[20][4]*t4+P[20][0]*t14+P[20][6]*t9-P[20][5]*t11-P[20][1]*t17+P[20][2]*t20+P[20][3]*t24);
|
||||
Kfusion[21] = t77*(P[21][4]*t4+P[21][0]*t14+P[21][6]*t9-P[21][5]*t11-P[21][1]*t17+P[21][2]*t20+P[21][3]*t24);
|
||||
Kfusion[22] = t77*(P[22][4]*t4+P[22][0]*t14+P[22][6]*t9-P[22][5]*t11-P[22][1]*t17+P[22][2]*t20+P[22][3]*t24);
|
||||
Kfusion[23] = t77*(P[23][4]*t4+P[23][0]*t14+P[23][6]*t9-P[23][5]*t11-P[23][1]*t17+P[23][2]*t20+P[23][3]*t24);
|
||||
|
|
@ -1,160 +0,0 @@
|
|||
float t2 = 1.0f/range;
|
||||
float t3 = Tbs.b.y*q0*2.0f;
|
||||
float t4 = Tbs.b.x*q3*2.0f;
|
||||
float t18 = Tbs.b.z*q1*2.0f;
|
||||
float t5 = t3+t4-t18;
|
||||
float t6 = Tbs.b.y*q1*2.0f;
|
||||
float t7 = Tbs.b.z*q0*2.0f;
|
||||
float t16 = Tbs.b.x*q2*2.0f;
|
||||
float t8 = t6+t7-t16;
|
||||
float t9 = Tbs.b.x*q0*2.0f;
|
||||
float t10 = Tbs.b.z*q2*2.0f;
|
||||
float t17 = Tbs.b.y*q3*2.0f;
|
||||
float t11 = t9+t10-t17;
|
||||
float t12 = Tbs.b.x*q1*2.0f;
|
||||
float t13 = Tbs.b.y*q2*2.0f;
|
||||
float t14 = Tbs.b.z*q3*2.0f;
|
||||
float t15 = t12+t13+t14;
|
||||
float t19 = q0*q0;
|
||||
float t20 = q1*q1;
|
||||
float t21 = q2*q2;
|
||||
float t22 = q3*q3;
|
||||
float t23 = q0*q3*2.0f;
|
||||
float t24 = q0*q2*2.0f;
|
||||
float t25 = q1*q3*2.0f;
|
||||
float t26 = q0*q1*2.0f;
|
||||
float t27 = t19+t20-t21-t22;
|
||||
float t28 = Tbs.b.x*t27;
|
||||
float t29 = q1*q2*2.0f;
|
||||
float t30 = t24+t25;
|
||||
float t31 = Tbs.b.z*t30;
|
||||
float t32 = t19-t20+t21-t22;
|
||||
float t33 = Tbs.b.y*t32;
|
||||
float t34 = t23+t29;
|
||||
float t35 = Tbs.b.x*t34;
|
||||
float t36 = q2*q3*2.0f;
|
||||
float t37 = t19-t20-t21+t22;
|
||||
float t38 = Tbs.b.z*t37;
|
||||
float t39 = t24-t25;
|
||||
float t40 = t26+t36;
|
||||
float t41 = Tbs.b.y*t40;
|
||||
float t60 = Tbs.b.x*t39;
|
||||
float t42 = t38+t41-t60;
|
||||
float t43 = t8*vd;
|
||||
float t44 = t5*ve;
|
||||
float t45 = t11*vn;
|
||||
float t46 = t43+t44+t45;
|
||||
float t47 = t5*vd;
|
||||
float t48 = t15*vn;
|
||||
float t62 = t8*ve;
|
||||
float t49 = t47+t48-t62;
|
||||
float t50 = t15*ve;
|
||||
float t51 = t8*vn;
|
||||
float t63 = t11*vd;
|
||||
float t52 = t50+t51-t63;
|
||||
float t53 = t15*vd;
|
||||
float t54 = t11*ve;
|
||||
float t64 = t5*vn;
|
||||
float t55 = t53+t54-t64;
|
||||
float t56 = t23-t29;
|
||||
float t65 = Tbs.b.y*t56;
|
||||
float t57 = t28+t31-t65;
|
||||
float t58 = t26-t36;
|
||||
float t66 = Tbs.b.z*t58;
|
||||
float t59 = t33+t35-t66;
|
||||
float t61 = P[0][0]*t2*t46;
|
||||
float t67 = P[1][1]*t2*t49;
|
||||
float t68 = P[4][0]*t2*t57;
|
||||
float t69 = P[5][0]*t2*t59;
|
||||
float t70 = P[6][0]*t2*t42;
|
||||
float t71 = P[1][0]*t2*t49;
|
||||
float t72 = P[2][0]*t2*t52;
|
||||
float t73 = P[3][0]*t2*t55;
|
||||
float t74 = t61+t68+t69+t70+t71+t72+t73;
|
||||
float t75 = t2*t46*t74;
|
||||
float t76 = P[4][1]*t2*t57;
|
||||
float t77 = P[5][1]*t2*t59;
|
||||
float t78 = P[6][1]*t2*t42;
|
||||
float t79 = P[0][1]*t2*t46;
|
||||
float t80 = P[2][1]*t2*t52;
|
||||
float t81 = P[3][1]*t2*t55;
|
||||
float t82 = t67+t76+t77+t78+t79+t80+t81;
|
||||
float t83 = t2*t49*t82;
|
||||
float t84 = P[4][2]*t2*t57;
|
||||
float t85 = P[5][2]*t2*t59;
|
||||
float t86 = P[6][2]*t2*t42;
|
||||
float t87 = P[0][2]*t2*t46;
|
||||
float t88 = P[1][2]*t2*t49;
|
||||
float t89 = P[2][2]*t2*t52;
|
||||
float t90 = P[3][2]*t2*t55;
|
||||
float t91 = t84+t85+t86+t87+t88+t89+t90;
|
||||
float t92 = t2*t52*t91;
|
||||
float t93 = P[4][3]*t2*t57;
|
||||
float t94 = P[5][3]*t2*t59;
|
||||
float t95 = P[6][3]*t2*t42;
|
||||
float t96 = P[0][3]*t2*t46;
|
||||
float t97 = P[1][3]*t2*t49;
|
||||
float t98 = P[2][3]*t2*t52;
|
||||
float t99 = P[3][3]*t2*t55;
|
||||
float t100 = t93+t94+t95+t96+t97+t98+t99;
|
||||
float t101 = t2*t55*t100;
|
||||
float t102 = P[4][4]*t2*t57;
|
||||
float t103 = P[5][4]*t2*t59;
|
||||
float t104 = P[6][4]*t2*t42;
|
||||
float t105 = P[0][4]*t2*t46;
|
||||
float t106 = P[1][4]*t2*t49;
|
||||
float t107 = P[2][4]*t2*t52;
|
||||
float t108 = P[3][4]*t2*t55;
|
||||
float t109 = t102+t103+t104+t105+t106+t107+t108;
|
||||
float t110 = t2*t57*t109;
|
||||
float t111 = P[4][5]*t2*t57;
|
||||
float t112 = P[5][5]*t2*t59;
|
||||
float t113 = P[6][5]*t2*t42;
|
||||
float t114 = P[0][5]*t2*t46;
|
||||
float t115 = P[1][5]*t2*t49;
|
||||
float t116 = P[2][5]*t2*t52;
|
||||
float t117 = P[3][5]*t2*t55;
|
||||
float t118 = t111+t112+t113+t114+t115+t116+t117;
|
||||
float t119 = t2*t59*t118;
|
||||
float t120 = P[4][6]*t2*t57;
|
||||
float t121 = P[5][6]*t2*t59;
|
||||
float t122 = P[6][6]*t2*t42;
|
||||
float t123 = P[0][6]*t2*t46;
|
||||
float t124 = P[1][6]*t2*t49;
|
||||
float t125 = P[2][6]*t2*t52;
|
||||
float t126 = P[3][6]*t2*t55;
|
||||
float t127 = t120+t121+t122+t123+t124+t125+t126;
|
||||
float t128 = t2*t42*t127;
|
||||
float t129 = R_LOS+t75+t83+t92+t101+t110+t119+t128;
|
||||
float t130 = 1.0f/t129;
|
||||
H_LOS[0] = t2*t46;
|
||||
H_LOS[1] = t2*t49;
|
||||
H_LOS[2] = t2*t52;
|
||||
H_LOS[3] = t2*t55;
|
||||
H_LOS[4] = t2*(t28+t31-Tbs.b.y*(t23-q1*q2*2.0));
|
||||
H_LOS[5] = t2*(t33+t35-Tbs.b.z*(t26-q2*q3*2.0));
|
||||
H_LOS[6] = t2*t42;
|
||||
Kfusion[0] = t130*(t61+P[0][6]*t2*t42+P[0][1]*t2*t49+P[0][2]*t2*t52+P[0][3]*t2*t55+P[0][4]*t2*t57+P[0][5]*t2*t59);
|
||||
Kfusion[1] = t130*(t67+P[1][0]*t2*t46+P[1][6]*t2*t42+P[1][2]*t2*t52+P[1][3]*t2*t55+P[1][4]*t2*t57+P[1][5]*t2*t59);
|
||||
Kfusion[2] = t130*(t89+P[2][0]*t2*t46+P[2][6]*t2*t42+P[2][1]*t2*t49+P[2][3]*t2*t55+P[2][4]*t2*t57+P[2][5]*t2*t59);
|
||||
Kfusion[3] = t130*(t99+P[3][0]*t2*t46+P[3][6]*t2*t42+P[3][1]*t2*t49+P[3][2]*t2*t52+P[3][4]*t2*t57+P[3][5]*t2*t59);
|
||||
Kfusion[4] = t130*(t102+P[4][0]*t2*t46+P[4][6]*t2*t42+P[4][1]*t2*t49+P[4][2]*t2*t52+P[4][3]*t2*t55+P[4][5]*t2*t59);
|
||||
Kfusion[5] = t130*(t112+P[5][0]*t2*t46+P[5][6]*t2*t42+P[5][1]*t2*t49+P[5][2]*t2*t52+P[5][3]*t2*t55+P[5][4]*t2*t57);
|
||||
Kfusion[6] = t130*(t122+P[6][0]*t2*t46+P[6][1]*t2*t49+P[6][2]*t2*t52+P[6][3]*t2*t55+P[6][4]*t2*t57+P[6][5]*t2*t59);
|
||||
Kfusion[7] = t130*(P[7][0]*t2*t46+P[7][6]*t2*t42+P[7][1]*t2*t49+P[7][2]*t2*t52+P[7][3]*t2*t55+P[7][4]*t2*t57+P[7][5]*t2*t59);
|
||||
Kfusion[8] = t130*(P[8][0]*t2*t46+P[8][6]*t2*t42+P[8][1]*t2*t49+P[8][2]*t2*t52+P[8][3]*t2*t55+P[8][4]*t2*t57+P[8][5]*t2*t59);
|
||||
Kfusion[9] = t130*(P[9][0]*t2*t46+P[9][6]*t2*t42+P[9][1]*t2*t49+P[9][2]*t2*t52+P[9][3]*t2*t55+P[9][4]*t2*t57+P[9][5]*t2*t59);
|
||||
Kfusion[10] = t130*(P[10][0]*t2*t46+P[10][6]*t2*t42+P[10][1]*t2*t49+P[10][2]*t2*t52+P[10][3]*t2*t55+P[10][4]*t2*t57+P[10][5]*t2*t59);
|
||||
Kfusion[11] = t130*(P[11][0]*t2*t46+P[11][6]*t2*t42+P[11][1]*t2*t49+P[11][2]*t2*t52+P[11][3]*t2*t55+P[11][4]*t2*t57+P[11][5]*t2*t59);
|
||||
Kfusion[12] = t130*(P[12][0]*t2*t46+P[12][6]*t2*t42+P[12][1]*t2*t49+P[12][2]*t2*t52+P[12][3]*t2*t55+P[12][4]*t2*t57+P[12][5]*t2*t59);
|
||||
Kfusion[13] = t130*(P[13][0]*t2*t46+P[13][6]*t2*t42+P[13][1]*t2*t49+P[13][2]*t2*t52+P[13][3]*t2*t55+P[13][4]*t2*t57+P[13][5]*t2*t59);
|
||||
Kfusion[14] = t130*(P[14][0]*t2*t46+P[14][6]*t2*t42+P[14][1]*t2*t49+P[14][2]*t2*t52+P[14][3]*t2*t55+P[14][4]*t2*t57+P[14][5]*t2*t59);
|
||||
Kfusion[15] = t130*(P[15][0]*t2*t46+P[15][6]*t2*t42+P[15][1]*t2*t49+P[15][2]*t2*t52+P[15][3]*t2*t55+P[15][4]*t2*t57+P[15][5]*t2*t59);
|
||||
Kfusion[16] = t130*(P[16][0]*t2*t46+P[16][6]*t2*t42+P[16][1]*t2*t49+P[16][2]*t2*t52+P[16][3]*t2*t55+P[16][4]*t2*t57+P[16][5]*t2*t59);
|
||||
Kfusion[17] = t130*(P[17][0]*t2*t46+P[17][6]*t2*t42+P[17][1]*t2*t49+P[17][2]*t2*t52+P[17][3]*t2*t55+P[17][4]*t2*t57+P[17][5]*t2*t59);
|
||||
Kfusion[18] = t130*(P[18][0]*t2*t46+P[18][6]*t2*t42+P[18][1]*t2*t49+P[18][2]*t2*t52+P[18][3]*t2*t55+P[18][4]*t2*t57+P[18][5]*t2*t59);
|
||||
Kfusion[19] = t130*(P[19][0]*t2*t46+P[19][6]*t2*t42+P[19][1]*t2*t49+P[19][2]*t2*t52+P[19][3]*t2*t55+P[19][4]*t2*t57+P[19][5]*t2*t59);
|
||||
Kfusion[20] = t130*(P[20][0]*t2*t46+P[20][6]*t2*t42+P[20][1]*t2*t49+P[20][2]*t2*t52+P[20][3]*t2*t55+P[20][4]*t2*t57+P[20][5]*t2*t59);
|
||||
Kfusion[21] = t130*(P[21][0]*t2*t46+P[21][6]*t2*t42+P[21][1]*t2*t49+P[21][2]*t2*t52+P[21][3]*t2*t55+P[21][4]*t2*t57+P[21][5]*t2*t59);
|
||||
Kfusion[22] = t130*(P[22][0]*t2*t46+P[22][6]*t2*t42+P[22][1]*t2*t49+P[22][2]*t2*t52+P[22][3]*t2*t55+P[22][4]*t2*t57+P[22][5]*t2*t59);
|
||||
Kfusion[23] = t130*(P[23][0]*t2*t46+P[23][6]*t2*t42+P[23][1]*t2*t49+P[23][2]*t2*t52+P[23][3]*t2*t55+P[23][4]*t2*t57+P[23][5]*t2*t59);
|
||||
|
|
@ -1,162 +0,0 @@
|
|||
float t2 = 1.0f/range;
|
||||
float t3 = Tbs.a.y*q0*2.0f;
|
||||
float t4 = Tbs.a.x*q3*2.0f;
|
||||
float t18 = Tbs.a.z*q1*2.0f;
|
||||
float t5 = t3+t4-t18;
|
||||
float t6 = Tbs.a.y*q1*2.0f;
|
||||
float t7 = Tbs.a.z*q0*2.0f;
|
||||
float t16 = Tbs.a.x*q2*2.0f;
|
||||
float t8 = t6+t7-t16;
|
||||
float t9 = Tbs.a.x*q0*2.0f;
|
||||
float t10 = Tbs.a.z*q2*2.0f;
|
||||
float t17 = Tbs.a.y*q3*2.0f;
|
||||
float t11 = t9+t10-t17;
|
||||
float t12 = Tbs.a.x*q1*2.0f;
|
||||
float t13 = Tbs.a.y*q2*2.0f;
|
||||
float t14 = Tbs.a.z*q3*2.0f;
|
||||
float t15 = t12+t13+t14;
|
||||
float t19 = q0*q0;
|
||||
float t20 = q1*q1;
|
||||
float t21 = q2*q2;
|
||||
float t22 = q3*q3;
|
||||
float t23 = q0*q3*2.0f;
|
||||
float t24 = q0*q2*2.0f;
|
||||
float t25 = q1*q3*2.0f;
|
||||
float t26 = q0*q1*2.0f;
|
||||
float t27 = t19+t20-t21-t22;
|
||||
float t28 = Tbs.a.x*t27;
|
||||
float t29 = q1*q2*2.0f;
|
||||
float t30 = t24+t25;
|
||||
float t31 = Tbs.a.z*t30;
|
||||
float t32 = t19-t20+t21-t22;
|
||||
float t33 = Tbs.a.y*t32;
|
||||
float t34 = t23+t29;
|
||||
float t35 = Tbs.a.x*t34;
|
||||
float t36 = q2*q3*2.0f;
|
||||
float t37 = t19-t20-t21+t22;
|
||||
float t38 = Tbs.a.z*t37;
|
||||
float t39 = t24-t25;
|
||||
float t40 = t26+t36;
|
||||
float t41 = Tbs.a.y*t40;
|
||||
float t60 = Tbs.a.x*t39;
|
||||
float t42 = t38+t41-t60;
|
||||
float t43 = t8*vd;
|
||||
float t44 = t5*ve;
|
||||
float t45 = t11*vn;
|
||||
float t46 = t43+t44+t45;
|
||||
float t47 = t5*vd;
|
||||
float t48 = t15*vn;
|
||||
float t62 = t8*ve;
|
||||
float t49 = t47+t48-t62;
|
||||
float t50 = t15*ve;
|
||||
float t51 = t8*vn;
|
||||
float t63 = t11*vd;
|
||||
float t52 = t50+t51-t63;
|
||||
float t53 = t15*vd;
|
||||
float t54 = t11*ve;
|
||||
float t64 = t5*vn;
|
||||
float t55 = t53+t54-t64;
|
||||
float t56 = t23-t29;
|
||||
float t65 = Tbs.a.y*t56;
|
||||
float t57 = t28+t31-t65;
|
||||
float t58 = t26-t36;
|
||||
float t66 = Tbs.a.z*t58;
|
||||
float t59 = t33+t35-t66;
|
||||
float t61 = P[0][0]*t2*t46;
|
||||
float t67 = P[1][1]*t2*t49;
|
||||
float t68 = P[4][0]*t2*t57;
|
||||
float t69 = P[5][0]*t2*t59;
|
||||
float t70 = P[6][0]*t2*t42;
|
||||
float t71 = P[1][0]*t2*t49;
|
||||
float t72 = P[2][0]*t2*t52;
|
||||
float t73 = P[3][0]*t2*t55;
|
||||
float t74 = t61+t68+t69+t70+t71+t72+t73;
|
||||
float t75 = t2*t46*t74;
|
||||
float t76 = P[4][1]*t2*t57;
|
||||
float t77 = P[5][1]*t2*t59;
|
||||
float t78 = P[6][1]*t2*t42;
|
||||
float t79 = P[0][1]*t2*t46;
|
||||
float t80 = P[2][1]*t2*t52;
|
||||
float t81 = P[3][1]*t2*t55;
|
||||
float t82 = t67+t76+t77+t78+t79+t80+t81;
|
||||
float t83 = t2*t49*t82;
|
||||
float t84 = P[4][2]*t2*t57;
|
||||
float t85 = P[5][2]*t2*t59;
|
||||
float t86 = P[6][2]*t2*t42;
|
||||
float t87 = P[0][2]*t2*t46;
|
||||
float t88 = P[1][2]*t2*t49;
|
||||
float t89 = P[2][2]*t2*t52;
|
||||
float t90 = P[3][2]*t2*t55;
|
||||
float t91 = t84+t85+t86+t87+t88+t89+t90;
|
||||
float t92 = t2*t52*t91;
|
||||
float t93 = P[4][3]*t2*t57;
|
||||
float t94 = P[5][3]*t2*t59;
|
||||
float t95 = P[6][3]*t2*t42;
|
||||
float t96 = P[0][3]*t2*t46;
|
||||
float t97 = P[1][3]*t2*t49;
|
||||
float t98 = P[2][3]*t2*t52;
|
||||
float t99 = P[3][3]*t2*t55;
|
||||
float t100 = t93+t94+t95+t96+t97+t98+t99;
|
||||
float t101 = t2*t55*t100;
|
||||
float t102 = P[4][4]*t2*t57;
|
||||
float t103 = P[5][4]*t2*t59;
|
||||
float t104 = P[6][4]*t2*t42;
|
||||
float t105 = P[0][4]*t2*t46;
|
||||
float t106 = P[1][4]*t2*t49;
|
||||
float t107 = P[2][4]*t2*t52;
|
||||
float t108 = P[3][4]*t2*t55;
|
||||
float t109 = t102+t103+t104+t105+t106+t107+t108;
|
||||
float t110 = t2*t57*t109;
|
||||
float t111 = P[4][5]*t2*t57;
|
||||
float t112 = P[5][5]*t2*t59;
|
||||
float t113 = P[6][5]*t2*t42;
|
||||
float t114 = P[0][5]*t2*t46;
|
||||
float t115 = P[1][5]*t2*t49;
|
||||
float t116 = P[2][5]*t2*t52;
|
||||
float t117 = P[3][5]*t2*t55;
|
||||
float t118 = t111+t112+t113+t114+t115+t116+t117;
|
||||
float t119 = t2*t59*t118;
|
||||
float t120 = P[4][6]*t2*t57;
|
||||
float t121 = P[5][6]*t2*t59;
|
||||
float t122 = P[6][6]*t2*t42;
|
||||
float t123 = P[0][6]*t2*t46;
|
||||
float t124 = P[1][6]*t2*t49;
|
||||
float t125 = P[2][6]*t2*t52;
|
||||
float t126 = P[3][6]*t2*t55;
|
||||
float t127 = t120+t121+t122+t123+t124+t125+t126;
|
||||
float t128 = t2*t42*t127;
|
||||
float t129 = R_LOS+t75+t83+t92+t101+t110+t119+t128;
|
||||
float t130 = 1.0f/t129;
|
||||
|
||||
H_LOS[0] = -t2*t46;
|
||||
H_LOS[1] = -t2*t49;
|
||||
H_LOS[2] = -t2*t52;
|
||||
H_LOS[3] = -t2*t55;
|
||||
H_LOS[4] = -t2*(t28+t31-Tbs.a.y*(t23-q1*q2*2.0f));
|
||||
H_LOS[5] = -t2*(t33+t35-Tbs.a.z*(t26-q2*q3*2.0f));
|
||||
H_LOS[6] = -t2*t42;
|
||||
|
||||
Kfusion[0] = -t130*(t61+P[0][6]*t2*t42+P[0][1]*t2*t49+P[0][2]*t2*t52+P[0][3]*t2*t55+P[0][4]*t2*t57+P[0][5]*t2*t59);
|
||||
Kfusion[1] = -t130*(t67+P[1][0]*t2*t46+P[1][6]*t2*t42+P[1][2]*t2*t52+P[1][3]*t2*t55+P[1][4]*t2*t57+P[1][5]*t2*t59);
|
||||
Kfusion[2] = -t130*(t89+P[2][0]*t2*t46+P[2][6]*t2*t42+P[2][1]*t2*t49+P[2][3]*t2*t55+P[2][4]*t2*t57+P[2][5]*t2*t59);
|
||||
Kfusion[3] = -t130*(t99+P[3][0]*t2*t46+P[3][6]*t2*t42+P[3][1]*t2*t49+P[3][2]*t2*t52+P[3][4]*t2*t57+P[3][5]*t2*t59);
|
||||
Kfusion[4] = -t130*(t102+P[4][0]*t2*t46+P[4][6]*t2*t42+P[4][1]*t2*t49+P[4][2]*t2*t52+P[4][3]*t2*t55+P[4][5]*t2*t59);
|
||||
Kfusion[5] = -t130*(t112+P[5][0]*t2*t46+P[5][6]*t2*t42+P[5][1]*t2*t49+P[5][2]*t2*t52+P[5][3]*t2*t55+P[5][4]*t2*t57);
|
||||
Kfusion[6] = -t130*(t122+P[6][0]*t2*t46+P[6][1]*t2*t49+P[6][2]*t2*t52+P[6][3]*t2*t55+P[6][4]*t2*t57+P[6][5]*t2*t59);
|
||||
Kfusion[7] = -t130*(P[7][0]*t2*t46+P[7][6]*t2*t42+P[7][1]*t2*t49+P[7][2]*t2*t52+P[7][3]*t2*t55+P[7][4]*t2*t57+P[7][5]*t2*t59);
|
||||
Kfusion[8] = -t130*(P[8][0]*t2*t46+P[8][6]*t2*t42+P[8][1]*t2*t49+P[8][2]*t2*t52+P[8][3]*t2*t55+P[8][4]*t2*t57+P[8][5]*t2*t59);
|
||||
Kfusion[9] = -t130*(P[9][0]*t2*t46+P[9][6]*t2*t42+P[9][1]*t2*t49+P[9][2]*t2*t52+P[9][3]*t2*t55+P[9][4]*t2*t57+P[9][5]*t2*t59);
|
||||
Kfusion[10] = -t130*(P[10][0]*t2*t46+P[10][6]*t2*t42+P[10][1]*t2*t49+P[10][2]*t2*t52+P[10][3]*t2*t55+P[10][4]*t2*t57+P[10][5]*t2*t59);
|
||||
Kfusion[11] = -t130*(P[11][0]*t2*t46+P[11][6]*t2*t42+P[11][1]*t2*t49+P[11][2]*t2*t52+P[11][3]*t2*t55+P[11][4]*t2*t57+P[11][5]*t2*t59);
|
||||
Kfusion[12] = -t130*(P[12][0]*t2*t46+P[12][6]*t2*t42+P[12][1]*t2*t49+P[12][2]*t2*t52+P[12][3]*t2*t55+P[12][4]*t2*t57+P[12][5]*t2*t59);
|
||||
Kfusion[13] = -t130*(P[13][0]*t2*t46+P[13][6]*t2*t42+P[13][1]*t2*t49+P[13][2]*t2*t52+P[13][3]*t2*t55+P[13][4]*t2*t57+P[13][5]*t2*t59);
|
||||
Kfusion[14] = -t130*(P[14][0]*t2*t46+P[14][6]*t2*t42+P[14][1]*t2*t49+P[14][2]*t2*t52+P[14][3]*t2*t55+P[14][4]*t2*t57+P[14][5]*t2*t59);
|
||||
Kfusion[15] = -t130*(P[15][0]*t2*t46+P[15][6]*t2*t42+P[15][1]*t2*t49+P[15][2]*t2*t52+P[15][3]*t2*t55+P[15][4]*t2*t57+P[15][5]*t2*t59);
|
||||
Kfusion[16] = -t130*(P[16][0]*t2*t46+P[16][6]*t2*t42+P[16][1]*t2*t49+P[16][2]*t2*t52+P[16][3]*t2*t55+P[16][4]*t2*t57+P[16][5]*t2*t59);
|
||||
Kfusion[17] = -t130*(P[17][0]*t2*t46+P[17][6]*t2*t42+P[17][1]*t2*t49+P[17][2]*t2*t52+P[17][3]*t2*t55+P[17][4]*t2*t57+P[17][5]*t2*t59);
|
||||
Kfusion[18] = -t130*(P[18][0]*t2*t46+P[18][6]*t2*t42+P[18][1]*t2*t49+P[18][2]*t2*t52+P[18][3]*t2*t55+P[18][4]*t2*t57+P[18][5]*t2*t59);
|
||||
Kfusion[19] = -t130*(P[19][0]*t2*t46+P[19][6]*t2*t42+P[19][1]*t2*t49+P[19][2]*t2*t52+P[19][3]*t2*t55+P[19][4]*t2*t57+P[19][5]*t2*t59);
|
||||
Kfusion[20] = -t130*(P[20][0]*t2*t46+P[20][6]*t2*t42+P[20][1]*t2*t49+P[20][2]*t2*t52+P[20][3]*t2*t55+P[20][4]*t2*t57+P[20][5]*t2*t59);
|
||||
Kfusion[21] = -t130*(P[21][0]*t2*t46+P[21][6]*t2*t42+P[21][1]*t2*t49+P[21][2]*t2*t52+P[21][3]*t2*t55+P[21][4]*t2*t57+P[21][5]*t2*t59);
|
||||
Kfusion[22] = -t130*(P[22][0]*t2*t46+P[22][6]*t2*t42+P[22][1]*t2*t49+P[22][2]*t2*t52+P[22][3]*t2*t55+P[22][4]*t2*t57+P[22][5]*t2*t59);
|
||||
Kfusion[23] = -t130*(P[23][0]*t2*t46+P[23][6]*t2*t42+P[23][1]*t2*t49+P[23][2]*t2*t52+P[23][3]*t2*t55+P[23][4]*t2*t57+P[23][5]*t2*t59);
|
||||
File diff suppressed because one or more lines are too long
Binary file not shown.
|
|
@ -1,5 +0,0 @@
|
|||
% normalise the quaternion
|
||||
function quaternion = normQuat(quaternion)
|
||||
|
||||
quatMag = sqrt(quaternion(1)^2 + quaternion(2)^2 + quaternion(3)^2 + quaternion(4)^2);
|
||||
quaternion(1:4) = quaternion / quatMag;
|
||||
|
|
@ -1,29 +0,0 @@
|
|||
function [SymExpOut,SubExpArray] = OptimiseAlgebra(SymExpIn,SubExpName)
|
||||
|
||||
% Loop through symbolic expression, identifying repeated expressions and
|
||||
% bringing them out as shared expression or sub expressions
|
||||
% do this until no further repeated expressions found
|
||||
% This can significantly reduce computations
|
||||
|
||||
syms SubExpIn SubExpArray ;
|
||||
|
||||
SubExpArray(1,1) = 'invalid';
|
||||
index = 0;
|
||||
f_complete = 0;
|
||||
while f_complete==0
|
||||
index = index + 1;
|
||||
SubExpIn = [SubExpName,'(',num2str(index),')'];
|
||||
SubExpInStore{index} = SubExpIn;
|
||||
[SymExpOut,SubExpOut]=subexpr(SymExpIn,SubExpIn);
|
||||
for k = 1:index
|
||||
if SubExpOut == SubExpInStore{k}
|
||||
f_complete = 1;
|
||||
end
|
||||
end
|
||||
if f_complete || index > 100
|
||||
SymExpOut = SymExpIn;
|
||||
else
|
||||
SubExpArray(index,1) = SubExpOut;
|
||||
SymExpIn = SymExpOut;
|
||||
end
|
||||
end
|
||||
|
|
@ -1,89 +0,0 @@
|
|||
float t3 = ve-vwe;
|
||||
float t4 = q0*q0;
|
||||
float t5 = q1*q1;
|
||||
float t6 = q2*q2;
|
||||
float t7 = q3*q3;
|
||||
float t8 = vd-vwd;
|
||||
float t9 = q0*q1*2.0f;
|
||||
float t10 = q2*q3*2.0f;
|
||||
float t11 = vn-vwn;
|
||||
float t13 = q0*q2*2.0f;
|
||||
float t14 = q1*q3*2.0f;
|
||||
float t18 = t4-t5-t6+t7;
|
||||
float t19 = t8*t18;
|
||||
float t20 = t9-t10;
|
||||
float t21 = t3*t20;
|
||||
float t22 = t13+t14;
|
||||
float t23 = t11*t22;
|
||||
float t2 = t19-t21+t23;
|
||||
float t15 = q0*q3*2.0f;
|
||||
float t16 = q1*q2*2.0f;
|
||||
float t24 = t9+t10;
|
||||
float t25 = t4-t5+t6-t7;
|
||||
float t26 = t3*t25;
|
||||
float t27 = t8*t24;
|
||||
float t28 = t15-t16;
|
||||
float t29 = t11*t28;
|
||||
float t12 = t26+t27-t29;
|
||||
float t30 = t13-t14;
|
||||
float t31 = t4+t5-t6-t7;
|
||||
float t32 = t11*t31;
|
||||
float t33 = t8*t30;
|
||||
float t34 = t15+t16;
|
||||
float t35 = t3*t34;
|
||||
float t17 = t32-t33+t35;
|
||||
float t44 = t2*t18*2.0f;
|
||||
float t45 = t12*t24*2.0f;
|
||||
float t46 = t17*t30*2.0f;
|
||||
float t36 = t44+t45-t46;
|
||||
float t37 = t2*t2;
|
||||
float t38 = t12*t12;
|
||||
float t39 = t17*t17;
|
||||
float t40 = t37+t38+t39;
|
||||
float t41 = 1.0f/t40;
|
||||
float t48 = t12*t25*2.0f;
|
||||
float t49 = t2*t20*2.0f;
|
||||
float t50 = t17*t34*2.0f;
|
||||
float t42 = t48-t49+t50;
|
||||
float t52 = t17*t31*2.0f;
|
||||
float t53 = t2*t22*2.0f;
|
||||
float t54 = t12*t28*2.0f;
|
||||
float t43 = t52+t53-t54;
|
||||
float t47 = t36*t36;
|
||||
float t51 = t42*t42;
|
||||
float t55 = t43*t43;
|
||||
float t57 = 1.0f/(t17*t17);
|
||||
float t58 = 1.0f/t17;
|
||||
float t63 = t18*t58;
|
||||
float t64 = t2*t30*t57;
|
||||
float t56 = t63+t64;
|
||||
float t66 = t22*t58;
|
||||
float t67 = t2*t31*t57;
|
||||
float t59 = t66-t67;
|
||||
float t60 = t37*t57;
|
||||
float t61 = t60+1.0f;
|
||||
float t62 = 1.0f/(t61*t61);
|
||||
float t65 = t56*t56;
|
||||
float t68 = t59*t59;
|
||||
float t70 = t20*t58;
|
||||
float t71 = t2*t34*t57;
|
||||
float t69 = t70+t71;
|
||||
float t72 = t69*t69;
|
||||
float t78 = t25*t58;
|
||||
float t79 = t12*t34*t57;
|
||||
float t73 = t78-t79;
|
||||
float t81 = t28*t58;
|
||||
float t82 = t12*t31*t57;
|
||||
float t74 = t81+t82;
|
||||
float t75 = t38*t57;
|
||||
float t76 = t75+1.0f;
|
||||
float t77 = 1.0f/(t76*t76);
|
||||
float t80 = t73*t73;
|
||||
float t83 = t74*t74;
|
||||
float t85 = t24*t58;
|
||||
float t86 = t12*t30*t57;
|
||||
float t84 = t85+t86;
|
||||
float t87 = t84*t84;
|
||||
float tas_var = t41*t47*vd_var*0.25f+t41*t51*ve_var*0.25f+t41*t55*vn_var*0.25f+t41*t47*vwd_var*0.25f+t41*t51*vwe_var*0.25f+t41*t55*vwn_var*0.25f;
|
||||
float aoa_var = t62*t65*vd_var+t62*t72*ve_var+t62*t68*vn_var+t62*t65*vwd_var+t62*t72*vwe_var+t62*t68*vwn_var;
|
||||
float aos_var = t77*t87*vd_var+t77*t80*ve_var+t77*t83*vn_var+t77*t87*vwd_var+t77*t80*vwe_var+t77*t83*vwn_var;
|
||||
|
|
@ -1,14 +0,0 @@
|
|||
function Tbn = Quat2Tbn(quat)
|
||||
|
||||
% Convert from quaternions defining the flight vehicles rotation to
|
||||
% the direction cosine matrix defining the rotation from body to navigation
|
||||
% coordinates
|
||||
|
||||
q0 = quat(1);
|
||||
q1 = quat(2);
|
||||
q2 = quat(3);
|
||||
q3 = quat(4);
|
||||
|
||||
Tbn = [q0^2 + q1^2 - q2^2 - q3^2, 2*(q1*q2 - q0*q3), 2*(q1*q3 + q0*q2); ...
|
||||
2*(q1*q2 + q0*q3), q0^2 - q1^2 + q2^2 - q3^2, 2*(q2*q3 - q0*q1); ...
|
||||
2*(q1*q3-q0*q2), 2*(q2*q3 + q0*q1), q0^2 - q1^2 - q2^2 + q3^2];
|
||||
|
|
@ -1,16 +0,0 @@
|
|||
function q_out = QuatDivide(qin1,qin2)
|
||||
|
||||
q0 = qin1(1);
|
||||
q1 = qin1(2);
|
||||
q2 = qin1(3);
|
||||
q3 = qin1(4);
|
||||
|
||||
r0 = qin2(1);
|
||||
r1 = qin2(2);
|
||||
r2 = qin2(3);
|
||||
r3 = qin2(4);
|
||||
|
||||
q_out(1,1) = (qin2(1)*qin1(1) + qin2(2)*qin1(2) + qin2(3)*qin1(3) + qin2(4)*qin1(4));
|
||||
q_out(2,1) = (r0*q1 - r1*q0 - r2*q3 + r3*q2);
|
||||
q_out(3,1) = (r0*q2 + r1*q3 - r2*q0 - r3*q1);
|
||||
q_out(4,1) = (r0*q3 - r1*q2 + r2*q1 - r3*q0);
|
||||
|
|
@ -1,62 +0,0 @@
|
|||
%% calculate the rotation vector variances from an equivalent quaternion
|
||||
% inputs are the quaternion orientation and the 4x4 covariance matrix for the quaternions
|
||||
% output is a vector of variances for the rotation vector that is equivalent to the quaternion
|
||||
clear all;
|
||||
reset(symengine);
|
||||
syms q0 q1 q2 q3 real % quaternions defining attitude of body axes relative to local NED
|
||||
|
||||
% define quaternion rotation
|
||||
quat = [q0;q1;q2;q3];
|
||||
|
||||
% convert to a rotation vector
|
||||
delta = 2*acos(q0);
|
||||
rotVec = (delta/sin(delta/2))*[q1;q2;q3];
|
||||
|
||||
% calculate transfer matrix from quaternion to rotation vector
|
||||
G = jacobian(rotVec, quat);
|
||||
|
||||
% define a symbolic covariance matrix using strings to represent
|
||||
% '_l_' to represent '( '
|
||||
% '_c_' to represent ,
|
||||
% '_r_' to represent ')'
|
||||
% these can be substituted later to create executable code
|
||||
for rowIndex = 1:4
|
||||
for colIndex = 1:4
|
||||
eval(['syms P_l_',num2str(rowIndex-1),'_c_',num2str(colIndex-1), '_r_ real']);
|
||||
eval(['quatCovMat(',num2str(rowIndex),',',num2str(colIndex), ') = P_l_',num2str(rowIndex-1),'_c_',num2str(colIndex-1),'_r_;']);
|
||||
end
|
||||
end
|
||||
|
||||
% rotate the covariance from quaternion to rotation vector
|
||||
rotCovMat = G*quatCovMat*transpose(G);
|
||||
|
||||
% take the variances
|
||||
rotVarVec = [rotCovMat(1,1);rotCovMat(2,2);rotCovMat(3,3)];
|
||||
|
||||
% convert to c-code
|
||||
ccode(rotVarVec,'file','rotVarVec.c');
|
||||
|
||||
%% calculate the quaternion variances from an equivalent rotation vector
|
||||
|
||||
% define a rotation vector
|
||||
syms rotX rotY rotZ real;
|
||||
rotVec = [rotX;rotY;rotZ];
|
||||
|
||||
% convert to a quaternion
|
||||
vecLength = sqrt(rotVec(1)^2 + rotVec(2)^2 + rotVec(3)^2);
|
||||
quat = [cos(0.5*vecLength); rotVec/vecLength*sin(0.5*vecLength)];
|
||||
|
||||
% calculate transfer matrix from rotation vector to quaternion
|
||||
G = jacobian(quat, rotVec);
|
||||
|
||||
% define the rotation vector variances
|
||||
syms rotVarX rotVarY rotVarZ real;
|
||||
|
||||
% define the rotation vector covariance matrix
|
||||
rotCovMat = diag([rotVarX;rotVarY;rotVarZ]);
|
||||
|
||||
% rotate the covariance matrix into quaternion coordinates
|
||||
quatCovMat = G*rotCovMat*transpose(G);
|
||||
|
||||
% convert to c-code
|
||||
ccode(quatCovMat,'file','quatCovMat.c');
|
||||
|
|
@ -1,5 +0,0 @@
|
|||
function quatOut = QuatMult(quatA,quatB)
|
||||
% Calculate the following quaternion product quatA * quatB using the
|
||||
% standard identity
|
||||
|
||||
quatOut = [quatA(1)*quatB(1)-quatA(2:4)'*quatB(2:4); quatA(1)*quatB(2:4) + quatB(1)*quatA(2:4) + cross(quatA(2:4),quatB(2:4))];
|
||||
|
|
@ -1,9 +0,0 @@
|
|||
% Convert from a quaternion to a 321 Euler rotation sequence in radians
|
||||
|
||||
function Euler = QuatToEul(quat)
|
||||
|
||||
Euler = zeros(3,1);
|
||||
|
||||
Euler(1) = atan2(2*(quat(3)*quat(4)+quat(1)*quat(2)), quat(1)*quat(1) - quat(2)*quat(2) - quat(3)*quat(3) + quat(4)*quat(4));
|
||||
Euler(2) = -asin(2*(quat(2)*quat(4)-quat(1)*quat(3)));
|
||||
Euler(3) = atan2(2*(quat(2)*quat(3)+quat(1)*quat(4)), quat(1)*quat(1) + quat(2)*quat(2) - quat(3)*quat(3) - quat(4)*quat(4));
|
||||
|
|
@ -1,10 +0,0 @@
|
|||
% convert froma rotation vector in radians to a quaternion
|
||||
function quaternion = RotToQuat(rotVec)
|
||||
|
||||
vecLength = sqrt(rotVec(1)^2 + rotVec(2)^2 + rotVec(3)^2);
|
||||
|
||||
if vecLength < 1e-6
|
||||
quaternion = [1;0;0;0];
|
||||
else
|
||||
quaternion = [cos(0.5*vecLength); rotVec/vecLength*sin(0.5*vecLength)];
|
||||
end
|
||||
|
|
@ -1,666 +0,0 @@
|
|||
function SaveScriptCode(nStates)
|
||||
%% Load Data
|
||||
fileName = strcat('SymbolicOutput',int2str(nStates),'.mat');
|
||||
load(fileName);
|
||||
|
||||
%% Open output file
|
||||
fileName = strcat('SymbolicOutput',int2str(nStates),'.txt');
|
||||
fid = fopen(fileName,'wt');
|
||||
|
||||
%% Write equation for state transition matrix
|
||||
if exist('SF','var')
|
||||
|
||||
fprintf(fid,'SF = zeros(%d,1);\n',numel(SF));
|
||||
for rowIndex = 1:numel(SF)
|
||||
string = char(SF(rowIndex,1));
|
||||
fprintf(fid,'SF(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
|
||||
% fprintf(fid,'\n');
|
||||
% fprintf(fid,'F = zeros(%d,%d);\n',nStates,nStates);
|
||||
% for rowIndex = 1:nStates
|
||||
% for colIndex = 1:nStates
|
||||
% string = char(F(rowIndex,colIndex));
|
||||
% % don't write out a zero-assignment
|
||||
% if ~strcmpi(string,'0')
|
||||
% fprintf(fid,'F(%d,%d) = %s;\n',rowIndex,colIndex,string);
|
||||
% end
|
||||
% end
|
||||
% end
|
||||
% fprintf(fid,'\n');
|
||||
|
||||
end
|
||||
%% Write equations for control influence (disturbance) matrix
|
||||
if exist('SG','var')
|
||||
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'SG = zeros(%d,1);\n',numel(SG));
|
||||
for rowIndex = 1:numel(SG)
|
||||
string = char(SG(rowIndex,1));
|
||||
fprintf(fid,'SG(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
% fprintf(fid,'\n');
|
||||
% fprintf(fid,'G = zeros(%d,%d);\n',nStates,numel([da;dv]));
|
||||
% for rowIndex = 1:nStates
|
||||
% for colIndex = 1:numel([da;dv])
|
||||
% string = char(G(rowIndex,colIndex));
|
||||
% % don't write out a zero-assignment
|
||||
% if ~strcmpi(string,'0')
|
||||
% fprintf(fid,'G(%d,%d) = %s;\n',rowIndex,colIndex,string);
|
||||
% end
|
||||
% end
|
||||
% end
|
||||
% fprintf(fid,'\n');
|
||||
|
||||
end
|
||||
%% Write equations for state error matrix
|
||||
if exist('SQ','var')
|
||||
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'SQ = zeros(%d,1);\n',numel(SQ));
|
||||
for rowIndex = 1:numel(SQ)
|
||||
string = char(SQ(rowIndex,1));
|
||||
fprintf(fid,'SQ(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
% fprintf(fid,'\n');
|
||||
% fprintf(fid,'Q = zeros(%d,%d);\n',nStates,nStates);
|
||||
% for rowIndex = 1:nStates
|
||||
% for colIndex = 1:nStates
|
||||
% string = char(Q(rowIndex,colIndex));
|
||||
% % don't write out a zero-assignment
|
||||
% if ~strcmpi(string,'0')
|
||||
% fprintf(fid,'Q(%d,%d) = %s;\n',rowIndex,colIndex,string);
|
||||
% end
|
||||
% end
|
||||
% end
|
||||
% fprintf(fid,'\n');
|
||||
|
||||
end
|
||||
%% Write equations for covariance prediction
|
||||
% Only write out upper diagonal (matrix is symmetric)
|
||||
if exist('SPP','var')
|
||||
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'SPP = zeros(%d,1);\n',numel(SPP));
|
||||
for rowIndex = 1:numel(SPP)
|
||||
string = char(SPP(rowIndex,1));
|
||||
fprintf(fid,'SPP(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
end
|
||||
|
||||
if exist('PP','var')
|
||||
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'nextP = zeros(%d,%d);\n',nStates,nStates);
|
||||
for colIndex = 1:nStates
|
||||
for rowIndex = 1:colIndex
|
||||
string = char(PP(rowIndex,colIndex));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'nextP(%d,%d) = %s;\n',rowIndex,colIndex,string);
|
||||
end
|
||||
end
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
end
|
||||
|
||||
%% Write equations for velocity and position data fusion
|
||||
if exist('H_VP','var')
|
||||
|
||||
[nRow,nCol] = size(H_VP);
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'H_VP = zeros(%d,%d);\n',nRow,nCol);
|
||||
for rowIndex = 1:nRow
|
||||
for colIndex = 1:nCol
|
||||
string = char(H_VP(rowIndex,colIndex));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'H_VP(%d,%d) = %s;\n',rowIndex,colIndex,string);
|
||||
end
|
||||
end
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
[nRow,nCol] = size(SK_VP);
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'SK_VP = zeros(%d,%d);\n',nRow,nCol);
|
||||
for rowIndex = 1:nRow
|
||||
for colIndex = 1:nCol
|
||||
string = char(SK_VP(rowIndex,colIndex));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'SK_VP(%d,%d) = %s;\n',rowIndex,colIndex,string);
|
||||
end
|
||||
end
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
[nRow,nCol] = size(K_VP);
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'Kfusion = zeros(%d,%d);\n',nRow,nCol);
|
||||
for rowIndex = 1:nRow
|
||||
for colIndex = 1:nCol
|
||||
string = char(K_VP(rowIndex,colIndex));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'Kfusion(%d,%d) = %s;\n',rowIndex,colIndex,string);
|
||||
end
|
||||
end
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
end
|
||||
%% Write equations for true airspeed data fusion
|
||||
if exist('SH_TAS','var')
|
||||
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'SH_TAS = zeros(%d,1);\n',numel(SH_TAS));
|
||||
for rowIndex = 1:numel(SH_TAS)
|
||||
string = char(SH_TAS(rowIndex,1));
|
||||
fprintf(fid,'SH_TAS(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
|
||||
[nRow,nCol] = size(H_TAS);
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'H_TAS = zeros(1,%d);\n',nCol);
|
||||
for rowIndex = 1:nRow
|
||||
for colIndex = 1:nCol
|
||||
string = char(H_TAS(rowIndex,colIndex));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'H_TAS(1,%d) = %s;\n',colIndex,string);
|
||||
end
|
||||
end
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'SK_TAS = zeros(%d,1);\n',numel(SK_TAS));
|
||||
for rowIndex = 1:numel(SK_TAS)
|
||||
string = char(SK_TAS(rowIndex,1));
|
||||
fprintf(fid,'SK_TAS(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
[nRow,nCol] = size(K_TAS);
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol);
|
||||
for rowIndex = 1:nRow
|
||||
string = char(K_TAS(rowIndex,1));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
end
|
||||
%% Write equations for sideslip data fusion
|
||||
if exist('SH_BETA','var')
|
||||
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'SH_BETA = zeros(%d,1);\n',numel(SH_BETA));
|
||||
for rowIndex = 1:numel(SH_BETA)
|
||||
string = char(SH_BETA(rowIndex,1));
|
||||
fprintf(fid,'SH_BETA(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
|
||||
[nRow,nCol] = size(H_BETA);
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'H_BETA = zeros(1,%d);\n',nCol);
|
||||
for rowIndex = 1:nRow
|
||||
for colIndex = 1:nCol
|
||||
string = char(H_BETA(rowIndex,colIndex));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'H_BETA(1,%d) = %s;\n',colIndex,string);
|
||||
end
|
||||
end
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'SK_BETA = zeros(%d,1);\n',numel(SK_BETA));
|
||||
for rowIndex = 1:numel(SK_BETA)
|
||||
string = char(SK_BETA(rowIndex,1));
|
||||
fprintf(fid,'SK_BETA(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
[nRow,nCol] = size(K_BETA);
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol);
|
||||
for rowIndex = 1:nRow
|
||||
string = char(K_BETA(rowIndex,1));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
end
|
||||
%% Write equations for magnetometer data fusion
|
||||
if exist('SH_MAG','var')
|
||||
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'SH_MAG = zeros(%d,1);\n',numel(SH_MAG));
|
||||
for rowIndex = 1:numel(SH_MAG)
|
||||
string = char(SH_MAG(rowIndex,1));
|
||||
fprintf(fid,'SH_MAG(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
[nRow,nCol] = size(H_MAG);
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'H_MAG = zeros(1,%d);\n',nCol);
|
||||
for colIndex = 1:nCol
|
||||
string = char(H_MAG(1,colIndex));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'H_MAG(%d) = %s;\n',colIndex,string);
|
||||
end
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'SK_MX = zeros(%d,1);\n',numel(SK_MX));
|
||||
for rowIndex = 1:numel(SK_MX)
|
||||
string = char(SK_MX(rowIndex,1));
|
||||
fprintf(fid,'SK_MX(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
[nRow,nCol] = size(K_MX);
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol);
|
||||
for rowIndex = 1:nRow
|
||||
string = char(K_MX(rowIndex,1));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
[nRow,nCol] = size(H_MAG);
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'H_MAG = zeros(1,%d);\n',nCol);
|
||||
for colIndex = 1:nCol
|
||||
string = char(H_MAG(2,colIndex));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'H_MAG(%d) = %s;\n',colIndex,string);
|
||||
end
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'SK_MY = zeros(%d,1);\n',numel(SK_MY));
|
||||
for rowIndex = 1:numel(SK_MY)
|
||||
string = char(SK_MY(rowIndex,1));
|
||||
fprintf(fid,'SK_MY(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
[nRow,nCol] = size(K_MY);
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol);
|
||||
for rowIndex = 1:nRow
|
||||
string = char(K_MY(rowIndex,1));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
[nRow,nCol] = size(H_MAG);
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'H_MAG = zeros(1,%d);\n',nCol);
|
||||
for colIndex = 1:nCol
|
||||
string = char(H_MAG(3,colIndex));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'H_MAG(%d) = %s;\n',colIndex,string);
|
||||
end
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'SK_MZ = zeros(%d,1);\n',numel(SK_MZ));
|
||||
for rowIndex = 1:numel(SK_MZ)
|
||||
string = char(SK_MZ(rowIndex,1));
|
||||
fprintf(fid,'SK_MZ(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
[nRow,nCol] = size(K_MZ);
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol);
|
||||
for rowIndex = 1:nRow
|
||||
string = char(K_MZ(rowIndex,1));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
end
|
||||
%% Write equations for optical flow sensor angular LOS data fusion
|
||||
if exist('SH_LOS','var')
|
||||
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'SH_LOS = zeros(%d,1);\n',numel(SH_LOS));
|
||||
for rowIndex = 1:numel(SH_LOS)
|
||||
string = char(SH_LOS(rowIndex,1));
|
||||
fprintf(fid,'SH_LOS(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
fprintf(fid,'\n');
|
||||
[nRow,nCol] = size(H_LOS);
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'H_LOS = zeros(1,%d);\n',nCol);
|
||||
for colIndex = 1:nCol
|
||||
string = char(H_LOS(1,colIndex));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'H_LOS(%d) = %s;\n',colIndex,string);
|
||||
end
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
fprintf(fid,'\n');
|
||||
[nRow,nCol] = size(H_LOS);
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'H_LOS = zeros(1,%d);\n',nCol);
|
||||
for colIndex = 1:nCol
|
||||
string = char(H_LOS(2,colIndex));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'H_LOS(%d) = %s;\n',colIndex,string);
|
||||
end
|
||||
end
|
||||
|
||||
% fprintf(fid,'\n');
|
||||
% fprintf(fid,'SKK_LOS = zeros(%d,1);\n',numel(SKK_LOS));
|
||||
% for rowIndex = 1:numel(SKK_LOS)
|
||||
% string = char(SKK_LOS(rowIndex,1));
|
||||
% fprintf(fid,'SKK_LOS(%d) = %s;\n',rowIndex,string);
|
||||
% end
|
||||
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'SK_LOS = zeros(%d,1);\n',numel(SK_LOS));
|
||||
for rowIndex = 1:numel(SK_LOS)
|
||||
string = char(SK_LOS(rowIndex,1));
|
||||
fprintf(fid,'SK_LOS(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
|
||||
[nRow,nCol] = size(K_LOSX);
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol);
|
||||
for rowIndex = 1:nRow
|
||||
string = char(K_LOSX(rowIndex));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
[nRow,nCol] = size(K_LOSY);
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol);
|
||||
for rowIndex = 1:nRow
|
||||
string = char(K_LOSY(rowIndex));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
end
|
||||
|
||||
end
|
||||
%% Write observation fusion equations for optical flow sensor scale factor error estimation
|
||||
if exist('SH_OPT','var')
|
||||
|
||||
fprintf(fid,'\n');
|
||||
for rowIndex = 1:numel(SH_OPT)
|
||||
string = char(SH_OPT(rowIndex,1));
|
||||
fprintf(fid,'SH_OPT(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
fprintf(fid,'\n');
|
||||
string = char(H_OPT(1));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'H_OPT(1) = %s;\n',1,string);
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
fprintf(fid,'\n');
|
||||
string = char(H_OPT(2));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'H_OPT(2) = %s;\n',1,string);
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
fprintf(fid,'\n');
|
||||
for rowIndex = 1:numel(SK_OPT)
|
||||
string = char(SK_OPT(rowIndex,1));
|
||||
fprintf(fid,'SK_OPT(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
|
||||
fprintf(fid,'\n');
|
||||
string = char(K_OPT(1));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'K_OPT(1) = %s;\n',1,string);
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
fprintf(fid,'\n');
|
||||
string = char(K_OPT(2));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'K_OPT(2) = %s;\n',1,string);
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
end
|
||||
%% Write equations for laser range finder data fusion
|
||||
if exist('SH_RNG','var')
|
||||
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'SH_RNG = zeros(%d,1);\n',numel(SH_RNG));
|
||||
for rowIndex = 1:numel(SH_RNG)
|
||||
string = char(SH_RNG(rowIndex,1));
|
||||
fprintf(fid,'SH_RNG(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
|
||||
[nRow,nCol] = size(H_RNG);
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'H_RNG = zeros(1,%d);\n',nCol);
|
||||
for rowIndex = 1:nRow
|
||||
for colIndex = 1:nCol
|
||||
string = char(H_RNG(rowIndex,colIndex));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'H_RNG(1,%d) = %s;\n',colIndex,string);
|
||||
end
|
||||
end
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'SK_RNG = zeros(%d,1);\n',numel(SK_RNG));
|
||||
for rowIndex = 1:numel(SK_RNG)
|
||||
string = char(SK_RNG(rowIndex,1));
|
||||
fprintf(fid,'SK_RNG(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
[nRow,nCol] = size(K_RNG);
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol);
|
||||
for rowIndex = 1:nRow
|
||||
string = char(K_RNG(rowIndex,1));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
end
|
||||
|
||||
%% Write equations for simple magnetomter data fusion
|
||||
if exist('SH_MAGS','var')
|
||||
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'SH_MAGS = zeros(%d,1);\n',numel(SH_MAGS));
|
||||
for rowIndex = 1:numel(SH_MAGS)
|
||||
string = char(SH_MAGS(rowIndex,1));
|
||||
fprintf(fid,'SH_MAGS(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
[nRow,nCol] = size(H_MAGS);
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'H_MAGS = zeros(1,%d);\n',nCol);
|
||||
for colIndex = 1:nCol
|
||||
string = char(H_MAGS(1,colIndex));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'H_MAGS(%d) = %s;\n',colIndex,string);
|
||||
end
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'SK_MAGS = zeros(%d,1);\n',numel(SK_MAGS));
|
||||
for rowIndex = 1:numel(SK_MAGS)
|
||||
string = char(SK_MAGS(rowIndex,1));
|
||||
fprintf(fid,'SK_MAGS(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
[nRow,nCol] = size(K_MAGS);
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol);
|
||||
for rowIndex = 1:nRow
|
||||
string = char(K_MAGS(rowIndex,1));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
end
|
||||
|
||||
%% Write equations for X accel fusion
|
||||
if exist('SH_ACCX','var')
|
||||
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'SH_ACCX = zeros(%d,1);\n',numel(SH_ACCX));
|
||||
for rowIndex = 1:numel(SH_ACCX)
|
||||
string = char(SH_ACCX(rowIndex,1));
|
||||
fprintf(fid,'SH_ACCX(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
|
||||
[nRow,nCol] = size(H_ACCX);
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'H_ACCX = zeros(1,%d);\n',nCol);
|
||||
for rowIndex = 1:nRow
|
||||
for colIndex = 1:nCol
|
||||
string = char(H_ACCX(rowIndex,colIndex));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'H_ACCX(1,%d) = %s;\n',colIndex,string);
|
||||
end
|
||||
end
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'SK_ACCX = zeros(%d,1);\n',numel(SK_ACCX));
|
||||
for rowIndex = 1:numel(SK_ACCX)
|
||||
string = char(SK_ACCX(rowIndex,1));
|
||||
fprintf(fid,'SK_ACCX(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
[nRow,nCol] = size(K_ACCX);
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol);
|
||||
for rowIndex = 1:nRow
|
||||
string = char(K_ACCX(rowIndex,1));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
end
|
||||
|
||||
%% Write equations for Y accel fusion
|
||||
if exist('SH_ACCY','var')
|
||||
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'SH_ACCY = zeros(%d,1);\n',numel(SH_ACCY));
|
||||
for rowIndex = 1:numel(SH_ACCY)
|
||||
string = char(SH_ACCY(rowIndex,1));
|
||||
fprintf(fid,'SH_ACCY(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
|
||||
[nRow,nCol] = size(H_ACCY);
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'H_ACCY = zeros(1,%d);\n',nCol);
|
||||
for rowIndex = 1:nRow
|
||||
for colIndex = 1:nCol
|
||||
string = char(H_ACCY(rowIndex,colIndex));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'H_ACCY(1,%d) = %s;\n',colIndex,string);
|
||||
end
|
||||
end
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'SK_ACCY = zeros(%d,1);\n',numel(SK_ACCY));
|
||||
for rowIndex = 1:numel(SK_ACCY)
|
||||
string = char(SK_ACCY(rowIndex,1));
|
||||
fprintf(fid,'SK_ACCY(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
[nRow,nCol] = size(K_ACCY);
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol);
|
||||
for rowIndex = 1:nRow
|
||||
string = char(K_ACCY(rowIndex,1));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
end
|
||||
|
||||
%% Close output file
|
||||
fclose(fid);
|
||||
|
||||
end
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
File diff suppressed because one or more lines are too long
|
|
@ -1,15 +0,0 @@
|
|||
t2 = sin(yaw);
|
||||
t3 = cos(yaw);
|
||||
t4 = sin(pitch);
|
||||
t5 = cos(pitch);
|
||||
t6 = sin(roll);
|
||||
t7 = cos(roll);
|
||||
A0[0][0] = t3*t5-t2*t4*t6;
|
||||
A0[0][1] = -t2*t7;
|
||||
A0[0][2] = t3*t4+t2*t5*t6;
|
||||
A0[1][0] = t2*t5+t3*t4*t6;
|
||||
A0[1][1] = t3*t7;
|
||||
A0[1][2] = t2*t4-t3*t5*t6;
|
||||
A0[2][0] = -t4*t7;
|
||||
A0[2][1] = t6;
|
||||
A0[2][2] = t5*t7;
|
||||
|
|
@ -1,15 +0,0 @@
|
|||
t2 = cos(yaw);
|
||||
t3 = sin(roll);
|
||||
t4 = sin(yaw);
|
||||
t5 = cos(roll);
|
||||
t6 = sin(pitch);
|
||||
t7 = cos(pitch);
|
||||
A0[0][0] = t2*t7;
|
||||
A0[0][1] = -t4*t5+t2*t3*t6;
|
||||
A0[0][2] = t3*t4+t2*t5*t6;
|
||||
A0[1][0] = t4*t7;
|
||||
A0[1][1] = t2*t5+t3*t4*t6;
|
||||
A0[1][2] = -t2*t3+t4*t5*t6;
|
||||
A0[2][0] = -t6;
|
||||
A0[2][1] = t3*t7;
|
||||
A0[2][2] = t5*t7;
|
||||
|
|
@ -1,17 +0,0 @@
|
|||
float t2 = q1*q2*2.0;
|
||||
float t3 = q0*q0;
|
||||
float t4 = q1*q1;
|
||||
float t5 = q2*q2;
|
||||
float t6 = q3*q3;
|
||||
float t7 = q0*q2*2.0;
|
||||
float t8 = q1*q3*2.0;
|
||||
float t9 = q2*q3*2.0;
|
||||
A0[0][0] = t3+t4-t5-t6;
|
||||
A0[0][1] = t2-q0*q3*2.0;
|
||||
A0[0][2] = t7+t8;
|
||||
A0[1][0] = t2+q0*q3*2.0;
|
||||
A0[1][1] = t3-t4+t5-t6;
|
||||
A0[1][2] = t9-q0*q1*2.0;
|
||||
A0[2][0] = -t7+t8;
|
||||
A0[2][1] = t9+q0*q1*2.0;
|
||||
A0[2][2] = t3-t4-t5+t6;
|
||||
|
|
@ -1,42 +0,0 @@
|
|||
/*
|
||||
C code fragment for function that enables the yaw uncertainty to be increased following a yaw reset.
|
||||
The variables _state.quat_nominal(0) -> _state.quat_nominal(3) are the attitude quaternions
|
||||
The variable daYawVar is the variance of the yaw angle uncertainty in rad**2
|
||||
See DeriveYawResetEquations.m for the derivation
|
||||
The gnerate autocode has been cleaned up with removal of 0 coefficient terms and mirroring of lower
|
||||
diagonal terms missing from the derivation script raw autocode output of C_code4.txt
|
||||
*/
|
||||
|
||||
// Intermediate variables
|
||||
float SG[3];
|
||||
SG[0] = sq(_state.quat_nominal(0)) - sq(_state.quat_nominal(1)) - sq(_state.quat_nominal(2)) + sq(_state.quat_nominal(3));
|
||||
SG[1] = 2*_state.quat_nominal(0)*_state.quat_nominal(2) - 2*_state.quat_nominal(1)*_state.quat_nominal(3);
|
||||
SG[2] = 2*_state.quat_nominal(0)*_state.quat_nominal(1) + 2*_state.quat_nominal(2)*_state.quat_nominal(3);
|
||||
|
||||
float SQ[4];
|
||||
SQ[0] = 0.5f * ((_state.quat_nominal(1)*SG[0]) - (_state.quat_nominal(0)*SG[2]) + (_state.quat_nominal(3)*SG[1]));
|
||||
SQ[1] = 0.5f * ((_state.quat_nominal(0)*SG[1]) - (_state.quat_nominal(2)*SG[0]) + (_state.quat_nominal(3)*SG[2]));
|
||||
SQ[2] = 0.5f * ((_state.quat_nominal(3)*SG[0]) - (_state.quat_nominal(1)*SG[1]) + (_state.quat_nominal(2)*SG[2]));
|
||||
SQ[3] = 0.5f * ((_state.quat_nominal(0)*SG[0]) + (_state.quat_nominal(1)*SG[2]) + (_state.quat_nominal(2)*SG[1]));
|
||||
|
||||
// Variance of yaw angle uncertainty (rad**2)
|
||||
const float daYawVar = TBD;
|
||||
|
||||
// Add covariances for additonal yaw uncertainty to existing covariances.
|
||||
// This assumes that the additional yaw error is uncorrrelated
|
||||
P[0][0] += yaw_variance*sq(SQ[2]);
|
||||
P[0][1] += yaw_variance*SQ[1]*SQ[2];
|
||||
P[1][1] += yaw_variance*sq(SQ[1]);
|
||||
P[0][2] += yaw_variance*SQ[0]*SQ[2];
|
||||
P[1][2] += yaw_variance*SQ[0]*SQ[1];
|
||||
P[2][2] += yaw_variance*sq(SQ[0]);
|
||||
P[0][3] -= yaw_variance*SQ[2]*SQ[3];
|
||||
P[1][3] -= yaw_variance*SQ[1]*SQ[3];
|
||||
P[2][3] -= yaw_variance*SQ[0]*SQ[3];
|
||||
P[3][3] += yaw_variance*sq(SQ[3]);
|
||||
P[1][0] += yaw_variance*SQ[1]*SQ[2];
|
||||
P[2][0] += yaw_variance*SQ[0]*SQ[2];
|
||||
P[2][1] += yaw_variance*SQ[0]*SQ[1];
|
||||
P[3][0] -= yaw_variance*SQ[2]*SQ[3];
|
||||
P[3][1] -= yaw_variance*SQ[1]*SQ[3];
|
||||
P[3][2] -= yaw_variance*SQ[0]*SQ[3];
|
||||
|
|
@ -1,38 +0,0 @@
|
|||
t9 = q0*q3;
|
||||
t10 = q1*q2;
|
||||
t2 = t9-t10;
|
||||
t3 = q0*q0;
|
||||
t4 = q1*q1;
|
||||
t5 = q2*q2;
|
||||
t6 = q3*q3;
|
||||
t7 = t3-t4+t5-t6;
|
||||
t8 = 1.0/(t7*t7);
|
||||
t11 = t2*t2;
|
||||
t12 = t8*t11*4.0;
|
||||
t13 = t12+1.0;
|
||||
t14 = 1.0/t13;
|
||||
t15 = 1.0/(t2*t2);
|
||||
t16 = q3*t3;
|
||||
t17 = q3*t4;
|
||||
t18 = t16+t17-q3*t5+q3*t6-q0*q1*q2*2.0;
|
||||
t19 = t7*t7;
|
||||
t20 = t15*t19*(1.0/4.0);
|
||||
t21 = t20+1.0;
|
||||
t22 = 1.0/t21;
|
||||
t23 = q2*t3;
|
||||
t24 = q2*t4;
|
||||
t25 = t23+t24+q2*t5-q2*t6-q0*q1*q3*2.0;
|
||||
t26 = q1*t5;
|
||||
t27 = q1*t6;
|
||||
t28 = t26+t27-q1*t3+q1*t4-q0*q2*q3*2.0;
|
||||
t29 = q0*t5;
|
||||
t30 = q0*t6;
|
||||
t31 = t29+t30+q0*t3-q0*t4-q1*q2*q3*2.0;
|
||||
A0[0][0] = t8*t14*t18*-2.0;
|
||||
A0[0][1] = t8*t14*t25*-2.0;
|
||||
A0[0][2] = t8*t14*t28*2.0;
|
||||
A0[0][3] = t8*t14*t31*2.0;
|
||||
A0[1][0] = t15*t18*t22*(-1.0/2.0);
|
||||
A0[1][1] = t15*t22*t25*(-1.0/2.0);
|
||||
A0[1][2] = t15*t22*t28*(1.0/2.0);
|
||||
A0[1][3] = t15*t22*t31*(1.0/2.0);
|
||||
|
|
@ -1,42 +0,0 @@
|
|||
t9 = q0*q3;
|
||||
t10 = q1*q2;
|
||||
t2 = t9+t10;
|
||||
t3 = q0*q0;
|
||||
t4 = q1*q1;
|
||||
t5 = q2*q2;
|
||||
t6 = q3*q3;
|
||||
t7 = t3+t4-t5-t6;
|
||||
t8 = 1.0/(t7*t7);
|
||||
t11 = t2*t2;
|
||||
t12 = t8*t11*4.0;
|
||||
t13 = t12+1.0;
|
||||
t14 = 1.0/t13;
|
||||
t15 = 1.0/(t2*t2);
|
||||
t16 = q3*t3;
|
||||
t17 = q3*t5;
|
||||
t18 = q0*q1*q2*2.0;
|
||||
t19 = t16+t17+t18-q3*t4+q3*t6;
|
||||
t20 = t7*t7;
|
||||
t21 = t15*t20*(1.0/4.0);
|
||||
t22 = t21+1.0;
|
||||
t23 = 1.0/t22;
|
||||
t24 = q2*t4;
|
||||
t25 = q2*t6;
|
||||
t26 = q0*q1*q3*2.0;
|
||||
t27 = t24+t25+t26-q2*t3+q2*t5;
|
||||
t28 = q1*t3;
|
||||
t29 = q1*t5;
|
||||
t30 = q0*q2*q3*2.0;
|
||||
t31 = t28+t29+t30+q1*t4-q1*t6;
|
||||
t32 = q0*t4;
|
||||
t33 = q0*t6;
|
||||
t34 = q1*q2*q3*2.0;
|
||||
t35 = t32+t33+t34+q0*t3-q0*t5;
|
||||
A0[0][0] = t8*t14*t19*-2.0;
|
||||
A0[0][1] = t8*t14*t27*-2.0;
|
||||
A0[0][2] = t8*t14*t31*2.0;
|
||||
A0[0][3] = t8*t14*t35*2.0;
|
||||
A0[1][0] = t15*t19*t23*(-1.0/2.0);
|
||||
A0[1][1] = t15*t23*t27*(-1.0/2.0);
|
||||
A0[1][2] = t15*t23*t31*(1.0/2.0);
|
||||
A0[1][3] = t15*t23*t35*(1.0/2.0);
|
||||
|
|
@ -1,38 +0,0 @@
|
|||
t2 = sin(ant_yaw);
|
||||
t3 = cos(ant_yaw);
|
||||
t4 = q0*q3*2.0;
|
||||
t5 = q0*q0;
|
||||
t6 = q1*q1;
|
||||
t7 = q2*q2;
|
||||
t8 = q3*q3;
|
||||
t9 = q1*q2*2.0;
|
||||
t10 = t5+t6-t7-t8;
|
||||
t11 = t3*t10;
|
||||
t12 = t4+t9;
|
||||
t13 = t3*t12;
|
||||
t14 = t5-t6+t7-t8;
|
||||
t15 = t2*t14;
|
||||
t16 = t13+t15;
|
||||
t17 = t4-t9;
|
||||
t19 = t2*t17;
|
||||
t20 = t11-t19;
|
||||
t18 = 1.0/(t20*t20);
|
||||
t21 = t16*t16;
|
||||
t22 = 1.0/pow(t11-t19][2.0);
|
||||
t23 = q1*t3*2.0;
|
||||
t24 = q2*t2*2.0;
|
||||
t25 = t23+t24;
|
||||
t26 = 1.0/t20;
|
||||
t27 = q1*t2*2.0;
|
||||
t28 = t21*t22;
|
||||
t29 = t28+1.0;
|
||||
t30 = 1.0/t29;
|
||||
t31 = q0*t3*2.0;
|
||||
t32 = t31-q3*t2*2.0;
|
||||
t33 = q3*t3*2.0;
|
||||
t34 = q0*t2*2.0;
|
||||
t35 = t33+t34;
|
||||
A0[0][0] = (t35/(t11-t2*(t4-q1*q2*2.0))-t16*t18*t32)/(t18*t21+1.0);
|
||||
A0[0][1] = -t30*(t26*(t27-q2*t3*2.0)+t16*t22*t25);
|
||||
A0[0][2] = t30*(t25*t26-t16*t22*(t27-q2*t3*2.0));
|
||||
A0[0][3] = t30*(t26*t32+t16*t22*t35);
|
||||
|
|
@ -1,46 +0,0 @@
|
|||
t2 = magE*magE;
|
||||
t3 = magN*magN;
|
||||
t4 = t2+t3;
|
||||
t5 = P[16][16]*t2;
|
||||
t6 = P[17][17]*t3;
|
||||
t7 = t2*t2;
|
||||
t8 = R_DECL*t7;
|
||||
t9 = t3*t3;
|
||||
t10 = R_DECL*t9;
|
||||
t11 = R_DECL*t2*t3*2.0;
|
||||
t14 = P[16][17]*magE*magN;
|
||||
t15 = P[17][16]*magE*magN;
|
||||
t12 = t5+t6+t8+t10+t11-t14-t15;
|
||||
t13 = 1.0/t12;
|
||||
t16 = conjugate(magE);
|
||||
t17 = conjugate(magN);
|
||||
t18 = t16*t16;
|
||||
t19 = t17*t17;
|
||||
t20 = t18+t19;
|
||||
t21 = 1.0/t20;
|
||||
A0[0][0] = -t4*t13*(P[0][16]*magE-P[0][17]*magN);
|
||||
A0[1][0] = -t4*t13*(P[1][16]*magE-P[1][17]*magN);
|
||||
A0[2][0] = -t4*t13*(P[2][16]*magE-P[2][17]*magN);
|
||||
A0[3][0] = -t4*t13*(P[3][16]*magE-P[3][17]*magN);
|
||||
A0[4][0] = -t4*t13*(P[4][16]*magE-P[4][17]*magN);
|
||||
A0[5][0] = -t4*t13*(P[5][16]*magE-P[5][17]*magN);
|
||||
A0[6][0] = -t4*t13*(P[6][16]*magE-P[6][17]*magN);
|
||||
A0[7][0] = -t4*t13*(P[7][16]*magE-P[7][17]*magN);
|
||||
A0[8][0] = -t4*t13*(P[8][16]*magE-P[8][17]*magN);
|
||||
A0[9][0] = -t4*t13*(P[9][16]*magE-P[9][17]*magN);
|
||||
A0[10][0] = -t4*t13*(P[10][16]*magE-P[10][17]*magN);
|
||||
A0[11][0] = -t4*t13*(P[11][16]*magE-P[11][17]*magN);
|
||||
A0[12][0] = -t4*t13*(P[12][16]*magE-P[12][17]*magN);
|
||||
A0[13][0] = -t4*t13*(P[13][16]*magE-P[13][17]*magN);
|
||||
A0[14][0] = -t4*t13*(P[14][16]*magE-P[14][17]*magN);
|
||||
A0[15][0] = -t4*t13*(P[15][16]*magE-P[15][17]*magN);
|
||||
A0[16][0] = -t4*t13*(P[16][16]*magE-P[16][17]*magN);
|
||||
A0[16][1] = -t16*t21;
|
||||
A0[17][0] = -t4*t13*(P[17][16]*magE-P[17][17]*magN);
|
||||
A0[17][1] = t17*t21;
|
||||
A0[18][0] = -t4*t13*(P[18][16]*magE-P[18][17]*magN);
|
||||
A0[19][0] = -t4*t13*(P[19][16]*magE-P[19][17]*magN);
|
||||
A0[20][0] = -t4*t13*(P[20][16]*magE-P[20][17]*magN);
|
||||
A0[21][0] = -t4*t13*(P[21][16]*magE-P[21][17]*magN);
|
||||
A0[22][0] = -t4*t13*(P[22][16]*magE-P[22][17]*magN);
|
||||
A0[23][0] = -t4*t13*(P[23][16]*magE-P[23][17]*magN);
|
||||
|
|
@ -1,97 +0,0 @@
|
|||
% This script generates c code required to calculate the variance of the
|
||||
% TAS, AoA and AoS estimates calculated from the vehicle quaternions, NED
|
||||
% velocity and NED wind velocity. Uncertainty in the quaternions is
|
||||
% ignored. Variance in vehicle velocity and wind velocity is accounted for.
|
||||
|
||||
%% calculate TAS error terms
|
||||
clear all;
|
||||
reset(symengine);
|
||||
|
||||
syms vn ve vd 'real' % navigation frame NED velocity (m/s)
|
||||
syms vwn vwe vwd 'real' % navigation frame NED wind velocity (m/s)
|
||||
syms vn_var ve_var vd_var 'real' % navigation frame NED velocity variances (m/s)^2
|
||||
syms vwn_var vwe_var vwd_var 'real' % navigation frame NED wind velocity variances (m/s)^2
|
||||
syms q0 q1 q2 q3 'real' % quaternions defining rotation from navigation NED frame to body XYZ frame
|
||||
|
||||
quat = [q0;q1;q2;q3];
|
||||
|
||||
% rotation matrix from navigation to body frame
|
||||
Tnb = transpose(Quat2Tbn(quat));
|
||||
|
||||
% crete velocity vectors
|
||||
ground_velocity_truth = [vn;ve;vd];
|
||||
wind_velocity_truth = [vwn;vwe;vwd];
|
||||
|
||||
% calcuate wind relative velocity
|
||||
rel_vel_ef = ground_velocity_truth - wind_velocity_truth;
|
||||
|
||||
% rotate into body frame
|
||||
rel_vel_bf = Tnb * rel_vel_ef;
|
||||
|
||||
% calculate the true airspeed
|
||||
TAS = sqrt(rel_vel_bf(1)^2 + rel_vel_bf(2)^2 + rel_vel_bf(3)^2);
|
||||
|
||||
% derive the control(disturbance) influence matrix from velocity error to
|
||||
% TAS error
|
||||
G_TAS = jacobian(TAS, [ground_velocity_truth;wind_velocity_truth]);
|
||||
|
||||
% derive the error matrix
|
||||
TAS_dist_matrix = diag([vn_var ve_var vd_var vwn_var vwe_var vwd_var]);
|
||||
Q_TAS = G_TAS*TAS_dist_matrix*transpose(G_TAS);
|
||||
|
||||
% save as C code
|
||||
ccode(Q_TAS,'file','Q_TAS.c');
|
||||
|
||||
save temp.mat;
|
||||
|
||||
%% calculate AoA error equations
|
||||
clear all;
|
||||
reset(symengine);
|
||||
|
||||
load temp.mat;
|
||||
|
||||
AoA = atan(rel_vel_bf(3) / rel_vel_bf(1));
|
||||
|
||||
% derive the control(disturbance) influence matrix from velocity error to
|
||||
% AoA error
|
||||
G_AoA = jacobian(AoA, [ground_velocity_truth;wind_velocity_truth]);
|
||||
|
||||
% derive the error matrix
|
||||
AoA_dist_matrix = diag([vn_var ve_var vd_var vwn_var vwe_var vwd_var]);
|
||||
Q_AoA = G_AoA*AoA_dist_matrix*transpose(G_AoA);
|
||||
|
||||
% save as C code
|
||||
ccode(Q_AoA,'file','Q_AoA.c');
|
||||
|
||||
save temp.mat;
|
||||
|
||||
%% Calculate AoS error equations
|
||||
|
||||
clear all;
|
||||
reset(symengine);
|
||||
|
||||
load temp.mat;
|
||||
|
||||
AoS = atan(rel_vel_bf(2) / rel_vel_bf(1));
|
||||
% derive the control(disturbance) influence matrix from velocity error to
|
||||
% AoS error
|
||||
G_AoS = jacobian(AoS, [ground_velocity_truth;wind_velocity_truth]);
|
||||
|
||||
% derive the error matrix
|
||||
AoS_dist_matrix = diag([vn_var ve_var vd_var vwn_var vwe_var vwd_var]);
|
||||
Q_AoS = G_AoS*AoS_dist_matrix*transpose(G_AoS);
|
||||
|
||||
% save as C code
|
||||
ccode(Q_AoS,'file','Q_AoS.c');
|
||||
|
||||
save temp.mat;
|
||||
|
||||
%% convert them combined to take advantage of shared terms in the optimiser
|
||||
|
||||
clear all;
|
||||
reset(symengine);
|
||||
|
||||
load temp.mat;
|
||||
|
||||
% save as C code
|
||||
ccode([Q_TAS;Q_AoA;Q_AoS],'file','Q_airdata.c');
|
||||
|
|
@ -1,92 +0,0 @@
|
|||
function fix_c_code(fileName)
|
||||
%% Initialize variables
|
||||
delimiter = '';
|
||||
|
||||
%% Format string for each line of text:
|
||||
% column1: text (%s)
|
||||
% For more information, see the TEXTSCAN documentation.
|
||||
formatSpec = '%s%[^\n\r]';
|
||||
|
||||
%% Open the text file.
|
||||
fileID = fopen(fileName,'r');
|
||||
|
||||
%% Read columns of data according to format string.
|
||||
% This call is based on the structure of the file used to generate this
|
||||
% code. If an error occurs for a different file, try regenerating the code
|
||||
% from the Import Tool.
|
||||
dataArray = textscan(fileID, formatSpec, 'Delimiter', delimiter, 'ReturnOnError', false);
|
||||
|
||||
%% Close the text file.
|
||||
fclose(fileID);
|
||||
|
||||
%% Create output variable
|
||||
SymbolicOutput = [dataArray{1:end-1}];
|
||||
|
||||
%% Clear temporary variables
|
||||
clearvars filename delimiter formatSpec fileID dataArray ans;
|
||||
|
||||
%% replace brackets and commas
|
||||
for lineIndex = 1:length(SymbolicOutput)
|
||||
SymbolicOutput(lineIndex) = regexprep(SymbolicOutput(lineIndex), '_l_', '(');
|
||||
SymbolicOutput(lineIndex) = regexprep(SymbolicOutput(lineIndex), '_c_', ',');
|
||||
SymbolicOutput(lineIndex) = regexprep(SymbolicOutput(lineIndex), '_r_', ')');
|
||||
end
|
||||
|
||||
%% Convert indexing and replace brackets
|
||||
|
||||
% replace 1-D indexes
|
||||
for arrayIndex = 1:99
|
||||
strIndex = int2str(arrayIndex);
|
||||
strRep = sprintf('[%d]',(arrayIndex-1));
|
||||
strPat = strcat('\(',strIndex,'\)');
|
||||
for lineIndex = 1:length(SymbolicOutput)
|
||||
str = char(SymbolicOutput(lineIndex));
|
||||
SymbolicOutput(lineIndex) = {regexprep(str, strPat, strRep)};
|
||||
end
|
||||
end
|
||||
|
||||
% replace 2-D left indexes
|
||||
for arrayIndex = 1:99
|
||||
strIndex = int2str(arrayIndex);
|
||||
strRep = sprintf('[%d,',(arrayIndex-1));
|
||||
strPat = strcat('\(',strIndex,'\,');
|
||||
for lineIndex = 1:length(SymbolicOutput)
|
||||
str = char(SymbolicOutput(lineIndex));
|
||||
SymbolicOutput(lineIndex) = {regexprep(str, strPat, strRep)};
|
||||
end
|
||||
end
|
||||
|
||||
% replace 2-D right indexes
|
||||
for arrayIndex = 1:99
|
||||
strIndex = int2str(arrayIndex);
|
||||
strRep = sprintf(',%d]',(arrayIndex-1));
|
||||
strPat = strcat('\,',strIndex,'\)');
|
||||
for lineIndex = 1:length(SymbolicOutput)
|
||||
str = char(SymbolicOutput(lineIndex));
|
||||
SymbolicOutput(lineIndex) = {regexprep(str, strPat, strRep)};
|
||||
end
|
||||
end
|
||||
|
||||
% replace commas
|
||||
for lineIndex = 1:length(SymbolicOutput)
|
||||
str = char(SymbolicOutput(lineIndex));
|
||||
SymbolicOutput(lineIndex) = {regexprep(str, '\,', '][')};
|
||||
end
|
||||
|
||||
%% Change covariance matrix variable name to P
|
||||
for lineIndex = 1:length(SymbolicOutput)
|
||||
strIn = char(SymbolicOutput(lineIndex));
|
||||
strIn = regexprep(strIn,'OP\[','P[');
|
||||
SymbolicOutput(lineIndex) = cellstr(strIn);
|
||||
end
|
||||
|
||||
%% Write to file
|
||||
fid = fopen(fileName,'wt');
|
||||
for lineIndex = 1:length(SymbolicOutput)
|
||||
fprintf(fid,char(SymbolicOutput(lineIndex)));
|
||||
fprintf(fid,'\n');
|
||||
end
|
||||
fclose(fid);
|
||||
clear all;
|
||||
|
||||
end
|
||||
|
|
@ -1,8 +0,0 @@
|
|||
clear all;
|
||||
syms spd yaw real;
|
||||
syms R_spd R_yaw real;
|
||||
vx = spd*cos(yaw);
|
||||
vy = spd*sin(yaw);
|
||||
Tpc = jacobian([vx;vy],[spd;yaw]);
|
||||
R_polar = [R_spd 0;0 R_yaw];
|
||||
R_cartesian = Tpc*R_polar*Tpc';
|
||||
|
|
@ -1,29 +0,0 @@
|
|||
% define roll, pitch and yaw variables
|
||||
syms roll pitch yaw 'real'
|
||||
|
||||
% Define yransformaton matrices for rotations about the X,Y and Z body axes
|
||||
Xrot = [1 0 0 ; 0 cos(roll) sin(roll) ; 0 -sin(roll) cos(roll)];
|
||||
Yrot = [cos(pitch) 0 -sin(pitch) ; 0 1 0 ; sin(pitch) 0 cos(pitch)];
|
||||
Zrot = [cos(yaw) sin(yaw) 0 ; -sin(yaw) cos(yaw) 0 ; 0 0 1];
|
||||
|
||||
% calculate the tranformation matrix from body to navigation frame resulting from
|
||||
% a rotation in yaw, roll, pitch order
|
||||
Tbn_312 = transpose(Yrot*Xrot*Zrot)
|
||||
|
||||
% convert to c code and save
|
||||
ccode(Tbn_312,'file','Tbn_312.c');
|
||||
fix_c_code('Tbn_312.c');
|
||||
|
||||
% define the quaternion elements
|
||||
syms q0 q1 q2 q3 'real'
|
||||
|
||||
% calculate the tranformation matrix from body to navigation frame as a
|
||||
% function of the quaternions
|
||||
Tbn_quat = Quat2Tbn([q0;q1;q2;q3]);
|
||||
|
||||
% collect the y,x terms required for calculation of the yaw angle
|
||||
yaw_input_312 = [-Tbn_quat(1,2);Tbn_quat(2,2)];
|
||||
|
||||
% convert to c code and save
|
||||
ccode(yaw_input_312,'file','yaw_input_312.c');
|
||||
fix_c_code('yaw_input_312.c');
|
||||
|
|
@ -1,29 +0,0 @@
|
|||
% define roll, pitch and yaw variables
|
||||
syms roll pitch yaw 'real'
|
||||
|
||||
% Define yransformaton matrices for rotations about the X,Y and Z body axes
|
||||
Xrot = [1 0 0 ; 0 cos(roll) sin(roll) ; 0 -sin(roll) cos(roll)];
|
||||
Yrot = [cos(pitch) 0 -sin(pitch) ; 0 1 0 ; sin(pitch) 0 cos(pitch)];
|
||||
Zrot = [cos(yaw) sin(yaw) 0 ; -sin(yaw) cos(yaw) 0 ; 0 0 1];
|
||||
|
||||
% calculate the tranformation matrix from body to navigation frame resulting from
|
||||
% a rotation in yaw, pitch, roll order
|
||||
Tbn_321 = transpose(Xrot*Yrot*Zrot)
|
||||
|
||||
% convert to c code and save
|
||||
ccode(Tbn_321,'file','Tbn_321.c');
|
||||
fix_c_code('Tbn_321.c');
|
||||
|
||||
% define the quaternion elements
|
||||
syms q0 q1 q2 q3 'real'
|
||||
|
||||
% calculate the tranformation matrix from body to navigation frame as a
|
||||
% function of the quaternions
|
||||
Tbn_quat = Quat2Tbn([q0;q1;q2;q3]);
|
||||
|
||||
% calculate the y,x terms required for calculation fo the yaw angle
|
||||
yaw_input_321 = [Tbn_quat(2,1);Tbn_quat(1,1)];
|
||||
|
||||
% convert to c code and save
|
||||
ccode(yaw_input_321,'file','yaw_input_321.c');
|
||||
fix_c_code('yaw_input_321.c');
|
||||
|
|
@ -1,59 +0,0 @@
|
|||
float t2 = rotX*rotX;
|
||||
float t4 = rotY*rotY;
|
||||
float t5 = rotZ*rotZ;
|
||||
float t6 = t2+t4+t5;
|
||||
float t7 = sqrtf(t6);
|
||||
float t8 = t7*0.5f;
|
||||
float t3 = sinf(t8);
|
||||
float t9 = t3*t3;
|
||||
float t10 = 1.0f/t6;
|
||||
float t11 = 1.0f/sqrtf(t6);
|
||||
float t12 = cosf(t8);
|
||||
float t13 = 1.0f/powf(t6,1.5f);
|
||||
float t14 = t3*t11;
|
||||
float t15 = rotX*rotY*t3*t13;
|
||||
float t16 = rotX*rotZ*t3*t13;
|
||||
float t17 = rotY*rotZ*t3*t13;
|
||||
float t18 = t2*t10*t12*0.5f;
|
||||
float t27 = t2*t3*t13;
|
||||
float t19 = t14+t18-t27;
|
||||
float t23 = rotX*rotY*t10*t12*0.5f;
|
||||
float t28 = t15-t23;
|
||||
float t20 = rotY*rotVarY*t3*t11*t28*0.5f;
|
||||
float t25 = rotX*rotZ*t10*t12*0.5f;
|
||||
float t31 = t16-t25;
|
||||
float t21 = rotZ*rotVarZ*t3*t11*t31*0.5f;
|
||||
float t22 = t20+t21-rotX*rotVarX*t3*t11*t19*0.5f;
|
||||
float t24 = t15-t23;
|
||||
float t26 = t16-t25;
|
||||
float t29 = t4*t10*t12*0.5f;
|
||||
float t34 = t3*t4*t13;
|
||||
float t30 = t14+t29-t34;
|
||||
float t32 = t5*t10*t12*0.5f;
|
||||
float t40 = t3*t5*t13;
|
||||
float t33 = t14+t32-t40;
|
||||
float t36 = rotY*rotZ*t10*t12*0.5f;
|
||||
float t39 = t17-t36;
|
||||
float t35 = rotZ*rotVarZ*t3*t11*t39*0.5f;
|
||||
float t37 = t15-t23;
|
||||
float t38 = t17-t36;
|
||||
float t41 = rotVarX*(t15-t23)*(t16-t25);
|
||||
float t42 = t41-rotVarY*t30*t39-rotVarZ*t33*t39;
|
||||
float t43 = t16-t25;
|
||||
float t44 = t17-t36;
|
||||
P[0][0] = rotVarX*t2*t9*t10*0.25f+rotVarY*t4*t9*t10*0.25f+rotVarZ*t5*t9*t10*0.25f;
|
||||
P[0][1] = t22;
|
||||
P[0][2] = t35+rotX*rotVarX*t3*t11*(t15-rotX*rotY*t10*t12*0.5f)*0.5f-rotY*rotVarY*t3*t11*t30*0.5f;
|
||||
P[0][3] = rotX*rotVarX*t3*t11*(t16-rotX*rotZ*t10*t12*0.5f)*0.5f+rotY*rotVarY*t3*t11*(t17-rotY*rotZ*t10*t12*0.5f)*0.5f-rotZ*rotVarZ*t3*t11*t33*0.5f;
|
||||
P[1][0] = t22;
|
||||
P[1][1] = rotVarX*(t19*t19)+rotVarY*(t24*t24)+rotVarZ*(t26*t26);
|
||||
P[1][2] = rotVarZ*(t16-t25)*(t17-rotY*rotZ*t10*t12*0.5f)-rotVarX*t19*t28-rotVarY*t28*t30;
|
||||
P[1][3] = rotVarY*(t15-t23)*(t17-rotY*rotZ*t10*t12*0.5f)-rotVarX*t19*t31-rotVarZ*t31*t33;
|
||||
P[2][0] = t35-rotY*rotVarY*t3*t11*t30*0.5f+rotX*rotVarX*t3*t11*(t15-t23)*0.5f;
|
||||
P[2][1] = rotVarZ*(t16-t25)*(t17-t36)-rotVarX*t19*t28-rotVarY*t28*t30;
|
||||
P[2][2] = rotVarY*(t30*t30)+rotVarX*(t37*t37)+rotVarZ*(t38*t38);
|
||||
P[2][3] = t42;
|
||||
P[3][0] = rotZ*rotVarZ*t3*t11*t33*(-1.0f/2.0f)+rotX*rotVarX*t3*t11*(t16-t25)*0.5f+rotY*rotVarY*t3*t11*(t17-t36)*0.5f;
|
||||
P[3][1] = rotVarY*(t15-t23)*(t17-t36)-rotVarX*t19*t31-rotVarZ*t31*t33;
|
||||
P[3][2] = t42;
|
||||
P[3][3] = rotVarZ*(t33*t33)+rotVarX*(t43*t43)+rotVarY*(t44*t44);
|
||||
|
|
@ -1,19 +0,0 @@
|
|||
t2 = q0*q0;
|
||||
t3 = acos(q0);
|
||||
t4 = -t2+1.0;
|
||||
t5 = t2-1.0;
|
||||
t6 = 1.0/t5;
|
||||
t7 = q1*t6*2.0;
|
||||
t8 = 1.0/pow(t4,3.0/2.0);
|
||||
t9 = q0*q1*t3*t8*2.0;
|
||||
t10 = t7+t9;
|
||||
t11 = 1.0/sqrt(t4);
|
||||
t12 = q2*t6*2.0;
|
||||
t13 = q0*q2*t3*t8*2.0;
|
||||
t14 = t12+t13;
|
||||
t15 = q3*t6*2.0;
|
||||
t16 = q0*q3*t3*t8*2.0;
|
||||
t17 = t15+t16;
|
||||
A0[0][0] = t10*(P_l_0_c_0_r_*t10+P_l_1_c_0_r_*t3*t11*2.0)+t3*t11*(P_l_0_c_1_r_*t10+P_l_1_c_1_r_*t3*t11*2.0)*2.0;
|
||||
A0[1][0] = t14*(P_l_0_c_0_r_*t14+P_l_2_c_0_r_*t3*t11*2.0)+t3*t11*(P_l_0_c_2_r_*t14+P_l_2_c_2_r_*t3*t11*2.0)*2.0;
|
||||
A0[2][0] = t17*(P_l_0_c_0_r_*t17+P_l_3_c_0_r_*t3*t11*2.0)+t3*t11*(P_l_0_c_3_r_*t17+P_l_3_c_3_r_*t3*t11*2.0)*2.0;
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
|
@ -1,35 +0,0 @@
|
|||
% script to test complementary output filter gain structure
|
||||
vel_state = 0.0;
|
||||
pos_state = 1.0;
|
||||
vel_demand = 0.0;
|
||||
pos_demand = 0.0;
|
||||
dt = 0.01;
|
||||
buffer_delay = 30;
|
||||
vel_state_history = zeros(1,10000);
|
||||
pos_state_history = zeros(1,10000);
|
||||
time = [0:dt:9999*dt];
|
||||
vel_correction = 0;
|
||||
pos_correction = 0;
|
||||
omega = 1.0/(dt*buffer_delay);
|
||||
prev_vel = 1.0;
|
||||
for i=1:10000
|
||||
prev_vel = vel_state;
|
||||
vel_state = vel_state + vel_correction*dt;
|
||||
vel_state_history(i) = vel_state;
|
||||
pos_state = pos_state + 0.5*dt*(vel_state + prev_vel) + pos_correction*dt;
|
||||
pos_state_history(i) = pos_state;
|
||||
if i > buffer_delay
|
||||
vel_correction = (vel_demand - vel_state_history(i-buffer_delay))*omega*0.5;
|
||||
pos_correction = (pos_demand - pos_state_history(i-buffer_delay))*omega*0.6;
|
||||
else
|
||||
vel_correction = 0;
|
||||
pos_correction = 0;
|
||||
end
|
||||
end
|
||||
min(vel_state_history)
|
||||
close all;
|
||||
figure;
|
||||
subplot(2,1,1)
|
||||
plot(time,vel_state_history);
|
||||
subplot(2,1,2);
|
||||
plot(time,pos_state_history);
|
||||
|
|
@ -1,20 +0,0 @@
|
|||
import sympy as sp
|
||||
Wx, Wy, yaw, R_TAS, initial_wind_var_body_y, R_yaw = sp.symbols('Wx Wy yaw R_TAS initial_wind_var_body_y R_yaw')
|
||||
Wn = Wx * sp.cos(yaw) - Wy * sp.sin(yaw)
|
||||
We = Wx * sp.sin(yaw) + Wy * sp.cos(yaw)
|
||||
|
||||
Wn_Wx = sp.diff(Wn, Wx)
|
||||
Wn_Wy = sp.diff(Wn, Wy)
|
||||
Wn_yaw = sp.diff(Wn, yaw)
|
||||
We_Wx = sp.diff(We, Wx)
|
||||
We_Wy = sp.diff(We, Wy)
|
||||
We_yaw = sp.diff(We, yaw)
|
||||
|
||||
G = sp.Matrix([[Wn_Wx, Wn_Wy, Wn_yaw],[We_Wx, We_Wy, We_yaw]])
|
||||
b_wind_cov = sp.Matrix([[R_TAS, 0.0, 0.0], [0.0,initial_wind_var_body_y, 0.0], [0.0, 0.0, R_yaw]])
|
||||
i_wind_cov = G * b_wind_cov * G.T
|
||||
|
||||
print('P[22][22] = ' + str(i_wind_cov[0,0]))
|
||||
print('P[22][23] = ' + str(i_wind_cov[0,1]))
|
||||
print('P[23][22] = ' + str(i_wind_cov[1,0]))
|
||||
print('P[23][23] = ' + str(i_wind_cov[1,1]))
|
||||
|
|
@ -1,2 +0,0 @@
|
|||
A0[0][0] = q0*q3*2.0-q1*q2*2.0;
|
||||
A0[1][0] = q0*q0-q1*q1+q2*q2-q3*q3;
|
||||
|
|
@ -1,2 +0,0 @@
|
|||
A0[0][0] = q0*q3*2.0+q1*q2*2.0;
|
||||
A0[1][0] = q0*q0+q1*q1-q2*q2-q3*q3;
|
||||
Loading…
Reference in New Issue