mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-01 09:24:10 +01:00
Merged in corvusvcorax/librepilot/LP-443_insgps14state_fixes (pull request #396)
LP-443 modified insgps14state to use spare matrix optimization (similar to insgps13state) as opposed to precompiled prediction equation code Approved-by: Alessio Morale <alessiomorale@gmail.com> Approved-by: Philippe Renon <philippe_renon@yahoo.fr> Approved-by: Eric Price <corvuscorax@cybertrench.com> Approved-by: Lalanne Laurent <f5soh@free.fr>
This commit is contained in:
Eric Price
Lalanne Laurent
commit
86d6bec7d1
@ -44,28 +44,52 @@
|
||||
#define NUMW 10 // number of plant noise inputs, w is disturbance noise vector
|
||||
#define NUMV 10 // number of measurements, v is the measurement noise vector
|
||||
#define NUMU 6 // number of deterministic inputs, U is the input vector
|
||||
|
||||
#if defined(GENERAL_COV)
|
||||
// This might trick people so I have a note here. There is a slower but bigger version of the
|
||||
// code here but won't fit when debugging disabled (requires -Os)
|
||||
#define COVARIANCE_PREDICTION_GENERAL
|
||||
#endif
|
||||
|
||||
#pragma GCC optimize "O3"
|
||||
// Private functions
|
||||
void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
|
||||
float Q[NUMW], float dT, float P[NUMX][NUMX]);
|
||||
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);
|
||||
void RungeKutta(float X[NUMX], float U[NUMU], float dT);
|
||||
void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX]);
|
||||
void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX],
|
||||
float G[NUMX][NUMW]);
|
||||
void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV]);
|
||||
void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX]);
|
||||
|
||||
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);
|
||||
static void RungeKutta(float X[NUMX], float U[NUMU], float dT);
|
||||
static void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX]);
|
||||
static void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX],
|
||||
float G[NUMX][NUMW]);
|
||||
static void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV]);
|
||||
static void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX]);
|
||||
|
||||
// Private variables
|
||||
|
||||
// speed optimizations, describe matrix sparsity
|
||||
// derived from state equations in
|
||||
// LinearizeFG() and LinearizeH():
|
||||
//
|
||||
// usage F: usage G: usage H:
|
||||
// -0123456789abcd 0123456789 0123456789abcd
|
||||
// 0...X.......... .......... X.............
|
||||
// 1....X......... .......... .X............
|
||||
// 2.....X........ .......... ..X...........
|
||||
// 3......XXXX...X ...XXX.... ...X..........
|
||||
// 4......XXXX...X ...XXX.... ....X.........
|
||||
// 5......XXXX...X ...XXX.... .....X........
|
||||
// 6.....ooXXXXXX. XXX....... ......XXXX....
|
||||
// 7.....oXoXXXXX. XXX....... ......XXXX....
|
||||
// 8.....oXXoXXXX. XXX....... ......XXXX....
|
||||
// 9.....oXXXoXXX. XXX....... ..X...........
|
||||
// a.............. ..........
|
||||
// b.............. ..........
|
||||
// c.............. ..........
|
||||
// d.............. ..........
|
||||
|
||||
static int8_t FrowMin[NUMX] = { 3, 4, 5, 6, 6, 6, 5, 5, 5, 5, 14, 14, 14, 14 };
|
||||
static int8_t FrowMax[NUMX] = { 3, 4, 5, 13, 13, 13, 12, 12, 12, 12, -1, -1, -1, -1 };
|
||||
|
||||
static int8_t GrowMin[NUMX] = { 10, 10, 10, 3, 3, 3, 0, 0, 0, 0, 10, 10, 10, 10 };
|
||||
static int8_t GrowMax[NUMX] = { -1, -1, -1, 5, 5, 5, 2, 2, 2, 2, -1, -1, -1, -1 };
|
||||
|
||||
static int8_t HrowMin[NUMV] = { 0, 1, 2, 3, 4, 5, 6, 6, 6, 2 };
|
||||
static int8_t HrowMax[NUMV] = { 0, 1, 2, 3, 4, 5, 9, 9, 9, 2 };
|
||||
|
||||
static struct EKFData {
|
||||
float F[NUMX][NUMX];
|
||||
float G[NUMX][NUMW];
|
||||
@ -101,6 +125,7 @@ void INSGPSInit()
|
||||
ekf.P[i][j] = 0.0f; // zero all terms
|
||||
ekf.F[i][j] = 0.0f;
|
||||
}
|
||||
|
||||
for (int j = 0; j < NUMW; j++) {
|
||||
ekf.G[i][j] = 0.0f;
|
||||
}
|
||||
@ -352,7 +377,7 @@ void INSLimitBias()
|
||||
void INSStatePrediction(const float gyro_data[3], const float accel_data[3], float dT)
|
||||
{
|
||||
float U[6];
|
||||
float qmag;
|
||||
float invqmag;
|
||||
|
||||
// rate gyro inputs in units of rad/s
|
||||
U[0] = gyro_data[0];
|
||||
@ -367,11 +392,11 @@ void INSStatePrediction(const float gyro_data[3], const float accel_data[3], flo
|
||||
// EKF prediction step
|
||||
LinearizeFG(ekf.X, U, ekf.F, ekf.G);
|
||||
RungeKutta(ekf.X, U, dT);
|
||||
qmag = sqrtf(ekf.X[6] * ekf.X[6] + ekf.X[7] * ekf.X[7] + ekf.X[8] * ekf.X[8] + ekf.X[9] * ekf.X[9]);
|
||||
ekf.X[6] /= qmag;
|
||||
ekf.X[7] /= qmag;
|
||||
ekf.X[8] /= qmag;
|
||||
ekf.X[9] /= qmag;
|
||||
invqmag = invsqrtf(ekf.X[6] * ekf.X[6] + ekf.X[7] * ekf.X[7] + ekf.X[8] * ekf.X[8] + ekf.X[9] * ekf.X[9]);
|
||||
ekf.X[6] *= invqmag;
|
||||
ekf.X[7] *= invqmag;
|
||||
ekf.X[8] *= invqmag;
|
||||
ekf.X[9] *= invqmag;
|
||||
|
||||
// Update Nav solution structure
|
||||
Nav.Pos[0] = ekf.X[0];
|
||||
@ -384,12 +409,12 @@ void INSStatePrediction(const float gyro_data[3], const float accel_data[3], flo
|
||||
Nav.q[1] = ekf.X[7];
|
||||
Nav.q[2] = ekf.X[8];
|
||||
Nav.q[3] = ekf.X[9];
|
||||
Nav.gyro_bias[0] = ekf.X[10];
|
||||
Nav.gyro_bias[1] = ekf.X[11];
|
||||
Nav.gyro_bias[2] = ekf.X[12];
|
||||
Nav.gyro_bias[0] = 0.0f;
|
||||
Nav.gyro_bias[1] = 0.0f;
|
||||
Nav.gyro_bias[2] = ekf.X[13];
|
||||
Nav.gyro_bias[0] = ekf.X[10];
|
||||
Nav.gyro_bias[1] = ekf.X[11];
|
||||
Nav.gyro_bias[2] = ekf.X[12];
|
||||
Nav.accel_bias[0] = 0.0f;
|
||||
Nav.accel_bias[1] = 0.0f;
|
||||
Nav.accel_bias[2] = ekf.X[13];
|
||||
}
|
||||
|
||||
void INSCovariancePrediction(float dT)
|
||||
@ -398,10 +423,10 @@ void INSCovariancePrediction(float dT)
|
||||
}
|
||||
|
||||
void INSCorrection(const float mag_data[3], const float Pos[3], const float Vel[3],
|
||||
float BaroAlt, uint16_t SensorsUsed)
|
||||
const float BaroAlt, uint16_t SensorsUsed)
|
||||
{
|
||||
float Z[10], Y[10];
|
||||
float qmag;
|
||||
float invqmag;
|
||||
|
||||
// GPS Position in meters and in local NED frame
|
||||
Z[0] = Pos[0];
|
||||
@ -436,6 +461,9 @@ void INSCorrection(const float mag_data[3], const float Pos[3], const float Vel[
|
||||
Z[6] = Rbe_a[0][0] * mag_data[0] + Rbe_a[1][0] * mag_data[1] + Rbe_a[2][0] * mag_data[2];
|
||||
Z[7] = Rbe_a[0][1] * mag_data[0] + Rbe_a[1][1] * mag_data[1] + Rbe_a[2][1] * mag_data[2];
|
||||
Z[8] = Rbe_a[0][2] * mag_data[0] + Rbe_a[1][2] * mag_data[1] + Rbe_a[2][2] * mag_data[2];
|
||||
if (!(IS_REAL(Z[6]) && IS_REAL(Z[7]) && IS_REAL(Z[8]))) {
|
||||
SensorsUsed = SensorsUsed & ~MAG_SENSORS;
|
||||
}
|
||||
}
|
||||
|
||||
// barometric altimeter in meters and in local NED frame
|
||||
@ -445,11 +473,11 @@ void INSCorrection(const float mag_data[3], const float Pos[3], const float Vel[
|
||||
LinearizeH(ekf.X, ekf.Be, ekf.H);
|
||||
MeasurementEq(ekf.X, ekf.Be, Y);
|
||||
SerialUpdate(ekf.H, ekf.R, Z, Y, ekf.P, ekf.X, SensorsUsed);
|
||||
qmag = sqrtf(ekf.X[6] * ekf.X[6] + ekf.X[7] * ekf.X[7] + ekf.X[8] * ekf.X[8] + ekf.X[9] * ekf.X[9]);
|
||||
ekf.X[6] /= qmag;
|
||||
ekf.X[7] /= qmag;
|
||||
ekf.X[8] /= qmag;
|
||||
ekf.X[9] /= qmag;
|
||||
invqmag = invsqrtf(ekf.X[6] * ekf.X[6] + ekf.X[7] * ekf.X[7] + ekf.X[8] * ekf.X[8] + ekf.X[9] * ekf.X[9]);
|
||||
ekf.X[6] *= invqmag;
|
||||
ekf.X[7] *= invqmag;
|
||||
ekf.X[8] *= invqmag;
|
||||
ekf.X[9] *= invqmag;
|
||||
|
||||
INSLimitBias();
|
||||
}
|
||||
@ -465,168 +493,86 @@ void INSCorrection(const float mag_data[3], const float Pos[3], const float Vel[
|
||||
// The first Method is very specific to this implementation
|
||||
// ************************************************
|
||||
|
||||
#ifdef COVARIANCE_PREDICTION_GENERAL
|
||||
|
||||
void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
|
||||
float Q[NUMW], float dT, float P[NUMX][NUMX])
|
||||
{
|
||||
float Dummy[NUMX][NUMX], dTsq;
|
||||
uint8_t i, j, k;
|
||||
// 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')]
|
||||
|
||||
// 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')]
|
||||
const float dT1 = 1.0f / dT; // multiplication is faster than division on fpu.
|
||||
const float dTsq = dT * dT;
|
||||
|
||||
dTsq = dT * dT;
|
||||
float Dummy[NUMX][NUMX];
|
||||
int8_t i;
|
||||
int8_t k;
|
||||
|
||||
for (i = 0; i < NUMX; i++) { // Calculate Dummy = (P/T +F*P)
|
||||
float *Firow = F[i];
|
||||
float *Pirow = P[i];
|
||||
float *Dirow = Dummy[i];
|
||||
const int8_t Fistart = FrowMin[i];
|
||||
const int8_t Fiend = FrowMax[i];
|
||||
int8_t j;
|
||||
|
||||
for (j = 0; j < NUMX; j++) {
|
||||
Dummy[i][j] = P[i][j] / dT;
|
||||
for (k = 0; k < NUMX; k++) {
|
||||
Dummy[i][j] += F[i][k] * P[k][j];
|
||||
Dirow[j] = Pirow[j] * dT1; // Dummy = P / T ...
|
||||
}
|
||||
for (k = Fistart; k <= Fiend; k++) {
|
||||
for (j = 0; j < NUMX; j++) {
|
||||
Dirow[j] += Firow[k] * P[k][j]; // [] + F * P
|
||||
}
|
||||
}
|
||||
}
|
||||
for (i = 0; i < NUMX; i++) { // Calculate Pnew = Dummy/T + Dummy*F' + G*Qw*G'
|
||||
for (i = 0; i < NUMX; i++) { // Calculate Pnew = (T^2) [Dummy/T + Dummy*F' + G*Qw*G']
|
||||
float *Dirow = Dummy[i];
|
||||
float *Girow = G[i];
|
||||
float *Pirow = P[i];
|
||||
const int8_t Gistart = GrowMin[i];
|
||||
const int8_t Giend = GrowMax[i];
|
||||
int8_t j;
|
||||
|
||||
|
||||
for (j = i; j < NUMX; j++) { // Use symmetry, ie only find upper triangular
|
||||
P[i][j] = Dummy[i][j] / dT;
|
||||
for (k = 0; k < NUMX; k++) {
|
||||
P[i][j] += Dummy[i][k] * F[j][k]; // P = Dummy/T + Dummy*F'
|
||||
float Ptmp = Dirow[j] * dT1; // Pnew = Dummy / T ...
|
||||
|
||||
const float *Fjrow = F[j];
|
||||
int8_t Fjstart = FrowMin[j];
|
||||
int8_t Fjend = FrowMax[j];
|
||||
k = Fjstart;
|
||||
|
||||
while (k <= Fjend - 3) {
|
||||
Ptmp += Dirow[k] * Fjrow[k]; // [] + Dummy*F' ...
|
||||
Ptmp += Dirow[k + 1] * Fjrow[k + 1];
|
||||
Ptmp += Dirow[k + 2] * Fjrow[k + 2];
|
||||
Ptmp += Dirow[k + 3] * Fjrow[k + 3];
|
||||
k += 4;
|
||||
}
|
||||
for (k = 0; k < NUMW; k++) {
|
||||
P[i][j] += Q[k] * G[i][k] * G[j][k]; // P = Dummy/T + Dummy*F' + G*Q*G'
|
||||
while (k <= Fjend) {
|
||||
Ptmp += Dirow[k] * Fjrow[k];
|
||||
k++;
|
||||
}
|
||||
P[j][i] = P[i][j] = P[i][j] * dTsq; // Pnew = T^2*P and fill in lower triangular;
|
||||
|
||||
float *Gjrow = G[j];
|
||||
const int8_t Gjstart = MAX(Gistart, GrowMin[j]);
|
||||
const int8_t Gjend = MIN(Giend, GrowMax[j]);
|
||||
k = Gjstart;
|
||||
while (k <= Gjend - 2) {
|
||||
Ptmp += Q[k] * Girow[k] * Gjrow[k]; // [] + G*Q*G' ...
|
||||
Ptmp += Q[k + 1] * Girow[k + 1] * Gjrow[k + 1];
|
||||
Ptmp += Q[k + 2] * Girow[k + 2] * Gjrow[k + 2];
|
||||
k += 3;
|
||||
}
|
||||
if (k <= Gjend) {
|
||||
Ptmp += Q[k] * Girow[k] * Gjrow[k];
|
||||
if (k <= Gjend - 1) {
|
||||
Ptmp += Q[k + 1] * Girow[k + 1] * Gjrow[k + 1];
|
||||
}
|
||||
}
|
||||
|
||||
P[j][i] = Pirow[j] = Ptmp * dTsq; // [] * (T^2)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#else /* ifdef COVARIANCE_PREDICTION_GENERAL */
|
||||
|
||||
void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
|
||||
float Q[NUMW], float dT, float P[NUMX][NUMX])
|
||||
{
|
||||
float D[NUMX][NUMX], T, Tsq;
|
||||
uint8_t i, j;
|
||||
|
||||
// Pnew = (I+F*T)*P*(I+F*T)' + T^2*G*Q*G' = scalar expansion from symbolic manipulator
|
||||
|
||||
T = dT;
|
||||
Tsq = dT * dT;
|
||||
|
||||
for (i = 0; i < NUMX; i++) { // Create a copy of the upper triangular of P
|
||||
for (j = i; j < NUMX; j++) {
|
||||
D[i][j] = P[i][j];
|
||||
}
|
||||
}
|
||||
|
||||
// Brute force calculation of the elements of P
|
||||
P[0][0] = D[3][3] * Tsq + (2 * D[0][3]) * T + D[0][0];
|
||||
P[0][1] = P[1][0] = D[3][4] * Tsq + (D[0][4] + D[1][3]) * T + D[0][1];
|
||||
P[0][2] = P[2][0] = D[3][5] * Tsq + (D[0][5] + D[2][3]) * T + D[0][2];
|
||||
P[0][3] = P[3][0] = (F[3][6] * D[3][6] + F[3][7] * D[3][7] + F[3][8] * D[3][8] + F[3][9] * D[3][9] + F[3][13] * D[3][13]) * Tsq + (D[3][3] + F[3][6] * D[0][6] + F[3][7] * D[0][7] + F[3][8] * D[0][8] + F[3][9] * D[0][9] + F[3][13] * D[0][13]) * T + D[0][3];
|
||||
P[0][4] = P[4][0] = (F[4][6] * D[3][6] + F[4][7] * D[3][7] + F[4][8] * D[3][8] + F[4][9] * D[3][9] + F[4][13] * D[3][13]) * Tsq + (D[3][4] + F[4][6] * D[0][6] + F[4][7] * D[0][7] + F[4][8] * D[0][8] + F[4][9] * D[0][9] + F[4][13] * D[0][13]) * T + D[0][4];
|
||||
P[0][5] = P[5][0] = (F[5][6] * D[3][6] + F[5][7] * D[3][7] + F[5][8] * D[3][8] + F[5][9] * D[3][9] + F[5][13] * D[3][13]) * Tsq + (D[3][5] + F[5][6] * D[0][6] + F[5][7] * D[0][7] + F[5][8] * D[0][8] + F[5][9] * D[0][9] + F[5][13] * D[0][13]) * T + D[0][5];
|
||||
P[0][6] = P[6][0] = (F[6][7] * D[3][7] + F[6][8] * D[3][8] + F[6][9] * D[3][9] + F[6][10] * D[3][10] + F[6][11] * D[3][11] + F[6][12] * D[3][12]) * Tsq + (D[3][6] + F[6][7] * D[0][7] + F[6][8] * D[0][8] + F[6][9] * D[0][9] + F[6][10] * D[0][10] + F[6][11] * D[0][11] + F[6][12] * D[0][12]) * T + D[0][6];
|
||||
P[0][7] = P[7][0] = (F[7][6] * D[3][6] + F[7][8] * D[3][8] + F[7][9] * D[3][9] + F[7][10] * D[3][10] + F[7][11] * D[3][11] + F[7][12] * D[3][12]) * Tsq + (D[3][7] + F[7][6] * D[0][6] + F[7][8] * D[0][8] + F[7][9] * D[0][9] + F[7][10] * D[0][10] + F[7][11] * D[0][11] + F[7][12] * D[0][12]) * T + D[0][7];
|
||||
P[0][8] = P[8][0] = (F[8][6] * D[3][6] + F[8][7] * D[3][7] + F[8][9] * D[3][9] + F[8][10] * D[3][10] + F[8][11] * D[3][11] + F[8][12] * D[3][12]) * Tsq + (D[3][8] + F[8][6] * D[0][6] + F[8][7] * D[0][7] + F[8][9] * D[0][9] + F[8][10] * D[0][10] + F[8][11] * D[0][11] + F[8][12] * D[0][12]) * T + D[0][8];
|
||||
P[0][9] = P[9][0] = (F[9][6] * D[3][6] + F[9][7] * D[3][7] + F[9][8] * D[3][8] + F[9][10] * D[3][10] + F[9][11] * D[3][11] + F[9][12] * D[3][12]) * Tsq + (D[3][9] + F[9][6] * D[0][6] + F[9][7] * D[0][7] + F[9][8] * D[0][8] + F[9][10] * D[0][10] + F[9][11] * D[0][11] + F[9][12] * D[0][12]) * T + D[0][9];
|
||||
P[0][10] = P[10][0] = D[3][10] * T + D[0][10];
|
||||
P[0][11] = P[11][0] = D[3][11] * T + D[0][11];
|
||||
P[0][12] = P[12][0] = D[3][12] * T + D[0][12];
|
||||
P[0][13] = P[13][0] = D[3][13] * T + D[0][13];
|
||||
P[1][1] = D[4][4] * Tsq + (2 * D[1][4]) * T + D[1][1];
|
||||
P[1][2] = P[2][1] = D[4][5] * Tsq + (D[1][5] + D[2][4]) * T + D[1][2];
|
||||
P[1][3] = P[3][1] = (F[3][6] * D[4][6] + F[3][7] * D[4][7] + F[3][8] * D[4][8] + F[3][9] * D[4][9] + F[3][13] * D[4][13]) * Tsq + (D[3][4] + F[3][6] * D[1][6] + F[3][7] * D[1][7] + F[3][8] * D[1][8] + F[3][9] * D[1][9] + F[3][13] * D[1][13]) * T + D[1][3];
|
||||
P[1][4] = P[4][1] = (F[4][6] * D[4][6] + F[4][7] * D[4][7] + F[4][8] * D[4][8] + F[4][9] * D[4][9] + F[4][13] * D[4][13]) * Tsq + (D[4][4] + F[4][6] * D[1][6] + F[4][7] * D[1][7] + F[4][8] * D[1][8] + F[4][9] * D[1][9] + F[4][13] * D[1][13]) * T + D[1][4];
|
||||
P[1][5] = P[5][1] = (F[5][6] * D[4][6] + F[5][7] * D[4][7] + F[5][8] * D[4][8] + F[5][9] * D[4][9] + F[5][13] * D[4][13]) * Tsq + (D[4][5] + F[5][6] * D[1][6] + F[5][7] * D[1][7] + F[5][8] * D[1][8] + F[5][9] * D[1][9] + F[5][13] * D[1][13]) * T + D[1][5];
|
||||
P[1][6] = P[6][1] = (F[6][7] * D[4][7] + F[6][8] * D[4][8] + F[6][9] * D[4][9] + F[6][10] * D[4][10] + F[6][11] * D[4][11] + F[6][12] * D[4][12]) * Tsq + (D[4][6] + F[6][7] * D[1][7] + F[6][8] * D[1][8] + F[6][9] * D[1][9] + F[6][10] * D[1][10] + F[6][11] * D[1][11] + F[6][12] * D[1][12]) * T + D[1][6];
|
||||
P[1][7] = P[7][1] = (F[7][6] * D[4][6] + F[7][8] * D[4][8] + F[7][9] * D[4][9] + F[7][10] * D[4][10] + F[7][11] * D[4][11] + F[7][12] * D[4][12]) * Tsq + (D[4][7] + F[7][6] * D[1][6] + F[7][8] * D[1][8] + F[7][9] * D[1][9] + F[7][10] * D[1][10] + F[7][11] * D[1][11] + F[7][12] * D[1][12]) * T + D[1][7];
|
||||
P[1][8] = P[8][1] = (F[8][6] * D[4][6] + F[8][7] * D[4][7] + F[8][9] * D[4][9] + F[8][10] * D[4][10] + F[8][11] * D[4][11] + F[8][12] * D[4][12]) * Tsq + (D[4][8] + F[8][6] * D[1][6] + F[8][7] * D[1][7] + F[8][9] * D[1][9] + F[8][10] * D[1][10] + F[8][11] * D[1][11] + F[8][12] * D[1][12]) * T + D[1][8];
|
||||
P[1][9] = P[9][1] = (F[9][6] * D[4][6] + F[9][7] * D[4][7] + F[9][8] * D[4][8] + F[9][10] * D[4][10] + F[9][11] * D[4][11] + F[9][12] * D[4][12]) * Tsq + (D[4][9] + F[9][6] * D[1][6] + F[9][7] * D[1][7] + F[9][8] * D[1][8] + F[9][10] * D[1][10] + F[9][11] * D[1][11] + F[9][12] * D[1][12]) * T + D[1][9];
|
||||
P[1][10] = P[10][1] = D[4][10] * T + D[1][10];
|
||||
P[1][11] = P[11][1] = D[4][11] * T + D[1][11];
|
||||
P[1][12] = P[12][1] = D[4][12] * T + D[1][12];
|
||||
P[1][13] = P[13][1] = D[4][13] * T + D[1][13];
|
||||
P[2][2] = D[5][5] * Tsq + (2 * D[2][5]) * T + D[2][2];
|
||||
P[2][3] = P[3][2] = (F[3][6] * D[5][6] + F[3][7] * D[5][7] + F[3][8] * D[5][8] + F[3][9] * D[5][9] + F[3][13] * D[5][13]) * Tsq + (D[3][5] + F[3][6] * D[2][6] + F[3][7] * D[2][7] + F[3][8] * D[2][8] + F[3][9] * D[2][9] + F[3][13] * D[2][13]) * T + D[2][3];
|
||||
P[2][4] = P[4][2] = (F[4][6] * D[5][6] + F[4][7] * D[5][7] + F[4][8] * D[5][8] + F[4][9] * D[5][9] + F[4][13] * D[5][13]) * Tsq + (D[4][5] + F[4][6] * D[2][6] + F[4][7] * D[2][7] + F[4][8] * D[2][8] + F[4][9] * D[2][9] + F[4][13] * D[2][13]) * T + D[2][4];
|
||||
P[2][5] = P[5][2] = (F[5][6] * D[5][6] + F[5][7] * D[5][7] + F[5][8] * D[5][8] + F[5][9] * D[5][9] + F[5][13] * D[5][13]) * Tsq + (D[5][5] + F[5][6] * D[2][6] + F[5][7] * D[2][7] + F[5][8] * D[2][8] + F[5][9] * D[2][9] + F[5][13] * D[2][13]) * T + D[2][5];
|
||||
P[2][6] = P[6][2] = (F[6][7] * D[5][7] + F[6][8] * D[5][8] + F[6][9] * D[5][9] + F[6][10] * D[5][10] + F[6][11] * D[5][11] + F[6][12] * D[5][12]) * Tsq + (D[5][6] + F[6][7] * D[2][7] + F[6][8] * D[2][8] + F[6][9] * D[2][9] + F[6][10] * D[2][10] + F[6][11] * D[2][11] + F[6][12] * D[2][12]) * T + D[2][6];
|
||||
P[2][7] = P[7][2] = (F[7][6] * D[5][6] + F[7][8] * D[5][8] + F[7][9] * D[5][9] + F[7][10] * D[5][10] + F[7][11] * D[5][11] + F[7][12] * D[5][12]) * Tsq + (D[5][7] + F[7][6] * D[2][6] + F[7][8] * D[2][8] + F[7][9] * D[2][9] + F[7][10] * D[2][10] + F[7][11] * D[2][11] + F[7][12] * D[2][12]) * T + D[2][7];
|
||||
P[2][8] = P[8][2] = (F[8][6] * D[5][6] + F[8][7] * D[5][7] + F[8][9] * D[5][9] + F[8][10] * D[5][10] + F[8][11] * D[5][11] + F[8][12] * D[5][12]) * Tsq + (D[5][8] + F[8][6] * D[2][6] + F[8][7] * D[2][7] + F[8][9] * D[2][9] + F[8][10] * D[2][10] + F[8][11] * D[2][11] + F[8][12] * D[2][12]) * T + D[2][8];
|
||||
P[2][9] = P[9][2] = (F[9][6] * D[5][6] + F[9][7] * D[5][7] + F[9][8] * D[5][8] + F[9][10] * D[5][10] + F[9][11] * D[5][11] + F[9][12] * D[5][12]) * Tsq + (D[5][9] + F[9][6] * D[2][6] + F[9][7] * D[2][7] + F[9][8] * D[2][8] + F[9][10] * D[2][10] + F[9][11] * D[2][11] + F[9][12] * D[2][12]) * T + D[2][9];
|
||||
P[2][10] = P[10][2] = D[5][10] * T + D[2][10];
|
||||
P[2][11] = P[11][2] = D[5][11] * T + D[2][11];
|
||||
P[2][12] = P[12][2] = D[5][12] * T + D[2][12];
|
||||
P[2][13] = P[13][2] = D[5][13] * T + D[2][13];
|
||||
P[3][3] = (Q[3] * G[3][3] * G[3][3] + Q[4] * G[3][4] * G[3][4] + Q[5] * G[3][5] * G[3][5] + F[3][6] * (F[3][6] * D[6][6] + F[3][7] * D[6][7] + F[3][8] * D[6][8] + F[3][9] * D[6][9] + F[3][13] * D[6][13]) + F[3][7] * (F[3][6] * D[6][7] + F[3][7] * D[7][7] + F[3][8] * D[7][8] + F[3][9] * D[7][9] + F[3][13] * D[7][13]) + F[3][8] * (F[3][6] * D[6][8] + F[3][7] * D[7][8] + F[3][8] * D[8][8] + F[3][9] * D[8][9] + F[3][13] * D[8][13]) + F[3][9] * (F[3][6] * D[6][9] + F[3][7] * D[7][9] + F[3][8] * D[8][9] + F[3][9] * D[9][9] + F[3][13] * D[9][13]) + F[3][13] * (F[3][6] * D[6][13] + F[3][7] * D[7][13] + F[3][8] * D[8][13] + F[3][9] * D[9][13] + F[3][13] * D[13][13])) * Tsq + (2 * F[3][6] * D[3][6] + 2 * F[3][7] * D[3][7] + 2 * F[3][8] * D[3][8] + 2 * F[3][9] * D[3][9] + 2 * F[3][13] * D[3][13]) * T + D[3][3];
|
||||
P[3][4] = P[4][3] = (F[4][6] * (F[3][6] * D[6][6] + F[3][7] * D[6][7] + F[3][8] * D[6][8] + F[3][9] * D[6][9] + F[3][13] * D[6][13]) + F[4][7] * (F[3][6] * D[6][7] + F[3][7] * D[7][7] + F[3][8] * D[7][8] + F[3][9] * D[7][9] + F[3][13] * D[7][13]) + F[4][8] * (F[3][6] * D[6][8] + F[3][7] * D[7][8] + F[3][8] * D[8][8] + F[3][9] * D[8][9] + F[3][13] * D[8][13]) + F[4][9] * (F[3][6] * D[6][9] + F[3][7] * D[7][9] + F[3][8] * D[8][9] + F[3][9] * D[9][9] + F[3][13] * D[9][13]) + F[4][13] * (F[3][6] * D[6][13] + F[3][7] * D[7][13] + F[3][8] * D[8][13] + F[3][9] * D[9][13] + F[3][13] * D[13][13]) + G[3][3] * G[4][3] * Q[3] + G[3][4] * G[4][4] * Q[4] + G[3][5] * G[4][5] * Q[5]) * Tsq + (F[3][6] * D[4][6] + F[4][6] * D[3][6] + F[3][7] * D[4][7] + F[4][7] * D[3][7] + F[3][8] * D[4][8] + F[4][8] * D[3][8] + F[3][9] * D[4][9] + F[4][9] * D[3][9] + F[3][13] * D[4][13] + F[4][13] * D[3][13]) * T + D[3][4];
|
||||
P[3][5] = P[5][3] = (F[5][6] * (F[3][6] * D[6][6] + F[3][7] * D[6][7] + F[3][8] * D[6][8] + F[3][9] * D[6][9] + F[3][13] * D[6][13]) + F[5][7] * (F[3][6] * D[6][7] + F[3][7] * D[7][7] + F[3][8] * D[7][8] + F[3][9] * D[7][9] + F[3][13] * D[7][13]) + F[5][8] * (F[3][6] * D[6][8] + F[3][7] * D[7][8] + F[3][8] * D[8][8] + F[3][9] * D[8][9] + F[3][13] * D[8][13]) + F[5][9] * (F[3][6] * D[6][9] + F[3][7] * D[7][9] + F[3][8] * D[8][9] + F[3][9] * D[9][9] + F[3][13] * D[9][13]) + F[5][13] * (F[3][6] * D[6][13] + F[3][7] * D[7][13] + F[3][8] * D[8][13] + F[3][9] * D[9][13] + F[3][13] * D[13][13]) + G[3][3] * G[5][3] * Q[3] + G[3][4] * G[5][4] * Q[4] + G[3][5] * G[5][5] * Q[5]) * Tsq + (F[3][6] * D[5][6] + F[5][6] * D[3][6] + F[3][7] * D[5][7] + F[5][7] * D[3][7] + F[3][8] * D[5][8] + F[5][8] * D[3][8] + F[3][9] * D[5][9] + F[5][9] * D[3][9] + F[3][13] * D[5][13] + F[5][13] * D[3][13]) * T + D[3][5];
|
||||
P[3][6] = P[6][3] = (F[6][7] * (F[3][6] * D[6][7] + F[3][7] * D[7][7] + F[3][8] * D[7][8] + F[3][9] * D[7][9] + F[3][13] * D[7][13]) + F[6][8] * (F[3][6] * D[6][8] + F[3][7] * D[7][8] + F[3][8] * D[8][8] + F[3][9] * D[8][9] + F[3][13] * D[8][13]) + F[6][9] * (F[3][6] * D[6][9] + F[3][7] * D[7][9] + F[3][8] * D[8][9] + F[3][9] * D[9][9] + F[3][13] * D[9][13]) + F[6][10] * (F[3][6] * D[6][10] + F[3][7] * D[7][10] + F[3][8] * D[8][10] + F[3][9] * D[9][10] + F[3][13] * D[10][13]) + F[6][11] * (F[3][6] * D[6][11] + F[3][7] * D[7][11] + F[3][8] * D[8][11] + F[3][9] * D[9][11] + F[3][13] * D[11][13]) + F[6][12] * (F[3][6] * D[6][12] + F[3][7] * D[7][12] + F[3][8] * D[8][12] + F[3][9] * D[9][12] + F[3][13] * D[12][13])) * Tsq + (F[3][6] * D[6][6] + F[3][7] * D[6][7] + F[6][7] * D[3][7] + F[3][8] * D[6][8] + F[6][8] * D[3][8] + F[3][9] * D[6][9] + F[6][9] * D[3][9] + F[6][10] * D[3][10] + F[6][11] * D[3][11] + F[6][12] * D[3][12] + F[3][13] * D[6][13]) * T + D[3][6];
|
||||
P[3][7] = P[7][3] = (F[7][6] * (F[3][6] * D[6][6] + F[3][7] * D[6][7] + F[3][8] * D[6][8] + F[3][9] * D[6][9] + F[3][13] * D[6][13]) + F[7][8] * (F[3][6] * D[6][8] + F[3][7] * D[7][8] + F[3][8] * D[8][8] + F[3][9] * D[8][9] + F[3][13] * D[8][13]) + F[7][9] * (F[3][6] * D[6][9] + F[3][7] * D[7][9] + F[3][8] * D[8][9] + F[3][9] * D[9][9] + F[3][13] * D[9][13]) + F[7][10] * (F[3][6] * D[6][10] + F[3][7] * D[7][10] + F[3][8] * D[8][10] + F[3][9] * D[9][10] + F[3][13] * D[10][13]) + F[7][11] * (F[3][6] * D[6][11] + F[3][7] * D[7][11] + F[3][8] * D[8][11] + F[3][9] * D[9][11] + F[3][13] * D[11][13]) + F[7][12] * (F[3][6] * D[6][12] + F[3][7] * D[7][12] + F[3][8] * D[8][12] + F[3][9] * D[9][12] + F[3][13] * D[12][13])) * Tsq + (F[3][6] * D[6][7] + F[7][6] * D[3][6] + F[3][7] * D[7][7] + F[3][8] * D[7][8] + F[7][8] * D[3][8] + F[3][9] * D[7][9] + F[7][9] * D[3][9] + F[7][10] * D[3][10] + F[7][11] * D[3][11] + F[7][12] * D[3][12] + F[3][13] * D[7][13]) * T + D[3][7];
|
||||
P[3][8] = P[8][3] = (F[8][6] * (F[3][6] * D[6][6] + F[3][7] * D[6][7] + F[3][8] * D[6][8] + F[3][9] * D[6][9] + F[3][13] * D[6][13]) + F[8][7] * (F[3][6] * D[6][7] + F[3][7] * D[7][7] + F[3][8] * D[7][8] + F[3][9] * D[7][9] + F[3][13] * D[7][13]) + F[8][9] * (F[3][6] * D[6][9] + F[3][7] * D[7][9] + F[3][8] * D[8][9] + F[3][9] * D[9][9] + F[3][13] * D[9][13]) + F[8][10] * (F[3][6] * D[6][10] + F[3][7] * D[7][10] + F[3][8] * D[8][10] + F[3][9] * D[9][10] + F[3][13] * D[10][13]) + F[8][11] * (F[3][6] * D[6][11] + F[3][7] * D[7][11] + F[3][8] * D[8][11] + F[3][9] * D[9][11] + F[3][13] * D[11][13]) + F[8][12] * (F[3][6] * D[6][12] + F[3][7] * D[7][12] + F[3][8] * D[8][12] + F[3][9] * D[9][12] + F[3][13] * D[12][13])) * Tsq + (F[3][6] * D[6][8] + F[3][7] * D[7][8] + F[8][6] * D[3][6] + F[8][7] * D[3][7] + F[3][8] * D[8][8] + F[3][9] * D[8][9] + F[8][9] * D[3][9] + F[8][10] * D[3][10] + F[8][11] * D[3][11] + F[8][12] * D[3][12] + F[3][13] * D[8][13]) * T + D[3][8];
|
||||
P[3][9] = P[9][3] = (F[9][6] * (F[3][6] * D[6][6] + F[3][7] * D[6][7] + F[3][8] * D[6][8] + F[3][9] * D[6][9] + F[3][13] * D[6][13]) + F[9][7] * (F[3][6] * D[6][7] + F[3][7] * D[7][7] + F[3][8] * D[7][8] + F[3][9] * D[7][9] + F[3][13] * D[7][13]) + F[9][8] * (F[3][6] * D[6][8] + F[3][7] * D[7][8] + F[3][8] * D[8][8] + F[3][9] * D[8][9] + F[3][13] * D[8][13]) + F[9][10] * (F[3][6] * D[6][10] + F[3][7] * D[7][10] + F[3][8] * D[8][10] + F[3][9] * D[9][10] + F[3][13] * D[10][13]) + F[9][11] * (F[3][6] * D[6][11] + F[3][7] * D[7][11] + F[3][8] * D[8][11] + F[3][9] * D[9][11] + F[3][13] * D[11][13]) + F[9][12] * (F[3][6] * D[6][12] + F[3][7] * D[7][12] + F[3][8] * D[8][12] + F[3][9] * D[9][12] + F[3][13] * D[12][13])) * Tsq + (F[9][6] * D[3][6] + F[9][7] * D[3][7] + F[9][8] * D[3][8] + F[3][6] * D[6][9] + F[3][7] * D[7][9] + F[3][8] * D[8][9] + F[3][9] * D[9][9] + F[9][10] * D[3][10] + F[9][11] * D[3][11] + F[9][12] * D[3][12] + F[3][13] * D[9][13]) * T + D[3][9];
|
||||
P[3][10] = P[10][3] = (F[3][6] * D[6][10] + F[3][7] * D[7][10] + F[3][8] * D[8][10] + F[3][9] * D[9][10] + F[3][13] * D[10][13]) * T + D[3][10];
|
||||
P[3][11] = P[11][3] = (F[3][6] * D[6][11] + F[3][7] * D[7][11] + F[3][8] * D[8][11] + F[3][9] * D[9][11] + F[3][13] * D[11][13]) * T + D[3][11];
|
||||
P[3][12] = P[12][3] = (F[3][6] * D[6][12] + F[3][7] * D[7][12] + F[3][8] * D[8][12] + F[3][9] * D[9][12] + F[3][13] * D[12][13]) * T + D[3][12];
|
||||
P[3][13] = P[13][3] = (F[3][6] * D[6][13] + F[3][7] * D[7][13] + F[3][8] * D[8][13] + F[3][9] * D[9][13] + F[3][13] * D[13][13]) * T + D[3][13];
|
||||
P[4][4] = (Q[3] * G[4][3] * G[4][3] + Q[4] * G[4][4] * G[4][4] + Q[5] * G[4][5] * G[4][5] + F[4][6] * (F[4][6] * D[6][6] + F[4][7] * D[6][7] + F[4][8] * D[6][8] + F[4][9] * D[6][9] + F[4][13] * D[6][13]) + F[4][7] * (F[4][6] * D[6][7] + F[4][7] * D[7][7] + F[4][8] * D[7][8] + F[4][9] * D[7][9] + F[4][13] * D[7][13]) + F[4][8] * (F[4][6] * D[6][8] + F[4][7] * D[7][8] + F[4][8] * D[8][8] + F[4][9] * D[8][9] + F[4][13] * D[8][13]) + F[4][9] * (F[4][6] * D[6][9] + F[4][7] * D[7][9] + F[4][8] * D[8][9] + F[4][9] * D[9][9] + F[4][13] * D[9][13]) + F[4][13] * (F[4][6] * D[6][13] + F[4][7] * D[7][13] + F[4][8] * D[8][13] + F[4][9] * D[9][13] + F[4][13] * D[13][13])) * Tsq + (2 * F[4][6] * D[4][6] + 2 * F[4][7] * D[4][7] + 2 * F[4][8] * D[4][8] + 2 * F[4][9] * D[4][9] + 2 * F[4][13] * D[4][13]) * T + D[4][4];
|
||||
P[4][5] = P[5][4] = (F[5][6] * (F[4][6] * D[6][6] + F[4][7] * D[6][7] + F[4][8] * D[6][8] + F[4][9] * D[6][9] + F[4][13] * D[6][13]) + F[5][7] * (F[4][6] * D[6][7] + F[4][7] * D[7][7] + F[4][8] * D[7][8] + F[4][9] * D[7][9] + F[4][13] * D[7][13]) + F[5][8] * (F[4][6] * D[6][8] + F[4][7] * D[7][8] + F[4][8] * D[8][8] + F[4][9] * D[8][9] + F[4][13] * D[8][13]) + F[5][9] * (F[4][6] * D[6][9] + F[4][7] * D[7][9] + F[4][8] * D[8][9] + F[4][9] * D[9][9] + F[4][13] * D[9][13]) + F[5][13] * (F[4][6] * D[6][13] + F[4][7] * D[7][13] + F[4][8] * D[8][13] + F[4][9] * D[9][13] + F[4][13] * D[13][13]) + G[4][3] * G[5][3] * Q[3] + G[4][4] * G[5][4] * Q[4] + G[4][5] * G[5][5] * Q[5]) * Tsq + (F[4][6] * D[5][6] + F[5][6] * D[4][6] + F[4][7] * D[5][7] + F[5][7] * D[4][7] + F[4][8] * D[5][8] + F[5][8] * D[4][8] + F[4][9] * D[5][9] + F[5][9] * D[4][9] + F[4][13] * D[5][13] + F[5][13] * D[4][13]) * T + D[4][5];
|
||||
P[4][6] = P[6][4] = (F[6][7] * (F[4][6] * D[6][7] + F[4][7] * D[7][7] + F[4][8] * D[7][8] + F[4][9] * D[7][9] + F[4][13] * D[7][13]) + F[6][8] * (F[4][6] * D[6][8] + F[4][7] * D[7][8] + F[4][8] * D[8][8] + F[4][9] * D[8][9] + F[4][13] * D[8][13]) + F[6][9] * (F[4][6] * D[6][9] + F[4][7] * D[7][9] + F[4][8] * D[8][9] + F[4][9] * D[9][9] + F[4][13] * D[9][13]) + F[6][10] * (F[4][6] * D[6][10] + F[4][7] * D[7][10] + F[4][8] * D[8][10] + F[4][9] * D[9][10] + F[4][13] * D[10][13]) + F[6][11] * (F[4][6] * D[6][11] + F[4][7] * D[7][11] + F[4][8] * D[8][11] + F[4][9] * D[9][11] + F[4][13] * D[11][13]) + F[6][12] * (F[4][6] * D[6][12] + F[4][7] * D[7][12] + F[4][8] * D[8][12] + F[4][9] * D[9][12] + F[4][13] * D[12][13])) * Tsq + (F[4][6] * D[6][6] + F[4][7] * D[6][7] + F[6][7] * D[4][7] + F[4][8] * D[6][8] + F[6][8] * D[4][8] + F[4][9] * D[6][9] + F[6][9] * D[4][9] + F[6][10] * D[4][10] + F[6][11] * D[4][11] + F[6][12] * D[4][12] + F[4][13] * D[6][13]) * T + D[4][6];
|
||||
P[4][7] = P[7][4] = (F[7][6] * (F[4][6] * D[6][6] + F[4][7] * D[6][7] + F[4][8] * D[6][8] + F[4][9] * D[6][9] + F[4][13] * D[6][13]) + F[7][8] * (F[4][6] * D[6][8] + F[4][7] * D[7][8] + F[4][8] * D[8][8] + F[4][9] * D[8][9] + F[4][13] * D[8][13]) + F[7][9] * (F[4][6] * D[6][9] + F[4][7] * D[7][9] + F[4][8] * D[8][9] + F[4][9] * D[9][9] + F[4][13] * D[9][13]) + F[7][10] * (F[4][6] * D[6][10] + F[4][7] * D[7][10] + F[4][8] * D[8][10] + F[4][9] * D[9][10] + F[4][13] * D[10][13]) + F[7][11] * (F[4][6] * D[6][11] + F[4][7] * D[7][11] + F[4][8] * D[8][11] + F[4][9] * D[9][11] + F[4][13] * D[11][13]) + F[7][12] * (F[4][6] * D[6][12] + F[4][7] * D[7][12] + F[4][8] * D[8][12] + F[4][9] * D[9][12] + F[4][13] * D[12][13])) * Tsq + (F[4][6] * D[6][7] + F[7][6] * D[4][6] + F[4][7] * D[7][7] + F[4][8] * D[7][8] + F[7][8] * D[4][8] + F[4][9] * D[7][9] + F[7][9] * D[4][9] + F[7][10] * D[4][10] + F[7][11] * D[4][11] + F[7][12] * D[4][12] + F[4][13] * D[7][13]) * T + D[4][7];
|
||||
P[4][8] = P[8][4] = (F[8][6] * (F[4][6] * D[6][6] + F[4][7] * D[6][7] + F[4][8] * D[6][8] + F[4][9] * D[6][9] + F[4][13] * D[6][13]) + F[8][7] * (F[4][6] * D[6][7] + F[4][7] * D[7][7] + F[4][8] * D[7][8] + F[4][9] * D[7][9] + F[4][13] * D[7][13]) + F[8][9] * (F[4][6] * D[6][9] + F[4][7] * D[7][9] + F[4][8] * D[8][9] + F[4][9] * D[9][9] + F[4][13] * D[9][13]) + F[8][10] * (F[4][6] * D[6][10] + F[4][7] * D[7][10] + F[4][8] * D[8][10] + F[4][9] * D[9][10] + F[4][13] * D[10][13]) + F[8][11] * (F[4][6] * D[6][11] + F[4][7] * D[7][11] + F[4][8] * D[8][11] + F[4][9] * D[9][11] + F[4][13] * D[11][13]) + F[8][12] * (F[4][6] * D[6][12] + F[4][7] * D[7][12] + F[4][8] * D[8][12] + F[4][9] * D[9][12] + F[4][13] * D[12][13])) * Tsq + (F[4][6] * D[6][8] + F[4][7] * D[7][8] + F[8][6] * D[4][6] + F[8][7] * D[4][7] + F[4][8] * D[8][8] + F[4][9] * D[8][9] + F[8][9] * D[4][9] + F[8][10] * D[4][10] + F[8][11] * D[4][11] + F[8][12] * D[4][12] + F[4][13] * D[8][13]) * T + D[4][8];
|
||||
P[4][9] = P[9][4] = (F[9][6] * (F[4][6] * D[6][6] + F[4][7] * D[6][7] + F[4][8] * D[6][8] + F[4][9] * D[6][9] + F[4][13] * D[6][13]) + F[9][7] * (F[4][6] * D[6][7] + F[4][7] * D[7][7] + F[4][8] * D[7][8] + F[4][9] * D[7][9] + F[4][13] * D[7][13]) + F[9][8] * (F[4][6] * D[6][8] + F[4][7] * D[7][8] + F[4][8] * D[8][8] + F[4][9] * D[8][9] + F[4][13] * D[8][13]) + F[9][10] * (F[4][6] * D[6][10] + F[4][7] * D[7][10] + F[4][8] * D[8][10] + F[4][9] * D[9][10] + F[4][13] * D[10][13]) + F[9][11] * (F[4][6] * D[6][11] + F[4][7] * D[7][11] + F[4][8] * D[8][11] + F[4][9] * D[9][11] + F[4][13] * D[11][13]) + F[9][12] * (F[4][6] * D[6][12] + F[4][7] * D[7][12] + F[4][8] * D[8][12] + F[4][9] * D[9][12] + F[4][13] * D[12][13])) * Tsq + (F[9][6] * D[4][6] + F[9][7] * D[4][7] + F[9][8] * D[4][8] + F[4][6] * D[6][9] + F[4][7] * D[7][9] + F[4][8] * D[8][9] + F[4][9] * D[9][9] + F[9][10] * D[4][10] + F[9][11] * D[4][11] + F[9][12] * D[4][12] + F[4][13] * D[9][13]) * T + D[4][9];
|
||||
P[4][10] = P[10][4] = (F[4][6] * D[6][10] + F[4][7] * D[7][10] + F[4][8] * D[8][10] + F[4][9] * D[9][10] + F[4][13] * D[10][13]) * T + D[4][10];
|
||||
P[4][11] = P[11][4] = (F[4][6] * D[6][11] + F[4][7] * D[7][11] + F[4][8] * D[8][11] + F[4][9] * D[9][11] + F[4][13] * D[11][13]) * T + D[4][11];
|
||||
P[4][12] = P[12][4] = (F[4][6] * D[6][12] + F[4][7] * D[7][12] + F[4][8] * D[8][12] + F[4][9] * D[9][12] + F[4][13] * D[12][13]) * T + D[4][12];
|
||||
P[4][13] = P[13][4] = (F[4][6] * D[6][13] + F[4][7] * D[7][13] + F[4][8] * D[8][13] + F[4][9] * D[9][13] + F[4][13] * D[13][13]) * T + D[4][13];
|
||||
P[5][5] = (Q[3] * G[5][3] * G[5][3] + Q[4] * G[5][4] * G[5][4] + Q[5] * G[5][5] * G[5][5] + F[5][6] * (F[5][6] * D[6][6] + F[5][7] * D[6][7] + F[5][8] * D[6][8] + F[5][9] * D[6][9] + F[5][13] * D[6][13]) + F[5][7] * (F[5][6] * D[6][7] + F[5][7] * D[7][7] + F[5][8] * D[7][8] + F[5][9] * D[7][9] + F[5][13] * D[7][13]) + F[5][8] * (F[5][6] * D[6][8] + F[5][7] * D[7][8] + F[5][8] * D[8][8] + F[5][9] * D[8][9] + F[5][13] * D[8][13]) + F[5][9] * (F[5][6] * D[6][9] + F[5][7] * D[7][9] + F[5][8] * D[8][9] + F[5][9] * D[9][9] + F[5][13] * D[9][13]) + F[5][13] * (F[5][6] * D[6][13] + F[5][7] * D[7][13] + F[5][8] * D[8][13] + F[5][9] * D[9][13] + F[5][13] * D[13][13])) * Tsq + (2 * F[5][6] * D[5][6] + 2 * F[5][7] * D[5][7] + 2 * F[5][8] * D[5][8] + 2 * F[5][9] * D[5][9] + 2 * F[5][13] * D[5][13]) * T + D[5][5];
|
||||
P[5][6] = P[6][5] = (F[6][7] * (F[5][6] * D[6][7] + F[5][7] * D[7][7] + F[5][8] * D[7][8] + F[5][9] * D[7][9] + F[5][13] * D[7][13]) + F[6][8] * (F[5][6] * D[6][8] + F[5][7] * D[7][8] + F[5][8] * D[8][8] + F[5][9] * D[8][9] + F[5][13] * D[8][13]) + F[6][9] * (F[5][6] * D[6][9] + F[5][7] * D[7][9] + F[5][8] * D[8][9] + F[5][9] * D[9][9] + F[5][13] * D[9][13]) + F[6][10] * (F[5][6] * D[6][10] + F[5][7] * D[7][10] + F[5][8] * D[8][10] + F[5][9] * D[9][10] + F[5][13] * D[10][13]) + F[6][11] * (F[5][6] * D[6][11] + F[5][7] * D[7][11] + F[5][8] * D[8][11] + F[5][9] * D[9][11] + F[5][13] * D[11][13]) + F[6][12] * (F[5][6] * D[6][12] + F[5][7] * D[7][12] + F[5][8] * D[8][12] + F[5][9] * D[9][12] + F[5][13] * D[12][13])) * Tsq + (F[5][6] * D[6][6] + F[5][7] * D[6][7] + F[6][7] * D[5][7] + F[5][8] * D[6][8] + F[6][8] * D[5][8] + F[5][9] * D[6][9] + F[6][9] * D[5][9] + F[6][10] * D[5][10] + F[6][11] * D[5][11] + F[6][12] * D[5][12] + F[5][13] * D[6][13]) * T + D[5][6];
|
||||
P[5][7] = P[7][5] = (F[7][6] * (F[5][6] * D[6][6] + F[5][7] * D[6][7] + F[5][8] * D[6][8] + F[5][9] * D[6][9] + F[5][13] * D[6][13]) + F[7][8] * (F[5][6] * D[6][8] + F[5][7] * D[7][8] + F[5][8] * D[8][8] + F[5][9] * D[8][9] + F[5][13] * D[8][13]) + F[7][9] * (F[5][6] * D[6][9] + F[5][7] * D[7][9] + F[5][8] * D[8][9] + F[5][9] * D[9][9] + F[5][13] * D[9][13]) + F[7][10] * (F[5][6] * D[6][10] + F[5][7] * D[7][10] + F[5][8] * D[8][10] + F[5][9] * D[9][10] + F[5][13] * D[10][13]) + F[7][11] * (F[5][6] * D[6][11] + F[5][7] * D[7][11] + F[5][8] * D[8][11] + F[5][9] * D[9][11] + F[5][13] * D[11][13]) + F[7][12] * (F[5][6] * D[6][12] + F[5][7] * D[7][12] + F[5][8] * D[8][12] + F[5][9] * D[9][12] + F[5][13] * D[12][13])) * Tsq + (F[5][6] * D[6][7] + F[7][6] * D[5][6] + F[5][7] * D[7][7] + F[5][8] * D[7][8] + F[7][8] * D[5][8] + F[5][9] * D[7][9] + F[7][9] * D[5][9] + F[7][10] * D[5][10] + F[7][11] * D[5][11] + F[7][12] * D[5][12] + F[5][13] * D[7][13]) * T + D[5][7];
|
||||
P[5][8] = P[8][5] = (F[8][6] * (F[5][6] * D[6][6] + F[5][7] * D[6][7] + F[5][8] * D[6][8] + F[5][9] * D[6][9] + F[5][13] * D[6][13]) + F[8][7] * (F[5][6] * D[6][7] + F[5][7] * D[7][7] + F[5][8] * D[7][8] + F[5][9] * D[7][9] + F[5][13] * D[7][13]) + F[8][9] * (F[5][6] * D[6][9] + F[5][7] * D[7][9] + F[5][8] * D[8][9] + F[5][9] * D[9][9] + F[5][13] * D[9][13]) + F[8][10] * (F[5][6] * D[6][10] + F[5][7] * D[7][10] + F[5][8] * D[8][10] + F[5][9] * D[9][10] + F[5][13] * D[10][13]) + F[8][11] * (F[5][6] * D[6][11] + F[5][7] * D[7][11] + F[5][8] * D[8][11] + F[5][9] * D[9][11] + F[5][13] * D[11][13]) + F[8][12] * (F[5][6] * D[6][12] + F[5][7] * D[7][12] + F[5][8] * D[8][12] + F[5][9] * D[9][12] + F[5][13] * D[12][13])) * Tsq + (F[5][6] * D[6][8] + F[5][7] * D[7][8] + F[8][6] * D[5][6] + F[8][7] * D[5][7] + F[5][8] * D[8][8] + F[5][9] * D[8][9] + F[8][9] * D[5][9] + F[8][10] * D[5][10] + F[8][11] * D[5][11] + F[8][12] * D[5][12] + F[5][13] * D[8][13]) * T + D[5][8];
|
||||
P[5][9] = P[9][5] = (F[9][6] * (F[5][6] * D[6][6] + F[5][7] * D[6][7] + F[5][8] * D[6][8] + F[5][9] * D[6][9] + F[5][13] * D[6][13]) + F[9][7] * (F[5][6] * D[6][7] + F[5][7] * D[7][7] + F[5][8] * D[7][8] + F[5][9] * D[7][9] + F[5][13] * D[7][13]) + F[9][8] * (F[5][6] * D[6][8] + F[5][7] * D[7][8] + F[5][8] * D[8][8] + F[5][9] * D[8][9] + F[5][13] * D[8][13]) + F[9][10] * (F[5][6] * D[6][10] + F[5][7] * D[7][10] + F[5][8] * D[8][10] + F[5][9] * D[9][10] + F[5][13] * D[10][13]) + F[9][11] * (F[5][6] * D[6][11] + F[5][7] * D[7][11] + F[5][8] * D[8][11] + F[5][9] * D[9][11] + F[5][13] * D[11][13]) + F[9][12] * (F[5][6] * D[6][12] + F[5][7] * D[7][12] + F[5][8] * D[8][12] + F[5][9] * D[9][12] + F[5][13] * D[12][13])) * Tsq + (F[9][6] * D[5][6] + F[9][7] * D[5][7] + F[9][8] * D[5][8] + F[5][6] * D[6][9] + F[5][7] * D[7][9] + F[5][8] * D[8][9] + F[5][9] * D[9][9] + F[9][10] * D[5][10] + F[9][11] * D[5][11] + F[9][12] * D[5][12] + F[5][13] * D[9][13]) * T + D[5][9];
|
||||
P[5][10] = P[10][5] = (F[5][6] * D[6][10] + F[5][7] * D[7][10] + F[5][8] * D[8][10] + F[5][9] * D[9][10] + F[5][13] * D[10][13]) * T + D[5][10];
|
||||
P[5][11] = P[11][5] = (F[5][6] * D[6][11] + F[5][7] * D[7][11] + F[5][8] * D[8][11] + F[5][9] * D[9][11] + F[5][13] * D[11][13]) * T + D[5][11];
|
||||
P[5][12] = P[12][5] = (F[5][6] * D[6][12] + F[5][7] * D[7][12] + F[5][8] * D[8][12] + F[5][9] * D[9][12] + F[5][13] * D[12][13]) * T + D[5][12];
|
||||
P[5][13] = P[13][5] = (F[5][6] * D[6][13] + F[5][7] * D[7][13] + F[5][8] * D[8][13] + F[5][9] * D[9][13] + F[5][13] * D[13][13]) * T + D[5][13];
|
||||
P[6][6] = (Q[0] * G[6][0] * G[6][0] + Q[1] * G[6][1] * G[6][1] + Q[2] * G[6][2] * G[6][2] + F[6][7] * (F[6][7] * D[7][7] + F[6][8] * D[7][8] + F[6][9] * D[7][9] + F[6][10] * D[7][10] + F[6][11] * D[7][11] + F[6][12] * D[7][12]) + F[6][8] * (F[6][7] * D[7][8] + F[6][8] * D[8][8] + F[6][9] * D[8][9] + F[6][10] * D[8][10] + F[6][11] * D[8][11] + F[6][12] * D[8][12]) + F[6][9] * (F[6][7] * D[7][9] + F[6][8] * D[8][9] + F[6][9] * D[9][9] + F[6][10] * D[9][10] + F[6][11] * D[9][11] + F[6][12] * D[9][12]) + F[6][10] * (F[6][7] * D[7][10] + F[6][8] * D[8][10] + F[6][9] * D[9][10] + F[6][10] * D[10][10] + F[6][11] * D[10][11] + F[6][12] * D[10][12]) + F[6][11] * (F[6][7] * D[7][11] + F[6][8] * D[8][11] + F[6][9] * D[9][11] + F[6][10] * D[10][11] + F[6][11] * D[11][11] + F[6][12] * D[11][12]) + F[6][12] * (F[6][7] * D[7][12] + F[6][8] * D[8][12] + F[6][9] * D[9][12] + F[6][10] * D[10][12] + F[6][11] * D[11][12] + F[6][12] * D[12][12])) * Tsq + (2 * F[6][7] * D[6][7] + 2 * F[6][8] * D[6][8] + 2 * F[6][9] * D[6][9] + 2 * F[6][10] * D[6][10] + 2 * F[6][11] * D[6][11] + 2 * F[6][12] * D[6][12]) * T + D[6][6];
|
||||
P[6][7] = P[7][6] = (F[7][6] * (F[6][7] * D[6][7] + F[6][8] * D[6][8] + F[6][9] * D[6][9] + F[6][10] * D[6][10] + F[6][11] * D[6][11] + F[6][12] * D[6][12]) + F[7][8] * (F[6][7] * D[7][8] + F[6][8] * D[8][8] + F[6][9] * D[8][9] + F[6][10] * D[8][10] + F[6][11] * D[8][11] + F[6][12] * D[8][12]) + F[7][9] * (F[6][7] * D[7][9] + F[6][8] * D[8][9] + F[6][9] * D[9][9] + F[6][10] * D[9][10] + F[6][11] * D[9][11] + F[6][12] * D[9][12]) + F[7][10] * (F[6][7] * D[7][10] + F[6][8] * D[8][10] + F[6][9] * D[9][10] + F[6][10] * D[10][10] + F[6][11] * D[10][11] + F[6][12] * D[10][12]) + F[7][11] * (F[6][7] * D[7][11] + F[6][8] * D[8][11] + F[6][9] * D[9][11] + F[6][10] * D[10][11] + F[6][11] * D[11][11] + F[6][12] * D[11][12]) + F[7][12] * (F[6][7] * D[7][12] + F[6][8] * D[8][12] + F[6][9] * D[9][12] + F[6][10] * D[10][12] + F[6][11] * D[11][12] + F[6][12] * D[12][12]) + G[6][0] * G[7][0] * Q[0] + G[6][1] * G[7][1] * Q[1] + G[6][2] * G[7][2] * Q[2]) * Tsq + (F[7][6] * D[6][6] + F[6][7] * D[7][7] + F[6][8] * D[7][8] + F[7][8] * D[6][8] + F[6][9] * D[7][9] + F[7][9] * D[6][9] + F[6][10] * D[7][10] + F[7][10] * D[6][10] + F[6][11] * D[7][11] + F[7][11] * D[6][11] + F[6][12] * D[7][12] + F[7][12] * D[6][12]) * T + D[6][7];
|
||||
P[6][8] = P[8][6] = (F[8][6] * (F[6][7] * D[6][7] + F[6][8] * D[6][8] + F[6][9] * D[6][9] + F[6][10] * D[6][10] + F[6][11] * D[6][11] + F[6][12] * D[6][12]) + F[8][7] * (F[6][7] * D[7][7] + F[6][8] * D[7][8] + F[6][9] * D[7][9] + F[6][10] * D[7][10] + F[6][11] * D[7][11] + F[6][12] * D[7][12]) + F[8][9] * (F[6][7] * D[7][9] + F[6][8] * D[8][9] + F[6][9] * D[9][9] + F[6][10] * D[9][10] + F[6][11] * D[9][11] + F[6][12] * D[9][12]) + F[8][10] * (F[6][7] * D[7][10] + F[6][8] * D[8][10] + F[6][9] * D[9][10] + F[6][10] * D[10][10] + F[6][11] * D[10][11] + F[6][12] * D[10][12]) + F[8][11] * (F[6][7] * D[7][11] + F[6][8] * D[8][11] + F[6][9] * D[9][11] + F[6][10] * D[10][11] + F[6][11] * D[11][11] + F[6][12] * D[11][12]) + F[8][12] * (F[6][7] * D[7][12] + F[6][8] * D[8][12] + F[6][9] * D[9][12] + F[6][10] * D[10][12] + F[6][11] * D[11][12] + F[6][12] * D[12][12]) + G[6][0] * G[8][0] * Q[0] + G[6][1] * G[8][1] * Q[1] + G[6][2] * G[8][2] * Q[2]) * Tsq + (F[6][7] * D[7][8] + F[8][6] * D[6][6] + F[8][7] * D[6][7] + F[6][8] * D[8][8] + F[6][9] * D[8][9] + F[8][9] * D[6][9] + F[6][10] * D[8][10] + F[8][10] * D[6][10] + F[6][11] * D[8][11] + F[8][11] * D[6][11] + F[6][12] * D[8][12] + F[8][12] * D[6][12]) * T + D[6][8];
|
||||
P[6][9] = P[9][6] = (F[9][6] * (F[6][7] * D[6][7] + F[6][8] * D[6][8] + F[6][9] * D[6][9] + F[6][10] * D[6][10] + F[6][11] * D[6][11] + F[6][12] * D[6][12]) + F[9][7] * (F[6][7] * D[7][7] + F[6][8] * D[7][8] + F[6][9] * D[7][9] + F[6][10] * D[7][10] + F[6][11] * D[7][11] + F[6][12] * D[7][12]) + F[9][8] * (F[6][7] * D[7][8] + F[6][8] * D[8][8] + F[6][9] * D[8][9] + F[6][10] * D[8][10] + F[6][11] * D[8][11] + F[6][12] * D[8][12]) + F[9][10] * (F[6][7] * D[7][10] + F[6][8] * D[8][10] + F[6][9] * D[9][10] + F[6][10] * D[10][10] + F[6][11] * D[10][11] + F[6][12] * D[10][12]) + F[9][11] * (F[6][7] * D[7][11] + F[6][8] * D[8][11] + F[6][9] * D[9][11] + F[6][10] * D[10][11] + F[6][11] * D[11][11] + F[6][12] * D[11][12]) + F[9][12] * (F[6][7] * D[7][12] + F[6][8] * D[8][12] + F[6][9] * D[9][12] + F[6][10] * D[10][12] + F[6][11] * D[11][12] + F[6][12] * D[12][12]) + G[6][0] * G[9][0] * Q[0] + G[6][1] * G[9][1] * Q[1] + G[6][2] * G[9][2] * Q[2]) * Tsq + (F[9][6] * D[6][6] + F[9][7] * D[6][7] + F[9][8] * D[6][8] + F[6][7] * D[7][9] + F[6][8] * D[8][9] + F[6][9] * D[9][9] + F[6][10] * D[9][10] + F[9][10] * D[6][10] + F[6][11] * D[9][11] + F[9][11] * D[6][11] + F[6][12] * D[9][12] + F[9][12] * D[6][12]) * T + D[6][9];
|
||||
P[6][10] = P[10][6] = (F[6][7] * D[7][10] + F[6][8] * D[8][10] + F[6][9] * D[9][10] + F[6][10] * D[10][10] + F[6][11] * D[10][11] + F[6][12] * D[10][12]) * T + D[6][10];
|
||||
P[6][11] = P[11][6] = (F[6][7] * D[7][11] + F[6][8] * D[8][11] + F[6][9] * D[9][11] + F[6][10] * D[10][11] + F[6][11] * D[11][11] + F[6][12] * D[11][12]) * T + D[6][11];
|
||||
P[6][12] = P[12][6] = (F[6][7] * D[7][12] + F[6][8] * D[8][12] + F[6][9] * D[9][12] + F[6][10] * D[10][12] + F[6][11] * D[11][12] + F[6][12] * D[12][12]) * T + D[6][12];
|
||||
P[6][13] = P[13][6] = (F[6][7] * D[7][13] + F[6][8] * D[8][13] + F[6][9] * D[9][13] + F[6][10] * D[10][13] + F[6][11] * D[11][13] + F[6][12] * D[12][13]) * T + D[6][13];
|
||||
P[7][7] = (Q[0] * G[7][0] * G[7][0] + Q[1] * G[7][1] * G[7][1] + Q[2] * G[7][2] * G[7][2] + F[7][6] * (F[7][6] * D[6][6] + F[7][8] * D[6][8] + F[7][9] * D[6][9] + F[7][10] * D[6][10] + F[7][11] * D[6][11] + F[7][12] * D[6][12]) + F[7][8] * (F[7][6] * D[6][8] + F[7][8] * D[8][8] + F[7][9] * D[8][9] + F[7][10] * D[8][10] + F[7][11] * D[8][11] + F[7][12] * D[8][12]) + F[7][9] * (F[7][6] * D[6][9] + F[7][8] * D[8][9] + F[7][9] * D[9][9] + F[7][10] * D[9][10] + F[7][11] * D[9][11] + F[7][12] * D[9][12]) + F[7][10] * (F[7][6] * D[6][10] + F[7][8] * D[8][10] + F[7][9] * D[9][10] + F[7][10] * D[10][10] + F[7][11] * D[10][11] + F[7][12] * D[10][12]) + F[7][11] * (F[7][6] * D[6][11] + F[7][8] * D[8][11] + F[7][9] * D[9][11] + F[7][10] * D[10][11] + F[7][11] * D[11][11] + F[7][12] * D[11][12]) + F[7][12] * (F[7][6] * D[6][12] + F[7][8] * D[8][12] + F[7][9] * D[9][12] + F[7][10] * D[10][12] + F[7][11] * D[11][12] + F[7][12] * D[12][12])) * Tsq + (2 * F[7][6] * D[6][7] + 2 * F[7][8] * D[7][8] + 2 * F[7][9] * D[7][9] + 2 * F[7][10] * D[7][10] + 2 * F[7][11] * D[7][11] + 2 * F[7][12] * D[7][12]) * T + D[7][7];
|
||||
P[7][8] = P[8][7] = (F[8][6] * (F[7][6] * D[6][6] + F[7][8] * D[6][8] + F[7][9] * D[6][9] + F[7][10] * D[6][10] + F[7][11] * D[6][11] + F[7][12] * D[6][12]) + F[8][7] * (F[7][6] * D[6][7] + F[7][8] * D[7][8] + F[7][9] * D[7][9] + F[7][10] * D[7][10] + F[7][11] * D[7][11] + F[7][12] * D[7][12]) + F[8][9] * (F[7][6] * D[6][9] + F[7][8] * D[8][9] + F[7][9] * D[9][9] + F[7][10] * D[9][10] + F[7][11] * D[9][11] + F[7][12] * D[9][12]) + F[8][10] * (F[7][6] * D[6][10] + F[7][8] * D[8][10] + F[7][9] * D[9][10] + F[7][10] * D[10][10] + F[7][11] * D[10][11] + F[7][12] * D[10][12]) + F[8][11] * (F[7][6] * D[6][11] + F[7][8] * D[8][11] + F[7][9] * D[9][11] + F[7][10] * D[10][11] + F[7][11] * D[11][11] + F[7][12] * D[11][12]) + F[8][12] * (F[7][6] * D[6][12] + F[7][8] * D[8][12] + F[7][9] * D[9][12] + F[7][10] * D[10][12] + F[7][11] * D[11][12] + F[7][12] * D[12][12]) + G[7][0] * G[8][0] * Q[0] + G[7][1] * G[8][1] * Q[1] + G[7][2] * G[8][2] * Q[2]) * Tsq + (F[7][6] * D[6][8] + F[8][6] * D[6][7] + F[8][7] * D[7][7] + F[7][8] * D[8][8] + F[7][9] * D[8][9] + F[8][9] * D[7][9] + F[7][10] * D[8][10] + F[8][10] * D[7][10] + F[7][11] * D[8][11] + F[8][11] * D[7][11] + F[7][12] * D[8][12] + F[8][12] * D[7][12]) * T + D[7][8];
|
||||
P[7][9] = P[9][7] = (F[9][6] * (F[7][6] * D[6][6] + F[7][8] * D[6][8] + F[7][9] * D[6][9] + F[7][10] * D[6][10] + F[7][11] * D[6][11] + F[7][12] * D[6][12]) + F[9][7] * (F[7][6] * D[6][7] + F[7][8] * D[7][8] + F[7][9] * D[7][9] + F[7][10] * D[7][10] + F[7][11] * D[7][11] + F[7][12] * D[7][12]) + F[9][8] * (F[7][6] * D[6][8] + F[7][8] * D[8][8] + F[7][9] * D[8][9] + F[7][10] * D[8][10] + F[7][11] * D[8][11] + F[7][12] * D[8][12]) + F[9][10] * (F[7][6] * D[6][10] + F[7][8] * D[8][10] + F[7][9] * D[9][10] + F[7][10] * D[10][10] + F[7][11] * D[10][11] + F[7][12] * D[10][12]) + F[9][11] * (F[7][6] * D[6][11] + F[7][8] * D[8][11] + F[7][9] * D[9][11] + F[7][10] * D[10][11] + F[7][11] * D[11][11] + F[7][12] * D[11][12]) + F[9][12] * (F[7][6] * D[6][12] + F[7][8] * D[8][12] + F[7][9] * D[9][12] + F[7][10] * D[10][12] + F[7][11] * D[11][12] + F[7][12] * D[12][12]) + G[7][0] * G[9][0] * Q[0] + G[7][1] * G[9][1] * Q[1] + G[7][2] * G[9][2] * Q[2]) * Tsq + (F[9][6] * D[6][7] + F[9][7] * D[7][7] + F[9][8] * D[7][8] + F[7][6] * D[6][9] + F[7][8] * D[8][9] + F[7][9] * D[9][9] + F[7][10] * D[9][10] + F[9][10] * D[7][10] + F[7][11] * D[9][11] + F[9][11] * D[7][11] + F[7][12] * D[9][12] + F[9][12] * D[7][12]) * T + D[7][9];
|
||||
P[7][10] = P[10][7] = (F[7][6] * D[6][10] + F[7][8] * D[8][10] + F[7][9] * D[9][10] + F[7][10] * D[10][10] + F[7][11] * D[10][11] + F[7][12] * D[10][12]) * T + D[7][10];
|
||||
P[7][11] = P[11][7] = (F[7][6] * D[6][11] + F[7][8] * D[8][11] + F[7][9] * D[9][11] + F[7][10] * D[10][11] + F[7][11] * D[11][11] + F[7][12] * D[11][12]) * T + D[7][11];
|
||||
P[7][12] = P[12][7] = (F[7][6] * D[6][12] + F[7][8] * D[8][12] + F[7][9] * D[9][12] + F[7][10] * D[10][12] + F[7][11] * D[11][12] + F[7][12] * D[12][12]) * T + D[7][12];
|
||||
P[7][13] = P[13][7] = (F[7][6] * D[6][13] + F[7][8] * D[8][13] + F[7][9] * D[9][13] + F[7][10] * D[10][13] + F[7][11] * D[11][13] + F[7][12] * D[12][13]) * T + D[7][13];
|
||||
P[8][8] = (Q[0] * G[8][0] * G[8][0] + Q[1] * G[8][1] * G[8][1] + Q[2] * G[8][2] * G[8][2] + F[8][6] * (F[8][6] * D[6][6] + F[8][7] * D[6][7] + F[8][9] * D[6][9] + F[8][10] * D[6][10] + F[8][11] * D[6][11] + F[8][12] * D[6][12]) + F[8][7] * (F[8][6] * D[6][7] + F[8][7] * D[7][7] + F[8][9] * D[7][9] + F[8][10] * D[7][10] + F[8][11] * D[7][11] + F[8][12] * D[7][12]) + F[8][9] * (F[8][6] * D[6][9] + F[8][7] * D[7][9] + F[8][9] * D[9][9] + F[8][10] * D[9][10] + F[8][11] * D[9][11] + F[8][12] * D[9][12]) + F[8][10] * (F[8][6] * D[6][10] + F[8][7] * D[7][10] + F[8][9] * D[9][10] + F[8][10] * D[10][10] + F[8][11] * D[10][11] + F[8][12] * D[10][12]) + F[8][11] * (F[8][6] * D[6][11] + F[8][7] * D[7][11] + F[8][9] * D[9][11] + F[8][10] * D[10][11] + F[8][11] * D[11][11] + F[8][12] * D[11][12]) + F[8][12] * (F[8][6] * D[6][12] + F[8][7] * D[7][12] + F[8][9] * D[9][12] + F[8][10] * D[10][12] + F[8][11] * D[11][12] + F[8][12] * D[12][12])) * Tsq + (2 * F[8][6] * D[6][8] + 2 * F[8][7] * D[7][8] + 2 * F[8][9] * D[8][9] + 2 * F[8][10] * D[8][10] + 2 * F[8][11] * D[8][11] + 2 * F[8][12] * D[8][12]) * T + D[8][8];
|
||||
P[8][9] = P[9][8] = (F[9][6] * (F[8][6] * D[6][6] + F[8][7] * D[6][7] + F[8][9] * D[6][9] + F[8][10] * D[6][10] + F[8][11] * D[6][11] + F[8][12] * D[6][12]) + F[9][7] * (F[8][6] * D[6][7] + F[8][7] * D[7][7] + F[8][9] * D[7][9] + F[8][10] * D[7][10] + F[8][11] * D[7][11] + F[8][12] * D[7][12]) + F[9][8] * (F[8][6] * D[6][8] + F[8][7] * D[7][8] + F[8][9] * D[8][9] + F[8][10] * D[8][10] + F[8][11] * D[8][11] + F[8][12] * D[8][12]) + F[9][10] * (F[8][6] * D[6][10] + F[8][7] * D[7][10] + F[8][9] * D[9][10] + F[8][10] * D[10][10] + F[8][11] * D[10][11] + F[8][12] * D[10][12]) + F[9][11] * (F[8][6] * D[6][11] + F[8][7] * D[7][11] + F[8][9] * D[9][11] + F[8][10] * D[10][11] + F[8][11] * D[11][11] + F[8][12] * D[11][12]) + F[9][12] * (F[8][6] * D[6][12] + F[8][7] * D[7][12] + F[8][9] * D[9][12] + F[8][10] * D[10][12] + F[8][11] * D[11][12] + F[8][12] * D[12][12]) + G[8][0] * G[9][0] * Q[0] + G[8][1] * G[9][1] * Q[1] + G[8][2] * G[9][2] * Q[2]) * Tsq + (F[9][6] * D[6][8] + F[9][7] * D[7][8] + F[9][8] * D[8][8] + F[8][6] * D[6][9] + F[8][7] * D[7][9] + F[8][9] * D[9][9] + F[8][10] * D[9][10] + F[9][10] * D[8][10] + F[8][11] * D[9][11] + F[9][11] * D[8][11] + F[8][12] * D[9][12] + F[9][12] * D[8][12]) * T + D[8][9];
|
||||
P[8][10] = P[10][8] = (F[8][6] * D[6][10] + F[8][7] * D[7][10] + F[8][9] * D[9][10] + F[8][10] * D[10][10] + F[8][11] * D[10][11] + F[8][12] * D[10][12]) * T + D[8][10];
|
||||
P[8][11] = P[11][8] = (F[8][6] * D[6][11] + F[8][7] * D[7][11] + F[8][9] * D[9][11] + F[8][10] * D[10][11] + F[8][11] * D[11][11] + F[8][12] * D[11][12]) * T + D[8][11];
|
||||
P[8][12] = P[12][8] = (F[8][6] * D[6][12] + F[8][7] * D[7][12] + F[8][9] * D[9][12] + F[8][10] * D[10][12] + F[8][11] * D[11][12] + F[8][12] * D[12][12]) * T + D[8][12];
|
||||
P[8][13] = P[13][8] = (F[8][6] * D[6][13] + F[8][7] * D[7][13] + F[8][9] * D[9][13] + F[8][10] * D[10][13] + F[8][11] * D[11][13] + F[8][12] * D[12][13]) * T + D[8][13];
|
||||
P[9][9] = (Q[0] * G[9][0] * G[9][0] + Q[1] * G[9][1] * G[9][1] + Q[2] * G[9][2] * G[9][2] + F[9][6] * (F[9][6] * D[6][6] + F[9][7] * D[6][7] + F[9][8] * D[6][8] + F[9][10] * D[6][10] + F[9][11] * D[6][11] + F[9][12] * D[6][12]) + F[9][7] * (F[9][6] * D[6][7] + F[9][7] * D[7][7] + F[9][8] * D[7][8] + F[9][10] * D[7][10] + F[9][11] * D[7][11] + F[9][12] * D[7][12]) + F[9][8] * (F[9][6] * D[6][8] + F[9][7] * D[7][8] + F[9][8] * D[8][8] + F[9][10] * D[8][10] + F[9][11] * D[8][11] + F[9][12] * D[8][12]) + F[9][10] * (F[9][6] * D[6][10] + F[9][7] * D[7][10] + F[9][8] * D[8][10] + F[9][10] * D[10][10] + F[9][11] * D[10][11] + F[9][12] * D[10][12]) + F[9][11] * (F[9][6] * D[6][11] + F[9][7] * D[7][11] + F[9][8] * D[8][11] + F[9][10] * D[10][11] + F[9][11] * D[11][11] + F[9][12] * D[11][12]) + F[9][12] * (F[9][6] * D[6][12] + F[9][7] * D[7][12] + F[9][8] * D[8][12] + F[9][10] * D[10][12] + F[9][11] * D[11][12] + F[9][12] * D[12][12])) * Tsq + (2 * F[9][6] * D[6][9] + 2 * F[9][7] * D[7][9] + 2 * F[9][8] * D[8][9] + 2 * F[9][10] * D[9][10] + 2 * F[9][11] * D[9][11] + 2 * F[9][12] * D[9][12]) * T + D[9][9];
|
||||
P[9][10] = P[10][9] = (F[9][6] * D[6][10] + F[9][7] * D[7][10] + F[9][8] * D[8][10] + F[9][10] * D[10][10] + F[9][11] * D[10][11] + F[9][12] * D[10][12]) * T + D[9][10];
|
||||
P[9][11] = P[11][9] = (F[9][6] * D[6][11] + F[9][7] * D[7][11] + F[9][8] * D[8][11] + F[9][10] * D[10][11] + F[9][11] * D[11][11] + F[9][12] * D[11][12]) * T + D[9][11];
|
||||
P[9][12] = P[12][9] = (F[9][6] * D[6][12] + F[9][7] * D[7][12] + F[9][8] * D[8][12] + F[9][10] * D[10][12] + F[9][11] * D[11][12] + F[9][12] * D[12][12]) * T + D[9][12];
|
||||
P[9][13] = P[13][9] = (F[9][6] * D[6][13] + F[9][7] * D[7][13] + F[9][8] * D[8][13] + F[9][10] * D[10][13] + F[9][11] * D[11][13] + F[9][12] * D[12][13]) * T + D[9][13];
|
||||
P[10][10] = Q[6] * Tsq + D[10][10];
|
||||
P[10][11] = P[11][10] = D[10][11];
|
||||
P[10][12] = P[12][10] = D[10][12];
|
||||
P[10][13] = P[13][10] = D[10][13];
|
||||
P[11][11] = Q[7] * Tsq + D[11][11];
|
||||
P[11][12] = P[12][11] = D[11][12];
|
||||
P[11][13] = P[13][11] = D[11][13];
|
||||
P[12][12] = Q[8] * Tsq + D[12][12];
|
||||
P[12][13] = P[13][12] = D[12][13];
|
||||
P[13][13] = Q[9] * Tsq + D[13][13];
|
||||
}
|
||||
#endif /* ifdef COVARIANCE_PREDICTION_GENERAL */
|
||||
|
||||
// ************* SerialUpdate *******************
|
||||
// Does the update step of the Kalman filter for the covariance and estimate
|
||||
// Outputs are Xnew & Pnew, and are written over P and X
|
||||
@ -642,47 +588,47 @@ void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
|
||||
// The SensorsUsed variable is a bitwise mask indicating which sensors
|
||||
// should be used in the update.
|
||||
// ************************************************
|
||||
|
||||
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)
|
||||
{
|
||||
float HP[NUMX], HPHR, Error;
|
||||
uint8_t i, j, k, m;
|
||||
float Km[NUMX];
|
||||
|
||||
// Iterate through all the possible measurements and apply the
|
||||
// appropriate corrections
|
||||
for (m = 0; m < NUMV; m++) {
|
||||
if (SensorsUsed & (0x01 << m)) { // use this sensor for update
|
||||
for (j = 0; j < NUMX; j++) { // Find Hp = H*P
|
||||
HP[j] = 0.0f;
|
||||
for (k = 0; k < NUMX; k++) {
|
||||
HP[j] = 0;
|
||||
}
|
||||
|
||||
for (k = HrowMin[m]; k <= HrowMax[m]; k++) {
|
||||
for (j = 0; j < NUMX; j++) { // Find Hp = H*P
|
||||
HP[j] += H[m][k] * P[k][j];
|
||||
}
|
||||
}
|
||||
HPHR = R[m]; // Find HPHR = H*P*H' + R
|
||||
for (k = 0; k < NUMX; k++) {
|
||||
for (k = HrowMin[m]; k <= HrowMax[m]; k++) {
|
||||
HPHR += HP[k] * H[m][k];
|
||||
}
|
||||
|
||||
float invHPHR = 1.0f / HPHR;
|
||||
for (k = 0; k < NUMX; k++) {
|
||||
ekf.K[k][m] = HP[k] / HPHR; // find K = HP/HPHR
|
||||
Km[k] = HP[k] * invHPHR; // find K = HP/HPHR
|
||||
}
|
||||
for (i = 0; i < NUMX; i++) { // Find P(m)= P(m-1) + K*HP
|
||||
for (j = i; j < NUMX; j++) {
|
||||
P[i][j] = P[j][i] =
|
||||
P[i][j] - ekf.K[i][m] * HP[j];
|
||||
P[i][j] = P[j][i] = P[i][j] - Km[i] * HP[j];
|
||||
}
|
||||
}
|
||||
|
||||
Error = Z[m] - Y[m];
|
||||
for (i = 0; i < NUMX; i++) { // Find X(m)= X(m-1) + K*Error
|
||||
X[i] = X[i] + ekf.K[i][m] * Error;
|
||||
X[i] = X[i] + Km[i] * Error;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
INSLimitBias();
|
||||
}
|
||||
|
||||
// ************* RungeKutta **********************
|
||||
@ -694,8 +640,8 @@ void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
|
||||
|
||||
void RungeKutta(float X[NUMX], float U[NUMU], float dT)
|
||||
{
|
||||
float dT2 =
|
||||
dT / 2.0f, K1[NUMX], K2[NUMX], K3[NUMX], K4[NUMX], Xlast[NUMX];
|
||||
const float dT2 = dT / 2.0f;
|
||||
float K1[NUMX], K2[NUMX], K3[NUMX], K4[NUMX], Xlast[NUMX];
|
||||
uint8_t i;
|
||||
|
||||
for (i = 0; i < NUMX; i++) {
|
||||
@ -719,7 +665,7 @@ void RungeKutta(float X[NUMX], float U[NUMU], float dT)
|
||||
for (i = 0; i < NUMX; i++) {
|
||||
X[i] =
|
||||
Xlast[i] + dT * (K1[i] + 2.0f * K2[i] + 2.0f * K3[i] +
|
||||
K4[i]) / 6.0f;
|
||||
K4[i]) * (1.0f / 6.0f);
|
||||
}
|
||||
}
|
||||
|
||||
@ -873,23 +819,29 @@ void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX],
|
||||
F[9][12] = -q0 / 2.0f;
|
||||
|
||||
// dVdot/dna
|
||||
G[3][3] = -q0 * q0 - q1 * q1 + q2 * q2 + q3 * q3; G[3][4] = 2 * (-q1 * q2 + q0 * q3); G[3][5] = -2 * (q1 * q3 + q0 * q2);
|
||||
G[4][3] = -2 * (q1 * q2 + q0 * q3); G[4][4] = -q0 * q0 + q1 * q1 - q2 * q2 + q3 * q3; G[4][5] = 2 * (-q2 * q3 + q0 * q1);
|
||||
G[5][3] = 2 * (-q1 * q3 + q0 * q2); G[5][4] = -2 * (q2 * q3 + q0 * q1); G[5][5] = -q0 * q0 + q1 * q1 + q2 * q2 - q3 * q3;
|
||||
G[3][3] = -q0 * q0 - q1 * q1 + q2 * q2 + q3 * q3;
|
||||
G[3][4] = 2 * (-q1 * q2 + q0 * q3);
|
||||
// G[3][5] = -2 * (q1 * q3 + q0 * q2); // already assigned above
|
||||
G[4][3] = -2 * (q1 * q2 + q0 * q3);
|
||||
G[4][4] = -q0 * q0 + q1 * q1 - q2 * q2 + q3 * q3;
|
||||
// G[4][5] = 2 * (-q2 * q3 + q0 * q1); // already assigned above
|
||||
G[5][3] = 2 * (-q1 * q3 + q0 * q2);
|
||||
G[5][4] = -2 * (q2 * q3 + q0 * q1);
|
||||
// G[5][5] = -q0 * q0 + q1 * q1 + q2 * q2 - q3 * q3; // already assigned above
|
||||
|
||||
// dqdot/dnw
|
||||
G[6][0] = q1 / 2.0f;
|
||||
G[6][1] = q2 / 2.0f;
|
||||
G[6][2] = q3 / 2.0f;
|
||||
G[7][0] = -q0 / 2.0f;
|
||||
G[7][1] = q3 / 2.0f;
|
||||
G[7][2] = -q2 / 2.0f;
|
||||
G[8][0] = -q3 / 2.0f;
|
||||
G[8][1] = -q0 / 2.0f;
|
||||
G[8][2] = q1 / 2.0f;
|
||||
G[9][0] = q2 / 2.0f;
|
||||
G[9][1] = -q1 / 2.0f;
|
||||
G[9][2] = -q0 / 2.0f;
|
||||
G[6][0] = q1 / 2.0f;
|
||||
G[6][1] = q2 / 2.0f;
|
||||
G[6][2] = q3 / 2.0f;
|
||||
G[7][0] = -q0 / 2.0f;
|
||||
G[7][1] = q3 / 2.0f;
|
||||
G[7][2] = -q2 / 2.0f;
|
||||
G[8][0] = -q3 / 2.0f;
|
||||
G[8][1] = -q0 / 2.0f;
|
||||
G[8][2] = q1 / 2.0f;
|
||||
G[9][0] = q2 / 2.0f;
|
||||
G[9][1] = -q1 / 2.0f;
|
||||
G[9][2] = -q0 / 2.0f;
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
Loading…
Reference in New Issue
Block a user