dRonin  adbada4
dRonin firmware
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Groups Pages
insgps16state.c
Go to the documentation of this file.
1 
17 /*
18  * This program is free software; you can redistribute it and/or modify
19  * it under the terms of the GNU General Public License as published by
20  * the Free Software Foundation; either version 3 of the License, or
21  * (at your option) any later version.
22  *
23  * This program is distributed in the hope that it will be useful, but
24  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
25  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
26  * for more details.
27  *
28  * You should have received a copy of the GNU General Public License along
29  * with this program; if not, see <http://www.gnu.org/licenses/>
30  */
31 
32 #include "insgps.h"
33 #include "physical_constants.h"
34 #include <math.h>
35 #include <stdint.h>
36 
37 // constants/macros/typdefs
38 #define NUMX 16 // number of states, X is the state vector
39 #define NUMW 12 // number of plant noise inputs, w is disturbance noise vector
40 #define NUMV 10 // number of measurements, v is the measurement noise vector
41 #define NUMU 6 // number of deterministic inputs, U is the input vector
42 
43 #if defined(GENERAL_COV)
44 // This might trick people so I have a note here. There is a slower but bigger version of the
45 // code here but won't fit when debugging disabled (requires -Os)
46 #define COVARIANCE_PREDICTION_GENERAL
47 #endif
48 
49 // Private functions
50 void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
51  float Q[NUMW], float dT, float P[NUMX][NUMX]);
52 void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
53  float Y[NUMV], float P[NUMX][NUMX], float X[NUMX],
54  uint16_t SensorsUsed);
55 void RungeKutta(float X[NUMX], float U[NUMU], float dT);
56 void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX]);
57 void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX],
58  float G[NUMX][NUMW]);
59 void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV]);
60 void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX]);
61 
62 // Private variables
63 float F[NUMX][NUMX], G[NUMX][NUMW], H[NUMV][NUMX]; // linearized system matrices
64  // global to init to zero and maintain zero elements
65 float Be[3]; // local magnetic unit vector in NED frame
66 float P[NUMX][NUMX], X[NUMX]; // covariance matrix and state vector
67 float Q[NUMW], R[NUMV]; // input noise and measurement noise variances
68 float K[NUMX][NUMV]; // feedback gain matrix
69 
70 // ************* Exposed Functions ****************
71 // *************************************************
72 
73 uint16_t ins_get_num_states()
74 {
75  return NUMX;
76 }
77 
78 void INSGPSInit() //pretty much just a place holder for now
79 {
80  Be[0] = 1.0f;
81  Be[1] = 0;
82  Be[2] = 0; // local magnetic unit vector
83 
84  for (int i = 0; i < NUMX; i++) {
85  for (int j = 0; j < NUMX; j++) {
86  P[i][j] = 0.0f; // zero all terms
87  F[i][j] = 0.0f;
88  }
89  for (int j = 0; j < NUMW; j++)
90  G[i][j] = 0.0f;
91 
92  for (int j = 0; j < NUMV; j++) {
93  H[j][i] = 0.0f;
94  K[i][j] = 0.0f;
95  }
96 
97  X[i] = 0.0f;
98  }
99  for (int i = 0; i < NUMW; i++)
100  Q[i] = 0.0f;
101  for (int i = 0; i < NUMV; i++)
102  R[i] = 0.0f;
103 
104  P[0][0] = P[1][1] = P[2][2] = 25.0f; // initial position variance (m^2)
105  P[3][3] = P[4][4] = P[5][5] = 5.0f; // initial velocity variance (m/s)^2
106  P[6][6] = P[7][7] = P[8][8] = P[9][9] = 1e-5f; // initial quaternion variance
107  P[10][10] = P[11][11] = P[12][12] = 1e-6f; // initial gyro bias variance (rad/s)^2
108  P[13][13] = P[14][14] = P[15][15] = 1e-5f; // initial accel bias variance (deg/s)^2
109 
110  X[0] = X[1] = X[2] = X[3] = X[4] = X[5] = 0.0f; // initial pos and vel (m)
111  X[6] = 1.0f;
112  X[7] = X[8] = X[9] = 0.0f; // initial quaternion (level and North) (m/s)
113  X[10] = X[11] = X[12] = 0.0f; // initial gyro bias (rad/s)
114  X[13] = X[14] = X[15] = 0.0f; // initial accel bias
115 
116  Q[0] = Q[1] = Q[2] = 1e-5f; // gyro noise variance (rad/s)^2
117  Q[3] = Q[4] = Q[5] = 1e-5f; // accelerometer noise variance (m/s^2)^2
118  Q[6] = Q[7] = 1e-6f; // gyro x and y bias random walk variance (rad/s^2)^2
119  Q[8] = 1e-6f; // gyro z bias random walk variance (rad/s^2)^2
120  Q[9] = Q[10] = Q[11] = 5e-4f; // accel bias random walk variance (m/s^3)^2
121 
122  R[0] = R[1] = 0.004f; // High freq GPS horizontal position noise variance (m^2)
123  R[2] = 0.036f; // High freq GPS vertical position noise variance (m^2)
124  R[3] = R[4] = 0.004f; // High freq GPS horizontal velocity noise variance (m/s)^2
125  R[5] = 100.0f; // High freq GPS vertical velocity noise variance (m/s)^2
126  R[6] = R[7] = R[8] = 0.005f; // magnetometer unit vector noise variance
127  R[9] = .05f; // High freq altimeter noise variance (m^2)
128 }
129 
131 void INSSetArmed(bool armed)
132 {
133  // Speed up convergence of accel and gyro bias when not armed
134  if (armed) {
135  Q[11] = 1e-5f;
136  Q[8] = 2e-4f;
137  } else {
138  Q[11] = 1e-2f;
139  Q[8] = 2e-8f;
140  }
141 
142 
143 }
144 
145 
154 void INSGetState(float *pos, float *vel, float *attitude, float *gyro_bias, float *accel_bias)
155 {
156  if (pos) {
157  pos[0] = X[0];
158  pos[1] = X[1];
159  pos[2] = X[2];
160  }
161 
162  if (vel) {
163  vel[0] = X[3];
164  vel[1] = X[4];
165  vel[2] = X[5];
166  }
167 
168  if (attitude) {
169  attitude[0] = X[6];
170  attitude[1] = X[7];
171  attitude[2] = X[8];
172  attitude[3] = X[9];
173  }
174 
175  if (gyro_bias) {
176  gyro_bias[0] = X[10];
177  gyro_bias[1] = X[11];
178  gyro_bias[2] = X[12];
179  }
180 
181  if (accel_bias) {
182  accel_bias[0] = X[13];
183  accel_bias[1] = X[14];
184  accel_bias[2] = X[15];
185  }
186 }
187 
192 void INSGetVariance(float *var_out)
193  {
194  for (uint32_t i = 0; i < NUMX; i++)
195  var_out[i] = P[i][i];
196  }
197 
198 void INSResetP(const float *PDiag)
199 {
200  uint8_t i,j;
201 
202  // if PDiag[i] nonzero then clear row and column and set diagonal element
203  for (i=0;i<NUMX;i++){
204  if (PDiag != 0){
205  for (j=0;j<NUMX;j++)
206  P[i][j]=P[j][i]=0.0f;
207  P[i][i]=PDiag[i];
208  }
209  }
210 }
211 
212 void INSSetState(const float pos[3], const float vel[3], const float q[4], const float gyro_bias[3], const float accel_bias[3])
213 {
214  X[0] = pos[0];
215  X[1] = pos[1];
216  X[2] = pos[2];
217  X[3] = vel[0];
218  X[4] = vel[1];
219  X[5] = vel[2];
220  X[6] = q[0];
221  X[7] = q[1];
222  X[8] = q[2];
223  X[9] = q[3];
224  X[10] = gyro_bias[0];
225  X[11] = gyro_bias[1];
226  X[12] = gyro_bias[2];
227  X[13] = accel_bias[0];
228  X[14] = accel_bias[1];
229  X[15] = accel_bias[2];
230 }
231 
232 void INSPosVelReset(const float pos[3], const float vel[3])
233 {
234  for (int i = 0; i < 6; i++) {
235  for(int j = i; j < NUMX; j++) {
236  P[i][j] = 0.0f; // zero the first 6 rows and columns
237  P[j][i] = 0.0f;
238  }
239  }
240 
241  P[0][0] = P[1][1] = P[2][2] = 25.0f; // initial position variance (m^2)
242  P[3][3] = P[4][4] = P[5][5] = 5.0f; // initial velocity variance (m/s)^2
243 
244  X[0] = pos[0];
245  X[1] = pos[1];
246  X[2] = pos[2];
247  X[3] = vel[0];
248  X[4] = vel[1];
249  X[5] = vel[2];
250 }
251 
252 void INSSetPosVelVar(float PosVar, float VelVar, float VertPosVar)
253 {
254  R[0] = PosVar;
255  R[1] = PosVar;
256  R[2] = VertPosVar;
257  R[3] = VelVar;
258  R[4] = VelVar;
259  R[5] = VelVar; // Don't change vertical velocity, not measured
260 }
261 
262 void INSSetGyroBias(const float gyro_bias[3])
263 {
264  X[10] = gyro_bias[0];
265  X[11] = gyro_bias[1];
266  X[12] = gyro_bias[2];
267 }
268 
269 void INSSetAccelBias(const float accel_bias[3])
270 {
271  X[13] = accel_bias[0];
272  X[14] = accel_bias[1];
273  X[15] = accel_bias[2];
274 }
275 
276 void INSSetAccelVar(const float accel_var[3])
277 {
278  Q[3] = accel_var[0];
279  Q[4] = accel_var[1];
280  Q[5] = accel_var[2];
281 }
282 
283 void INSSetGyroVar(const float gyro_var[3])
284 {
285  Q[0] = gyro_var[0];
286  Q[1] = gyro_var[1];
287  Q[2] = gyro_var[2];
288 }
289 
290 void INSSetMagVar(const float scaled_mag_var[3])
291 {
292  R[6] = scaled_mag_var[0];
293  R[7] = scaled_mag_var[1];
294  R[8] = scaled_mag_var[2];
295 }
296 
297 void INSSetBaroVar(const float baro_var)
298 {
299  R[9] = baro_var;
300 }
301 
302 void INSSetMagNorth(const float B[3])
303 {
304  Be[0] = B[0];
305  Be[1] = B[1];
306  Be[2] = B[2];
307 }
308 
309 void INSStatePrediction(const float gyro_data[3], const float accel_data[3], float dT)
310 {
311  float U[6];
312  float qmag;
313 
314  // rate gyro inputs in units of rad/s
315  U[0] = gyro_data[0];
316  U[1] = gyro_data[1];
317  U[2] = gyro_data[2];
318 
319  // accelerometer inputs in units of m/s
320  U[3] = accel_data[0];
321  U[4] = accel_data[1];
322  U[5] = accel_data[2];
323 
324  // EKF prediction step
325  LinearizeFG(X, U, F, G);
326  RungeKutta(X, U, dT);
327  qmag = sqrtf(X[6] * X[6] + X[7] * X[7] + X[8] * X[8] + X[9] * X[9]);
328  X[6] /= qmag;
329  X[7] /= qmag;
330  X[8] /= qmag;
331  X[9] /= qmag;
332 }
333 
334 void INSCovariancePrediction(float dT)
335 {
336  CovariancePrediction(F, G, Q, dT, P);
337 }
338 
339 void INSCorrection(const float mag_data[3], const float Pos[3], const float Vel[3],
340  float BaroAlt, uint16_t SensorsUsed)
341 {
342  float Z[10], Y[10];
343  float qmag;
344 
345  // GPS Position in meters and in local NED frame
346  Z[0] = Pos[0];
347  Z[1] = Pos[1];
348  Z[2] = Pos[2];
349 
350  // GPS Velocity in meters and in local NED frame
351  Z[3] = Vel[0];
352  Z[4] = Vel[1];
353  Z[5] = Vel[2];
354 
355  if (SensorsUsed & MAG_SENSORS) {
356  // magnetometer data in any units (use unit vector) and in body frame
357  float Rbe_a[3][3];
358  float q0 = X[6];
359  float q1 = X[7];
360  float q2 = X[8];
361  float q3 = X[9];
362  float k1 = 1.0f/sqrtf(powf(q0*q1*2.0f+q2*q3*2.0f,2.0f)+powf(q0*q0-q1*q1-q2*q2+q3*q3,2.0f));
363  float k2 = sqrtf(-powf(q0*q2*2.0f-q1*q3*2.0f,2.0f)+1.0f);
364 
365  Rbe_a[0][0] = k2;
366  Rbe_a[0][1] = 0.0f;
367  Rbe_a[0][2] = q0*q2*-2.0f+q1*q3*2.0f;
368  Rbe_a[1][0] = k1*(q0*q1*2.0f+q2*q3*2.0f)*(q0*q2*2.0f-q1*q3*2.0f);
369  Rbe_a[1][1] = k1*(q0*q0-q1*q1-q2*q2+q3*q3);
370  Rbe_a[1][2] = k1*sqrtf(-powf(q0*q2*2.0f-q1*q3*2.0f,2.0f)+1.0f)*(q0*q1*2.0f+q2*q3*2.0f);
371  Rbe_a[2][0] = k1*(q0*q2*2.0f-q1*q3*2.0f)*(q0*q0-q1*q1-q2*q2+q3*q3);
372  Rbe_a[2][1] = -k1*(q0*q1*2.0f+q2*q3*2.0f);
373  Rbe_a[2][2] = k1*k2*(q0*q0-q1*q1-q2*q2+q3*q3);
374 
375  Z[6] = Rbe_a[0][0]*mag_data[0] + Rbe_a[1][0]*mag_data[1] + Rbe_a[2][0]*mag_data[2] ;
376  Z[7] = Rbe_a[0][1]*mag_data[0] + Rbe_a[1][1]*mag_data[1] + Rbe_a[2][1]*mag_data[2] ;
377  Z[8] = Rbe_a[0][2]*mag_data[0] + Rbe_a[1][2]*mag_data[1] + Rbe_a[2][2]*mag_data[2] ;
378  }
379 
380  // barometric altimeter in meters and in local NED frame
381  Z[9] = BaroAlt;
382 
383  // EKF correction step
384  LinearizeH(X, Be, H);
385  MeasurementEq(X, Be, Y);
386  SerialUpdate(H, R, Z, Y, P, X, SensorsUsed);
387  qmag = sqrtf(X[6] * X[6] + X[7] * X[7] + X[8] * X[8] + X[9] * X[9]);
388  X[6] /= qmag;
389  X[7] /= qmag;
390  X[8] /= qmag;
391  X[9] /= qmag;
392 }
393 
394 // ************* CovariancePrediction *************
395 // Does the prediction step of the Kalman filter for the covariance matrix
396 // Output, Pnew, overwrites P, the input covariance
397 // Pnew = (I+F*T)*P*(I+F*T)' + T^2*G*Q*G'
398 // Q is the discrete time covariance of process noise
399 // Q is vector of the diagonal for a square matrix with
400 // dimensions equal to the number of disturbance noise variables
401 // The General Method is very inefficient,not taking advantage of the sparse F and G
402 // The first Method is very specific to this implementation
403 // ************************************************
404 
405 #ifdef COVARIANCE_PREDICTION_GENERAL
406 
407 void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
408  float Q[NUMW], float dT, float P[NUMX][NUMX])
409 {
410  float Dummy[NUMX][NUMX], dTsq;
411  uint8_t i, j, k;
412 
413  // Pnew = (I+F*T)*P*(I+F*T)' + T^2*G*Q*G' = T^2[(P/T + F*P)*(I/T + F') + G*Q*G')]
414 
415  dTsq = dT * dT;
416 
417  for (i = 0; i < NUMX; i++) // Calculate Dummy = (P/T +F*P)
418  for (j = 0; j < NUMX; j++) {
419  Dummy[i][j] = P[i][j] / dT;
420  for (k = 0; k < NUMX; k++)
421  Dummy[i][j] += F[i][k] * P[k][j];
422  }
423  for (i = 0; i < NUMX; i++) // Calculate Pnew = Dummy/T + Dummy*F' + G*Qw*G'
424  for (j = i; j < NUMX; j++) { // Use symmetry, ie only find upper triangular
425  P[i][j] = Dummy[i][j] / dT;
426  for (k = 0; k < NUMX; k++)
427  P[i][j] += Dummy[i][k] * F[j][k]; // P = Dummy/T + Dummy*F'
428  for (k = 0; k < NUMW; k++)
429  P[i][j] += Q[k] * G[i][k] * G[j][k]; // P = Dummy/T + Dummy*F' + G*Q*G'
430  P[j][i] = P[i][j] = P[i][j] * dTsq; // Pnew = T^2*P and fill in lower triangular;
431  }
432 }
433 
434 #else
435 
436 void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
437  float Q[NUMW], float dT, float P[NUMX][NUMX])
438 {
439  float D[NUMX][NUMX], T, Tsq;
440  uint8_t i, j;
441 
442  // Pnew = (I+F*T)*P*(I+F*T)' + T^2*G*Q*G' = scalar expansion from symbolic manipulator
443 
444  T = dT;
445  Tsq = dT * dT;
446 
447  for (i = 0; i < NUMX; i++) // Create a copy of the upper triangular of P
448  for (j = i; j < NUMX; j++)
449  D[i][j] = P[i][j];
450 
451  // Brute force calculation of the elements of P
452  P[0][0] = D[3][3]*Tsq + (2*D[0][3])*T + D[0][0];
453  P[0][1] = P[1][0] = D[3][4]*Tsq + (D[0][4] + D[1][3])*T + D[0][1];
454  P[0][2] = P[2][0] = D[3][5]*Tsq + (D[0][5] + D[2][3])*T + D[0][2];
455  P[0][3] = P[3][0] = (F[3][6]*D[3][6] + F[3][7]*D[3][7] + F[3][8]*D[3][8] + F[3][9]*D[3][9] + F[3][13]*D[3][13] + F[3][14]*D[3][14] + F[3][15]*D[3][15])*Tsq + (D[3][3] + F[3][6]*D[0][6] + F[3][7]*D[0][7] + F[3][8]*D[0][8] + F[3][9]*D[0][9] + F[3][13]*D[0][13] + F[3][14]*D[0][14] + F[3][15]*D[0][15])*T + D[0][3];
456  P[0][4] = P[4][0] = (F[4][6]*D[3][6] + F[4][7]*D[3][7] + F[4][8]*D[3][8] + F[4][9]*D[3][9] + F[4][13]*D[3][13] + F[4][14]*D[3][14] + F[4][15]*D[3][15])*Tsq + (D[3][4] + F[4][6]*D[0][6] + F[4][7]*D[0][7] + F[4][8]*D[0][8] + F[4][9]*D[0][9] + F[4][13]*D[0][13] + F[4][14]*D[0][14] + F[4][15]*D[0][15])*T + D[0][4];
457  P[0][5] = P[5][0] = (F[5][6]*D[3][6] + F[5][7]*D[3][7] + F[5][8]*D[3][8] + F[5][9]*D[3][9] + F[5][13]*D[3][13] + F[5][14]*D[3][14] + F[5][15]*D[3][15])*Tsq + (D[3][5] + F[5][6]*D[0][6] + F[5][7]*D[0][7] + F[5][8]*D[0][8] + F[5][9]*D[0][9] + F[5][13]*D[0][13] + F[5][14]*D[0][14] + F[5][15]*D[0][15])*T + D[0][5];
458  P[0][6] = P[6][0] = (F[6][7]*D[3][7] + F[6][8]*D[3][8] + F[6][9]*D[3][9] + F[6][10]*D[3][10] + F[6][11]*D[3][11] + F[6][12]*D[3][12])*Tsq + (D[3][6] + F[6][7]*D[0][7] + F[6][8]*D[0][8] + F[6][9]*D[0][9] + F[6][10]*D[0][10] + F[6][11]*D[0][11] + F[6][12]*D[0][12])*T + D[0][6];
459  P[0][7] = P[7][0] = (F[7][6]*D[3][6] + F[7][8]*D[3][8] + F[7][9]*D[3][9] + F[7][10]*D[3][10] + F[7][11]*D[3][11] + F[7][12]*D[3][12])*Tsq + (D[3][7] + F[7][6]*D[0][6] + F[7][8]*D[0][8] + F[7][9]*D[0][9] + F[7][10]*D[0][10] + F[7][11]*D[0][11] + F[7][12]*D[0][12])*T + D[0][7];
460  P[0][8] = P[8][0] = (F[8][6]*D[3][6] + F[8][7]*D[3][7] + F[8][9]*D[3][9] + F[8][10]*D[3][10] + F[8][11]*D[3][11] + F[8][12]*D[3][12])*Tsq + (D[3][8] + F[8][6]*D[0][6] + F[8][7]*D[0][7] + F[8][9]*D[0][9] + F[8][10]*D[0][10] + F[8][11]*D[0][11] + F[8][12]*D[0][12])*T + D[0][8];
461  P[0][9] = P[9][0] = (F[9][6]*D[3][6] + F[9][7]*D[3][7] + F[9][8]*D[3][8] + F[9][10]*D[3][10] + F[9][11]*D[3][11] + F[9][12]*D[3][12])*Tsq + (D[3][9] + F[9][6]*D[0][6] + F[9][7]*D[0][7] + F[9][8]*D[0][8] + F[9][10]*D[0][10] + F[9][11]*D[0][11] + F[9][12]*D[0][12])*T + D[0][9];
462  P[0][10] = P[10][0] = D[3][10]*T + D[0][10];
463  P[0][11] = P[11][0] = D[3][11]*T + D[0][11];
464  P[0][12] = P[12][0] = D[3][12]*T + D[0][12];
465  P[0][13] = P[13][0] = D[3][13]*T + D[0][13];
466  P[0][14] = P[14][0] = D[3][14]*T + D[0][14];
467  P[0][15] = P[15][0] = D[3][15]*T + D[0][15];
468  P[1][1] = D[4][4]*Tsq + (2*D[1][4])*T + D[1][1];
469  P[1][2] = P[2][1] = D[4][5]*Tsq + (D[1][5] + D[2][4])*T + D[1][2];
470  P[1][3] = P[3][1] = (F[3][6]*D[4][6] + F[3][7]*D[4][7] + F[3][8]*D[4][8] + F[3][9]*D[4][9] + F[3][13]*D[4][13] + F[3][14]*D[4][14] + F[3][15]*D[4][15])*Tsq + (D[3][4] + F[3][6]*D[1][6] + F[3][7]*D[1][7] + F[3][8]*D[1][8] + F[3][9]*D[1][9] + F[3][13]*D[1][13] + F[3][14]*D[1][14] + F[3][15]*D[1][15])*T + D[1][3];
471  P[1][4] = P[4][1] = (F[4][6]*D[4][6] + F[4][7]*D[4][7] + F[4][8]*D[4][8] + F[4][9]*D[4][9] + F[4][13]*D[4][13] + F[4][14]*D[4][14] + F[4][15]*D[4][15])*Tsq + (D[4][4] + F[4][6]*D[1][6] + F[4][7]*D[1][7] + F[4][8]*D[1][8] + F[4][9]*D[1][9] + F[4][13]*D[1][13] + F[4][14]*D[1][14] + F[4][15]*D[1][15])*T + D[1][4];
472  P[1][5] = P[5][1] = (F[5][6]*D[4][6] + F[5][7]*D[4][7] + F[5][8]*D[4][8] + F[5][9]*D[4][9] + F[5][13]*D[4][13] + F[5][14]*D[4][14] + F[5][15]*D[4][15])*Tsq + (D[4][5] + F[5][6]*D[1][6] + F[5][7]*D[1][7] + F[5][8]*D[1][8] + F[5][9]*D[1][9] + F[5][13]*D[1][13] + F[5][14]*D[1][14] + F[5][15]*D[1][15])*T + D[1][5];
473  P[1][6] = P[6][1] = (F[6][7]*D[4][7] + F[6][8]*D[4][8] + F[6][9]*D[4][9] + F[6][10]*D[4][10] + F[6][11]*D[4][11] + F[6][12]*D[4][12])*Tsq + (D[4][6] + F[6][7]*D[1][7] + F[6][8]*D[1][8] + F[6][9]*D[1][9] + F[6][10]*D[1][10] + F[6][11]*D[1][11] + F[6][12]*D[1][12])*T + D[1][6];
474  P[1][7] = P[7][1] = (F[7][6]*D[4][6] + F[7][8]*D[4][8] + F[7][9]*D[4][9] + F[7][10]*D[4][10] + F[7][11]*D[4][11] + F[7][12]*D[4][12])*Tsq + (D[4][7] + F[7][6]*D[1][6] + F[7][8]*D[1][8] + F[7][9]*D[1][9] + F[7][10]*D[1][10] + F[7][11]*D[1][11] + F[7][12]*D[1][12])*T + D[1][7];
475  P[1][8] = P[8][1] = (F[8][6]*D[4][6] + F[8][7]*D[4][7] + F[8][9]*D[4][9] + F[8][10]*D[4][10] + F[8][11]*D[4][11] + F[8][12]*D[4][12])*Tsq + (D[4][8] + F[8][6]*D[1][6] + F[8][7]*D[1][7] + F[8][9]*D[1][9] + F[8][10]*D[1][10] + F[8][11]*D[1][11] + F[8][12]*D[1][12])*T + D[1][8];
476  P[1][9] = P[9][1] = (F[9][6]*D[4][6] + F[9][7]*D[4][7] + F[9][8]*D[4][8] + F[9][10]*D[4][10] + F[9][11]*D[4][11] + F[9][12]*D[4][12])*Tsq + (D[4][9] + F[9][6]*D[1][6] + F[9][7]*D[1][7] + F[9][8]*D[1][8] + F[9][10]*D[1][10] + F[9][11]*D[1][11] + F[9][12]*D[1][12])*T + D[1][9];
477  P[1][10] = P[10][1] = D[4][10]*T + D[1][10];
478  P[1][11] = P[11][1] = D[4][11]*T + D[1][11];
479  P[1][12] = P[12][1] = D[4][12]*T + D[1][12];
480  P[1][13] = P[13][1] = D[4][13]*T + D[1][13];
481  P[1][14] = P[14][1] = D[4][14]*T + D[1][14];
482  P[1][15] = P[15][1] = D[4][15]*T + D[1][15];
483  P[2][2] = D[5][5]*Tsq + (2*D[2][5])*T + D[2][2];
484  P[2][3] = P[3][2] = (F[3][6]*D[5][6] + F[3][7]*D[5][7] + F[3][8]*D[5][8] + F[3][9]*D[5][9] + F[3][13]*D[5][13] + F[3][14]*D[5][14] + F[3][15]*D[5][15])*Tsq + (D[3][5] + F[3][6]*D[2][6] + F[3][7]*D[2][7] + F[3][8]*D[2][8] + F[3][9]*D[2][9] + F[3][13]*D[2][13] + F[3][14]*D[2][14] + F[3][15]*D[2][15])*T + D[2][3];
485  P[2][4] = P[4][2] = (F[4][6]*D[5][6] + F[4][7]*D[5][7] + F[4][8]*D[5][8] + F[4][9]*D[5][9] + F[4][13]*D[5][13] + F[4][14]*D[5][14] + F[4][15]*D[5][15])*Tsq + (D[4][5] + F[4][6]*D[2][6] + F[4][7]*D[2][7] + F[4][8]*D[2][8] + F[4][9]*D[2][9] + F[4][13]*D[2][13] + F[4][14]*D[2][14] + F[4][15]*D[2][15])*T + D[2][4];
486  P[2][5] = P[5][2] = (F[5][6]*D[5][6] + F[5][7]*D[5][7] + F[5][8]*D[5][8] + F[5][9]*D[5][9] + F[5][13]*D[5][13] + F[5][14]*D[5][14] + F[5][15]*D[5][15])*Tsq + (D[5][5] + F[5][6]*D[2][6] + F[5][7]*D[2][7] + F[5][8]*D[2][8] + F[5][9]*D[2][9] + F[5][13]*D[2][13] + F[5][14]*D[2][14] + F[5][15]*D[2][15])*T + D[2][5];
487  P[2][6] = P[6][2] = (F[6][7]*D[5][7] + F[6][8]*D[5][8] + F[6][9]*D[5][9] + F[6][10]*D[5][10] + F[6][11]*D[5][11] + F[6][12]*D[5][12])*Tsq + (D[5][6] + F[6][7]*D[2][7] + F[6][8]*D[2][8] + F[6][9]*D[2][9] + F[6][10]*D[2][10] + F[6][11]*D[2][11] + F[6][12]*D[2][12])*T + D[2][6];
488  P[2][7] = P[7][2] = (F[7][6]*D[5][6] + F[7][8]*D[5][8] + F[7][9]*D[5][9] + F[7][10]*D[5][10] + F[7][11]*D[5][11] + F[7][12]*D[5][12])*Tsq + (D[5][7] + F[7][6]*D[2][6] + F[7][8]*D[2][8] + F[7][9]*D[2][9] + F[7][10]*D[2][10] + F[7][11]*D[2][11] + F[7][12]*D[2][12])*T + D[2][7];
489  P[2][8] = P[8][2] = (F[8][6]*D[5][6] + F[8][7]*D[5][7] + F[8][9]*D[5][9] + F[8][10]*D[5][10] + F[8][11]*D[5][11] + F[8][12]*D[5][12])*Tsq + (D[5][8] + F[8][6]*D[2][6] + F[8][7]*D[2][7] + F[8][9]*D[2][9] + F[8][10]*D[2][10] + F[8][11]*D[2][11] + F[8][12]*D[2][12])*T + D[2][8];
490  P[2][9] = P[9][2] = (F[9][6]*D[5][6] + F[9][7]*D[5][7] + F[9][8]*D[5][8] + F[9][10]*D[5][10] + F[9][11]*D[5][11] + F[9][12]*D[5][12])*Tsq + (D[5][9] + F[9][6]*D[2][6] + F[9][7]*D[2][7] + F[9][8]*D[2][8] + F[9][10]*D[2][10] + F[9][11]*D[2][11] + F[9][12]*D[2][12])*T + D[2][9];
491  P[2][10] = P[10][2] = D[5][10]*T + D[2][10];
492  P[2][11] = P[11][2] = D[5][11]*T + D[2][11];
493  P[2][12] = P[12][2] = D[5][12]*T + D[2][12];
494  P[2][13] = P[13][2] = D[5][13]*T + D[2][13];
495  P[2][14] = P[14][2] = D[5][14]*T + D[2][14];
496  P[2][15] = P[15][2] = D[5][15]*T + D[2][15];
497  P[3][3] = (Q[3]*G[3][3]*G[3][3] + Q[4]*G[3][4]*G[3][4] + Q[5]*G[3][5]*G[3][5] + F[3][6]*(F[3][6]*D[6][6] + F[3][7]*D[6][7] + F[3][8]*D[6][8] + F[3][9]*D[6][9] + F[3][13]*D[6][13] + F[3][14]*D[6][14] + F[3][15]*D[6][15]) + F[3][7]*(F[3][6]*D[6][7] + F[3][7]*D[7][7] + F[3][8]*D[7][8] + F[3][9]*D[7][9] + F[3][13]*D[7][13] + F[3][14]*D[7][14] + F[3][15]*D[7][15]) + F[3][8]*(F[3][6]*D[6][8] + F[3][7]*D[7][8] + F[3][8]*D[8][8] + F[3][9]*D[8][9] + F[3][13]*D[8][13] + F[3][14]*D[8][14] + F[3][15]*D[8][15]) + F[3][9]*(F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9] + F[3][9]*D[9][9] + F[3][13]*D[9][13] + F[3][14]*D[9][14] + F[3][15]*D[9][15]) + F[3][13]*(F[3][6]*D[6][13] + F[3][7]*D[7][13] + F[3][8]*D[8][13] + F[3][9]*D[9][13] + F[3][13]*D[13][13] + F[3][14]*D[13][14] + F[3][15]*D[13][15]) + F[3][14]*(F[3][6]*D[6][14] + F[3][7]*D[7][14] + F[3][8]*D[8][14] + F[3][9]*D[9][14] + F[3][13]*D[13][14] + F[3][14]*D[14][14] + F[3][15]*D[14][15]) + F[3][15]*(F[3][6]*D[6][15] + F[3][7]*D[7][15] + F[3][8]*D[8][15] + F[3][9]*D[9][15] + F[3][13]*D[13][15] + F[3][14]*D[14][15] + F[3][15]*D[15][15]))*Tsq + (2*F[3][6]*D[3][6] + 2*F[3][7]*D[3][7] + 2*F[3][8]*D[3][8] + 2*F[3][9]*D[3][9] + 2*F[3][13]*D[3][13] + 2*F[3][14]*D[3][14] + 2*F[3][15]*D[3][15])*T + D[3][3];
498  P[3][4] = P[4][3] = (F[4][6]*(F[3][6]*D[6][6] + F[3][7]*D[6][7] + F[3][8]*D[6][8] + F[3][9]*D[6][9] + F[3][13]*D[6][13] + F[3][14]*D[6][14] + F[3][15]*D[6][15]) + F[4][7]*(F[3][6]*D[6][7] + F[3][7]*D[7][7] + F[3][8]*D[7][8] + F[3][9]*D[7][9] + F[3][13]*D[7][13] + F[3][14]*D[7][14] + F[3][15]*D[7][15]) + F[4][8]*(F[3][6]*D[6][8] + F[3][7]*D[7][8] + F[3][8]*D[8][8] + F[3][9]*D[8][9] + F[3][13]*D[8][13] + F[3][14]*D[8][14] + F[3][15]*D[8][15]) + F[4][9]*(F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9] + F[3][9]*D[9][9] + F[3][13]*D[9][13] + F[3][14]*D[9][14] + F[3][15]*D[9][15]) + F[4][13]*(F[3][6]*D[6][13] + F[3][7]*D[7][13] + F[3][8]*D[8][13] + F[3][9]*D[9][13] + F[3][13]*D[13][13] + F[3][14]*D[13][14] + F[3][15]*D[13][15]) + F[4][14]*(F[3][6]*D[6][14] + F[3][7]*D[7][14] + F[3][8]*D[8][14] + F[3][9]*D[9][14] + F[3][13]*D[13][14] + F[3][14]*D[14][14] + F[3][15]*D[14][15]) + F[4][15]*(F[3][6]*D[6][15] + F[3][7]*D[7][15] + F[3][8]*D[8][15] + F[3][9]*D[9][15] + F[3][13]*D[13][15] + F[3][14]*D[14][15] + F[3][15]*D[15][15]) + G[3][3]*G[4][3]*Q[3] + G[3][4]*G[4][4]*Q[4] + G[3][5]*G[4][5]*Q[5])*Tsq + (F[3][6]*D[4][6] + F[4][6]*D[3][6] + F[3][7]*D[4][7] + F[4][7]*D[3][7] + F[3][8]*D[4][8] + F[4][8]*D[3][8] + F[3][9]*D[4][9] + F[4][9]*D[3][9] + F[3][13]*D[4][13] + F[4][13]*D[3][13] + F[3][14]*D[4][14] + F[4][14]*D[3][14] + F[3][15]*D[4][15] + F[4][15]*D[3][15])*T + D[3][4];
499  P[3][5] = P[5][3] = (F[5][6]*(F[3][6]*D[6][6] + F[3][7]*D[6][7] + F[3][8]*D[6][8] + F[3][9]*D[6][9] + F[3][13]*D[6][13] + F[3][14]*D[6][14] + F[3][15]*D[6][15]) + F[5][7]*(F[3][6]*D[6][7] + F[3][7]*D[7][7] + F[3][8]*D[7][8] + F[3][9]*D[7][9] + F[3][13]*D[7][13] + F[3][14]*D[7][14] + F[3][15]*D[7][15]) + F[5][8]*(F[3][6]*D[6][8] + F[3][7]*D[7][8] + F[3][8]*D[8][8] + F[3][9]*D[8][9] + F[3][13]*D[8][13] + F[3][14]*D[8][14] + F[3][15]*D[8][15]) + F[5][9]*(F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9] + F[3][9]*D[9][9] + F[3][13]*D[9][13] + F[3][14]*D[9][14] + F[3][15]*D[9][15]) + F[5][13]*(F[3][6]*D[6][13] + F[3][7]*D[7][13] + F[3][8]*D[8][13] + F[3][9]*D[9][13] + F[3][13]*D[13][13] + F[3][14]*D[13][14] + F[3][15]*D[13][15]) + F[5][14]*(F[3][6]*D[6][14] + F[3][7]*D[7][14] + F[3][8]*D[8][14] + F[3][9]*D[9][14] + F[3][13]*D[13][14] + F[3][14]*D[14][14] + F[3][15]*D[14][15]) + F[5][15]*(F[3][6]*D[6][15] + F[3][7]*D[7][15] + F[3][8]*D[8][15] + F[3][9]*D[9][15] + F[3][13]*D[13][15] + F[3][14]*D[14][15] + F[3][15]*D[15][15]) + G[3][3]*G[5][3]*Q[3] + G[3][4]*G[5][4]*Q[4] + G[3][5]*G[5][5]*Q[5])*Tsq + (F[3][6]*D[5][6] + F[5][6]*D[3][6] + F[3][7]*D[5][7] + F[5][7]*D[3][7] + F[3][8]*D[5][8] + F[5][8]*D[3][8] + F[3][9]*D[5][9] + F[5][9]*D[3][9] + F[3][13]*D[5][13] + F[5][13]*D[3][13] + F[3][14]*D[5][14] + F[5][14]*D[3][14] + F[3][15]*D[5][15] + F[5][15]*D[3][15])*T + D[3][5];
500  P[3][6] = P[6][3] = (F[6][7]*(F[3][6]*D[6][7] + F[3][7]*D[7][7] + F[3][8]*D[7][8] + F[3][9]*D[7][9] + F[3][13]*D[7][13] + F[3][14]*D[7][14] + F[3][15]*D[7][15]) + F[6][8]*(F[3][6]*D[6][8] + F[3][7]*D[7][8] + F[3][8]*D[8][8] + F[3][9]*D[8][9] + F[3][13]*D[8][13] + F[3][14]*D[8][14] + F[3][15]*D[8][15]) + F[6][9]*(F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9] + F[3][9]*D[9][9] + F[3][13]*D[9][13] + F[3][14]*D[9][14] + F[3][15]*D[9][15]) + F[6][10]*(F[3][6]*D[6][10] + F[3][7]*D[7][10] + F[3][8]*D[8][10] + F[3][9]*D[9][10] + F[3][13]*D[10][13] + F[3][14]*D[10][14] + F[3][15]*D[10][15]) + F[6][11]*(F[3][6]*D[6][11] + F[3][7]*D[7][11] + F[3][8]*D[8][11] + F[3][9]*D[9][11] + F[3][13]*D[11][13] + F[3][14]*D[11][14] + F[3][15]*D[11][15]) + F[6][12]*(F[3][6]*D[6][12] + F[3][7]*D[7][12] + F[3][8]*D[8][12] + F[3][9]*D[9][12] + F[3][13]*D[12][13] + F[3][14]*D[12][14] + F[3][15]*D[12][15]))*Tsq + (F[3][6]*D[6][6] + F[3][7]*D[6][7] + F[6][7]*D[3][7] + F[3][8]*D[6][8] + F[6][8]*D[3][8] + F[3][9]*D[6][9] + F[6][9]*D[3][9] + F[6][10]*D[3][10] + F[6][11]*D[3][11] + F[6][12]*D[3][12] + F[3][13]*D[6][13] + F[3][14]*D[6][14] + F[3][15]*D[6][15])*T + D[3][6];
501  P[3][7] = P[7][3] = (F[7][6]*(F[3][6]*D[6][6] + F[3][7]*D[6][7] + F[3][8]*D[6][8] + F[3][9]*D[6][9] + F[3][13]*D[6][13] + F[3][14]*D[6][14] + F[3][15]*D[6][15]) + F[7][8]*(F[3][6]*D[6][8] + F[3][7]*D[7][8] + F[3][8]*D[8][8] + F[3][9]*D[8][9] + F[3][13]*D[8][13] + F[3][14]*D[8][14] + F[3][15]*D[8][15]) + F[7][9]*(F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9] + F[3][9]*D[9][9] + F[3][13]*D[9][13] + F[3][14]*D[9][14] + F[3][15]*D[9][15]) + F[7][10]*(F[3][6]*D[6][10] + F[3][7]*D[7][10] + F[3][8]*D[8][10] + F[3][9]*D[9][10] + F[3][13]*D[10][13] + F[3][14]*D[10][14] + F[3][15]*D[10][15]) + F[7][11]*(F[3][6]*D[6][11] + F[3][7]*D[7][11] + F[3][8]*D[8][11] + F[3][9]*D[9][11] + F[3][13]*D[11][13] + F[3][14]*D[11][14] + F[3][15]*D[11][15]) + F[7][12]*(F[3][6]*D[6][12] + F[3][7]*D[7][12] + F[3][8]*D[8][12] + F[3][9]*D[9][12] + F[3][13]*D[12][13] + F[3][14]*D[12][14] + F[3][15]*D[12][15]))*Tsq + (F[3][6]*D[6][7] + F[7][6]*D[3][6] + F[3][7]*D[7][7] + F[3][8]*D[7][8] + F[7][8]*D[3][8] + F[3][9]*D[7][9] + F[7][9]*D[3][9] + F[7][10]*D[3][10] + F[7][11]*D[3][11] + F[7][12]*D[3][12] + F[3][13]*D[7][13] + F[3][14]*D[7][14] + F[3][15]*D[7][15])*T + D[3][7];
502  P[3][8] = P[8][3] = (F[8][6]*(F[3][6]*D[6][6] + F[3][7]*D[6][7] + F[3][8]*D[6][8] + F[3][9]*D[6][9] + F[3][13]*D[6][13] + F[3][14]*D[6][14] + F[3][15]*D[6][15]) + F[8][7]*(F[3][6]*D[6][7] + F[3][7]*D[7][7] + F[3][8]*D[7][8] + F[3][9]*D[7][9] + F[3][13]*D[7][13] + F[3][14]*D[7][14] + F[3][15]*D[7][15]) + F[8][9]*(F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9] + F[3][9]*D[9][9] + F[3][13]*D[9][13] + F[3][14]*D[9][14] + F[3][15]*D[9][15]) + F[8][10]*(F[3][6]*D[6][10] + F[3][7]*D[7][10] + F[3][8]*D[8][10] + F[3][9]*D[9][10] + F[3][13]*D[10][13] + F[3][14]*D[10][14] + F[3][15]*D[10][15]) + F[8][11]*(F[3][6]*D[6][11] + F[3][7]*D[7][11] + F[3][8]*D[8][11] + F[3][9]*D[9][11] + F[3][13]*D[11][13] + F[3][14]*D[11][14] + F[3][15]*D[11][15]) + F[8][12]*(F[3][6]*D[6][12] + F[3][7]*D[7][12] + F[3][8]*D[8][12] + F[3][9]*D[9][12] + F[3][13]*D[12][13] + F[3][14]*D[12][14] + F[3][15]*D[12][15]))*Tsq + (F[3][6]*D[6][8] + F[3][7]*D[7][8] + F[8][6]*D[3][6] + F[8][7]*D[3][7] + F[3][8]*D[8][8] + F[3][9]*D[8][9] + F[8][9]*D[3][9] + F[8][10]*D[3][10] + F[8][11]*D[3][11] + F[8][12]*D[3][12] + F[3][13]*D[8][13] + F[3][14]*D[8][14] + F[3][15]*D[8][15])*T + D[3][8];
503  P[3][9] = P[9][3] = (F[9][6]*(F[3][6]*D[6][6] + F[3][7]*D[6][7] + F[3][8]*D[6][8] + F[3][9]*D[6][9] + F[3][13]*D[6][13] + F[3][14]*D[6][14] + F[3][15]*D[6][15]) + F[9][7]*(F[3][6]*D[6][7] + F[3][7]*D[7][7] + F[3][8]*D[7][8] + F[3][9]*D[7][9] + F[3][13]*D[7][13] + F[3][14]*D[7][14] + F[3][15]*D[7][15]) + F[9][8]*(F[3][6]*D[6][8] + F[3][7]*D[7][8] + F[3][8]*D[8][8] + F[3][9]*D[8][9] + F[3][13]*D[8][13] + F[3][14]*D[8][14] + F[3][15]*D[8][15]) + F[9][10]*(F[3][6]*D[6][10] + F[3][7]*D[7][10] + F[3][8]*D[8][10] + F[3][9]*D[9][10] + F[3][13]*D[10][13] + F[3][14]*D[10][14] + F[3][15]*D[10][15]) + F[9][11]*(F[3][6]*D[6][11] + F[3][7]*D[7][11] + F[3][8]*D[8][11] + F[3][9]*D[9][11] + F[3][13]*D[11][13] + F[3][14]*D[11][14] + F[3][15]*D[11][15]) + F[9][12]*(F[3][6]*D[6][12] + F[3][7]*D[7][12] + F[3][8]*D[8][12] + F[3][9]*D[9][12] + F[3][13]*D[12][13] + F[3][14]*D[12][14] + F[3][15]*D[12][15]))*Tsq + (F[9][6]*D[3][6] + F[9][7]*D[3][7] + F[9][8]*D[3][8] + F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9] + F[3][9]*D[9][9] + F[9][10]*D[3][10] + F[9][11]*D[3][11] + F[9][12]*D[3][12] + F[3][13]*D[9][13] + F[3][14]*D[9][14] + F[3][15]*D[9][15])*T + D[3][9];
504  P[3][10] = P[10][3] = (F[3][6]*D[6][10] + F[3][7]*D[7][10] + F[3][8]*D[8][10] + F[3][9]*D[9][10] + F[3][13]*D[10][13] + F[3][14]*D[10][14] + F[3][15]*D[10][15])*T + D[3][10];
505  P[3][11] = P[11][3] = (F[3][6]*D[6][11] + F[3][7]*D[7][11] + F[3][8]*D[8][11] + F[3][9]*D[9][11] + F[3][13]*D[11][13] + F[3][14]*D[11][14] + F[3][15]*D[11][15])*T + D[3][11];
506  P[3][12] = P[12][3] = (F[3][6]*D[6][12] + F[3][7]*D[7][12] + F[3][8]*D[8][12] + F[3][9]*D[9][12] + F[3][13]*D[12][13] + F[3][14]*D[12][14] + F[3][15]*D[12][15])*T + D[3][12];
507  P[3][13] = P[13][3] = (F[3][6]*D[6][13] + F[3][7]*D[7][13] + F[3][8]*D[8][13] + F[3][9]*D[9][13] + F[3][13]*D[13][13] + F[3][14]*D[13][14] + F[3][15]*D[13][15])*T + D[3][13];
508  P[3][14] = P[14][3] = (F[3][6]*D[6][14] + F[3][7]*D[7][14] + F[3][8]*D[8][14] + F[3][9]*D[9][14] + F[3][13]*D[13][14] + F[3][14]*D[14][14] + F[3][15]*D[14][15])*T + D[3][14];
509  P[3][15] = P[15][3] = (F[3][6]*D[6][15] + F[3][7]*D[7][15] + F[3][8]*D[8][15] + F[3][9]*D[9][15] + F[3][13]*D[13][15] + F[3][14]*D[14][15] + F[3][15]*D[15][15])*T + D[3][15];
510  P[4][4] = (Q[3]*G[4][3]*G[4][3] + Q[4]*G[4][4]*G[4][4] + Q[5]*G[4][5]*G[4][5] + F[4][6]*(F[4][6]*D[6][6] + F[4][7]*D[6][7] + F[4][8]*D[6][8] + F[4][9]*D[6][9] + F[4][13]*D[6][13] + F[4][14]*D[6][14] + F[4][15]*D[6][15]) + F[4][7]*(F[4][6]*D[6][7] + F[4][7]*D[7][7] + F[4][8]*D[7][8] + F[4][9]*D[7][9] + F[4][13]*D[7][13] + F[4][14]*D[7][14] + F[4][15]*D[7][15]) + F[4][8]*(F[4][6]*D[6][8] + F[4][7]*D[7][8] + F[4][8]*D[8][8] + F[4][9]*D[8][9] + F[4][13]*D[8][13] + F[4][14]*D[8][14] + F[4][15]*D[8][15]) + F[4][9]*(F[4][6]*D[6][9] + F[4][7]*D[7][9] + F[4][8]*D[8][9] + F[4][9]*D[9][9] + F[4][13]*D[9][13] + F[4][14]*D[9][14] + F[4][15]*D[9][15]) + F[4][13]*(F[4][6]*D[6][13] + F[4][7]*D[7][13] + F[4][8]*D[8][13] + F[4][9]*D[9][13] + F[4][13]*D[13][13] + F[4][14]*D[13][14] + F[4][15]*D[13][15]) + F[4][14]*(F[4][6]*D[6][14] + F[4][7]*D[7][14] + F[4][8]*D[8][14] + F[4][9]*D[9][14] + F[4][13]*D[13][14] + F[4][14]*D[14][14] + F[4][15]*D[14][15]) + F[4][15]*(F[4][6]*D[6][15] + F[4][7]*D[7][15] + F[4][8]*D[8][15] + F[4][9]*D[9][15] + F[4][13]*D[13][15] + F[4][14]*D[14][15] + F[4][15]*D[15][15]))*Tsq + (2*F[4][6]*D[4][6] + 2*F[4][7]*D[4][7] + 2*F[4][8]*D[4][8] + 2*F[4][9]*D[4][9] + 2*F[4][13]*D[4][13] + 2*F[4][14]*D[4][14] + 2*F[4][15]*D[4][15])*T + D[4][4];
511  P[4][5] = P[5][4] = (F[5][6]*(F[4][6]*D[6][6] + F[4][7]*D[6][7] + F[4][8]*D[6][8] + F[4][9]*D[6][9] + F[4][13]*D[6][13] + F[4][14]*D[6][14] + F[4][15]*D[6][15]) + F[5][7]*(F[4][6]*D[6][7] + F[4][7]*D[7][7] + F[4][8]*D[7][8] + F[4][9]*D[7][9] + F[4][13]*D[7][13] + F[4][14]*D[7][14] + F[4][15]*D[7][15]) + F[5][8]*(F[4][6]*D[6][8] + F[4][7]*D[7][8] + F[4][8]*D[8][8] + F[4][9]*D[8][9] + F[4][13]*D[8][13] + F[4][14]*D[8][14] + F[4][15]*D[8][15]) + F[5][9]*(F[4][6]*D[6][9] + F[4][7]*D[7][9] + F[4][8]*D[8][9] + F[4][9]*D[9][9] + F[4][13]*D[9][13] + F[4][14]*D[9][14] + F[4][15]*D[9][15]) + F[5][13]*(F[4][6]*D[6][13] + F[4][7]*D[7][13] + F[4][8]*D[8][13] + F[4][9]*D[9][13] + F[4][13]*D[13][13] + F[4][14]*D[13][14] + F[4][15]*D[13][15]) + F[5][14]*(F[4][6]*D[6][14] + F[4][7]*D[7][14] + F[4][8]*D[8][14] + F[4][9]*D[9][14] + F[4][13]*D[13][14] + F[4][14]*D[14][14] + F[4][15]*D[14][15]) + F[5][15]*(F[4][6]*D[6][15] + F[4][7]*D[7][15] + F[4][8]*D[8][15] + F[4][9]*D[9][15] + F[4][13]*D[13][15] + F[4][14]*D[14][15] + F[4][15]*D[15][15]) + G[4][3]*G[5][3]*Q[3] + G[4][4]*G[5][4]*Q[4] + G[4][5]*G[5][5]*Q[5])*Tsq + (F[4][6]*D[5][6] + F[5][6]*D[4][6] + F[4][7]*D[5][7] + F[5][7]*D[4][7] + F[4][8]*D[5][8] + F[5][8]*D[4][8] + F[4][9]*D[5][9] + F[5][9]*D[4][9] + F[4][13]*D[5][13] + F[5][13]*D[4][13] + F[4][14]*D[5][14] + F[5][14]*D[4][14] + F[4][15]*D[5][15] + F[5][15]*D[4][15])*T + D[4][5];
512  P[4][6] = P[6][4] = (F[6][7]*(F[4][6]*D[6][7] + F[4][7]*D[7][7] + F[4][8]*D[7][8] + F[4][9]*D[7][9] + F[4][13]*D[7][13] + F[4][14]*D[7][14] + F[4][15]*D[7][15]) + F[6][8]*(F[4][6]*D[6][8] + F[4][7]*D[7][8] + F[4][8]*D[8][8] + F[4][9]*D[8][9] + F[4][13]*D[8][13] + F[4][14]*D[8][14] + F[4][15]*D[8][15]) + F[6][9]*(F[4][6]*D[6][9] + F[4][7]*D[7][9] + F[4][8]*D[8][9] + F[4][9]*D[9][9] + F[4][13]*D[9][13] + F[4][14]*D[9][14] + F[4][15]*D[9][15]) + F[6][10]*(F[4][6]*D[6][10] + F[4][7]*D[7][10] + F[4][8]*D[8][10] + F[4][9]*D[9][10] + F[4][13]*D[10][13] + F[4][14]*D[10][14] + F[4][15]*D[10][15]) + F[6][11]*(F[4][6]*D[6][11] + F[4][7]*D[7][11] + F[4][8]*D[8][11] + F[4][9]*D[9][11] + F[4][13]*D[11][13] + F[4][14]*D[11][14] + F[4][15]*D[11][15]) + F[6][12]*(F[4][6]*D[6][12] + F[4][7]*D[7][12] + F[4][8]*D[8][12] + F[4][9]*D[9][12] + F[4][13]*D[12][13] + F[4][14]*D[12][14] + F[4][15]*D[12][15]))*Tsq + (F[4][6]*D[6][6] + F[4][7]*D[6][7] + F[6][7]*D[4][7] + F[4][8]*D[6][8] + F[6][8]*D[4][8] + F[4][9]*D[6][9] + F[6][9]*D[4][9] + F[6][10]*D[4][10] + F[6][11]*D[4][11] + F[6][12]*D[4][12] + F[4][13]*D[6][13] + F[4][14]*D[6][14] + F[4][15]*D[6][15])*T + D[4][6];
513  P[4][7] = P[7][4] = (F[7][6]*(F[4][6]*D[6][6] + F[4][7]*D[6][7] + F[4][8]*D[6][8] + F[4][9]*D[6][9] + F[4][13]*D[6][13] + F[4][14]*D[6][14] + F[4][15]*D[6][15]) + F[7][8]*(F[4][6]*D[6][8] + F[4][7]*D[7][8] + F[4][8]*D[8][8] + F[4][9]*D[8][9] + F[4][13]*D[8][13] + F[4][14]*D[8][14] + F[4][15]*D[8][15]) + F[7][9]*(F[4][6]*D[6][9] + F[4][7]*D[7][9] + F[4][8]*D[8][9] + F[4][9]*D[9][9] + F[4][13]*D[9][13] + F[4][14]*D[9][14] + F[4][15]*D[9][15]) + F[7][10]*(F[4][6]*D[6][10] + F[4][7]*D[7][10] + F[4][8]*D[8][10] + F[4][9]*D[9][10] + F[4][13]*D[10][13] + F[4][14]*D[10][14] + F[4][15]*D[10][15]) + F[7][11]*(F[4][6]*D[6][11] + F[4][7]*D[7][11] + F[4][8]*D[8][11] + F[4][9]*D[9][11] + F[4][13]*D[11][13] + F[4][14]*D[11][14] + F[4][15]*D[11][15]) + F[7][12]*(F[4][6]*D[6][12] + F[4][7]*D[7][12] + F[4][8]*D[8][12] + F[4][9]*D[9][12] + F[4][13]*D[12][13] + F[4][14]*D[12][14] + F[4][15]*D[12][15]))*Tsq + (F[4][6]*D[6][7] + F[7][6]*D[4][6] + F[4][7]*D[7][7] + F[4][8]*D[7][8] + F[7][8]*D[4][8] + F[4][9]*D[7][9] + F[7][9]*D[4][9] + F[7][10]*D[4][10] + F[7][11]*D[4][11] + F[7][12]*D[4][12] + F[4][13]*D[7][13] + F[4][14]*D[7][14] + F[4][15]*D[7][15])*T + D[4][7];
514  P[4][8] = P[8][4] = (F[8][6]*(F[4][6]*D[6][6] + F[4][7]*D[6][7] + F[4][8]*D[6][8] + F[4][9]*D[6][9] + F[4][13]*D[6][13] + F[4][14]*D[6][14] + F[4][15]*D[6][15]) + F[8][7]*(F[4][6]*D[6][7] + F[4][7]*D[7][7] + F[4][8]*D[7][8] + F[4][9]*D[7][9] + F[4][13]*D[7][13] + F[4][14]*D[7][14] + F[4][15]*D[7][15]) + F[8][9]*(F[4][6]*D[6][9] + F[4][7]*D[7][9] + F[4][8]*D[8][9] + F[4][9]*D[9][9] + F[4][13]*D[9][13] + F[4][14]*D[9][14] + F[4][15]*D[9][15]) + F[8][10]*(F[4][6]*D[6][10] + F[4][7]*D[7][10] + F[4][8]*D[8][10] + F[4][9]*D[9][10] + F[4][13]*D[10][13] + F[4][14]*D[10][14] + F[4][15]*D[10][15]) + F[8][11]*(F[4][6]*D[6][11] + F[4][7]*D[7][11] + F[4][8]*D[8][11] + F[4][9]*D[9][11] + F[4][13]*D[11][13] + F[4][14]*D[11][14] + F[4][15]*D[11][15]) + F[8][12]*(F[4][6]*D[6][12] + F[4][7]*D[7][12] + F[4][8]*D[8][12] + F[4][9]*D[9][12] + F[4][13]*D[12][13] + F[4][14]*D[12][14] + F[4][15]*D[12][15]))*Tsq + (F[4][6]*D[6][8] + F[4][7]*D[7][8] + F[8][6]*D[4][6] + F[8][7]*D[4][7] + F[4][8]*D[8][8] + F[4][9]*D[8][9] + F[8][9]*D[4][9] + F[8][10]*D[4][10] + F[8][11]*D[4][11] + F[8][12]*D[4][12] + F[4][13]*D[8][13] + F[4][14]*D[8][14] + F[4][15]*D[8][15])*T + D[4][8];
515  P[4][9] = P[9][4] = (F[9][6]*(F[4][6]*D[6][6] + F[4][7]*D[6][7] + F[4][8]*D[6][8] + F[4][9]*D[6][9] + F[4][13]*D[6][13] + F[4][14]*D[6][14] + F[4][15]*D[6][15]) + F[9][7]*(F[4][6]*D[6][7] + F[4][7]*D[7][7] + F[4][8]*D[7][8] + F[4][9]*D[7][9] + F[4][13]*D[7][13] + F[4][14]*D[7][14] + F[4][15]*D[7][15]) + F[9][8]*(F[4][6]*D[6][8] + F[4][7]*D[7][8] + F[4][8]*D[8][8] + F[4][9]*D[8][9] + F[4][13]*D[8][13] + F[4][14]*D[8][14] + F[4][15]*D[8][15]) + F[9][10]*(F[4][6]*D[6][10] + F[4][7]*D[7][10] + F[4][8]*D[8][10] + F[4][9]*D[9][10] + F[4][13]*D[10][13] + F[4][14]*D[10][14] + F[4][15]*D[10][15]) + F[9][11]*(F[4][6]*D[6][11] + F[4][7]*D[7][11] + F[4][8]*D[8][11] + F[4][9]*D[9][11] + F[4][13]*D[11][13] + F[4][14]*D[11][14] + F[4][15]*D[11][15]) + F[9][12]*(F[4][6]*D[6][12] + F[4][7]*D[7][12] + F[4][8]*D[8][12] + F[4][9]*D[9][12] + F[4][13]*D[12][13] + F[4][14]*D[12][14] + F[4][15]*D[12][15]))*Tsq + (F[9][6]*D[4][6] + F[9][7]*D[4][7] + F[9][8]*D[4][8] + F[4][6]*D[6][9] + F[4][7]*D[7][9] + F[4][8]*D[8][9] + F[4][9]*D[9][9] + F[9][10]*D[4][10] + F[9][11]*D[4][11] + F[9][12]*D[4][12] + F[4][13]*D[9][13] + F[4][14]*D[9][14] + F[4][15]*D[9][15])*T + D[4][9];
516  P[4][10] = P[10][4] = (F[4][6]*D[6][10] + F[4][7]*D[7][10] + F[4][8]*D[8][10] + F[4][9]*D[9][10] + F[4][13]*D[10][13] + F[4][14]*D[10][14] + F[4][15]*D[10][15])*T + D[4][10];
517  P[4][11] = P[11][4] = (F[4][6]*D[6][11] + F[4][7]*D[7][11] + F[4][8]*D[8][11] + F[4][9]*D[9][11] + F[4][13]*D[11][13] + F[4][14]*D[11][14] + F[4][15]*D[11][15])*T + D[4][11];
518  P[4][12] = P[12][4] = (F[4][6]*D[6][12] + F[4][7]*D[7][12] + F[4][8]*D[8][12] + F[4][9]*D[9][12] + F[4][13]*D[12][13] + F[4][14]*D[12][14] + F[4][15]*D[12][15])*T + D[4][12];
519  P[4][13] = P[13][4] = (F[4][6]*D[6][13] + F[4][7]*D[7][13] + F[4][8]*D[8][13] + F[4][9]*D[9][13] + F[4][13]*D[13][13] + F[4][14]*D[13][14] + F[4][15]*D[13][15])*T + D[4][13];
520  P[4][14] = P[14][4] = (F[4][6]*D[6][14] + F[4][7]*D[7][14] + F[4][8]*D[8][14] + F[4][9]*D[9][14] + F[4][13]*D[13][14] + F[4][14]*D[14][14] + F[4][15]*D[14][15])*T + D[4][14];
521  P[4][15] = P[15][4] = (F[4][6]*D[6][15] + F[4][7]*D[7][15] + F[4][8]*D[8][15] + F[4][9]*D[9][15] + F[4][13]*D[13][15] + F[4][14]*D[14][15] + F[4][15]*D[15][15])*T + D[4][15];
522  P[5][5] = (Q[3]*G[5][3]*G[5][3] + Q[4]*G[5][4]*G[5][4] + Q[5]*G[5][5]*G[5][5] + F[5][6]*(F[5][6]*D[6][6] + F[5][7]*D[6][7] + F[5][8]*D[6][8] + F[5][9]*D[6][9] + F[5][13]*D[6][13] + F[5][14]*D[6][14] + F[5][15]*D[6][15]) + F[5][7]*(F[5][6]*D[6][7] + F[5][7]*D[7][7] + F[5][8]*D[7][8] + F[5][9]*D[7][9] + F[5][13]*D[7][13] + F[5][14]*D[7][14] + F[5][15]*D[7][15]) + F[5][8]*(F[5][6]*D[6][8] + F[5][7]*D[7][8] + F[5][8]*D[8][8] + F[5][9]*D[8][9] + F[5][13]*D[8][13] + F[5][14]*D[8][14] + F[5][15]*D[8][15]) + F[5][9]*(F[5][6]*D[6][9] + F[5][7]*D[7][9] + F[5][8]*D[8][9] + F[5][9]*D[9][9] + F[5][13]*D[9][13] + F[5][14]*D[9][14] + F[5][15]*D[9][15]) + F[5][13]*(F[5][6]*D[6][13] + F[5][7]*D[7][13] + F[5][8]*D[8][13] + F[5][9]*D[9][13] + F[5][13]*D[13][13] + F[5][14]*D[13][14] + F[5][15]*D[13][15]) + F[5][14]*(F[5][6]*D[6][14] + F[5][7]*D[7][14] + F[5][8]*D[8][14] + F[5][9]*D[9][14] + F[5][13]*D[13][14] + F[5][14]*D[14][14] + F[5][15]*D[14][15]) + F[5][15]*(F[5][6]*D[6][15] + F[5][7]*D[7][15] + F[5][8]*D[8][15] + F[5][9]*D[9][15] + F[5][13]*D[13][15] + F[5][14]*D[14][15] + F[5][15]*D[15][15]))*Tsq + (2*F[5][6]*D[5][6] + 2*F[5][7]*D[5][7] + 2*F[5][8]*D[5][8] + 2*F[5][9]*D[5][9] + 2*F[5][13]*D[5][13] + 2*F[5][14]*D[5][14] + 2*F[5][15]*D[5][15])*T + D[5][5];
523  P[5][6] = P[6][5] = (F[6][7]*(F[5][6]*D[6][7] + F[5][7]*D[7][7] + F[5][8]*D[7][8] + F[5][9]*D[7][9] + F[5][13]*D[7][13] + F[5][14]*D[7][14] + F[5][15]*D[7][15]) + F[6][8]*(F[5][6]*D[6][8] + F[5][7]*D[7][8] + F[5][8]*D[8][8] + F[5][9]*D[8][9] + F[5][13]*D[8][13] + F[5][14]*D[8][14] + F[5][15]*D[8][15]) + F[6][9]*(F[5][6]*D[6][9] + F[5][7]*D[7][9] + F[5][8]*D[8][9] + F[5][9]*D[9][9] + F[5][13]*D[9][13] + F[5][14]*D[9][14] + F[5][15]*D[9][15]) + F[6][10]*(F[5][6]*D[6][10] + F[5][7]*D[7][10] + F[5][8]*D[8][10] + F[5][9]*D[9][10] + F[5][13]*D[10][13] + F[5][14]*D[10][14] + F[5][15]*D[10][15]) + F[6][11]*(F[5][6]*D[6][11] + F[5][7]*D[7][11] + F[5][8]*D[8][11] + F[5][9]*D[9][11] + F[5][13]*D[11][13] + F[5][14]*D[11][14] + F[5][15]*D[11][15]) + F[6][12]*(F[5][6]*D[6][12] + F[5][7]*D[7][12] + F[5][8]*D[8][12] + F[5][9]*D[9][12] + F[5][13]*D[12][13] + F[5][14]*D[12][14] + F[5][15]*D[12][15]))*Tsq + (F[5][6]*D[6][6] + F[5][7]*D[6][7] + F[6][7]*D[5][7] + F[5][8]*D[6][8] + F[6][8]*D[5][8] + F[5][9]*D[6][9] + F[6][9]*D[5][9] + F[6][10]*D[5][10] + F[6][11]*D[5][11] + F[6][12]*D[5][12] + F[5][13]*D[6][13] + F[5][14]*D[6][14] + F[5][15]*D[6][15])*T + D[5][6];
524  P[5][7] = P[7][5] = (F[7][6]*(F[5][6]*D[6][6] + F[5][7]*D[6][7] + F[5][8]*D[6][8] + F[5][9]*D[6][9] + F[5][13]*D[6][13] + F[5][14]*D[6][14] + F[5][15]*D[6][15]) + F[7][8]*(F[5][6]*D[6][8] + F[5][7]*D[7][8] + F[5][8]*D[8][8] + F[5][9]*D[8][9] + F[5][13]*D[8][13] + F[5][14]*D[8][14] + F[5][15]*D[8][15]) + F[7][9]*(F[5][6]*D[6][9] + F[5][7]*D[7][9] + F[5][8]*D[8][9] + F[5][9]*D[9][9] + F[5][13]*D[9][13] + F[5][14]*D[9][14] + F[5][15]*D[9][15]) + F[7][10]*(F[5][6]*D[6][10] + F[5][7]*D[7][10] + F[5][8]*D[8][10] + F[5][9]*D[9][10] + F[5][13]*D[10][13] + F[5][14]*D[10][14] + F[5][15]*D[10][15]) + F[7][11]*(F[5][6]*D[6][11] + F[5][7]*D[7][11] + F[5][8]*D[8][11] + F[5][9]*D[9][11] + F[5][13]*D[11][13] + F[5][14]*D[11][14] + F[5][15]*D[11][15]) + F[7][12]*(F[5][6]*D[6][12] + F[5][7]*D[7][12] + F[5][8]*D[8][12] + F[5][9]*D[9][12] + F[5][13]*D[12][13] + F[5][14]*D[12][14] + F[5][15]*D[12][15]))*Tsq + (F[5][6]*D[6][7] + F[7][6]*D[5][6] + F[5][7]*D[7][7] + F[5][8]*D[7][8] + F[7][8]*D[5][8] + F[5][9]*D[7][9] + F[7][9]*D[5][9] + F[7][10]*D[5][10] + F[7][11]*D[5][11] + F[7][12]*D[5][12] + F[5][13]*D[7][13] + F[5][14]*D[7][14] + F[5][15]*D[7][15])*T + D[5][7];
525  P[5][8] = P[8][5] = (F[8][6]*(F[5][6]*D[6][6] + F[5][7]*D[6][7] + F[5][8]*D[6][8] + F[5][9]*D[6][9] + F[5][13]*D[6][13] + F[5][14]*D[6][14] + F[5][15]*D[6][15]) + F[8][7]*(F[5][6]*D[6][7] + F[5][7]*D[7][7] + F[5][8]*D[7][8] + F[5][9]*D[7][9] + F[5][13]*D[7][13] + F[5][14]*D[7][14] + F[5][15]*D[7][15]) + F[8][9]*(F[5][6]*D[6][9] + F[5][7]*D[7][9] + F[5][8]*D[8][9] + F[5][9]*D[9][9] + F[5][13]*D[9][13] + F[5][14]*D[9][14] + F[5][15]*D[9][15]) + F[8][10]*(F[5][6]*D[6][10] + F[5][7]*D[7][10] + F[5][8]*D[8][10] + F[5][9]*D[9][10] + F[5][13]*D[10][13] + F[5][14]*D[10][14] + F[5][15]*D[10][15]) + F[8][11]*(F[5][6]*D[6][11] + F[5][7]*D[7][11] + F[5][8]*D[8][11] + F[5][9]*D[9][11] + F[5][13]*D[11][13] + F[5][14]*D[11][14] + F[5][15]*D[11][15]) + F[8][12]*(F[5][6]*D[6][12] + F[5][7]*D[7][12] + F[5][8]*D[8][12] + F[5][9]*D[9][12] + F[5][13]*D[12][13] + F[5][14]*D[12][14] + F[5][15]*D[12][15]))*Tsq + (F[5][6]*D[6][8] + F[5][7]*D[7][8] + F[8][6]*D[5][6] + F[8][7]*D[5][7] + F[5][8]*D[8][8] + F[5][9]*D[8][9] + F[8][9]*D[5][9] + F[8][10]*D[5][10] + F[8][11]*D[5][11] + F[8][12]*D[5][12] + F[5][13]*D[8][13] + F[5][14]*D[8][14] + F[5][15]*D[8][15])*T + D[5][8];
526  P[5][9] = P[9][5] = (F[9][6]*(F[5][6]*D[6][6] + F[5][7]*D[6][7] + F[5][8]*D[6][8] + F[5][9]*D[6][9] + F[5][13]*D[6][13] + F[5][14]*D[6][14] + F[5][15]*D[6][15]) + F[9][7]*(F[5][6]*D[6][7] + F[5][7]*D[7][7] + F[5][8]*D[7][8] + F[5][9]*D[7][9] + F[5][13]*D[7][13] + F[5][14]*D[7][14] + F[5][15]*D[7][15]) + F[9][8]*(F[5][6]*D[6][8] + F[5][7]*D[7][8] + F[5][8]*D[8][8] + F[5][9]*D[8][9] + F[5][13]*D[8][13] + F[5][14]*D[8][14] + F[5][15]*D[8][15]) + F[9][10]*(F[5][6]*D[6][10] + F[5][7]*D[7][10] + F[5][8]*D[8][10] + F[5][9]*D[9][10] + F[5][13]*D[10][13] + F[5][14]*D[10][14] + F[5][15]*D[10][15]) + F[9][11]*(F[5][6]*D[6][11] + F[5][7]*D[7][11] + F[5][8]*D[8][11] + F[5][9]*D[9][11] + F[5][13]*D[11][13] + F[5][14]*D[11][14] + F[5][15]*D[11][15]) + F[9][12]*(F[5][6]*D[6][12] + F[5][7]*D[7][12] + F[5][8]*D[8][12] + F[5][9]*D[9][12] + F[5][13]*D[12][13] + F[5][14]*D[12][14] + F[5][15]*D[12][15]))*Tsq + (F[9][6]*D[5][6] + F[9][7]*D[5][7] + F[9][8]*D[5][8] + F[5][6]*D[6][9] + F[5][7]*D[7][9] + F[5][8]*D[8][9] + F[5][9]*D[9][9] + F[9][10]*D[5][10] + F[9][11]*D[5][11] + F[9][12]*D[5][12] + F[5][13]*D[9][13] + F[5][14]*D[9][14] + F[5][15]*D[9][15])*T + D[5][9];
527  P[5][10] = P[10][5] = (F[5][6]*D[6][10] + F[5][7]*D[7][10] + F[5][8]*D[8][10] + F[5][9]*D[9][10] + F[5][13]*D[10][13] + F[5][14]*D[10][14] + F[5][15]*D[10][15])*T + D[5][10];
528  P[5][11] = P[11][5] = (F[5][6]*D[6][11] + F[5][7]*D[7][11] + F[5][8]*D[8][11] + F[5][9]*D[9][11] + F[5][13]*D[11][13] + F[5][14]*D[11][14] + F[5][15]*D[11][15])*T + D[5][11];
529  P[5][12] = P[12][5] = (F[5][6]*D[6][12] + F[5][7]*D[7][12] + F[5][8]*D[8][12] + F[5][9]*D[9][12] + F[5][13]*D[12][13] + F[5][14]*D[12][14] + F[5][15]*D[12][15])*T + D[5][12];
530  P[5][13] = P[13][5] = (F[5][6]*D[6][13] + F[5][7]*D[7][13] + F[5][8]*D[8][13] + F[5][9]*D[9][13] + F[5][13]*D[13][13] + F[5][14]*D[13][14] + F[5][15]*D[13][15])*T + D[5][13];
531  P[5][14] = P[14][5] = (F[5][6]*D[6][14] + F[5][7]*D[7][14] + F[5][8]*D[8][14] + F[5][9]*D[9][14] + F[5][13]*D[13][14] + F[5][14]*D[14][14] + F[5][15]*D[14][15])*T + D[5][14];
532  P[5][15] = P[15][5] = (F[5][6]*D[6][15] + F[5][7]*D[7][15] + F[5][8]*D[8][15] + F[5][9]*D[9][15] + F[5][13]*D[13][15] + F[5][14]*D[14][15] + F[5][15]*D[15][15])*T + D[5][15];
533  P[6][6] = (Q[0]*G[6][0]*G[6][0] + Q[1]*G[6][1]*G[6][1] + Q[2]*G[6][2]*G[6][2] + F[6][7]*(F[6][7]*D[7][7] + F[6][8]*D[7][8] + F[6][9]*D[7][9] + F[6][10]*D[7][10] + F[6][11]*D[7][11] + F[6][12]*D[7][12]) + F[6][8]*(F[6][7]*D[7][8] + F[6][8]*D[8][8] + F[6][9]*D[8][9] + F[6][10]*D[8][10] + F[6][11]*D[8][11] + F[6][12]*D[8][12]) + F[6][9]*(F[6][7]*D[7][9] + F[6][8]*D[8][9] + F[6][9]*D[9][9] + F[6][10]*D[9][10] + F[6][11]*D[9][11] + F[6][12]*D[9][12]) + F[6][10]*(F[6][7]*D[7][10] + F[6][8]*D[8][10] + F[6][9]*D[9][10] + F[6][10]*D[10][10] + F[6][11]*D[10][11] + F[6][12]*D[10][12]) + F[6][11]*(F[6][7]*D[7][11] + F[6][8]*D[8][11] + F[6][9]*D[9][11] + F[6][10]*D[10][11] + F[6][11]*D[11][11] + F[6][12]*D[11][12]) + F[6][12]*(F[6][7]*D[7][12] + F[6][8]*D[8][12] + F[6][9]*D[9][12] + F[6][10]*D[10][12] + F[6][11]*D[11][12] + F[6][12]*D[12][12]))*Tsq + (2*F[6][7]*D[6][7] + 2*F[6][8]*D[6][8] + 2*F[6][9]*D[6][9] + 2*F[6][10]*D[6][10] + 2*F[6][11]*D[6][11] + 2*F[6][12]*D[6][12])*T + D[6][6];
534  P[6][7] = P[7][6] = (F[7][6]*(F[6][7]*D[6][7] + F[6][8]*D[6][8] + F[6][9]*D[6][9] + F[6][10]*D[6][10] + F[6][11]*D[6][11] + F[6][12]*D[6][12]) + F[7][8]*(F[6][7]*D[7][8] + F[6][8]*D[8][8] + F[6][9]*D[8][9] + F[6][10]*D[8][10] + F[6][11]*D[8][11] + F[6][12]*D[8][12]) + F[7][9]*(F[6][7]*D[7][9] + F[6][8]*D[8][9] + F[6][9]*D[9][9] + F[6][10]*D[9][10] + F[6][11]*D[9][11] + F[6][12]*D[9][12]) + F[7][10]*(F[6][7]*D[7][10] + F[6][8]*D[8][10] + F[6][9]*D[9][10] + F[6][10]*D[10][10] + F[6][11]*D[10][11] + F[6][12]*D[10][12]) + F[7][11]*(F[6][7]*D[7][11] + F[6][8]*D[8][11] + F[6][9]*D[9][11] + F[6][10]*D[10][11] + F[6][11]*D[11][11] + F[6][12]*D[11][12]) + F[7][12]*(F[6][7]*D[7][12] + F[6][8]*D[8][12] + F[6][9]*D[9][12] + F[6][10]*D[10][12] + F[6][11]*D[11][12] + F[6][12]*D[12][12]) + G[6][0]*G[7][0]*Q[0] + G[6][1]*G[7][1]*Q[1] + G[6][2]*G[7][2]*Q[2])*Tsq + (F[7][6]*D[6][6] + F[6][7]*D[7][7] + F[6][8]*D[7][8] + F[7][8]*D[6][8] + F[6][9]*D[7][9] + F[7][9]*D[6][9] + F[6][10]*D[7][10] + F[7][10]*D[6][10] + F[6][11]*D[7][11] + F[7][11]*D[6][11] + F[6][12]*D[7][12] + F[7][12]*D[6][12])*T + D[6][7];
535  P[6][8] = P[8][6] = (F[8][6]*(F[6][7]*D[6][7] + F[6][8]*D[6][8] + F[6][9]*D[6][9] + F[6][10]*D[6][10] + F[6][11]*D[6][11] + F[6][12]*D[6][12]) + F[8][7]*(F[6][7]*D[7][7] + F[6][8]*D[7][8] + F[6][9]*D[7][9] + F[6][10]*D[7][10] + F[6][11]*D[7][11] + F[6][12]*D[7][12]) + F[8][9]*(F[6][7]*D[7][9] + F[6][8]*D[8][9] + F[6][9]*D[9][9] + F[6][10]*D[9][10] + F[6][11]*D[9][11] + F[6][12]*D[9][12]) + F[8][10]*(F[6][7]*D[7][10] + F[6][8]*D[8][10] + F[6][9]*D[9][10] + F[6][10]*D[10][10] + F[6][11]*D[10][11] + F[6][12]*D[10][12]) + F[8][11]*(F[6][7]*D[7][11] + F[6][8]*D[8][11] + F[6][9]*D[9][11] + F[6][10]*D[10][11] + F[6][11]*D[11][11] + F[6][12]*D[11][12]) + F[8][12]*(F[6][7]*D[7][12] + F[6][8]*D[8][12] + F[6][9]*D[9][12] + F[6][10]*D[10][12] + F[6][11]*D[11][12] + F[6][12]*D[12][12]) + G[6][0]*G[8][0]*Q[0] + G[6][1]*G[8][1]*Q[1] + G[6][2]*G[8][2]*Q[2])*Tsq + (F[6][7]*D[7][8] + F[8][6]*D[6][6] + F[8][7]*D[6][7] + F[6][8]*D[8][8] + F[6][9]*D[8][9] + F[8][9]*D[6][9] + F[6][10]*D[8][10] + F[8][10]*D[6][10] + F[6][11]*D[8][11] + F[8][11]*D[6][11] + F[6][12]*D[8][12] + F[8][12]*D[6][12])*T + D[6][8];
536  P[6][9] = P[9][6] = (F[9][6]*(F[6][7]*D[6][7] + F[6][8]*D[6][8] + F[6][9]*D[6][9] + F[6][10]*D[6][10] + F[6][11]*D[6][11] + F[6][12]*D[6][12]) + F[9][7]*(F[6][7]*D[7][7] + F[6][8]*D[7][8] + F[6][9]*D[7][9] + F[6][10]*D[7][10] + F[6][11]*D[7][11] + F[6][12]*D[7][12]) + F[9][8]*(F[6][7]*D[7][8] + F[6][8]*D[8][8] + F[6][9]*D[8][9] + F[6][10]*D[8][10] + F[6][11]*D[8][11] + F[6][12]*D[8][12]) + F[9][10]*(F[6][7]*D[7][10] + F[6][8]*D[8][10] + F[6][9]*D[9][10] + F[6][10]*D[10][10] + F[6][11]*D[10][11] + F[6][12]*D[10][12]) + F[9][11]*(F[6][7]*D[7][11] + F[6][8]*D[8][11] + F[6][9]*D[9][11] + F[6][10]*D[10][11] + F[6][11]*D[11][11] + F[6][12]*D[11][12]) + F[9][12]*(F[6][7]*D[7][12] + F[6][8]*D[8][12] + F[6][9]*D[9][12] + F[6][10]*D[10][12] + F[6][11]*D[11][12] + F[6][12]*D[12][12]) + G[6][0]*G[9][0]*Q[0] + G[6][1]*G[9][1]*Q[1] + G[6][2]*G[9][2]*Q[2])*Tsq + (F[9][6]*D[6][6] + F[9][7]*D[6][7] + F[9][8]*D[6][8] + F[6][7]*D[7][9] + F[6][8]*D[8][9] + F[6][9]*D[9][9] + F[6][10]*D[9][10] + F[9][10]*D[6][10] + F[6][11]*D[9][11] + F[9][11]*D[6][11] + F[6][12]*D[9][12] + F[9][12]*D[6][12])*T + D[6][9];
537  P[6][10] = P[10][6] = (F[6][7]*D[7][10] + F[6][8]*D[8][10] + F[6][9]*D[9][10] + F[6][10]*D[10][10] + F[6][11]*D[10][11] + F[6][12]*D[10][12])*T + D[6][10];
538  P[6][11] = P[11][6] = (F[6][7]*D[7][11] + F[6][8]*D[8][11] + F[6][9]*D[9][11] + F[6][10]*D[10][11] + F[6][11]*D[11][11] + F[6][12]*D[11][12])*T + D[6][11];
539  P[6][12] = P[12][6] = (F[6][7]*D[7][12] + F[6][8]*D[8][12] + F[6][9]*D[9][12] + F[6][10]*D[10][12] + F[6][11]*D[11][12] + F[6][12]*D[12][12])*T + D[6][12];
540  P[6][13] = P[13][6] = (F[6][7]*D[7][13] + F[6][8]*D[8][13] + F[6][9]*D[9][13] + F[6][10]*D[10][13] + F[6][11]*D[11][13] + F[6][12]*D[12][13])*T + D[6][13];
541  P[6][14] = P[14][6] = (F[6][7]*D[7][14] + F[6][8]*D[8][14] + F[6][9]*D[9][14] + F[6][10]*D[10][14] + F[6][11]*D[11][14] + F[6][12]*D[12][14])*T + D[6][14];
542  P[6][15] = P[15][6] = (F[6][7]*D[7][15] + F[6][8]*D[8][15] + F[6][9]*D[9][15] + F[6][10]*D[10][15] + F[6][11]*D[11][15] + F[6][12]*D[12][15])*T + D[6][15];
543  P[7][7] = (Q[0]*G[7][0]*G[7][0] + Q[1]*G[7][1]*G[7][1] + Q[2]*G[7][2]*G[7][2] + F[7][6]*(F[7][6]*D[6][6] + F[7][8]*D[6][8] + F[7][9]*D[6][9] + F[7][10]*D[6][10] + F[7][11]*D[6][11] + F[7][12]*D[6][12]) + F[7][8]*(F[7][6]*D[6][8] + F[7][8]*D[8][8] + F[7][9]*D[8][9] + F[7][10]*D[8][10] + F[7][11]*D[8][11] + F[7][12]*D[8][12]) + F[7][9]*(F[7][6]*D[6][9] + F[7][8]*D[8][9] + F[7][9]*D[9][9] + F[7][10]*D[9][10] + F[7][11]*D[9][11] + F[7][12]*D[9][12]) + F[7][10]*(F[7][6]*D[6][10] + F[7][8]*D[8][10] + F[7][9]*D[9][10] + F[7][10]*D[10][10] + F[7][11]*D[10][11] + F[7][12]*D[10][12]) + F[7][11]*(F[7][6]*D[6][11] + F[7][8]*D[8][11] + F[7][9]*D[9][11] + F[7][10]*D[10][11] + F[7][11]*D[11][11] + F[7][12]*D[11][12]) + F[7][12]*(F[7][6]*D[6][12] + F[7][8]*D[8][12] + F[7][9]*D[9][12] + F[7][10]*D[10][12] + F[7][11]*D[11][12] + F[7][12]*D[12][12]))*Tsq + (2*F[7][6]*D[6][7] + 2*F[7][8]*D[7][8] + 2*F[7][9]*D[7][9] + 2*F[7][10]*D[7][10] + 2*F[7][11]*D[7][11] + 2*F[7][12]*D[7][12])*T + D[7][7];
544  P[7][8] = P[8][7] = (F[8][6]*(F[7][6]*D[6][6] + F[7][8]*D[6][8] + F[7][9]*D[6][9] + F[7][10]*D[6][10] + F[7][11]*D[6][11] + F[7][12]*D[6][12]) + F[8][7]*(F[7][6]*D[6][7] + F[7][8]*D[7][8] + F[7][9]*D[7][9] + F[7][10]*D[7][10] + F[7][11]*D[7][11] + F[7][12]*D[7][12]) + F[8][9]*(F[7][6]*D[6][9] + F[7][8]*D[8][9] + F[7][9]*D[9][9] + F[7][10]*D[9][10] + F[7][11]*D[9][11] + F[7][12]*D[9][12]) + F[8][10]*(F[7][6]*D[6][10] + F[7][8]*D[8][10] + F[7][9]*D[9][10] + F[7][10]*D[10][10] + F[7][11]*D[10][11] + F[7][12]*D[10][12]) + F[8][11]*(F[7][6]*D[6][11] + F[7][8]*D[8][11] + F[7][9]*D[9][11] + F[7][10]*D[10][11] + F[7][11]*D[11][11] + F[7][12]*D[11][12]) + F[8][12]*(F[7][6]*D[6][12] + F[7][8]*D[8][12] + F[7][9]*D[9][12] + F[7][10]*D[10][12] + F[7][11]*D[11][12] + F[7][12]*D[12][12]) + G[7][0]*G[8][0]*Q[0] + G[7][1]*G[8][1]*Q[1] + G[7][2]*G[8][2]*Q[2])*Tsq + (F[7][6]*D[6][8] + F[8][6]*D[6][7] + F[8][7]*D[7][7] + F[7][8]*D[8][8] + F[7][9]*D[8][9] + F[8][9]*D[7][9] + F[7][10]*D[8][10] + F[8][10]*D[7][10] + F[7][11]*D[8][11] + F[8][11]*D[7][11] + F[7][12]*D[8][12] + F[8][12]*D[7][12])*T + D[7][8];
545  P[7][9] = P[9][7] = (F[9][6]*(F[7][6]*D[6][6] + F[7][8]*D[6][8] + F[7][9]*D[6][9] + F[7][10]*D[6][10] + F[7][11]*D[6][11] + F[7][12]*D[6][12]) + F[9][7]*(F[7][6]*D[6][7] + F[7][8]*D[7][8] + F[7][9]*D[7][9] + F[7][10]*D[7][10] + F[7][11]*D[7][11] + F[7][12]*D[7][12]) + F[9][8]*(F[7][6]*D[6][8] + F[7][8]*D[8][8] + F[7][9]*D[8][9] + F[7][10]*D[8][10] + F[7][11]*D[8][11] + F[7][12]*D[8][12]) + F[9][10]*(F[7][6]*D[6][10] + F[7][8]*D[8][10] + F[7][9]*D[9][10] + F[7][10]*D[10][10] + F[7][11]*D[10][11] + F[7][12]*D[10][12]) + F[9][11]*(F[7][6]*D[6][11] + F[7][8]*D[8][11] + F[7][9]*D[9][11] + F[7][10]*D[10][11] + F[7][11]*D[11][11] + F[7][12]*D[11][12]) + F[9][12]*(F[7][6]*D[6][12] + F[7][8]*D[8][12] + F[7][9]*D[9][12] + F[7][10]*D[10][12] + F[7][11]*D[11][12] + F[7][12]*D[12][12]) + G[7][0]*G[9][0]*Q[0] + G[7][1]*G[9][1]*Q[1] + G[7][2]*G[9][2]*Q[2])*Tsq + (F[9][6]*D[6][7] + F[9][7]*D[7][7] + F[9][8]*D[7][8] + F[7][6]*D[6][9] + F[7][8]*D[8][9] + F[7][9]*D[9][9] + F[7][10]*D[9][10] + F[9][10]*D[7][10] + F[7][11]*D[9][11] + F[9][11]*D[7][11] + F[7][12]*D[9][12] + F[9][12]*D[7][12])*T + D[7][9];
546  P[7][10] = P[10][7] = (F[7][6]*D[6][10] + F[7][8]*D[8][10] + F[7][9]*D[9][10] + F[7][10]*D[10][10] + F[7][11]*D[10][11] + F[7][12]*D[10][12])*T + D[7][10];
547  P[7][11] = P[11][7] = (F[7][6]*D[6][11] + F[7][8]*D[8][11] + F[7][9]*D[9][11] + F[7][10]*D[10][11] + F[7][11]*D[11][11] + F[7][12]*D[11][12])*T + D[7][11];
548  P[7][12] = P[12][7] = (F[7][6]*D[6][12] + F[7][8]*D[8][12] + F[7][9]*D[9][12] + F[7][10]*D[10][12] + F[7][11]*D[11][12] + F[7][12]*D[12][12])*T + D[7][12];
549  P[7][13] = P[13][7] = (F[7][6]*D[6][13] + F[7][8]*D[8][13] + F[7][9]*D[9][13] + F[7][10]*D[10][13] + F[7][11]*D[11][13] + F[7][12]*D[12][13])*T + D[7][13];
550  P[7][14] = P[14][7] = (F[7][6]*D[6][14] + F[7][8]*D[8][14] + F[7][9]*D[9][14] + F[7][10]*D[10][14] + F[7][11]*D[11][14] + F[7][12]*D[12][14])*T + D[7][14];
551  P[7][15] = P[15][7] = (F[7][6]*D[6][15] + F[7][8]*D[8][15] + F[7][9]*D[9][15] + F[7][10]*D[10][15] + F[7][11]*D[11][15] + F[7][12]*D[12][15])*T + D[7][15];
552  P[8][8] = (Q[0]*G[8][0]*G[8][0] + Q[1]*G[8][1]*G[8][1] + Q[2]*G[8][2]*G[8][2] + F[8][6]*(F[8][6]*D[6][6] + F[8][7]*D[6][7] + F[8][9]*D[6][9] + F[8][10]*D[6][10] + F[8][11]*D[6][11] + F[8][12]*D[6][12]) + F[8][7]*(F[8][6]*D[6][7] + F[8][7]*D[7][7] + F[8][9]*D[7][9] + F[8][10]*D[7][10] + F[8][11]*D[7][11] + F[8][12]*D[7][12]) + F[8][9]*(F[8][6]*D[6][9] + F[8][7]*D[7][9] + F[8][9]*D[9][9] + F[8][10]*D[9][10] + F[8][11]*D[9][11] + F[8][12]*D[9][12]) + F[8][10]*(F[8][6]*D[6][10] + F[8][7]*D[7][10] + F[8][9]*D[9][10] + F[8][10]*D[10][10] + F[8][11]*D[10][11] + F[8][12]*D[10][12]) + F[8][11]*(F[8][6]*D[6][11] + F[8][7]*D[7][11] + F[8][9]*D[9][11] + F[8][10]*D[10][11] + F[8][11]*D[11][11] + F[8][12]*D[11][12]) + F[8][12]*(F[8][6]*D[6][12] + F[8][7]*D[7][12] + F[8][9]*D[9][12] + F[8][10]*D[10][12] + F[8][11]*D[11][12] + F[8][12]*D[12][12]))*Tsq + (2*F[8][6]*D[6][8] + 2*F[8][7]*D[7][8] + 2*F[8][9]*D[8][9] + 2*F[8][10]*D[8][10] + 2*F[8][11]*D[8][11] + 2*F[8][12]*D[8][12])*T + D[8][8];
553  P[8][9] = P[9][8] = (F[9][6]*(F[8][6]*D[6][6] + F[8][7]*D[6][7] + F[8][9]*D[6][9] + F[8][10]*D[6][10] + F[8][11]*D[6][11] + F[8][12]*D[6][12]) + F[9][7]*(F[8][6]*D[6][7] + F[8][7]*D[7][7] + F[8][9]*D[7][9] + F[8][10]*D[7][10] + F[8][11]*D[7][11] + F[8][12]*D[7][12]) + F[9][8]*(F[8][6]*D[6][8] + F[8][7]*D[7][8] + F[8][9]*D[8][9] + F[8][10]*D[8][10] + F[8][11]*D[8][11] + F[8][12]*D[8][12]) + F[9][10]*(F[8][6]*D[6][10] + F[8][7]*D[7][10] + F[8][9]*D[9][10] + F[8][10]*D[10][10] + F[8][11]*D[10][11] + F[8][12]*D[10][12]) + F[9][11]*(F[8][6]*D[6][11] + F[8][7]*D[7][11] + F[8][9]*D[9][11] + F[8][10]*D[10][11] + F[8][11]*D[11][11] + F[8][12]*D[11][12]) + F[9][12]*(F[8][6]*D[6][12] + F[8][7]*D[7][12] + F[8][9]*D[9][12] + F[8][10]*D[10][12] + F[8][11]*D[11][12] + F[8][12]*D[12][12]) + G[8][0]*G[9][0]*Q[0] + G[8][1]*G[9][1]*Q[1] + G[8][2]*G[9][2]*Q[2])*Tsq + (F[9][6]*D[6][8] + F[9][7]*D[7][8] + F[9][8]*D[8][8] + F[8][6]*D[6][9] + F[8][7]*D[7][9] + F[8][9]*D[9][9] + F[8][10]*D[9][10] + F[9][10]*D[8][10] + F[8][11]*D[9][11] + F[9][11]*D[8][11] + F[8][12]*D[9][12] + F[9][12]*D[8][12])*T + D[8][9];
554  P[8][10] = P[10][8] = (F[8][6]*D[6][10] + F[8][7]*D[7][10] + F[8][9]*D[9][10] + F[8][10]*D[10][10] + F[8][11]*D[10][11] + F[8][12]*D[10][12])*T + D[8][10];
555  P[8][11] = P[11][8] = (F[8][6]*D[6][11] + F[8][7]*D[7][11] + F[8][9]*D[9][11] + F[8][10]*D[10][11] + F[8][11]*D[11][11] + F[8][12]*D[11][12])*T + D[8][11];
556  P[8][12] = P[12][8] = (F[8][6]*D[6][12] + F[8][7]*D[7][12] + F[8][9]*D[9][12] + F[8][10]*D[10][12] + F[8][11]*D[11][12] + F[8][12]*D[12][12])*T + D[8][12];
557  P[8][13] = P[13][8] = (F[8][6]*D[6][13] + F[8][7]*D[7][13] + F[8][9]*D[9][13] + F[8][10]*D[10][13] + F[8][11]*D[11][13] + F[8][12]*D[12][13])*T + D[8][13];
558  P[8][14] = P[14][8] = (F[8][6]*D[6][14] + F[8][7]*D[7][14] + F[8][9]*D[9][14] + F[8][10]*D[10][14] + F[8][11]*D[11][14] + F[8][12]*D[12][14])*T + D[8][14];
559  P[8][15] = P[15][8] = (F[8][6]*D[6][15] + F[8][7]*D[7][15] + F[8][9]*D[9][15] + F[8][10]*D[10][15] + F[8][11]*D[11][15] + F[8][12]*D[12][15])*T + D[8][15];
560  P[9][9] = (Q[0]*G[9][0]*G[9][0] + Q[1]*G[9][1]*G[9][1] + Q[2]*G[9][2]*G[9][2] + F[9][6]*(F[9][6]*D[6][6] + F[9][7]*D[6][7] + F[9][8]*D[6][8] + F[9][10]*D[6][10] + F[9][11]*D[6][11] + F[9][12]*D[6][12]) + F[9][7]*(F[9][6]*D[6][7] + F[9][7]*D[7][7] + F[9][8]*D[7][8] + F[9][10]*D[7][10] + F[9][11]*D[7][11] + F[9][12]*D[7][12]) + F[9][8]*(F[9][6]*D[6][8] + F[9][7]*D[7][8] + F[9][8]*D[8][8] + F[9][10]*D[8][10] + F[9][11]*D[8][11] + F[9][12]*D[8][12]) + F[9][10]*(F[9][6]*D[6][10] + F[9][7]*D[7][10] + F[9][8]*D[8][10] + F[9][10]*D[10][10] + F[9][11]*D[10][11] + F[9][12]*D[10][12]) + F[9][11]*(F[9][6]*D[6][11] + F[9][7]*D[7][11] + F[9][8]*D[8][11] + F[9][10]*D[10][11] + F[9][11]*D[11][11] + F[9][12]*D[11][12]) + F[9][12]*(F[9][6]*D[6][12] + F[9][7]*D[7][12] + F[9][8]*D[8][12] + F[9][10]*D[10][12] + F[9][11]*D[11][12] + F[9][12]*D[12][12]))*Tsq + (2*F[9][6]*D[6][9] + 2*F[9][7]*D[7][9] + 2*F[9][8]*D[8][9] + 2*F[9][10]*D[9][10] + 2*F[9][11]*D[9][11] + 2*F[9][12]*D[9][12])*T + D[9][9];
561  P[9][10] = P[10][9] = (F[9][6]*D[6][10] + F[9][7]*D[7][10] + F[9][8]*D[8][10] + F[9][10]*D[10][10] + F[9][11]*D[10][11] + F[9][12]*D[10][12])*T + D[9][10];
562  P[9][11] = P[11][9] = (F[9][6]*D[6][11] + F[9][7]*D[7][11] + F[9][8]*D[8][11] + F[9][10]*D[10][11] + F[9][11]*D[11][11] + F[9][12]*D[11][12])*T + D[9][11];
563  P[9][12] = P[12][9] = (F[9][6]*D[6][12] + F[9][7]*D[7][12] + F[9][8]*D[8][12] + F[9][10]*D[10][12] + F[9][11]*D[11][12] + F[9][12]*D[12][12])*T + D[9][12];
564  P[9][13] = P[13][9] = (F[9][6]*D[6][13] + F[9][7]*D[7][13] + F[9][8]*D[8][13] + F[9][10]*D[10][13] + F[9][11]*D[11][13] + F[9][12]*D[12][13])*T + D[9][13];
565  P[9][14] = P[14][9] = (F[9][6]*D[6][14] + F[9][7]*D[7][14] + F[9][8]*D[8][14] + F[9][10]*D[10][14] + F[9][11]*D[11][14] + F[9][12]*D[12][14])*T + D[9][14];
566  P[9][15] = P[15][9] = (F[9][6]*D[6][15] + F[9][7]*D[7][15] + F[9][8]*D[8][15] + F[9][10]*D[10][15] + F[9][11]*D[11][15] + F[9][12]*D[12][15])*T + D[9][15];
567  P[10][10] = Q[6]*Tsq + D[10][10];
568  P[10][11] = P[11][10] = D[10][11];
569  P[10][12] = P[12][10] = D[10][12];
570  P[10][13] = P[13][10] = D[10][13];
571  P[10][14] = P[14][10] = D[10][14];
572  P[10][15] = P[15][10] = D[10][15];
573  P[11][11] = Q[7]*Tsq + D[11][11];
574  P[11][12] = P[12][11] = D[11][12];
575  P[11][13] = P[13][11] = D[11][13];
576  P[11][14] = P[14][11] = D[11][14];
577  P[11][15] = P[15][11] = D[11][15];
578  P[12][12] = Q[8]*Tsq + D[12][12];
579  P[12][13] = P[13][12] = D[12][13];
580  P[12][14] = P[14][12] = D[12][14];
581  P[12][15] = P[15][12] = D[12][15];
582  P[13][13] = Q[9]*Tsq + D[13][13];
583  P[13][14] = P[14][13] = D[13][14];
584  P[13][15] = P[15][13] = D[13][15];
585  P[14][14] = Q[10]*Tsq + D[14][14];
586  P[14][15] = P[15][14] = D[14][15];
587  P[15][15] = Q[11]*Tsq + D[15][15];
588 
589 
590 }
591 #endif
592 
593 // ************* SerialUpdate *******************
594 // Does the update step of the Kalman filter for the covariance and estimate
595 // Outputs are Xnew & Pnew, and are written over P and X
596 // Z is actual measurement, Y is predicted measurement
597 // Xnew = X + K*(Z-Y), Pnew=(I-K*H)*P,
598 // where K=P*H'*inv[H*P*H'+R]
599 // NOTE the algorithm assumes R (measurement covariance matrix) is diagonal
600 // i.e. the measurment noises are uncorrelated.
601 // It therefore uses a serial update that requires no matrix inversion by
602 // processing the measurements one at a time.
603 // Algorithm - see Grewal and Andrews, "Kalman Filtering,2nd Ed" p.121 & p.253
604 // - or see Simon, "Optimal State Estimation," 1st Ed, p.150
605 // The SensorsUsed variable is a bitwise mask indicating which sensors
606 // should be used in the update.
607 // ************************************************
608 
609 void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
610  float Y[NUMV], float P[NUMX][NUMX], float X[NUMX],
611  uint16_t SensorsUsed)
612 {
613  float HP[NUMX], HPHR, Error;
614  uint8_t i, j, k, m;
615 
616  // Iterate through all the possible measurements and apply the
617  // appropriate corrections
618  for (m = 0; m < NUMV; m++) {
619 
620  if (SensorsUsed & (0x01 << m)) { // use this sensor for update
621 
622  for (j = 0; j < NUMX; j++) { // Find Hp = H*P
623  HP[j] = 0.0f;
624  for (k = 0; k < NUMX; k++)
625  HP[j] += H[m][k] * P[k][j];
626  }
627  HPHR = R[m]; // Find HPHR = H*P*H' + R
628  for (k = 0; k < NUMX; k++)
629  HPHR += HP[k] * H[m][k];
630 
631  for (k = 0; k < NUMX; k++)
632  K[k][m] = HP[k] / HPHR; // find K = HP/HPHR
633 
634  for (i = 0; i < NUMX; i++) { // Find P(m)= P(m-1) + K*HP
635  for (j = i; j < NUMX; j++)
636  P[i][j] = P[j][i] =
637  P[i][j] - K[i][m] * HP[j];
638  }
639 
640  Error = Z[m] - Y[m];
641  for (i = 0; i < NUMX; i++) // Find X(m)= X(m-1) + K*Error
642  X[i] = X[i] + K[i][m] * Error;
643 
644  }
645  }
646 }
647 
648 // ************* RungeKutta **********************
649 // Does a 4th order Runge Kutta numerical integration step
650 // Output, Xnew, is written over X
651 // NOTE the algorithm assumes time invariant state equations and
652 // constant inputs over integration step
653 // ************************************************
654 
655 void RungeKutta(float X[NUMX], float U[NUMU], float dT)
656 {
657 
658  float dT2 =
659  dT / 2.0f, K1[NUMX], K2[NUMX], K3[NUMX], K4[NUMX], Xlast[NUMX];
660  uint8_t i;
661 
662  for (i = 0; i < NUMX; i++)
663  Xlast[i] = X[i]; // make a working copy
664 
665  StateEq(X, U, K1); // k1 = f(x,u)
666  for (i = 0; i < NUMX; i++)
667  X[i] = Xlast[i] + dT2 * K1[i];
668  StateEq(X, U, K2); // k2 = f(x+0.5*dT*k1,u)
669  for (i = 0; i < NUMX; i++)
670  X[i] = Xlast[i] + dT2 * K2[i];
671  StateEq(X, U, K3); // k3 = f(x+0.5*dT*k2,u)
672  for (i = 0; i < NUMX; i++)
673  X[i] = Xlast[i] + dT * K3[i];
674  StateEq(X, U, K4); // k4 = f(x+dT*k3,u)
675 
676  // Xnew = X + dT*(k1+2*k2+2*k3+k4)/6
677  for (i = 0; i < NUMX; i++)
678  X[i] =
679  Xlast[i] + dT * (K1[i] + 2.0f * K2[i] + 2.0f * K3[i] +
680  K4[i]) / 6.0f;
681 }
682 
683 // ************* Model Specific Stuff ***************************
684 // *** StateEq, MeasurementEq, LinerizeFG, and LinearizeH ********
685 //
686 // State Variables = [Pos Vel Quaternion GyroBias AccelBias]
687 // Deterministic Inputs = [AngularVel Accel]
688 // Disturbance Noise = [GyroNoise AccelNoise GyroRandomWalkNoise AccelRandomWalkNoise]
689 //
690 // Measurement Variables = [Pos Vel BodyFrameMagField Altimeter]
691 // Inputs to Measurement = [EarthFrameMagField]
692 //
693 // Notes: Pos and Vel in earth frame
694 // AngularVel and Accel in body frame
695 // MagFields are unit vectors
696 // Xdot is output of StateEq()
697 // F and G are outputs of LinearizeFG(), all elements not set should be zero
698 // y is output of OutputEq()
699 // H is output of LinearizeH(), all elements not set should be zero
700 // ************************************************
701 
702 void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX])
703 {
704  float ax, ay, az, wx, wy, wz, q0, q1, q2, q3;
705 
706  ax = U[3] - X[13];
707  ay = U[4] - X[14];
708  az = U[5] - X[15]; // subtract the biases on accels
709  wx = U[0] - X[10];
710  wy = U[1] - X[11];
711  wz = U[2] - X[12]; // subtract the biases on gyros
712  q0 = X[6];
713  q1 = X[7];
714  q2 = X[8];
715  q3 = X[9];
716 
717  // Pdot = V
718  Xdot[0] = X[3];
719  Xdot[1] = X[4];
720  Xdot[2] = X[5];
721 
722  // Vdot = Reb*a
723  Xdot[3] =
724  (q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * ax + 2.0f * (q1 * q2 -
725  q0 * q3) *
726  ay + 2.0f * (q1 * q3 + q0 * q2) * az;
727  Xdot[4] =
728  2.0f * (q1 * q2 + q0 * q3) * ax + (q0 * q0 - q1 * q1 + q2 * q2 -
729  q3 * q3) * ay + 2.0f * (q2 * q3 -
730  q0 * q1) *
731  az;
732  Xdot[5] =
733  2.0f * (q1 * q3 - q0 * q2) * ax + 2.0f * (q2 * q3 + q0 * q1) * ay +
734  (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * az + GRAVITY;
735 
736  // qdot = Q*w
737  Xdot[6] = (-q1 * wx - q2 * wy - q3 * wz) / 2.0f;
738  Xdot[7] = (q0 * wx - q3 * wy + q2 * wz) / 2.0f;
739  Xdot[8] = (q3 * wx + q0 * wy - q1 * wz) / 2.0f;
740  Xdot[9] = (-q2 * wx + q1 * wy + q0 * wz) / 2.0f;
741 
742  // best guess is that bias stays constant
743  Xdot[10] = Xdot[11] = Xdot[12] = 0;
744 
745  // For accels to make sure things stay stable, assume bias always walks weakly
746  // towards zero for the horizontal axis. This prevents drifting around an
747  // unobservable manifold of possible attitudes and gyro biases. The z-axis
748  // we assume no drift becaues this is teh one we want to estimate most accurately.
749  float const accel_zero_rate = -1.0f;
750  Xdot[13] = accel_zero_rate * X[13];
751  Xdot[14] = accel_zero_rate * X[14];
752  Xdot[15] = 0.0f;
753 }
754 
769 void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX],
770  float G[NUMX][NUMW])
771 {
772  float ax, ay, az, wx, wy, wz, q0, q1, q2, q3;
773 
774  wx = U[0] - X[10];
775  wy = U[1] - X[11];
776  wz = U[2] - X[12]; // subtract the biases on gyros
777  ax = U[3] - X[13];
778  ay = U[4] - X[14];
779  az = U[5] - X[15]; // subtract the biases on accels
780  q0 = X[6];
781  q1 = X[7];
782  q2 = X[8];
783  q3 = X[9];
784 
785  // Pdot = V
786  F[0][3] = F[1][4] = F[2][5] = 1.0f;
787 
788  // dVdot/dq
789  F[3][6] = 2.0f * (q0 * ax - q3 * ay + q2 * az);
790  F[3][7] = 2.0f * (q1 * ax + q2 * ay + q3 * az);
791  F[3][8] = 2.0f * (-q2 * ax + q1 * ay + q0 * az);
792  F[3][9] = 2.0f * (-q3 * ax - q0 * ay + q1 * az);
793  F[4][6] = 2.0f * (q3 * ax + q0 * ay - q1 * az);
794  F[4][7] = 2.0f * (q2 * ax - q1 * ay - q0 * az);
795  F[4][8] = 2.0f * (q1 * ax + q2 * ay + q3 * az);
796  F[4][9] = 2.0f * (q0 * ax - q3 * ay + q2 * az);
797  F[5][6] = 2.0f * (-q2 * ax + q1 * ay + q0 * az);
798  F[5][7] = 2.0f * (q3 * ax + q0 * ay - q1 * az);
799  F[5][8] = 2.0f * (-q0 * ax + q3 * ay - q2 * az);
800  F[5][9] = 2.0f * (q1 * ax + q2 * ay + q3 * az);
801 
802  // dVdot/dabias & dVdot/dna - the equations for how the accel input and accel bias influence velocity are the same
803  F[3][13]=G[3][3]=-q0*q0-q1*q1+q2*q2+q3*q3; F[3][14]=G[3][4]=2.0f*(-q1*q2+q0*q3); F[3][15]=G[3][5]=-2.0f*(q1*q3+q0*q2);
804  F[4][13]=G[4][3]=-2.0f*(q1*q2+q0*q3); F[4][14]=G[4][4]=-q0*q0+q1*q1-q2*q2+q3*q3; F[4][15]=G[4][5]=2.0f*(-q2*q3+q0*q1);
805  F[5][13]=G[5][3]=2.0f*(-q1*q3+q0*q2); F[5][14]=G[5][4]=-2.0f*(q2*q3+q0*q1); F[5][15]=G[5][5]=-q0*q0+q1*q1+q2*q2-q3*q3;
806 
807  // dqdot/dq
808  F[6][6] = 0;
809  F[6][7] = -wx / 2.0f;
810  F[6][8] = -wy / 2.0f;
811  F[6][9] = -wz / 2.0f;
812  F[7][6] = wx / 2.0f;
813  F[7][7] = 0;
814  F[7][8] = wz / 2.0f;
815  F[7][9] = -wy / 2.0f;
816  F[8][6] = wy / 2.0f;
817  F[8][7] = -wz / 2.0f;
818  F[8][8] = 0;
819  F[8][9] = wx / 2.0f;
820  F[9][6] = wz / 2.0f;
821  F[9][7] = wy / 2.0f;
822  F[9][8] = -wx / 2.0f;
823  F[9][9] = 0;
824 
825  // dqdot/dwbias
826  F[6][10] = q1 / 2.0f;
827  F[6][11] = q2 / 2.0f;
828  F[6][12] = q3 / 2.0f;
829  F[7][10] = -q0 / 2.0f;
830  F[7][11] = q3 / 2.0f;
831  F[7][12] = -q2 / 2.0f;
832  F[8][10] = -q3 / 2.0f;
833  F[8][11] = -q0 / 2.0f;
834  F[8][12] = q1 / 2.0f;
835  F[9][10] = q2 / 2.0f;
836  F[9][11] = -q1 / 2.0f;
837  F[9][12] = -q0 / 2.0f;
838 
839  // dVdot/dna - WITH BIAS STATES ON ACCELS - THIS DONE ABOVE
840  //G[3][3]=-q0*q0-q1*q1+q2*q2+q3*q3; G[3][4]=2*(-q1*q2+q0*q3); G[3][5]=-2*(q1*q3+q0*q2);
841  //G[4][3]=-2*(q1*q2+q0*q3); G[4][4]=-q0*q0+q1*q1-q2*q2+q3*q3; G[4][5]=2*(-q2*q3+q0*q1);
842  //G[5][3]=2*(-q1*q3+q0*q2); G[5][4]=-2*(q2*q3+q0*q1); G[5][5]=-q0*q0+q1*q1+q2*q2-q3*q3;
843 
844  // dqdot/dnw
845  G[6][0] = q1 / 2.0f;
846  G[6][1] = q2 / 2.0f;
847  G[6][2] = q3 / 2.0f;
848  G[7][0] = -q0 / 2.0f;
849  G[7][1] = q3 / 2.0f;
850  G[7][2] = -q2 / 2.0f;
851  G[8][0] = -q3 / 2.0f;
852  G[8][1] = -q0 / 2.0f;
853  G[8][2] = q1 / 2.0f;
854  G[9][0] = q2 / 2.0f;
855  G[9][1] = -q1 / 2.0f;
856  G[9][2] = -q0 / 2.0f;
857 }
858 
865 void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV])
866 {
867  float q0, q1, q2, q3;
868 
869  q0 = X[6];
870  q1 = X[7];
871  q2 = X[8];
872  q3 = X[9];
873 
874  // first six outputs are P and V
875  Y[0] = X[0];
876  Y[1] = X[1];
877  Y[2] = X[2];
878  Y[3] = X[3];
879  Y[4] = X[4];
880  Y[5] = X[5];
881 
882  // Rotate Be by only the yaw heading
883  float r = sqrtf( powf(2*q0*q3 + 2*q1*q2, 2) + powf(q0*q0 + q1*q1 - q2*q2 - q3*q3, 2) );
884  float cP = (q0*q0 + q1*q1 - q2*q2 - q3*q3) / r;
885  float sP = (2*q0*q3 + 2*q1*q2) / r;
886  Y[6] = Be[0] * cP + Be[1] * sP;
887  Y[7] = -Be[0] * sP + Be[1] * cP;
888  Y[8] = 0; // don't care
889 
890  // Alt = -Pz
891  Y[9] = X[2] * -1.0f;
892 }
893 
899 void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX])
900 {
901  float q0, q1, q2, q3;
902 
903  q0 = X[6];
904  q1 = X[7];
905  q2 = X[8];
906  q3 = X[9];
907 
908  // dP/dP=I; (expect position to measure the position)
909  H[0][0] = H[1][1] = H[2][2] = 1.0f;
910  // dV/dV=I; (expect velocity to measure the velocity)
911  H[3][3] = H[4][4] = H[5][5] = 1.0f;
912 
913  // dBb/dq (expected magnetometer readings in the horizontal plane)
914  // these equations were generated by Rhb(q)*Be which is the matrix that
915  // rotates the earth magnetic field into the horizontal plane, and then
916  // taking the partial derivative wrt each term in q. Maniuplated in
917  // matlab symbolic toolbox
918  float Be_0 = Be[0];
919  float Be_1 = Be[1];
920  float k1 = 1.0f/sqrtf(powf(q0*q3*2.0f+q1*q2*2.0f,2.0f)+powf(q0*q0+q1*q1-q2*q2-q3*q3,2.0f));
921  float k3 = 1.0f/powf(powf(q0*q3*2.0f+q1*q2*2.0f,2.0f)+powf(q0*q0+q1*q1-q2*q2-q3*q3,2.0f),3.0f/2.0f)*(q0*q0+q1*q1-q2*q2-q3*q3)*(1.0f/2.0f);
922  float k4 = (q0*q0+q1*q1-q2*q2-q3*q3)*4.0f;
923  float k5 = (q0*q3*2.0f+q1*q2*2.0f)*4.0f;
924  float k6 = 1.0f/powf(powf(q0*q3*2.0f+q1*q2*2.0f,2.0f)+powf(q0*q0+q1*q1-q2*q2-q3*q3,2.0f),3.0f/2.0f)*(q0*q3*2.0f+q1*q2*2.0f)*(1.0f/2.0f);
925 
926  H[6][6] = Be_0*q0*k1*2.0f + Be_1*q3*k1*2.0f - Be_0*(q0*k4+q3*k5)*k3 - Be_1*(q0*k4+q3*k5)*k6;
927  H[6][7] = Be_0*q1*k1*2.0f + Be_1*q2*k1*2.0f - Be_0*(q1*k4+q2*k5)*k3 - Be_1*(q1*k4+q2*k5)*k6;
928  H[6][8] = Be_0*q2*k1*-2.0f + Be_1*q1*k1*2.0f + Be_0*(q2*k4-q1*k5)*k3 + Be_1*(q2*k4-q1*k5)*k6;
929  H[6][9] = Be_1*q0*k1*2.0f - Be_0*q3*k1*2.0f + Be_0*(q3*k4-q0*k5)*k3 + Be_1*(q3*k4-q0*k5)*k6;
930  H[7][6] = Be_1*q0*k1*2.0f - Be_0*q3*k1*2.0f - Be_1*(q0*k4+q3*k5)*k3 + Be_0*(q0*k4+q3*k5)*k6;
931  H[7][7] = Be_0*q2*k1*-2.0f + Be_1*q1*k1*2.0f - Be_1*(q1*k4+q2*k5)*k3 + Be_0*(q1*k4+q2*k5)*k6;
932  H[7][8] = Be_0*q1*k1*-2.0f - Be_1*q2*k1*2.0f + Be_1*(q2*k4-q1*k5)*k3 - Be_0*(q2*k4-q1*k5)*k6;
933  H[7][9] = Be_0*q0*k1*-2.0f - Be_1*q3*k1*2.0f + Be_1*(q3*k4-q0*k5)*k3 - Be_0*(q3*k4-q0*k5)*k6;
934  H[8][6] = 0.0f;
935  H[8][7] = 0.0f;
936  H[8][9] = 0.0f;
937 
938  // dAlt/dPz = -1 (expected baro readings)
939  H[9][2] = -1.0f;
940 }
941 
INSGetState
void INSGetState(float *pos, float *vel, float *attitude, float *gyro_bias, float *accel_bias)
Get the current state estimate.
Definition: insgps13state.c:144
LinearizeH
void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX])
Definition: insgps14state.c:879
F
float F[NUMX][NUMX]
Definition: insgps16state.c:63
H
float H[NUMV][NUMX]
Definition: insgps16state.c:63
Q
float Q[NUMW]
Definition: insgps16state.c:67
insgps.h
Include file of the INSGPS exposed functionality.
INSSetAccelBias
void INSSetAccelBias(const float gyro_bias[3])
Definition: insgps13state.c:257
R
float R[NUMV]
Definition: insgps16state.c:67
StateEq
void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX])
Definition: insgps14state.c:689
INSGetVariance
void INSGetVariance(float *p)
Definition: insgps13state.c:182
j
volatile int j
Definition: loadabletest.c:12
P
float P[NUMX][NUMX]
Definition: insgps16state.c:66
INSSetMagVar
void INSSetMagVar(const float scaled_mag_var[3])
Definition: insgps13state.c:276
K2
#define K2
Definition: lqg.c:117
INSSetPosVelVar
void INSSetPosVelVar(float PosVar, float VelVar, float VertPosVar)
Definition: insgps13state.c:240
NUMX
#define NUMX
Definition: insgps16state.c:38
INSSetGyroBias
void INSSetGyroBias(const float gyro_bias[3])
Definition: insgps13state.c:250
NUMW
#define NUMW
Definition: insgps16state.c:39
T
static float T[3]
Scales used in NED transform (local tangent plane approx).
Definition: attitude.c:210
accel_bias
static float accel_bias[3]
Definition: sensors.c:114
CovariancePrediction
void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW], float Q[NUMW], float dT, float P[NUMX][NUMX])
Definition: insgps14state.c:453
INSStatePrediction
void INSStatePrediction(const float gyro_data[3], const float accel_data[3], float dT)
Compute an update of the state estimate.
Definition: insgps13state.c:295
INSSetMagNorth
void INSSetMagNorth(const float B[3])
Definition: insgps13state.c:288
pos
struct _msp_pid_item pos
Definition: msp_messages.h:100
INSSetBaroVar
void INSSetBaroVar(float baro_var)
Definition: insgps13state.c:283
NUMU
#define NUMU
Definition: insgps16state.c:41
i
uint8_t i
Definition: msp_messages.h:97
SerialUpdate
void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV], float Y[NUMV], float P[NUMX][NUMX], float X[NUMX], uint16_t SensorsUsed)
Definition: insgps14state.c:594
LinearizeFG
void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX], float G[NUMX][NUMW])
Definition: insgps14state.c:751
INSSetState
void INSSetState(const float pos[3], const float vel[3], const float q[4], const float gyro_bias[3], const float accel_bias[3])
Definition: insgps13state.c:202
NUMV
#define NUMV
Definition: insgps16state.c:40
INSGPSInit
void INSGPSInit()
Reset the internal state variables and variances.
Definition: insgps13state.c:74
MAG_SENSORS
#define MAG_SENSORS
Definition: insgps.h:44
Be
float Be[3]
Definition: insgps16state.c:65
INSCorrection
void INSCorrection(const float mag_data[3], const float Pos[3], const float Vel[3], float BaroAlt, uint16_t SensorsUsed)
Correct the state and covariance estimate based on the sensors that were updated. ...
Definition: insgps13state.c:325
INSSetArmed
void INSSetArmed(bool armed)
Set the current flight state.
Definition: insgps13state.c:126
MeasurementEq
void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV])
Definition: insgps14state.c:845
INSResetP
void INSResetP(const float *PDiag)
Definition: insgps13state.c:188
K1
#define K1
Definition: lqg.c:116
INSCovariancePrediction
void INSCovariancePrediction(float dT)
Compute an update of the state covariance.
Definition: insgps13state.c:320
px_mkfw.f
tuple f
Definition: px_mkfw.py:81
ins_get_num_states
uint16_t ins_get_num_states()
Definition: insgps13state.c:69
INSSetGyroVar
void INSSetGyroVar(const float gyro_var[3])
Definition: insgps13state.c:269
INSSetAccelVar
void INSSetAccelVar(const float accel_var[3])
Definition: insgps13state.c:262
X
float X[NUMX]
Definition: insgps16state.c:66
K
float K[NUMX][NUMV]
Definition: insgps16state.c:68
INSPosVelReset
void INSPosVelReset(const float pos[3], const float vel[3])
Definition: insgps13state.c:220
G
float G[NUMX][NUMW]
Definition: insgps16state.c:63
RungeKutta
void RungeKutta(float X[NUMX], float U[NUMU], float dT)
Definition: insgps14state.c:642
vel
struct _msp_pid_item vel
Definition: msp_messages.h:105