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