- T[1][1] = Q0*Q0 + Q1*Q1 - Q2*Q2 - Q3*Q3; // Page A-11
- T[1][2] = 2*(Q1*Q2 + Q0*Q3); // From
- T[1][3] = 2*(Q1*Q3 - Q0*Q2); // Reference [2]
- T[2][1] = 2*(Q1*Q2 - Q0*Q3);
- T[2][2] = Q0*Q0 - Q1*Q1 + Q2*Q2 - Q3*Q3;
- T[2][3] = 2*(Q2*Q3 + Q0*Q1);
- T[3][1] = 2*(Q1*Q3 + Q0*Q2);
- T[3][2] = 2*(Q2*Q3 - Q0*Q1);
- T[3][3] = Q0*Q0 - Q1*Q1 - Q2*Q2 + Q3*Q3;
-
- Fn = T[1][1]*Fx + T[2][1]*Fy + T[3][1]*Fz; // Eqn. 3.5
- Fe = T[1][2]*Fx + T[2][2]*Fy + T[3][2]*Fz; // From
- Fd = T[1][3]*Fx + T[2][3]*Fy + T[3][3]*Fz; // Reference [3]
-
- tanLat = tan(Latitude); // I made this up
- cosLat = cos(Latitude);
-
- lastAccelN = AccelN;
- lastAccelE = AccelE;
- lastAccelD = AccelD;
-
- Vn = T[1][1]*U + T[2][1]*V + T[3][1]*W;
- Ve = T[1][2]*U + T[2][2]*V + T[3][2]*W;
- Vd = T[1][3]*U + T[2][3]*V + T[3][3]*W;