?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
LP-106 Setup Wizard refresh : Add dual servo setup (dual aileron or
[librepilot.git] / matlab / ins / insgps.c
blob37296113c7a8e8da55c328a64b4538efce08a9b9
1 /**
2 ******************************************************************************
3 * @addtogroup AHRS
4 * @{
5 * @addtogroup INSGPS
6 * @{
7 * @brief INSGPS is a joint attitude and position estimation EKF
8 *
9 * @file insgps.c
10 * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
11 * @brief An INS/GPS algorithm implemented with an EKF.
12 *
13 * @see The GNU Public License (GPL) Version 3
14 *
15 *****************************************************************************/
16 /*
17 * This program is free software; you can redistribute it and/or modify
18 * it under the terms of the GNU General Public License as published by
19 * the Free Software Foundation; either version 3 of the License, or
20 * (at your option) any later version.
21 *
22 * This program is distributed in the hope that it will be useful, but
23 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
24 * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
25 * for more details.
26 *
27 * You should have received a copy of the GNU General Public License along
28 * with this program; if not, write to the Free Software Foundation, Inc.,
29 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
30 */
31
32 #include "insgps.h"
33 #include "mex.h"
34 #include <math.h>
35 #include <stdint.h>
36
37 // constants/macros/typdefs
38 #define NUMX 13 // number of states, X is the state vector
39 #define NUMW 9 // 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 // Nav structure containing current solution
44 struct NavStruct {
45 float Pos[3]; // Position in meters and relative to a local NED frame
46 float Vel[3]; // Velocity in meters and in NED
47 float q[4]; // unit quaternion rotation relative to NED
48 } Nav;
49
50 #if defined(GENERAL_COV)
51 // This might trick people so I have a note here. There is a slower but bigger version of the
52 // code here but won't fit when debugging disabled (requires -Os)
53 #define COVARIANCE_PREDICTION_GENERAL
54 #endif
55
56 // Private functions
57 void INSCorrection(float mag_data[3], float Pos[3], float Vel[3],
58 float BaroAlt, uint16_t SensorsUsed);
59 void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
60 float Q[NUMW], float dT, float P[NUMX][NUMX]);
61 void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
62 float Y[NUMV], float P[NUMX][NUMX], float X[NUMX],
63 uint16_t SensorsUsed);
64 void RungeKutta(float X[NUMX], float U[NUMU], float dT);
65 void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX]);
66 void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX],
67 float G[NUMX][NUMW]);
68 void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV]);
69 void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX]);
70
71 // Private variables
72 float F[NUMX][NUMX], G[NUMX][NUMW], H[NUMV][NUMX]; // linearized system matrices
73 // global to init to zero and maintain zero elements
74 float Be[3]; // local magnetic unit vector in NED frame
75 float P[NUMX][NUMX], X[NUMX]; // covariance matrix and state vector
76 float Q[NUMW], R[NUMV]; // input noise and measurement noise variances
77 float K[NUMX][NUMV]; // feedback gain matrix
78
79 // ************* Exposed Functions ****************
80 // *************************************************
81 void INSGPSInit() //pretty much just a place holder for now
82 {
83 uint8_t i,j;
84
85 Be[0] = 1;
86 Be[1] = 0;
87 Be[2] = 0; // local magnetic unit vector
88
89 for (i = 0; i < NUMX; i++) {
90 for (j = 0; j < NUMX; j++) {
91 P[i][j] = 0; // zero all terms
92 }
93 }
94
95 P[0][0] = P[1][1] = P[2][2] = 25; // initial position variance (m^2)
96 P[3][3] = P[4][4] = P[5][5] = 5; // initial velocity variance (m/s)^2
97 P[6][6] = P[7][7] = P[8][8] = P[9][9] = 1e-5; // initial quaternion variance
98 P[10][10] = P[11][11] = P[12][12] = 1e-5; // initial gyro bias variance (rad/s)^2
99
100 X[0] = X[1] = X[2] = X[3] = X[4] = X[5] = 0; // initial pos and vel (m)
101 X[6] = 1;
102 X[7] = X[8] = X[9] = 0; // initial quaternion (level and North) (m/s)
103 X[10] = X[11] = X[12] = 0; // initial gyro bias (rad/s)
104
105 Q[0] = Q[1] = Q[2] = 50e-8; // gyro noise variance (rad/s)^2
106 Q[3] = Q[4] = Q[5] = 0.01; // accelerometer noise variance (m/s^2)^2
107 Q[6] = Q[7] = Q[8] = 2e-6; // gyro bias random walk variance (rad/s^2)^2
108
109 R[0] = R[1] = 0.004; // High freq GPS horizontal position noise variance (m^2)
110 R[2] = 0.036; // High freq GPS vertical position noise variance (m^2)
111 R[3] = R[4] = 0.004; // High freq GPS horizontal velocity noise variance (m/s)^2
112 R[5] = 100; // High freq GPS vertical velocity noise variance (m/s)^2
113 R[6] = R[7] = R[8] = 0.005; // magnetometer unit vector noise variance
114 R[9] = .05; // High freq altimeter noise variance (m^2)
115 }
116
117 void INSPosVelReset(float pos[3], float vel[3])
118 {
119 uint8_t i, j;
120
121 for (i = 0; i < 6; i++) {
122 for(j = i; j < NUMX; j++) {
123 P[i][j] = 0; // zero the first 6 rows and columns
124 P[j][i] = 0;
125 }
126 }
127
128 P[0][0] = P[1][1] = P[2][2] = 25; // initial position variance (m^2)
129 P[3][3] = P[4][4] = P[5][5] = 5; // initial velocity variance (m/s)^2
130
131 X[0] = pos[0];
132 X[1] = pos[1];
133 X[2] = pos[2];
134 X[3] = vel[0];
135 X[4] = vel[1];
136 X[5] = vel[2];
137 }
138
139 void INSSetPosVelVar(float PosVar)
140 {
141 R[0] = PosVar;
142 R[1] = PosVar;
143 R[2] = PosVar;
144 R[3] = PosVar;
145 R[4] = PosVar;
146 // R[5] = PosVar; // Don't change vertical velocity, not measured
147 }
148
149 void INSSetGyroBias(float gyro_bias[3])
150 {
151 X[10] = gyro_bias[0];
152 X[11] = gyro_bias[1];
153 X[12] = gyro_bias[2];
154 }
155
156 void INSSetAccelVar(float accel_var[3])
157 {
158 Q[3] = accel_var[0];
159 Q[4] = accel_var[1];
160 Q[5] = accel_var[2];
161 }
162
163 void INSSetGyroVar(float gyro_var[3])
164 {
165 Q[0] = gyro_var[0];
166 Q[1] = gyro_var[1];
167 Q[2] = gyro_var[2];
168 }
169
170 void INSSetMagVar(float scaled_mag_var[3])
171 {
172 R[6] = scaled_mag_var[0];
173 R[7] = scaled_mag_var[1];
174 R[8] = scaled_mag_var[2];
175 }
176
177 void INSSetMagNorth(float B[3])
178 {
179 Be[0] = B[0];
180 Be[1] = B[1];
181 Be[2] = B[2];
182 }
183
184 void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT)
185 {
186 float U[6];
187 float qmag;
188
189 // rate gyro inputs in units of rad/s
190 U[0] = gyro_data[0];
191 U[1] = gyro_data[1];
192 U[2] = gyro_data[2];
193
194 // accelerometer inputs in units of m/s
195 U[3] = accel_data[0];
196 U[4] = accel_data[1];
197 U[5] = accel_data[2];
198
199 // EKF prediction step
200 LinearizeFG(X, U, F, G);
201 RungeKutta(X, U, dT);
202 qmag = sqrt(X[6] * X[6] + X[7] * X[7] + X[8] * X[8] + X[9] * X[9]);
203 X[6] /= qmag;
204 X[7] /= qmag;
205 X[8] /= qmag;
206 X[9] /= qmag;
207 //CovariancePrediction(F,G,Q,dT,P);
208
209 // Update Nav solution structure
210 Nav.Pos[0] = X[0];
211 Nav.Pos[1] = X[1];
212 Nav.Pos[2] = X[2];
213 Nav.Vel[0] = X[3];
214 Nav.Vel[1] = X[4];
215 Nav.Vel[2] = X[5];
216 Nav.q[0] = X[6];
217 Nav.q[1] = X[7];
218 Nav.q[2] = X[8];
219 Nav.q[3] = X[9];
220 }
221
222 void INSCovariancePrediction(float dT)
223 {
224 CovariancePrediction(F, G, Q, dT, P);
225 }
226
227 float zeros[3] = { 0, 0, 0 };
228
229 void MagCorrection(float mag_data[3])
230 {
231 INSCorrection(mag_data, zeros, zeros, zeros[0], MAG_SENSORS);
232 }
233
234 void BaroCorrection(float baro)
235 {
236 INSCorrection(zeros, zeros, zeros, baro, BARO_SENSOR);
237 }
238 void GpsCorrection(float Pos[3], float Vel[3])
239 {
240 INSCorrection(zeros, Pos, Vel, zeros[0],
241 POS_SENSORS); // | HORIZ_SENSORS | VERT_SENSORS);
242 }
243
244
245 void MagVelBaroCorrection(float mag_data[3], float Vel[3], float BaroAlt)
246 {
247 INSCorrection(mag_data, zeros, Vel, BaroAlt,
248 MAG_SENSORS | HORIZ_SENSORS | VERT_SENSORS |
249 BARO_SENSOR);
250 }
251
252 void GpsBaroCorrection(float Pos[3], float Vel[3], float BaroAlt)
253 {
254 INSCorrection(zeros, Pos, Vel, BaroAlt,
255 POS_SENSORS | HORIZ_SENSORS | VERT_SENSORS | BARO_SENSOR);
256 }
257
258 void FullCorrection(float mag_data[3], float Pos[3], float Vel[3],
259 float BaroAlt)
260 {
261 INSCorrection(mag_data, Pos, Vel, BaroAlt, FULL_SENSORS);
262 }
263
264 void GpsMagCorrection(float mag_data[3], float Pos[3], float Vel[3])
265 {
266 INSCorrection(mag_data, Pos, Vel, zeros[0],
267 POS_SENSORS | HORIZ_SENSORS | MAG_SENSORS);
268 }
269
270 void VelBaroCorrection(float Vel[3], float BaroAlt)
271 {
272 INSCorrection(zeros, zeros, Vel, BaroAlt,
273 HORIZ_SENSORS | VERT_SENSORS | BARO_SENSOR);
274 }
275
276 void INSCorrection(float mag_data[3], float Pos[3], float Vel[3],
277 float BaroAlt, uint16_t SensorsUsed)
278 {
279 float Z[10], Y[10];
280 float Bmag, qmag;
281
282 // GPS Position in meters and in local NED frame
283 Z[0] = Pos[0];
284 Z[1] = Pos[1];
285 Z[2] = Pos[2];
286
287 // GPS Velocity in meters and in local NED frame
288 Z[3] = Vel[0];
289 Z[4] = Vel[1];
290 Z[5] = Vel[2];
291
292 // magnetometer data in any units (use unit vector) and in body frame
293 Bmag =
294 sqrt(mag_data[0] * mag_data[0] + mag_data[1] * mag_data[1] +
295 mag_data[2] * mag_data[2]);
296 Z[6] = mag_data[0] / Bmag;
297 Z[7] = mag_data[1] / Bmag;
298 Z[8] = mag_data[2] / Bmag;
299
300
301 // barometric altimeter in meters and in local NED frame
302 Z[9] = BaroAlt;
303
304 // EKF correction step
305 LinearizeH(X, Be, H);
306 MeasurementEq(X, Be, Y);
307 SerialUpdate(H, R, Z, Y, P, X, SensorsUsed);
308 qmag = sqrt(X[6] * X[6] + X[7] * X[7] + X[8] * X[8] + X[9] * X[9]);
309 X[6] /= qmag;
310 X[7] /= qmag;
311 X[8] /= qmag;
312 X[9] /= qmag;
313
314 // Update Nav solution structure
315 Nav.Pos[0] = X[0];
316 Nav.Pos[1] = X[1];
317 Nav.Pos[2] = X[2];
318 Nav.Vel[0] = X[3];
319 Nav.Vel[1] = X[4];
320 Nav.Vel[2] = X[5];
321 Nav.q[0] = X[6];
322 Nav.q[1] = X[7];
323 Nav.q[2] = X[8];
324 Nav.q[3] = X[9];
325 }
326
327 // ************* CovariancePrediction *************
328 // Does the prediction step of the Kalman filter for the covariance matrix
329 // Output, Pnew, overwrites P, the input covariance
330 // Pnew = (I+F*T)*P*(I+F*T)' + T^2*G*Q*G'
331 // Q is the discrete time covariance of process noise
332 // Q is vector of the diagonal for a square matrix with
333 // dimensions equal to the number of disturbance noise variables
334 // The General Method is very inefficient,not taking advantage of the sparse F and G
335 // The first Method is very specific to this implementation
336 // ************************************************
337
338 #ifdef COVARIANCE_PREDICTION_GENERAL
339
340 void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
341 float Q[NUMW], float dT, float P[NUMX][NUMX])
342 {
343 float Dummy[NUMX][NUMX], dTsq;
344 uint8_t i, j, k;
345
346 // 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')]
347
348 dTsq = dT * dT;
349
350 for (i = 0; i < NUMX; i++) // Calculate Dummy = (P/T +F*P)
351 for (j = 0; j < NUMX; j++) {
352 Dummy[i][j] = P[i][j] / dT;
353 for (k = 0; k < NUMX; k++)
354 Dummy[i][j] += F[i][k] * P[k][j];
355 }
356 for (i = 0; i < NUMX; i++) // Calculate Pnew = Dummy/T + Dummy*F' + G*Qw*G'
357 for (j = i; j < NUMX; j++) { // Use symmetry, ie only find upper triangular
358 P[i][j] = Dummy[i][j] / dT;
359 for (k = 0; k < NUMX; k++)
360 P[i][j] += Dummy[i][k] * F[j][k]; // P = Dummy/T + Dummy*F'
361 for (k = 0; k < NUMW; k++)
362 P[i][j] += Q[k] * G[i][k] * G[j][k]; // P = Dummy/T + Dummy*F' + G*Q*G'
363 P[j][i] = P[i][j] = P[i][j] * dTsq; // Pnew = T^2*P and fill in lower triangular;
364 }
365 }
366
367 #else
368
369 void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
370 float Q[NUMW], float dT, float P[NUMX][NUMX])
371 {
372 float D[NUMX][NUMX], T, Tsq;
373 uint8_t i, j;
374
375 // Pnew = (I+F*T)*P*(I+F*T)' + T^2*G*Q*G' = scalar expansion from symbolic manipulator
376
377 T = dT;
378 Tsq = dT * dT;
379
380 for (i = 0; i < NUMX; i++) // Create a copy of the upper triangular of P
381 for (j = i; j < NUMX; j++)
382 D[i][j] = P[i][j];
383
384 // Brute force calculation of the elements of P
385 P[0][0] = D[3][3] * Tsq + (2 * D[0][3]) * T + D[0][0];
386 P[0][1] = P[1][0] =
387 D[3][4] * Tsq + (D[0][4] + D[1][3]) * T + D[0][1];
388 P[0][2] = P[2][0] =
389 D[3][5] * Tsq + (D[0][5] + D[2][3]) * T + D[0][2];
390 P[0][3] = P[3][0] =
391 (F[3][6] * D[3][6] + F[3][7] * D[3][7] + F[3][8] * D[3][8] +
392 F[3][9] * D[3][9]) * Tsq + (D[3][3] + F[3][6] * D[0][6] +
393 F[3][7] * D[0][7] +
394 F[3][8] * D[0][8] +
395 F[3][9] * D[0][9]) * T + D[0][3];
396 P[0][4] = P[4][0] =
397 (F[4][6] * D[3][6] + F[4][7] * D[3][7] + F[4][8] * D[3][8] +
398 F[4][9] * D[3][9]) * Tsq + (D[3][4] + F[4][6] * D[0][6] +
399 F[4][7] * D[0][7] +
400 F[4][8] * D[0][8] +
401 F[4][9] * D[0][9]) * T + D[0][4];
402 P[0][5] = P[5][0] =
403 (F[5][6] * D[3][6] + F[5][7] * D[3][7] + F[5][8] * D[3][8] +
404 F[5][9] * D[3][9]) * Tsq + (D[3][5] + F[5][6] * D[0][6] +
405 F[5][7] * D[0][7] +
406 F[5][8] * D[0][8] +
407 F[5][9] * D[0][9]) * T + D[0][5];
408 P[0][6] = P[6][0] =
409 (F[6][7] * D[3][7] + F[6][8] * D[3][8] + F[6][9] * D[3][9] +
410 F[6][10] * D[3][10] + F[6][11] * D[3][11] +
411 F[6][12] * D[3][12]) * Tsq + (D[3][6] + F[6][7] * D[0][7] +
412 F[6][8] * D[0][8] +
413 F[6][9] * D[0][9] +
414 F[6][10] * D[0][10] +
415 F[6][11] * D[0][11] +
416 F[6][12] * D[0][12]) * T +
417 D[0][6];
418 P[0][7] = P[7][0] =
419 (F[7][6] * D[3][6] + F[7][8] * D[3][8] + F[7][9] * D[3][9] +
420 F[7][10] * D[3][10] + F[7][11] * D[3][11] +
421 F[7][12] * D[3][12]) * Tsq + (D[3][7] + F[7][6] * D[0][6] +
422 F[7][8] * D[0][8] +
423 F[7][9] * D[0][9] +
424 F[7][10] * D[0][10] +
425 F[7][11] * D[0][11] +
426 F[7][12] * D[0][12]) * T +
427 D[0][7];
428 P[0][8] = P[8][0] =
429 (F[8][6] * D[3][6] + F[8][7] * D[3][7] + F[8][9] * D[3][9] +
430 F[8][10] * D[3][10] + F[8][11] * D[3][11] +
431 F[8][12] * D[3][12]) * Tsq + (D[3][8] + F[8][6] * D[0][6] +
432 F[8][7] * D[0][7] +
433 F[8][9] * D[0][9] +
434 F[8][10] * D[0][10] +
435 F[8][11] * D[0][11] +
436 F[8][12] * D[0][12]) * T +
437 D[0][8];
438 P[0][9] = P[9][0] =
439 (F[9][6] * D[3][6] + F[9][7] * D[3][7] + F[9][8] * D[3][8] +
440 F[9][10] * D[3][10] + F[9][11] * D[3][11] +
441 F[9][12] * D[3][12]) * Tsq + (D[3][9] + F[9][6] * D[0][6] +
442 F[9][7] * D[0][7] +
443 F[9][8] * D[0][8] +
444 F[9][10] * D[0][10] +
445 F[9][11] * D[0][11] +
446 F[9][12] * D[0][12]) * T +
447 D[0][9];
448 P[0][10] = P[10][0] = D[3][10] * T + D[0][10];
449 P[0][11] = P[11][0] = D[3][11] * T + D[0][11];
450 P[0][12] = P[12][0] = D[3][12] * T + D[0][12];
451 P[1][1] = D[4][4] * Tsq + (2 * D[1][4]) * T + D[1][1];
452 P[1][2] = P[2][1] =
453 D[4][5] * Tsq + (D[1][5] + D[2][4]) * T + D[1][2];
454 P[1][3] = P[3][1] =
455 (F[3][6] * D[4][6] + F[3][7] * D[4][7] + F[3][8] * D[4][8] +
456 F[3][9] * D[4][9]) * Tsq + (D[3][4] + F[3][6] * D[1][6] +
457 F[3][7] * D[1][7] +
458 F[3][8] * D[1][8] +
459 F[3][9] * D[1][9]) * T + D[1][3];
460 P[1][4] = P[4][1] =
461 (F[4][6] * D[4][6] + F[4][7] * D[4][7] + F[4][8] * D[4][8] +
462 F[4][9] * D[4][9]) * Tsq + (D[4][4] + F[4][6] * D[1][6] +
463 F[4][7] * D[1][7] +
464 F[4][8] * D[1][8] +
465 F[4][9] * D[1][9]) * T + D[1][4];
466 P[1][5] = P[5][1] =
467 (F[5][6] * D[4][6] + F[5][7] * D[4][7] + F[5][8] * D[4][8] +
468 F[5][9] * D[4][9]) * Tsq + (D[4][5] + F[5][6] * D[1][6] +
469 F[5][7] * D[1][7] +
470 F[5][8] * D[1][8] +
471 F[5][9] * D[1][9]) * T + D[1][5];
472 P[1][6] = P[6][1] =
473 (F[6][7] * D[4][7] + F[6][8] * D[4][8] + F[6][9] * D[4][9] +
474 F[6][10] * D[4][10] + F[6][11] * D[4][11] +
475 F[6][12] * D[4][12]) * Tsq + (D[4][6] + F[6][7] * D[1][7] +
476 F[6][8] * D[1][8] +
477 F[6][9] * D[1][9] +
478 F[6][10] * D[1][10] +
479 F[6][11] * D[1][11] +
480 F[6][12] * D[1][12]) * T +
481 D[1][6];
482 P[1][7] = P[7][1] =
483 (F[7][6] * D[4][6] + F[7][8] * D[4][8] + F[7][9] * D[4][9] +
484 F[7][10] * D[4][10] + F[7][11] * D[4][11] +
485 F[7][12] * D[4][12]) * Tsq + (D[4][7] + F[7][6] * D[1][6] +
486 F[7][8] * D[1][8] +
487 F[7][9] * D[1][9] +
488 F[7][10] * D[1][10] +
489 F[7][11] * D[1][11] +
490 F[7][12] * D[1][12]) * T +
491 D[1][7];
492 P[1][8] = P[8][1] =
493 (F[8][6] * D[4][6] + F[8][7] * D[4][7] + F[8][9] * D[4][9] +
494 F[8][10] * D[4][10] + F[8][11] * D[4][11] +
495 F[8][12] * D[4][12]) * Tsq + (D[4][8] + F[8][6] * D[1][6] +
496 F[8][7] * D[1][7] +
497 F[8][9] * D[1][9] +
498 F[8][10] * D[1][10] +
499 F[8][11] * D[1][11] +
500 F[8][12] * D[1][12]) * T +
501 D[1][8];
502 P[1][9] = P[9][1] =
503 (F[9][6] * D[4][6] + F[9][7] * D[4][7] + F[9][8] * D[4][8] +
504 F[9][10] * D[4][10] + F[9][11] * D[4][11] +
505 F[9][12] * D[4][12]) * Tsq + (D[4][9] + F[9][6] * D[1][6] +
506 F[9][7] * D[1][7] +
507 F[9][8] * D[1][8] +
508 F[9][10] * D[1][10] +
509 F[9][11] * D[1][11] +
510 F[9][12] * D[1][12]) * T +
511 D[1][9];
512 P[1][10] = P[10][1] = D[4][10] * T + D[1][10];
513 P[1][11] = P[11][1] = D[4][11] * T + D[1][11];
514 P[1][12] = P[12][1] = D[4][12] * T + D[1][12];
515 P[2][2] = D[5][5] * Tsq + (2 * D[2][5]) * T + D[2][2];
516 P[2][3] = P[3][2] =
517 (F[3][6] * D[5][6] + F[3][7] * D[5][7] + F[3][8] * D[5][8] +
518 F[3][9] * D[5][9]) * Tsq + (D[3][5] + F[3][6] * D[2][6] +
519 F[3][7] * D[2][7] +
520 F[3][8] * D[2][8] +
521 F[3][9] * D[2][9]) * T + D[2][3];
522 P[2][4] = P[4][2] =
523 (F[4][6] * D[5][6] + F[4][7] * D[5][7] + F[4][8] * D[5][8] +
524 F[4][9] * D[5][9]) * Tsq + (D[4][5] + F[4][6] * D[2][6] +
525 F[4][7] * D[2][7] +
526 F[4][8] * D[2][8] +
527 F[4][9] * D[2][9]) * T + D[2][4];
528 P[2][5] = P[5][2] =
529 (F[5][6] * D[5][6] + F[5][7] * D[5][7] + F[5][8] * D[5][8] +
530 F[5][9] * D[5][9]) * Tsq + (D[5][5] + F[5][6] * D[2][6] +
531 F[5][7] * D[2][7] +
532 F[5][8] * D[2][8] +
533 F[5][9] * D[2][9]) * T + D[2][5];
534 P[2][6] = P[6][2] =
535 (F[6][7] * D[5][7] + F[6][8] * D[5][8] + F[6][9] * D[5][9] +
536 F[6][10] * D[5][10] + F[6][11] * D[5][11] +
537 F[6][12] * D[5][12]) * Tsq + (D[5][6] + F[6][7] * D[2][7] +
538 F[6][8] * D[2][8] +
539 F[6][9] * D[2][9] +
540 F[6][10] * D[2][10] +
541 F[6][11] * D[2][11] +
542 F[6][12] * D[2][12]) * T +
543 D[2][6];
544 P[2][7] = P[7][2] =
545 (F[7][6] * D[5][6] + F[7][8] * D[5][8] + F[7][9] * D[5][9] +
546 F[7][10] * D[5][10] + F[7][11] * D[5][11] +
547 F[7][12] * D[5][12]) * Tsq + (D[5][7] + F[7][6] * D[2][6] +
548 F[7][8] * D[2][8] +
549 F[7][9] * D[2][9] +
550 F[7][10] * D[2][10] +
551 F[7][11] * D[2][11] +
552 F[7][12] * D[2][12]) * T +
553 D[2][7];
554 P[2][8] = P[8][2] =
555 (F[8][6] * D[5][6] + F[8][7] * D[5][7] + F[8][9] * D[5][9] +
556 F[8][10] * D[5][10] + F[8][11] * D[5][11] +
557 F[8][12] * D[5][12]) * Tsq + (D[5][8] + F[8][6] * D[2][6] +
558 F[8][7] * D[2][7] +
559 F[8][9] * D[2][9] +
560 F[8][10] * D[2][10] +
561 F[8][11] * D[2][11] +
562 F[8][12] * D[2][12]) * T +
563 D[2][8];
564 P[2][9] = P[9][2] =
565 (F[9][6] * D[5][6] + F[9][7] * D[5][7] + F[9][8] * D[5][8] +
566 F[9][10] * D[5][10] + F[9][11] * D[5][11] +
567 F[9][12] * D[5][12]) * Tsq + (D[5][9] + F[9][6] * D[2][6] +
568 F[9][7] * D[2][7] +
569 F[9][8] * D[2][8] +
570 F[9][10] * D[2][10] +
571 F[9][11] * D[2][11] +
572 F[9][12] * D[2][12]) * T +
573 D[2][9];
574 P[2][10] = P[10][2] = D[5][10] * T + D[2][10];
575 P[2][11] = P[11][2] = D[5][11] * T + D[2][11];
576 P[2][12] = P[12][2] = D[5][12] * T + D[2][12];
577 P[3][3] =
578 (Q[3] * G[3][3] * G[3][3] + Q[4] * G[3][4] * G[3][4] +
579 Q[5] * G[3][5] * G[3][5] + F[3][9] * (F[3][9] * D[9][9] +
580 F[3][6] * D[6][9] +
581 F[3][7] * D[7][9] +
582 F[3][8] * D[8][9]) +
583 F[3][6] * (F[3][6] * D[6][6] + F[3][7] * D[6][7] +
584 F[3][8] * D[6][8] + F[3][9] * D[6][9]) +
585 F[3][7] * (F[3][6] * D[6][7] + F[3][7] * D[7][7] +
586 F[3][8] * D[7][8] + F[3][9] * D[7][9]) +
587 F[3][8] * (F[3][6] * D[6][8] + F[3][7] * D[7][8] +
588 F[3][8] * D[8][8] + F[3][9] * D[8][9])) * Tsq +
589 (2 * F[3][6] * D[3][6] + 2 * F[3][7] * D[3][7] +
590 2 * F[3][8] * D[3][8] + 2 * F[3][9] * D[3][9]) * T + D[3][3];
591 P[3][4] = P[4][3] =
592 (F[4][9] *
593 (F[3][9] * D[9][9] + F[3][6] * D[6][9] + F[3][7] * D[7][9] +
594 F[3][8] * D[8][9]) + F[4][6] * (F[3][6] * D[6][6] +
595 F[3][7] * D[6][7] +
596 F[3][8] * D[6][8] +
597 F[3][9] * D[6][9]) +
598 F[4][7] * (F[3][6] * D[6][7] + F[3][7] * D[7][7] +
599 F[3][8] * D[7][8] + F[3][9] * D[7][9]) +
600 F[4][8] * (F[3][6] * D[6][8] + F[3][7] * D[7][8] +
601 F[3][8] * D[8][8] + F[3][9] * D[8][9]) +
602 G[3][3] * G[4][3] * Q[3] + G[3][4] * G[4][4] * Q[4] +
603 G[3][5] * G[4][5] * Q[5]) * Tsq + (F[3][6] * D[4][6] +
604 F[4][6] * D[3][6] +
605 F[3][7] * D[4][7] +
606 F[4][7] * D[3][7] +
607 F[3][8] * D[4][8] +
608 F[4][8] * D[3][8] +
609 F[3][9] * D[4][9] +
610 F[4][9] * D[3][9]) * T +
611 D[3][4];
612 P[3][5] = P[5][3] =
613 (F[5][9] *
614 (F[3][9] * D[9][9] + F[3][6] * D[6][9] + F[3][7] * D[7][9] +
615 F[3][8] * D[8][9]) + F[5][6] * (F[3][6] * D[6][6] +
616 F[3][7] * D[6][7] +
617 F[3][8] * D[6][8] +
618 F[3][9] * D[6][9]) +
619 F[5][7] * (F[3][6] * D[6][7] + F[3][7] * D[7][7] +
620 F[3][8] * D[7][8] + F[3][9] * D[7][9]) +
621 F[5][8] * (F[3][6] * D[6][8] + F[3][7] * D[7][8] +
622 F[3][8] * D[8][8] + F[3][9] * D[8][9]) +
623 G[3][3] * G[5][3] * Q[3] + G[3][4] * G[5][4] * Q[4] +
624 G[3][5] * G[5][5] * Q[5]) * Tsq + (F[3][6] * D[5][6] +
625 F[5][6] * D[3][6] +
626 F[3][7] * D[5][7] +
627 F[5][7] * D[3][7] +
628 F[3][8] * D[5][8] +
629 F[5][8] * D[3][8] +
630 F[3][9] * D[5][9] +
631 F[5][9] * D[3][9]) * T +
632 D[3][5];
633 P[3][6] = P[6][3] =
634 (F[6][9] *
635 (F[3][9] * D[9][9] + F[3][6] * D[6][9] + F[3][7] * D[7][9] +
636 F[3][8] * D[8][9]) + F[6][10] * (F[3][9] * D[9][10] +
637 F[3][6] * D[6][10] +
638 F[3][7] * D[7][10] +
639 F[3][8] * D[8][10]) +
640 F[6][11] * (F[3][9] * D[9][11] + F[3][6] * D[6][11] +
641 F[3][7] * D[7][11] + F[3][8] * D[8][11]) +
642 F[6][12] * (F[3][9] * D[9][12] + F[3][6] * D[6][12] +
643 F[3][7] * D[7][12] + F[3][8] * D[8][12]) +
644 F[6][7] * (F[3][6] * D[6][7] + F[3][7] * D[7][7] +
645 F[3][8] * D[7][8] + F[3][9] * D[7][9]) +
646 F[6][8] * (F[3][6] * D[6][8] + F[3][7] * D[7][8] +
647 F[3][8] * D[8][8] + F[3][9] * D[8][9])) * Tsq +
648 (F[3][6] * D[6][6] + F[3][7] * D[6][7] + F[6][7] * D[3][7] +
649 F[3][8] * D[6][8] + F[6][8] * D[3][8] + F[3][9] * D[6][9] +
650 F[6][9] * D[3][9] + F[6][10] * D[3][10] +
651 F[6][11] * D[3][11] + F[6][12] * D[3][12]) * T + D[3][6];
652 P[3][7] = P[7][3] =
653 (F[7][9] *
654 (F[3][9] * D[9][9] + F[3][6] * D[6][9] + F[3][7] * D[7][9] +
655 F[3][8] * D[8][9]) + F[7][10] * (F[3][9] * D[9][10] +
656 F[3][6] * D[6][10] +
657 F[3][7] * D[7][10] +
658 F[3][8] * D[8][10]) +
659 F[7][11] * (F[3][9] * D[9][11] + F[3][6] * D[6][11] +
660 F[3][7] * D[7][11] + F[3][8] * D[8][11]) +
661 F[7][12] * (F[3][9] * D[9][12] + F[3][6] * D[6][12] +
662 F[3][7] * D[7][12] + F[3][8] * D[8][12]) +
663 F[7][6] * (F[3][6] * D[6][6] + F[3][7] * D[6][7] +
664 F[3][8] * D[6][8] + F[3][9] * D[6][9]) +
665 F[7][8] * (F[3][6] * D[6][8] + F[3][7] * D[7][8] +
666 F[3][8] * D[8][8] + F[3][9] * D[8][9])) * Tsq +
667 (F[3][6] * D[6][7] + F[7][6] * D[3][6] + F[3][7] * D[7][7] +
668 F[3][8] * D[7][8] + F[7][8] * D[3][8] + F[3][9] * D[7][9] +
669 F[7][9] * D[3][9] + F[7][10] * D[3][10] +
670 F[7][11] * D[3][11] + F[7][12] * D[3][12]) * T + D[3][7];
671 P[3][8] = P[8][3] =
672 (F[8][9] *
673 (F[3][9] * D[9][9] + F[3][6] * D[6][9] + F[3][7] * D[7][9] +
674 F[3][8] * D[8][9]) + F[8][10] * (F[3][9] * D[9][10] +
675 F[3][6] * D[6][10] +
676 F[3][7] * D[7][10] +
677 F[3][8] * D[8][10]) +
678 F[8][11] * (F[3][9] * D[9][11] + F[3][6] * D[6][11] +
679 F[3][7] * D[7][11] + F[3][8] * D[8][11]) +
680 F[8][12] * (F[3][9] * D[9][12] + F[3][6] * D[6][12] +
681 F[3][7] * D[7][12] + F[3][8] * D[8][12]) +
682 F[8][6] * (F[3][6] * D[6][6] + F[3][7] * D[6][7] +
683 F[3][8] * D[6][8] + F[3][9] * D[6][9]) +
684 F[8][7] * (F[3][6] * D[6][7] + F[3][7] * D[7][7] +
685 F[3][8] * D[7][8] + F[3][9] * D[7][9])) * Tsq +
686 (F[3][6] * D[6][8] + F[3][7] * D[7][8] + F[8][6] * D[3][6] +
687 F[8][7] * D[3][7] + F[3][8] * D[8][8] + F[3][9] * D[8][9] +
688 F[8][9] * D[3][9] + F[8][10] * D[3][10] +
689 F[8][11] * D[3][11] + F[8][12] * D[3][12]) * T + D[3][8];
690 P[3][9] = P[9][3] =
691 (F[9][10] *
692 (F[3][9] * D[9][10] + F[3][6] * D[6][10] +
693 F[3][7] * D[7][10] + F[3][8] * D[8][10]) +
694 F[9][11] * (F[3][9] * D[9][11] + F[3][6] * D[6][11] +
695 F[3][7] * D[7][11] + F[3][8] * D[8][11]) +
696 F[9][12] * (F[3][9] * D[9][12] + F[3][6] * D[6][12] +
697 F[3][7] * D[7][12] + F[3][8] * D[8][12]) +
698 F[9][6] * (F[3][6] * D[6][6] + F[3][7] * D[6][7] +
699 F[3][8] * D[6][8] + F[3][9] * D[6][9]) +
700 F[9][7] * (F[3][6] * D[6][7] + F[3][7] * D[7][7] +
701 F[3][8] * D[7][8] + F[3][9] * D[7][9]) +
702 F[9][8] * (F[3][6] * D[6][8] + F[3][7] * D[7][8] +
703 F[3][8] * D[8][8] + F[3][9] * D[8][9])) * Tsq +
704 (F[9][6] * D[3][6] + F[9][7] * D[3][7] + F[9][8] * D[3][8] +
705 F[3][9] * D[9][9] + F[9][10] * D[3][10] +
706 F[9][11] * D[3][11] + F[9][12] * D[3][12] +
707 F[3][6] * D[6][9] + F[3][7] * D[7][9] +
708 F[3][8] * D[8][9]) * T + D[3][9];
709 P[3][10] = P[10][3] =
710 (F[3][9] * D[9][10] + F[3][6] * D[6][10] + F[3][7] * D[7][10] +
711 F[3][8] * D[8][10]) * T + D[3][10];
712 P[3][11] = P[11][3] =
713 (F[3][9] * D[9][11] + F[3][6] * D[6][11] + F[3][7] * D[7][11] +
714 F[3][8] * D[8][11]) * T + D[3][11];
715 P[3][12] = P[12][3] =
716 (F[3][9] * D[9][12] + F[3][6] * D[6][12] + F[3][7] * D[7][12] +
717 F[3][8] * D[8][12]) * T + D[3][12];
718 P[4][4] =
719 (Q[3] * G[4][3] * G[4][3] + Q[4] * G[4][4] * G[4][4] +
720 Q[5] * G[4][5] * G[4][5] + F[4][9] * (F[4][9] * D[9][9] +
721 F[4][6] * D[6][9] +
722 F[4][7] * D[7][9] +
723 F[4][8] * D[8][9]) +
724 F[4][6] * (F[4][6] * D[6][6] + F[4][7] * D[6][7] +
725 F[4][8] * D[6][8] + F[4][9] * D[6][9]) +
726 F[4][7] * (F[4][6] * D[6][7] + F[4][7] * D[7][7] +
727 F[4][8] * D[7][8] + F[4][9] * D[7][9]) +
728 F[4][8] * (F[4][6] * D[6][8] + F[4][7] * D[7][8] +
729 F[4][8] * D[8][8] + F[4][9] * D[8][9])) * Tsq +
730 (2 * F[4][6] * D[4][6] + 2 * F[4][7] * D[4][7] +
731 2 * F[4][8] * D[4][8] + 2 * F[4][9] * D[4][9]) * T + D[4][4];
732 P[4][5] = P[5][4] =
733 (F[5][9] *
734 (F[4][9] * D[9][9] + F[4][6] * D[6][9] + F[4][7] * D[7][9] +
735 F[4][8] * D[8][9]) + F[5][6] * (F[4][6] * D[6][6] +
736 F[4][7] * D[6][7] +
737 F[4][8] * D[6][8] +
738 F[4][9] * D[6][9]) +
739 F[5][7] * (F[4][6] * D[6][7] + F[4][7] * D[7][7] +
740 F[4][8] * D[7][8] + F[4][9] * D[7][9]) +
741 F[5][8] * (F[4][6] * D[6][8] + F[4][7] * D[7][8] +
742 F[4][8] * D[8][8] + F[4][9] * D[8][9]) +
743 G[4][3] * G[5][3] * Q[3] + G[4][4] * G[5][4] * Q[4] +
744 G[4][5] * G[5][5] * Q[5]) * Tsq + (F[4][6] * D[5][6] +
745 F[5][6] * D[4][6] +
746 F[4][7] * D[5][7] +
747 F[5][7] * D[4][7] +
748 F[4][8] * D[5][8] +
749 F[5][8] * D[4][8] +
750 F[4][9] * D[5][9] +
751 F[5][9] * D[4][9]) * T +
752 D[4][5];
753 P[4][6] = P[6][4] =
754 (F[6][9] *
755 (F[4][9] * D[9][9] + F[4][6] * D[6][9] + F[4][7] * D[7][9] +
756 F[4][8] * D[8][9]) + F[6][10] * (F[4][9] * D[9][10] +
757 F[4][6] * D[6][10] +
758 F[4][7] * D[7][10] +
759 F[4][8] * D[8][10]) +
760 F[6][11] * (F[4][9] * D[9][11] + F[4][6] * D[6][11] +
761 F[4][7] * D[7][11] + F[4][8] * D[8][11]) +
762 F[6][12] * (F[4][9] * D[9][12] + F[4][6] * D[6][12] +
763 F[4][7] * D[7][12] + F[4][8] * D[8][12]) +
764 F[6][7] * (F[4][6] * D[6][7] + F[4][7] * D[7][7] +
765 F[4][8] * D[7][8] + F[4][9] * D[7][9]) +
766 F[6][8] * (F[4][6] * D[6][8] + F[4][7] * D[7][8] +
767 F[4][8] * D[8][8] + F[4][9] * D[8][9])) * Tsq +
768 (F[4][6] * D[6][6] + F[4][7] * D[6][7] + F[6][7] * D[4][7] +
769 F[4][8] * D[6][8] + F[6][8] * D[4][8] + F[4][9] * D[6][9] +
770 F[6][9] * D[4][9] + F[6][10] * D[4][10] +
771 F[6][11] * D[4][11] + F[6][12] * D[4][12]) * T + D[4][6];
772 P[4][7] = P[7][4] =
773 (F[7][9] *
774 (F[4][9] * D[9][9] + F[4][6] * D[6][9] + F[4][7] * D[7][9] +
775 F[4][8] * D[8][9]) + F[7][10] * (F[4][9] * D[9][10] +
776 F[4][6] * D[6][10] +
777 F[4][7] * D[7][10] +
778 F[4][8] * D[8][10]) +
779 F[7][11] * (F[4][9] * D[9][11] + F[4][6] * D[6][11] +
780 F[4][7] * D[7][11] + F[4][8] * D[8][11]) +
781 F[7][12] * (F[4][9] * D[9][12] + F[4][6] * D[6][12] +
782 F[4][7] * D[7][12] + F[4][8] * D[8][12]) +
783 F[7][6] * (F[4][6] * D[6][6] + F[4][7] * D[6][7] +
784 F[4][8] * D[6][8] + F[4][9] * D[6][9]) +
785 F[7][8] * (F[4][6] * D[6][8] + F[4][7] * D[7][8] +
786 F[4][8] * D[8][8] + F[4][9] * D[8][9])) * Tsq +
787 (F[4][6] * D[6][7] + F[7][6] * D[4][6] + F[4][7] * D[7][7] +
788 F[4][8] * D[7][8] + F[7][8] * D[4][8] + F[4][9] * D[7][9] +
789 F[7][9] * D[4][9] + F[7][10] * D[4][10] +
790 F[7][11] * D[4][11] + F[7][12] * D[4][12]) * T + D[4][7];
791 P[4][8] = P[8][4] =
792 (F[8][9] *
793 (F[4][9] * D[9][9] + F[4][6] * D[6][9] + F[4][7] * D[7][9] +
794 F[4][8] * D[8][9]) + F[8][10] * (F[4][9] * D[9][10] +
795 F[4][6] * D[6][10] +
796 F[4][7] * D[7][10] +
797 F[4][8] * D[8][10]) +
798 F[8][11] * (F[4][9] * D[9][11] + F[4][6] * D[6][11] +
799 F[4][7] * D[7][11] + F[4][8] * D[8][11]) +
800 F[8][12] * (F[4][9] * D[9][12] + F[4][6] * D[6][12] +
801 F[4][7] * D[7][12] + F[4][8] * D[8][12]) +
802 F[8][6] * (F[4][6] * D[6][6] + F[4][7] * D[6][7] +
803 F[4][8] * D[6][8] + F[4][9] * D[6][9]) +
804 F[8][7] * (F[4][6] * D[6][7] + F[4][7] * D[7][7] +
805 F[4][8] * D[7][8] + F[4][9] * D[7][9])) * Tsq +
806 (F[4][6] * D[6][8] + F[4][7] * D[7][8] + F[8][6] * D[4][6] +
807 F[8][7] * D[4][7] + F[4][8] * D[8][8] + F[4][9] * D[8][9] +
808 F[8][9] * D[4][9] + F[8][10] * D[4][10] +
809 F[8][11] * D[4][11] + F[8][12] * D[4][12]) * T + D[4][8];
810 P[4][9] = P[9][4] =
811 (F[9][10] *
812 (F[4][9] * D[9][10] + F[4][6] * D[6][10] +
813 F[4][7] * D[7][10] + F[4][8] * D[8][10]) +
814 F[9][11] * (F[4][9] * D[9][11] + F[4][6] * D[6][11] +
815 F[4][7] * D[7][11] + F[4][8] * D[8][11]) +
816 F[9][12] * (F[4][9] * D[9][12] + F[4][6] * D[6][12] +
817 F[4][7] * D[7][12] + F[4][8] * D[8][12]) +
818 F[9][6] * (F[4][6] * D[6][6] + F[4][7] * D[6][7] +
819 F[4][8] * D[6][8] + F[4][9] * D[6][9]) +
820 F[9][7] * (F[4][6] * D[6][7] + F[4][7] * D[7][7] +
821 F[4][8] * D[7][8] + F[4][9] * D[7][9]) +
822 F[9][8] * (F[4][6] * D[6][8] + F[4][7] * D[7][8] +
823 F[4][8] * D[8][8] + F[4][9] * D[8][9])) * Tsq +
824 (F[9][6] * D[4][6] + F[9][7] * D[4][7] + F[9][8] * D[4][8] +
825 F[4][9] * D[9][9] + F[9][10] * D[4][10] +
826 F[9][11] * D[4][11] + F[9][12] * D[4][12] +
827 F[4][6] * D[6][9] + F[4][7] * D[7][9] +
828 F[4][8] * D[8][9]) * T + D[4][9];
829 P[4][10] = P[10][4] =
830 (F[4][9] * D[9][10] + F[4][6] * D[6][10] + F[4][7] * D[7][10] +
831 F[4][8] * D[8][10]) * T + D[4][10];
832 P[4][11] = P[11][4] =
833 (F[4][9] * D[9][11] + F[4][6] * D[6][11] + F[4][7] * D[7][11] +
834 F[4][8] * D[8][11]) * T + D[4][11];
835 P[4][12] = P[12][4] =
836 (F[4][9] * D[9][12] + F[4][6] * D[6][12] + F[4][7] * D[7][12] +
837 F[4][8] * D[8][12]) * T + D[4][12];
838 P[5][5] =
839 (Q[3] * G[5][3] * G[5][3] + Q[4] * G[5][4] * G[5][4] +
840 Q[5] * G[5][5] * G[5][5] + F[5][9] * (F[5][9] * D[9][9] +
841 F[5][6] * D[6][9] +
842 F[5][7] * D[7][9] +
843 F[5][8] * D[8][9]) +
844 F[5][6] * (F[5][6] * D[6][6] + F[5][7] * D[6][7] +
845 F[5][8] * D[6][8] + F[5][9] * D[6][9]) +
846 F[5][7] * (F[5][6] * D[6][7] + F[5][7] * D[7][7] +
847 F[5][8] * D[7][8] + F[5][9] * D[7][9]) +
848 F[5][8] * (F[5][6] * D[6][8] + F[5][7] * D[7][8] +
849 F[5][8] * D[8][8] + F[5][9] * D[8][9])) * Tsq +
850 (2 * F[5][6] * D[5][6] + 2 * F[5][7] * D[5][7] +
851 2 * F[5][8] * D[5][8] + 2 * F[5][9] * D[5][9]) * T + D[5][5];
852 P[5][6] = P[6][5] =
853 (F[6][9] *
854 (F[5][9] * D[9][9] + F[5][6] * D[6][9] + F[5][7] * D[7][9] +
855 F[5][8] * D[8][9]) + F[6][10] * (F[5][9] * D[9][10] +
856 F[5][6] * D[6][10] +
857 F[5][7] * D[7][10] +
858 F[5][8] * D[8][10]) +
859 F[6][11] * (F[5][9] * D[9][11] + F[5][6] * D[6][11] +
860 F[5][7] * D[7][11] + F[5][8] * D[8][11]) +
861 F[6][12] * (F[5][9] * D[9][12] + F[5][6] * D[6][12] +
862 F[5][7] * D[7][12] + F[5][8] * D[8][12]) +
863 F[6][7] * (F[5][6] * D[6][7] + F[5][7] * D[7][7] +
864 F[5][8] * D[7][8] + F[5][9] * D[7][9]) +
865 F[6][8] * (F[5][6] * D[6][8] + F[5][7] * D[7][8] +
866 F[5][8] * D[8][8] + F[5][9] * D[8][9])) * Tsq +
867 (F[5][6] * D[6][6] + F[5][7] * D[6][7] + F[6][7] * D[5][7] +
868 F[5][8] * D[6][8] + F[6][8] * D[5][8] + F[5][9] * D[6][9] +
869 F[6][9] * D[5][9] + F[6][10] * D[5][10] +
870 F[6][11] * D[5][11] + F[6][12] * D[5][12]) * T + D[5][6];
871 P[5][7] = P[7][5] =
872 (F[7][9] *
873 (F[5][9] * D[9][9] + F[5][6] * D[6][9] + F[5][7] * D[7][9] +
874 F[5][8] * D[8][9]) + F[7][10] * (F[5][9] * D[9][10] +
875 F[5][6] * D[6][10] +
876 F[5][7] * D[7][10] +
877 F[5][8] * D[8][10]) +
878 F[7][11] * (F[5][9] * D[9][11] + F[5][6] * D[6][11] +
879 F[5][7] * D[7][11] + F[5][8] * D[8][11]) +
880 F[7][12] * (F[5][9] * D[9][12] + F[5][6] * D[6][12] +
881 F[5][7] * D[7][12] + F[5][8] * D[8][12]) +
882 F[7][6] * (F[5][6] * D[6][6] + F[5][7] * D[6][7] +
883 F[5][8] * D[6][8] + F[5][9] * D[6][9]) +
884 F[7][8] * (F[5][6] * D[6][8] + F[5][7] * D[7][8] +
885 F[5][8] * D[8][8] + F[5][9] * D[8][9])) * Tsq +
886 (F[5][6] * D[6][7] + F[7][6] * D[5][6] + F[5][7] * D[7][7] +
887 F[5][8] * D[7][8] + F[7][8] * D[5][8] + F[5][9] * D[7][9] +
888 F[7][9] * D[5][9] + F[7][10] * D[5][10] +
889 F[7][11] * D[5][11] + F[7][12] * D[5][12]) * T + D[5][7];
890 P[5][8] = P[8][5] =
891 (F[8][9] *
892 (F[5][9] * D[9][9] + F[5][6] * D[6][9] + F[5][7] * D[7][9] +
893 F[5][8] * D[8][9]) + F[8][10] * (F[5][9] * D[9][10] +
894 F[5][6] * D[6][10] +
895 F[5][7] * D[7][10] +
896 F[5][8] * D[8][10]) +
897 F[8][11] * (F[5][9] * D[9][11] + F[5][6] * D[6][11] +
898 F[5][7] * D[7][11] + F[5][8] * D[8][11]) +
899 F[8][12] * (F[5][9] * D[9][12] + F[5][6] * D[6][12] +
900 F[5][7] * D[7][12] + F[5][8] * D[8][12]) +
901 F[8][6] * (F[5][6] * D[6][6] + F[5][7] * D[6][7] +
902 F[5][8] * D[6][8] + F[5][9] * D[6][9]) +
903 F[8][7] * (F[5][6] * D[6][7] + F[5][7] * D[7][7] +
904 F[5][8] * D[7][8] + F[5][9] * D[7][9])) * Tsq +
905 (F[5][6] * D[6][8] + F[5][7] * D[7][8] + F[8][6] * D[5][6] +
906 F[8][7] * D[5][7] + F[5][8] * D[8][8] + F[5][9] * D[8][9] +
907 F[8][9] * D[5][9] + F[8][10] * D[5][10] +
908 F[8][11] * D[5][11] + F[8][12] * D[5][12]) * T + D[5][8];
909 P[5][9] = P[9][5] =
910 (F[9][10] *
911 (F[5][9] * D[9][10] + F[5][6] * D[6][10] +
912 F[5][7] * D[7][10] + F[5][8] * D[8][10]) +
913 F[9][11] * (F[5][9] * D[9][11] + F[5][6] * D[6][11] +
914 F[5][7] * D[7][11] + F[5][8] * D[8][11]) +
915 F[9][12] * (F[5][9] * D[9][12] + F[5][6] * D[6][12] +
916 F[5][7] * D[7][12] + F[5][8] * D[8][12]) +
917 F[9][6] * (F[5][6] * D[6][6] + F[5][7] * D[6][7] +
918 F[5][8] * D[6][8] + F[5][9] * D[6][9]) +
919 F[9][7] * (F[5][6] * D[6][7] + F[5][7] * D[7][7] +
920 F[5][8] * D[7][8] + F[5][9] * D[7][9]) +
921 F[9][8] * (F[5][6] * D[6][8] + F[5][7] * D[7][8] +
922 F[5][8] * D[8][8] + F[5][9] * D[8][9])) * Tsq +
923 (F[9][6] * D[5][6] + F[9][7] * D[5][7] + F[9][8] * D[5][8] +
924 F[5][9] * D[9][9] + F[9][10] * D[5][10] +
925 F[9][11] * D[5][11] + F[9][12] * D[5][12] +
926 F[5][6] * D[6][9] + F[5][7] * D[7][9] +
927 F[5][8] * D[8][9]) * T + D[5][9];
928 P[5][10] = P[10][5] =
929 (F[5][9] * D[9][10] + F[5][6] * D[6][10] + F[5][7] * D[7][10] +
930 F[5][8] * D[8][10]) * T + D[5][10];
931 P[5][11] = P[11][5] =
932 (F[5][9] * D[9][11] + F[5][6] * D[6][11] + F[5][7] * D[7][11] +
933 F[5][8] * D[8][11]) * T + D[5][11];
934 P[5][12] = P[12][5] =
935 (F[5][9] * D[9][12] + F[5][6] * D[6][12] + F[5][7] * D[7][12] +
936 F[5][8] * D[8][12]) * T + D[5][12];
937 P[6][6] =
938 (Q[0] * G[6][0] * G[6][0] + Q[1] * G[6][1] * G[6][1] +
939 Q[2] * G[6][2] * G[6][2] + F[6][9] * (F[6][9] * D[9][9] +
940 F[6][10] * D[9][10] +
941 F[6][11] * D[9][11] +
942 F[6][12] * D[9][12] +
943 F[6][7] * D[7][9] +
944 F[6][8] * D[8][9]) +
945 F[6][10] * (F[6][9] * D[9][10] + F[6][10] * D[10][10] +
946 F[6][11] * D[10][11] + F[6][12] * D[10][12] +
947 F[6][7] * D[7][10] + F[6][8] * D[8][10]) +
948 F[6][11] * (F[6][9] * D[9][11] + F[6][10] * D[10][11] +
949 F[6][11] * D[11][11] + F[6][12] * D[11][12] +
950 F[6][7] * D[7][11] + F[6][8] * D[8][11]) +
951 F[6][12] * (F[6][9] * D[9][12] + F[6][10] * D[10][12] +
952 F[6][11] * D[11][12] + F[6][12] * D[12][12] +
953 F[6][7] * D[7][12] + F[6][8] * D[8][12]) +
954 F[6][7] * (F[6][7] * D[7][7] + F[6][8] * D[7][8] +
955 F[6][9] * D[7][9] + F[6][10] * D[7][10] +
956 F[6][11] * D[7][11] + F[6][12] * D[7][12]) +
957 F[6][8] * (F[6][7] * D[7][8] + F[6][8] * D[8][8] +
958 F[6][9] * D[8][9] + F[6][10] * D[8][10] +
959 F[6][11] * D[8][11] + F[6][12] * D[8][12])) * Tsq +
960 (2 * F[6][7] * D[6][7] + 2 * F[6][8] * D[6][8] +
961 2 * F[6][9] * D[6][9] + 2 * F[6][10] * D[6][10] +
962 2 * F[6][11] * D[6][11] + 2 * F[6][12] * D[6][12]) * T +
963 D[6][6];
964 P[6][7] = P[7][6] =
965 (F[7][9] *
966 (F[6][9] * D[9][9] + F[6][10] * D[9][10] +
967 F[6][11] * D[9][11] + F[6][12] * D[9][12] +
968 F[6][7] * D[7][9] + F[6][8] * D[8][9]) +
969 F[7][10] * (F[6][9] * D[9][10] + F[6][10] * D[10][10] +
970 F[6][11] * D[10][11] + F[6][12] * D[10][12] +
971 F[6][7] * D[7][10] + F[6][8] * D[8][10]) +
972 F[7][11] * (F[6][9] * D[9][11] + F[6][10] * D[10][11] +
973 F[6][11] * D[11][11] + F[6][12] * D[11][12] +
974 F[6][7] * D[7][11] + F[6][8] * D[8][11]) +
975 F[7][12] * (F[6][9] * D[9][12] + F[6][10] * D[10][12] +
976 F[6][11] * D[11][12] + F[6][12] * D[12][12] +
977 F[6][7] * D[7][12] + F[6][8] * D[8][12]) +
978 F[7][6] * (F[6][7] * D[6][7] + F[6][8] * D[6][8] +
979 F[6][9] * D[6][9] + F[6][10] * D[6][10] +
980 F[6][11] * D[6][11] + F[6][12] * D[6][12]) +
981 F[7][8] * (F[6][7] * D[7][8] + F[6][8] * D[8][8] +
982 F[6][9] * D[8][9] + F[6][10] * D[8][10] +
983 F[6][11] * D[8][11] + F[6][12] * D[8][12]) +
984 G[6][0] * G[7][0] * Q[0] + G[6][1] * G[7][1] * Q[1] +
985 G[6][2] * G[7][2] * Q[2]) * Tsq + (F[7][6] * D[6][6] +
986 F[6][7] * D[7][7] +
987 F[6][8] * D[7][8] +
988 F[7][8] * D[6][8] +
989 F[6][9] * D[7][9] +
990 F[7][9] * D[6][9] +
991 F[6][10] * D[7][10] +
992 F[7][10] * D[6][10] +
993 F[6][11] * D[7][11] +
994 F[7][11] * D[6][11] +
995 F[6][12] * D[7][12] +
996 F[7][12] * D[6][12]) * T +
997 D[6][7];
998 P[6][8] = P[8][6] =
999 (F[8][9] *
1000 (F[6][9] * D[9][9] + F[6][10] * D[9][10] +
1001 F[6][11] * D[9][11] + F[6][12] * D[9][12] +
1002 F[6][7] * D[7][9] + F[6][8] * D[8][9]) +
1003 F[8][10] * (F[6][9] * D[9][10] + F[6][10] * D[10][10] +
1004 F[6][11] * D[10][11] + F[6][12] * D[10][12] +
1005 F[6][7] * D[7][10] + F[6][8] * D[8][10]) +
1006 F[8][11] * (F[6][9] * D[9][11] + F[6][10] * D[10][11] +
1007 F[6][11] * D[11][11] + F[6][12] * D[11][12] +
1008 F[6][7] * D[7][11] + F[6][8] * D[8][11]) +
1009 F[8][12] * (F[6][9] * D[9][12] + F[6][10] * D[10][12] +
1010 F[6][11] * D[11][12] + F[6][12] * D[12][12] +
1011 F[6][7] * D[7][12] + F[6][8] * D[8][12]) +
1012 F[8][6] * (F[6][7] * D[6][7] + F[6][8] * D[6][8] +
1013 F[6][9] * D[6][9] + F[6][10] * D[6][10] +
1014 F[6][11] * D[6][11] + F[6][12] * D[6][12]) +
1015 F[8][7] * (F[6][7] * D[7][7] + F[6][8] * D[7][8] +
1016 F[6][9] * D[7][9] + F[6][10] * D[7][10] +
1017 F[6][11] * D[7][11] + F[6][12] * D[7][12]) +
1018 G[6][0] * G[8][0] * Q[0] + G[6][1] * G[8][1] * Q[1] +
1019 G[6][2] * G[8][2] * Q[2]) * Tsq + (F[6][7] * D[7][8] +
1020 F[8][6] * D[6][6] +
1021 F[8][7] * D[6][7] +
1022 F[6][8] * D[8][8] +
1023 F[6][9] * D[8][9] +
1024 F[8][9] * D[6][9] +
1025 F[6][10] * D[8][10] +
1026 F[8][10] * D[6][10] +
1027 F[6][11] * D[8][11] +
1028 F[8][11] * D[6][11] +
1029 F[6][12] * D[8][12] +
1030 F[8][12] * D[6][12]) * T +
1031 D[6][8];
1032 P[6][9] = P[9][6] =
1033 (F[9][10] *
1034 (F[6][9] * D[9][10] + F[6][10] * D[10][10] +
1035 F[6][11] * D[10][11] + F[6][12] * D[10][12] +
1036 F[6][7] * D[7][10] + F[6][8] * D[8][10]) +
1037 F[9][11] * (F[6][9] * D[9][11] + F[6][10] * D[10][11] +
1038 F[6][11] * D[11][11] + F[6][12] * D[11][12] +
1039 F[6][7] * D[7][11] + F[6][8] * D[8][11]) +
1040 F[9][12] * (F[6][9] * D[9][12] + F[6][10] * D[10][12] +
1041 F[6][11] * D[11][12] + F[6][12] * D[12][12] +
1042 F[6][7] * D[7][12] + F[6][8] * D[8][12]) +
1043 F[9][6] * (F[6][7] * D[6][7] + F[6][8] * D[6][8] +
1044 F[6][9] * D[6][9] + F[6][10] * D[6][10] +
1045 F[6][11] * D[6][11] + F[6][12] * D[6][12]) +
1046 F[9][7] * (F[6][7] * D[7][7] + F[6][8] * D[7][8] +
1047 F[6][9] * D[7][9] + F[6][10] * D[7][10] +
1048 F[6][11] * D[7][11] + F[6][12] * D[7][12]) +
1049 F[9][8] * (F[6][7] * D[7][8] + F[6][8] * D[8][8] +
1050 F[6][9] * D[8][9] + F[6][10] * D[8][10] +
1051 F[6][11] * D[8][11] + F[6][12] * D[8][12]) +
1052 G[9][0] * G[6][0] * Q[0] + G[9][1] * G[6][1] * Q[1] +
1053 G[9][2] * G[6][2] * Q[2]) * Tsq + (F[9][6] * D[6][6] +
1054 F[9][7] * D[6][7] +
1055 F[9][8] * D[6][8] +
1056 F[6][9] * D[9][9] +
1057 F[9][10] * D[6][10] +
1058 F[6][10] * D[9][10] +
1059 F[9][11] * D[6][11] +
1060 F[6][11] * D[9][11] +
1061 F[9][12] * D[6][12] +
1062 F[6][12] * D[9][12] +
1063 F[6][7] * D[7][9] +
1064 F[6][8] * D[8][9]) * T +
1065 D[6][9];
1066 P[6][10] = P[10][6] =
1067 (F[6][9] * D[9][10] + F[6][10] * D[10][10] +
1068 F[6][11] * D[10][11] + F[6][12] * D[10][12] +
1069 F[6][7] * D[7][10] + F[6][8] * D[8][10]) * T + D[6][10];
1070 P[6][11] = P[11][6] =
1071 (F[6][9] * D[9][11] + F[6][10] * D[10][11] +
1072 F[6][11] * D[11][11] + F[6][12] * D[11][12] +
1073 F[6][7] * D[7][11] + F[6][8] * D[8][11]) * T + D[6][11];
1074 P[6][12] = P[12][6] =
1075 (F[6][9] * D[9][12] + F[6][10] * D[10][12] +
1076 F[6][11] * D[11][12] + F[6][12] * D[12][12] +
1077 F[6][7] * D[7][12] + F[6][8] * D[8][12]) * T + D[6][12];
1078 P[7][7] =
1079 (Q[0] * G[7][0] * G[7][0] + Q[1] * G[7][1] * G[7][1] +
1080 Q[2] * G[7][2] * G[7][2] + F[7][9] * (F[7][9] * D[9][9] +
1081 F[7][10] * D[9][10] +
1082 F[7][11] * D[9][11] +
1083 F[7][12] * D[9][12] +
1084 F[7][6] * D[6][9] +
1085 F[7][8] * D[8][9]) +
1086 F[7][10] * (F[7][9] * D[9][10] + F[7][10] * D[10][10] +
1087 F[7][11] * D[10][11] + F[7][12] * D[10][12] +
1088 F[7][6] * D[6][10] + F[7][8] * D[8][10]) +
1089 F[7][11] * (F[7][9] * D[9][11] + F[7][10] * D[10][11] +
1090 F[7][11] * D[11][11] + F[7][12] * D[11][12] +
1091 F[7][6] * D[6][11] + F[7][8] * D[8][11]) +
1092 F[7][12] * (F[7][9] * D[9][12] + F[7][10] * D[10][12] +
1093 F[7][11] * D[11][12] + F[7][12] * D[12][12] +
1094 F[7][6] * D[6][12] + F[7][8] * D[8][12]) +
1095 F[7][6] * (F[7][6] * D[6][6] + F[7][8] * D[6][8] +
1096 F[7][9] * D[6][9] + F[7][10] * D[6][10] +
1097 F[7][11] * D[6][11] + F[7][12] * D[6][12]) +
1098 F[7][8] * (F[7][6] * D[6][8] + F[7][8] * D[8][8] +
1099 F[7][9] * D[8][9] + F[7][10] * D[8][10] +
1100 F[7][11] * D[8][11] + F[7][12] * D[8][12])) * Tsq +
1101 (2 * F[7][6] * D[6][7] + 2 * F[7][8] * D[7][8] +
1102 2 * F[7][9] * D[7][9] + 2 * F[7][10] * D[7][10] +
1103 2 * F[7][11] * D[7][11] + 2 * F[7][12] * D[7][12]) * T +
1104 D[7][7];
1105 P[7][8] = P[8][7] =
1106 (F[8][9] *
1107 (F[7][9] * D[9][9] + F[7][10] * D[9][10] +
1108 F[7][11] * D[9][11] + F[7][12] * D[9][12] +
1109 F[7][6] * D[6][9] + F[7][8] * D[8][9]) +
1110 F[8][10] * (F[7][9] * D[9][10] + F[7][10] * D[10][10] +
1111 F[7][11] * D[10][11] + F[7][12] * D[10][12] +
1112 F[7][6] * D[6][10] + F[7][8] * D[8][10]) +
1113 F[8][11] * (F[7][9] * D[9][11] + F[7][10] * D[10][11] +
1114 F[7][11] * D[11][11] + F[7][12] * D[11][12] +
1115 F[7][6] * D[6][11] + F[7][8] * D[8][11]) +
1116 F[8][12] * (F[7][9] * D[9][12] + F[7][10] * D[10][12] +
1117 F[7][11] * D[11][12] + F[7][12] * D[12][12] +
1118 F[7][6] * D[6][12] + F[7][8] * D[8][12]) +
1119 F[8][6] * (F[7][6] * D[6][6] + F[7][8] * D[6][8] +
1120 F[7][9] * D[6][9] + F[7][10] * D[6][10] +
1121 F[7][11] * D[6][11] + F[7][12] * D[6][12]) +
1122 F[8][7] * (F[7][6] * D[6][7] + F[7][8] * D[7][8] +
1123 F[7][9] * D[7][9] + F[7][10] * D[7][10] +
1124 F[7][11] * D[7][11] + F[7][12] * D[7][12]) +
1125 G[7][0] * G[8][0] * Q[0] + G[7][1] * G[8][1] * Q[1] +
1126 G[7][2] * G[8][2] * Q[2]) * Tsq + (F[7][6] * D[6][8] +
1127 F[8][6] * D[6][7] +
1128 F[8][7] * D[7][7] +
1129 F[7][8] * D[8][8] +
1130 F[7][9] * D[8][9] +
1131 F[8][9] * D[7][9] +
1132 F[7][10] * D[8][10] +
1133 F[8][10] * D[7][10] +
1134 F[7][11] * D[8][11] +
1135 F[8][11] * D[7][11] +
1136 F[7][12] * D[8][12] +
1137 F[8][12] * D[7][12]) * T +
1138 D[7][8];
1139 P[7][9] = P[9][7] =
1140 (F[9][10] *
1141 (F[7][9] * D[9][10] + F[7][10] * D[10][10] +
1142 F[7][11] * D[10][11] + F[7][12] * D[10][12] +
1143 F[7][6] * D[6][10] + F[7][8] * D[8][10]) +
1144 F[9][11] * (F[7][9] * D[9][11] + F[7][10] * D[10][11] +
1145 F[7][11] * D[11][11] + F[7][12] * D[11][12] +
1146 F[7][6] * D[6][11] + F[7][8] * D[8][11]) +
1147 F[9][12] * (F[7][9] * D[9][12] + F[7][10] * D[10][12] +
1148 F[7][11] * D[11][12] + F[7][12] * D[12][12] +
1149 F[7][6] * D[6][12] + F[7][8] * D[8][12]) +
1150 F[9][6] * (F[7][6] * D[6][6] + F[7][8] * D[6][8] +
1151 F[7][9] * D[6][9] + F[7][10] * D[6][10] +
1152 F[7][11] * D[6][11] + F[7][12] * D[6][12]) +
1153 F[9][7] * (F[7][6] * D[6][7] + F[7][8] * D[7][8] +
1154 F[7][9] * D[7][9] + F[7][10] * D[7][10] +
1155 F[7][11] * D[7][11] + F[7][12] * D[7][12]) +
1156 F[9][8] * (F[7][6] * D[6][8] + F[7][8] * D[8][8] +
1157 F[7][9] * D[8][9] + F[7][10] * D[8][10] +
1158 F[7][11] * D[8][11] + F[7][12] * D[8][12]) +
1159 G[9][0] * G[7][0] * Q[0] + G[9][1] * G[7][1] * Q[1] +
1160 G[9][2] * G[7][2] * Q[2]) * Tsq + (F[9][6] * D[6][7] +
1161 F[9][7] * D[7][7] +
1162 F[9][8] * D[7][8] +
1163 F[7][9] * D[9][9] +
1164 F[9][10] * D[7][10] +
1165 F[7][10] * D[9][10] +
1166 F[9][11] * D[7][11] +
1167 F[7][11] * D[9][11] +
1168 F[9][12] * D[7][12] +
1169 F[7][12] * D[9][12] +
1170 F[7][6] * D[6][9] +
1171 F[7][8] * D[8][9]) * T +
1172 D[7][9];
1173 P[7][10] = P[10][7] =
1174 (F[7][9] * D[9][10] + F[7][10] * D[10][10] +
1175 F[7][11] * D[10][11] + F[7][12] * D[10][12] +
1176 F[7][6] * D[6][10] + F[7][8] * D[8][10]) * T + D[7][10];
1177 P[7][11] = P[11][7] =
1178 (F[7][9] * D[9][11] + F[7][10] * D[10][11] +
1179 F[7][11] * D[11][11] + F[7][12] * D[11][12] +
1180 F[7][6] * D[6][11] + F[7][8] * D[8][11]) * T + D[7][11];
1181 P[7][12] = P[12][7] =
1182 (F[7][9] * D[9][12] + F[7][10] * D[10][12] +
1183 F[7][11] * D[11][12] + F[7][12] * D[12][12] +
1184 F[7][6] * D[6][12] + F[7][8] * D[8][12]) * T + D[7][12];
1185 P[8][8] =
1186 (Q[0] * G[8][0] * G[8][0] + Q[1] * G[8][1] * G[8][1] +
1187 Q[2] * G[8][2] * G[8][2] + F[8][9] * (F[8][9] * D[9][9] +
1188 F[8][10] * D[9][10] +
1189 F[8][11] * D[9][11] +
1190 F[8][12] * D[9][12] +
1191 F[8][6] * D[6][9] +
1192 F[8][7] * D[7][9]) +
1193 F[8][10] * (F[8][9] * D[9][10] + F[8][10] * D[10][10] +
1194 F[8][11] * D[10][11] + F[8][12] * D[10][12] +
1195 F[8][6] * D[6][10] + F[8][7] * D[7][10]) +
1196 F[8][11] * (F[8][9] * D[9][11] + F[8][10] * D[10][11] +
1197 F[8][11] * D[11][11] + F[8][12] * D[11][12] +
1198 F[8][6] * D[6][11] + F[8][7] * D[7][11]) +
1199 F[8][12] * (F[8][9] * D[9][12] + F[8][10] * D[10][12] +
1200 F[8][11] * D[11][12] + F[8][12] * D[12][12] +
1201 F[8][6] * D[6][12] + F[8][7] * D[7][12]) +
1202 F[8][6] * (F[8][6] * D[6][6] + F[8][7] * D[6][7] +
1203 F[8][9] * D[6][9] + F[8][10] * D[6][10] +
1204 F[8][11] * D[6][11] + F[8][12] * D[6][12]) +
1205 F[8][7] * (F[8][6] * D[6][7] + F[8][7] * D[7][7] +
1206 F[8][9] * D[7][9] + F[8][10] * D[7][10] +
1207 F[8][11] * D[7][11] + F[8][12] * D[7][12])) * Tsq +
1208 (2 * F[8][6] * D[6][8] + 2 * F[8][7] * D[7][8] +
1209 2 * F[8][9] * D[8][9] + 2 * F[8][10] * D[8][10] +
1210 2 * F[8][11] * D[8][11] + 2 * F[8][12] * D[8][12]) * T +
1211 D[8][8];
1212 P[8][9] = P[9][8] =
1213 (F[9][10] *
1214 (F[8][9] * D[9][10] + F[8][10] * D[10][10] +
1215 F[8][11] * D[10][11] + F[8][12] * D[10][12] +
1216 F[8][6] * D[6][10] + F[8][7] * D[7][10]) +
1217 F[9][11] * (F[8][9] * D[9][11] + F[8][10] * D[10][11] +
1218 F[8][11] * D[11][11] + F[8][12] * D[11][12] +
1219 F[8][6] * D[6][11] + F[8][7] * D[7][11]) +
1220 F[9][12] * (F[8][9] * D[9][12] + F[8][10] * D[10][12] +
1221 F[8][11] * D[11][12] + F[8][12] * D[12][12] +
1222 F[8][6] * D[6][12] + F[8][7] * D[7][12]) +
1223 F[9][6] * (F[8][6] * D[6][6] + F[8][7] * D[6][7] +
1224 F[8][9] * D[6][9] + F[8][10] * D[6][10] +
1225 F[8][11] * D[6][11] + F[8][12] * D[6][12]) +
1226 F[9][7] * (F[8][6] * D[6][7] + F[8][7] * D[7][7] +
1227 F[8][9] * D[7][9] + F[8][10] * D[7][10] +
1228 F[8][11] * D[7][11] + F[8][12] * D[7][12]) +
1229 F[9][8] * (F[8][6] * D[6][8] + F[8][7] * D[7][8] +
1230 F[8][9] * D[8][9] + F[8][10] * D[8][10] +
1231 F[8][11] * D[8][11] + F[8][12] * D[8][12]) +
1232 G[9][0] * G[8][0] * Q[0] + G[9][1] * G[8][1] * Q[1] +
1233 G[9][2] * G[8][2] * Q[2]) * Tsq + (F[9][6] * D[6][8] +
1234 F[9][7] * D[7][8] +
1235 F[9][8] * D[8][8] +
1236 F[8][9] * D[9][9] +
1237 F[9][10] * D[8][10] +
1238 F[8][10] * D[9][10] +
1239 F[9][11] * D[8][11] +
1240 F[8][11] * D[9][11] +
1241 F[9][12] * D[8][12] +
1242 F[8][12] * D[9][12] +
1243 F[8][6] * D[6][9] +
1244 F[8][7] * D[7][9]) * T +
1245 D[8][9];
1246 P[8][10] = P[10][8] =
1247 (F[8][9] * D[9][10] + F[8][10] * D[10][10] +
1248 F[8][11] * D[10][11] + F[8][12] * D[10][12] +
1249 F[8][6] * D[6][10] + F[8][7] * D[7][10]) * T + D[8][10];
1250 P[8][11] = P[11][8] =
1251 (F[8][9] * D[9][11] + F[8][10] * D[10][11] +
1252 F[8][11] * D[11][11] + F[8][12] * D[11][12] +
1253 F[8][6] * D[6][11] + F[8][7] * D[7][11]) * T + D[8][11];
1254 P[8][12] = P[12][8] =
1255 (F[8][9] * D[9][12] + F[8][10] * D[10][12] +
1256 F[8][11] * D[11][12] + F[8][12] * D[12][12] +
1257 F[8][6] * D[6][12] + F[8][7] * D[7][12]) * T + D[8][12];
1258 P[9][9] =
1259 (Q[0] * G[9][0] * G[9][0] + Q[1] * G[9][1] * G[9][1] +
1260 Q[2] * G[9][2] * G[9][2] + F[9][10] * (F[9][10] * D[10][10] +
1261 F[9][11] * D[10][11] +
1262 F[9][12] * D[10][12] +
1263 F[9][6] * D[6][10] +
1264 F[9][7] * D[7][10] +
1265 F[9][8] * D[8][10]) +
1266 F[9][11] * (F[9][10] * D[10][11] + F[9][11] * D[11][11] +
1267 F[9][12] * D[11][12] + F[9][6] * D[6][11] +
1268 F[9][7] * D[7][11] + F[9][8] * D[8][11]) +
1269 F[9][12] * (F[9][10] * D[10][12] + F[9][11] * D[11][12] +
1270 F[9][12] * D[12][12] + F[9][6] * D[6][12] +
1271 F[9][7] * D[7][12] + F[9][8] * D[8][12]) +
1272 F[9][6] * (F[9][6] * D[6][6] + F[9][7] * D[6][7] +
1273 F[9][8] * D[6][8] + F[9][10] * D[6][10] +
1274 F[9][11] * D[6][11] + F[9][12] * D[6][12]) +
1275 F[9][7] * (F[9][6] * D[6][7] + F[9][7] * D[7][7] +
1276 F[9][8] * D[7][8] + F[9][10] * D[7][10] +
1277 F[9][11] * D[7][11] + F[9][12] * D[7][12]) +
1278 F[9][8] * (F[9][6] * D[6][8] + F[9][7] * D[7][8] +
1279 F[9][8] * D[8][8] + F[9][10] * D[8][10] +
1280 F[9][11] * D[8][11] + F[9][12] * D[8][12])) * Tsq +
1281 (2 * F[9][10] * D[9][10] + 2 * F[9][11] * D[9][11] +
1282 2 * F[9][12] * D[9][12] + 2 * F[9][6] * D[6][9] +
1283 2 * F[9][7] * D[7][9] + 2 * F[9][8] * D[8][9]) * T + D[9][9];
1284 P[9][10] = P[10][9] =
1285 (F[9][10] * D[10][10] + F[9][11] * D[10][11] +
1286 F[9][12] * D[10][12] + F[9][6] * D[6][10] +
1287 F[9][7] * D[7][10] + F[9][8] * D[8][10]) * T + D[9][10];
1288 P[9][11] = P[11][9] =
1289 (F[9][10] * D[10][11] + F[9][11] * D[11][11] +
1290 F[9][12] * D[11][12] + F[9][6] * D[6][11] +
1291 F[9][7] * D[7][11] + F[9][8] * D[8][11]) * T + D[9][11];
1292 P[9][12] = P[12][9] =
1293 (F[9][10] * D[10][12] + F[9][11] * D[11][12] +
1294 F[9][12] * D[12][12] + F[9][6] * D[6][12] +
1295 F[9][7] * D[7][12] + F[9][8] * D[8][12]) * T + D[9][12];
1296 P[10][10] = Q[6] * Tsq + D[10][10];
1297 P[10][11] = P[11][10] = D[10][11];
1298 P[10][12] = P[12][10] = D[10][12];
1299 P[11][11] = Q[7] * Tsq + D[11][11];
1300 P[11][12] = P[12][11] = D[11][12];
1301 P[12][12] = Q[8] * Tsq + D[12][12];
1302 }
1303 #endif
1304
1305 // ************* SerialUpdate *******************
1306 // Does the update step of the Kalman filter for the covariance and estimate
1307 // Outputs are Xnew & Pnew, and are written over P and X
1308 // Z is actual measurement, Y is predicted measurement
1309 // Xnew = X + K*(Z-Y), Pnew=(I-K*H)*P,
1310 // where K=P*H'*inv[H*P*H'+R]
1311 // NOTE the algorithm assumes R (measurement covariance matrix) is diagonal
1312 // i.e. the measurment noises are uncorrelated.
1313 // It therefore uses a serial update that requires no matrix inversion by
1314 // processing the measurements one at a time.
1315 // Algorithm - see Grewal and Andrews, "Kalman Filtering,2nd Ed" p.121 & p.253
1316 // - or see Simon, "Optimal State Estimation," 1st Ed, p.150
1317 // The SensorsUsed variable is a bitwise mask indicating which sensors
1318 // should be used in the update.
1319 // ************************************************
1320
1321 void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
1322 float Y[NUMV], float P[NUMX][NUMX], float X[NUMX],
1323 uint16_t SensorsUsed)
1324 {
1325 float HP[NUMX], HPHR, Error;
1326 uint8_t i, j, k, m;
1327
1328 for (m = 0; m < NUMV; m++) {
1329
1330 if (SensorsUsed & (0x01 << m)) { // use this sensor for update
1331
1332 for (j = 0; j < NUMX; j++) { // Find Hp = H*P
1333 HP[j] = 0;
1334 for (k = 0; k < NUMX; k++)
1335 HP[j] += H[m][k] * P[k][j];
1336 }
1337 HPHR = R[m]; // Find HPHR = H*P*H' + R
1338 for (k = 0; k < NUMX; k++)
1339 HPHR += HP[k] * H[m][k];
1340
1341 for (k = 0; k < NUMX; k++)
1342 K[k][m] = HP[k] / HPHR; // find K = HP/HPHR
1343
1344 for (i = 0; i < NUMX; i++) { // Find P(m)= P(m-1) + K*HP
1345 for (j = i; j < NUMX; j++)
1346 P[i][j] = P[j][i] =
1347 P[i][j] - K[i][m] * HP[j];
1348 }
1349
1350 Error = Z[m] - Y[m];
1351 for (i = 0; i < NUMX; i++) // Find X(m)= X(m-1) + K*Error
1352 X[i] = X[i] + K[i][m] * Error;
1353
1354 }
1355 }
1356 }
1357
1358 // ************* RungeKutta **********************
1359 // Does a 4th order Runge Kutta numerical integration step
1360 // Output, Xnew, is written over X
1361 // NOTE the algorithm assumes time invariant state equations and
1362 // constant inputs over integration step
1363 // ************************************************
1364
1365 void RungeKutta(float X[NUMX], float U[NUMU], float dT)
1366 {
1367
1368 float dT2 =
1369 dT / 2, K1[NUMX], K2[NUMX], K3[NUMX], K4[NUMX], Xlast[NUMX];
1370 uint8_t i;
1371
1372 for (i = 0; i < NUMX; i++)
1373 Xlast[i] = X[i]; // make a working copy
1374
1375 StateEq(X, U, K1); // k1 = f(x,u)
1376 for (i = 0; i < NUMX; i++)
1377 X[i] = Xlast[i] + dT2 * K1[i];
1378 StateEq(X, U, K2); // k2 = f(x+0.5*dT*k1,u)
1379 for (i = 0; i < NUMX; i++)
1380 X[i] = Xlast[i] + dT2 * K2[i];
1381 StateEq(X, U, K3); // k3 = f(x+0.5*dT*k2,u)
1382 for (i = 0; i < NUMX; i++)
1383 X[i] = Xlast[i] + dT * K3[i];
1384 StateEq(X, U, K4); // k4 = f(x+dT*k3,u)
1385
1386 // Xnew = X + dT*(k1+2*k2+2*k3+k4)/6
1387 for (i = 0; i < NUMX; i++)
1388 X[i] =
1389 Xlast[i] + dT * (K1[i] + 2 * K2[i] + 2 * K3[i] +
1390 K4[i]) / 6;
1391 }
1392
1393 // ************* Model Specific Stuff ***************************
1394 // *** StateEq, MeasurementEq, LinerizeFG, and LinearizeH ********
1395 //
1396 // State Variables = [Pos Vel Quaternion GyroBias NO-AccelBias]
1397 // Deterministic Inputs = [AngularVel Accel]
1398 // Disturbance Noise = [GyroNoise AccelNoise GyroRandomWalkNoise NO-AccelRandomWalkNoise]
1399 //
1400 // Measurement Variables = [Pos Vel BodyFrameMagField Altimeter]
1401 // Inputs to Measurement = [EarthFrameMagField]
1402 //
1403 // Notes: Pos and Vel in earth frame
1404 // AngularVel and Accel in body frame
1405 // MagFields are unit vectors
1406 // Xdot is output of StateEq()
1407 // F and G are outputs of LinearizeFG(), all elements not set should be zero
1408 // y is output of OutputEq()
1409 // H is output of LinearizeH(), all elements not set should be zero
1410 // ************************************************
1411
1412 void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX])
1413 {
1414 float ax, ay, az, wx, wy, wz, q0, q1, q2, q3;
1415
1416 // ax=U[3]-X[13]; ay=U[4]-X[14]; az=U[5]-X[15]; // subtract the biases on accels
1417 ax = U[3];
1418 ay = U[4];
1419 az = U[5]; // NO BIAS STATES ON ACCELS
1420 wx = U[0] - X[10];
1421 wy = U[1] - X[11];
1422 wz = U[2] - X[12]; // subtract the biases on gyros
1423 q0 = X[6];
1424 q1 = X[7];
1425 q2 = X[8];
1426 q3 = X[9];
1427
1428 // Pdot = V
1429 Xdot[0] = X[3];
1430 Xdot[1] = X[4];
1431 Xdot[2] = X[5];
1432
1433 // Vdot = Reb*a
1434 Xdot[3] =
1435 (q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * ax + 2 * (q1 * q2 -
1436 q0 * q3) *
1437 ay + 2 * (q1 * q3 + q0 * q2) * az;
1438 Xdot[4] =
1439 2 * (q1 * q2 + q0 * q3) * ax + (q0 * q0 - q1 * q1 + q2 * q2 -
1440 q3 * q3) * ay + 2 * (q2 * q3 -
1441 q0 * q1) *
1442 az;
1443 Xdot[5] =
1444 2 * (q1 * q3 - q0 * q2) * ax + 2 * (q2 * q3 + q0 * q1) * ay +
1445 (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * az + 9.81;
1446
1447 // qdot = Q*w
1448 Xdot[6] = (-q1 * wx - q2 * wy - q3 * wz) / 2;
1449 Xdot[7] = (q0 * wx - q3 * wy + q2 * wz) / 2;
1450 Xdot[8] = (q3 * wx + q0 * wy - q1 * wz) / 2;
1451 Xdot[9] = (-q2 * wx + q1 * wy + q0 * wz) / 2;
1452
1453 // best guess is that bias stays constant
1454 Xdot[10] = Xdot[11] = Xdot[12] = 0;
1455 }
1456
1457 void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX],
1458 float G[NUMX][NUMW])
1459 {
1460 float ax, ay, az, wx, wy, wz, q0, q1, q2, q3;
1461
1462 // ax=U[3]-X[13]; ay=U[4]-X[14]; az=U[5]-X[15]; // subtract the biases on accels
1463 ax = U[3];
1464 ay = U[4];
1465 az = U[5]; // NO BIAS STATES ON ACCELS
1466 wx = U[0] - X[10];
1467 wy = U[1] - X[11];
1468 wz = U[2] - X[12]; // subtract the biases on gyros
1469 q0 = X[6];
1470 q1 = X[7];
1471 q2 = X[8];
1472 q3 = X[9];
1473
1474 // Pdot = V
1475 F[0][3] = F[1][4] = F[2][5] = 1;
1476
1477 // dVdot/dq
1478 F[3][6] = 2 * (q0 * ax - q3 * ay + q2 * az);
1479 F[3][7] = 2 * (q1 * ax + q2 * ay + q3 * az);
1480 F[3][8] = 2 * (-q2 * ax + q1 * ay + q0 * az);
1481 F[3][9] = 2 * (-q3 * ax - q0 * ay + q1 * az);
1482 F[4][6] = 2 * (q3 * ax + q0 * ay - q1 * az);
1483 F[4][7] = 2 * (q2 * ax - q1 * ay - q0 * az);
1484 F[4][8] = 2 * (q1 * ax + q2 * ay + q3 * az);
1485 F[4][9] = 2 * (q0 * ax - q3 * ay + q2 * az);
1486 F[5][6] = 2 * (-q2 * ax + q1 * ay + q0 * az);
1487 F[5][7] = 2 * (q3 * ax + q0 * ay - q1 * az);
1488 F[5][8] = 2 * (-q0 * ax + q3 * ay - q2 * az);
1489 F[5][9] = 2 * (q1 * ax + q2 * ay + q3 * az);
1490
1491 // dVdot/dabias & dVdot/dna - NO BIAS STATES ON ACCELS - S0 REPEAT FOR G BELOW
1492 // F[3][13]=G[3][3]=-q0*q0-q1*q1+q2*q2+q3*q3; F[3][14]=G[3][4]=2*(-q1*q2+q0*q3); F[3][15]=G[3][5]=-2*(q1*q3+q0*q2);
1493 // F[4][13]=G[4][3]=-2*(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*(-q2*q3+q0*q1);
1494 // F[5][13]=G[5][3]=2*(-q1*q3+q0*q2); F[5][14]=G[5][4]=-2*(q2*q3+q0*q1); F[5][15]=G[5][5]=-q0*q0+q1*q1+q2*q2-q3*q3;
1495
1496 // dqdot/dq
1497 F[6][6] = 0;
1498 F[6][7] = -wx / 2;
1499 F[6][8] = -wy / 2;
1500 F[6][9] = -wz / 2;
1501 F[7][6] = wx / 2;
1502 F[7][7] = 0;
1503 F[7][8] = wz / 2;
1504 F[7][9] = -wy / 2;
1505 F[8][6] = wy / 2;
1506 F[8][7] = -wz / 2;
1507 F[8][8] = 0;
1508 F[8][9] = wx / 2;
1509 F[9][6] = wz / 2;
1510 F[9][7] = wy / 2;
1511 F[9][8] = -wx / 2;
1512 F[9][9] = 0;
1513
1514 // dqdot/dwbias
1515 F[6][10] = q1 / 2;
1516 F[6][11] = q2 / 2;
1517 F[6][12] = q3 / 2;
1518 F[7][10] = -q0 / 2;
1519 F[7][11] = q3 / 2;
1520 F[7][12] = -q2 / 2;
1521 F[8][10] = -q3 / 2;
1522 F[8][11] = -q0 / 2;
1523 F[8][12] = q1 / 2;
1524 F[9][10] = q2 / 2;
1525 F[9][11] = -q1 / 2;
1526 F[9][12] = -q0 / 2;
1527
1528 // dVdot/dna - NO BIAS STATES ON ACCELS - S0 REPEAT FOR G HERE
1529 G[3][3] = -q0 * q0 - q1 * q1 + q2 * q2 + q3 * q3;
1530 G[3][4] = 2 * (-q1 * q2 + q0 * q3);
1531 G[3][5] = -2 * (q1 * q3 + q0 * q2);
1532 G[4][3] = -2 * (q1 * q2 + q0 * q3);
1533 G[4][4] = -q0 * q0 + q1 * q1 - q2 * q2 + q3 * q3;
1534 G[4][5] = 2 * (-q2 * q3 + q0 * q1);
1535 G[5][3] = 2 * (-q1 * q3 + q0 * q2);
1536 G[5][4] = -2 * (q2 * q3 + q0 * q1);
1537 G[5][5] = -q0 * q0 + q1 * q1 + q2 * q2 - q3 * q3;
1538
1539 // dqdot/dnw
1540 G[6][0] = q1 / 2;
1541 G[6][1] = q2 / 2;
1542 G[6][2] = q3 / 2;
1543 G[7][0] = -q0 / 2;
1544 G[7][1] = q3 / 2;
1545 G[7][2] = -q2 / 2;
1546 G[8][0] = -q3 / 2;
1547 G[8][1] = -q0 / 2;
1548 G[8][2] = q1 / 2;
1549 G[9][0] = q2 / 2;
1550 G[9][1] = -q1 / 2;
1551 G[9][2] = -q0 / 2;
1552
1553 // dwbias = random walk noise
1554 G[10][6] = G[11][7] = G[12][8] = 1;
1555 // dabias = random walk noise
1556 // G[13][9]=G[14][10]=G[15][11]=1; // NO BIAS STATES ON ACCELS
1557 }
1558
1559 void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV])
1560 {
1561 float q0, q1, q2, q3;
1562
1563 q0 = X[6];
1564 q1 = X[7];
1565 q2 = X[8];
1566 q3 = X[9];
1567
1568 // first six outputs are P and V
1569 Y[0] = X[0];
1570 Y[1] = X[1];
1571 Y[2] = X[2];
1572 Y[3] = X[3];
1573 Y[4] = X[4];
1574 Y[5] = X[5];
1575
1576 // Bb=Rbe*Be
1577 Y[6] =
1578 (q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * Be[0] +
1579 2 * (q1 * q2 + q0 * q3) * Be[1] + 2 * (q1 * q3 -
1580 q0 * q2) * Be[2];
1581 Y[7] =
1582 2 * (q1 * q2 - q0 * q3) * Be[0] + (q0 * q0 - q1 * q1 +
1583 q2 * q2 - q3 * q3) * Be[1] +
1584 2 * (q2 * q3 + q0 * q1) * Be[2];
1585 Y[8] =
1586 2 * (q1 * q3 + q0 * q2) * Be[0] + 2 * (q2 * q3 -
1587 q0 * q1) * Be[1] +
1588 (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * Be[2];
1589
1590 // Alt = -Pz
1591 Y[9] = -X[2];
1592 }
1593
1594 void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX])
1595 {
1596 float q0, q1, q2, q3;
1597
1598 q0 = X[6];
1599 q1 = X[7];
1600 q2 = X[8];
1601 q3 = X[9];
1602
1603 // dP/dP=I;
1604 H[0][0] = H[1][1] = H[2][2] = 1;
1605 // dV/dV=I;
1606 H[3][3] = H[4][4] = H[5][5] = 1;
1607
1608 // dBb/dq
1609 H[6][6] = 2 * (q0 * Be[0] + q3 * Be[1] - q2 * Be[2]);
1610 H[6][7] = 2 * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
1611 H[6][8] = 2 * (-q2 * Be[0] + q1 * Be[1] - q0 * Be[2]);
1612 H[6][9] = 2 * (-q3 * Be[0] + q0 * Be[1] + q1 * Be[2]);
1613 H[7][6] = 2 * (-q3 * Be[0] + q0 * Be[1] + q1 * Be[2]);
1614 H[7][7] = 2 * (q2 * Be[0] - q1 * Be[1] + q0 * Be[2]);
1615 H[7][8] = 2 * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
1616 H[7][9] = 2 * (-q0 * Be[0] - q3 * Be[1] + q2 * Be[2]);
1617 H[8][6] = 2 * (q2 * Be[0] - q1 * Be[1] + q0 * Be[2]);
1618 H[8][7] = 2 * (q3 * Be[0] - q0 * Be[1] - q1 * Be[2]);
1619 H[8][8] = 2 * (q0 * Be[0] + q3 * Be[1] - q2 * Be[2]);
1620 H[8][9] = 2 * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
1621
1622 // dAlt/dPz = -1
1623 H[9][2] = -1;
1624 }
1625
1626 /**
1627 * @}
1628 * @}
1629 */