mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-20 03:13:44 +08:00
Removing Matlab derivation
This commit is contained in:
Kamil Ritz
committed by
Paul Riseborough
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.
|
||||
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);
|
||||
File diff suppressed because one or more lines are too long
@@ -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;
|
||||
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -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
|
||||
File diff suppressed because it is too large
Load Diff
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user