mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-03-16 08:29:15 +01:00
AHRS: Merged in some new code from Dschin that expliclity writes the prediction terms in the covariance matrix. Also made changes to compile with -Os. Now runs around 125 - 150 Hz depending on corrections used (this will become adjustable from GCS shortly).
Squashed commit of the following:
commit 897e3365ab6c762584c34238efd12dc144e72af0
Author: James Cotton <peabody124@gmail.com>
Date: Mon Sep 13 22:15:21 2010 -0500
AHRS: Getting it running nice and quickly.
commit 53d72fb849164730c89873cab81c9e09be0b112b
Author: James Cotton <peabody124@gmail.com>
Date: Mon Sep 13 15:57:46 2010 -0500
AHRS: Make more variables volatile for -Os.
commit 4fa90821b5d3d8c2ea5e67756313a8a63c0eeb8c
Author: James Cotton <peabody124@gmail.com>
Date: Mon Sep 13 15:11:31 2010 -0500
AHRS/Makefile: -O0 not added to allow me to try various space flags that don't break code.
commit 5bd17a304dc28dc271c2f3e8c6cb8ad4a830404f
Author: James Cotton <peabody124@gmail.com>
Date: Mon Sep 13 14:44:05 2010 -0500
AHRS: Make debuggign USART easy to disable
commit 5453f2a7939492769fe9e9cc822e69c107320670
Author: James Cotton <peabody124@gmail.com>
Date: Mon Sep 13 14:38:37 2010 -0500
AHRS: Explicitly expand covariance prediction. Brings computations from 160 to 90. Makes it not fit in AHRS though.
commit b6712da7eece2a464a2073d24b77be22bfa47094
Author: James Cotton <peabody124@gmail.com>
Date: Sun Sep 12 22:59:42 2010 -0500
AHRS: Make it easy to comment out the I2C/Magnetometers.
commit 8c84d0091f3d6f6bedc7a0224a4ed9cb099b022b
Author: James Cotton <peabody124@gmail.com>
Date: Thu Sep 9 00:49:50 2010 -0500
AHRS: Small change to INSGPS analysis tools
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1613 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124
peabody124
parent
7da2fc2ba7
commit
1051d894b9
@ -190,11 +190,13 @@ LINKERSCRIPTPATH = $(PIOSSTM32F10X)
|
||||
# (Note: 3 is not always the best optimization level. See avr-libc FAQ.)
|
||||
|
||||
ifeq ($(DEBUG),YES)
|
||||
OPT = 0
|
||||
# Do nothing, default is -O0
|
||||
else
|
||||
OPT = s
|
||||
endif
|
||||
|
||||
# Now need -Os always for size reasons
|
||||
CFLAGS += -Os
|
||||
|
||||
# Output format. (can be ihex or binary or both)
|
||||
# binary to create a load-image in raw-binary format i.e. for SAM-BA,
|
||||
# ihex to create a load-image in Intel hex format
|
||||
@ -240,7 +242,8 @@ ifeq ($(DEBUG),YES)
|
||||
CFLAGS = -g$(DEBUGF)
|
||||
endif
|
||||
|
||||
CFLAGS += -O$(OPT)
|
||||
CFLAGS += -ffast-math
|
||||
|
||||
CFLAGS += -mcpu=$(MCU) -mthumb
|
||||
CFLAGS += $(CDEFS)
|
||||
CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) -I.
|
||||
|
||||
@ -46,7 +46,7 @@
|
||||
* @arg AHRS_PROCESSING - Performing update on the available data
|
||||
*/
|
||||
volatile enum {AHRS_IDLE, AHRS_DATA_READY, AHRS_PROCESSING} ahrs_state;
|
||||
enum algorithms ahrs_algorithm;
|
||||
volatile enum algorithms ahrs_algorithm;
|
||||
|
||||
/**
|
||||
* @addtogroup AHRS_ADC_Configuration ADC Configuration
|
||||
@ -69,7 +69,7 @@ void DMA1_Channel1_IRQHandler() __attribute__ ((alias ("AHRS_ADC_DMA_Handler")))
|
||||
* @{
|
||||
*/
|
||||
// Currently analog acquistion hard coded at 480 Hz
|
||||
#define ADC_OVERSAMPLE 48
|
||||
#define ADC_OVERSAMPLE 18
|
||||
#define EKF_RATE ((float) 4*480 / ADC_OVERSAMPLE)
|
||||
#define ADC_CONTINUOUS_CHANNELS PIOS_ADC_NUM_PINS
|
||||
#define CORRECTION_COUNT 4
|
||||
@ -155,12 +155,12 @@ struct gps_sensor {
|
||||
bool updated;
|
||||
};
|
||||
|
||||
static struct mag_sensor mag_data;
|
||||
static struct accel_sensor accel_data;
|
||||
static struct gyro_sensor gyro_data;
|
||||
static struct altitude_sensor altitude_data;
|
||||
static struct gps_sensor gps_data;
|
||||
static struct attitude_solution attitude_data;
|
||||
struct mag_sensor mag_data;
|
||||
volatile struct accel_sensor accel_data;
|
||||
volatile struct gyro_sensor gyro_data;
|
||||
volatile struct altitude_sensor altitude_data;
|
||||
struct gps_sensor gps_data;
|
||||
volatile struct attitude_solution attitude_data;
|
||||
|
||||
/**
|
||||
* @}
|
||||
@ -197,9 +197,9 @@ int16_t mag_bias[3] = {0,0,0};
|
||||
//! Filter coefficients used in decimation. Limited order so filter can't run between samples
|
||||
int16_t fir_coeffs[ADC_OVERSAMPLE+1];
|
||||
//! Raw buffer that DMA data is dumped into
|
||||
int16_t raw_data_buffer[ADC_CONTINUOUS_CHANNELS * ADC_OVERSAMPLE * 2]; // Double buffer that DMA just used
|
||||
volatile int16_t raw_data_buffer[ADC_CONTINUOUS_CHANNELS * ADC_OVERSAMPLE * 2]; // Double buffer that DMA just used
|
||||
//! Swapped by interrupt handler to achieve double buffering
|
||||
int16_t * valid_data_buffer;
|
||||
volatile int16_t * valid_data_buffer;
|
||||
//! Counts how many times the EKF wasn't idle when DMA handler called
|
||||
uint32_t ekf_too_slow = 0;
|
||||
//! Total number of data blocks converted
|
||||
@ -241,13 +241,14 @@ int main()
|
||||
/* ADC system */
|
||||
AHRS_ADC_Config(EKF_RATE, ADC_OVERSAMPLE);
|
||||
|
||||
/* Magnetic sensor system */
|
||||
PIOS_I2C_Init();
|
||||
PIOS_HMC5843_Init();
|
||||
|
||||
/* Setup the Accelerometer FS (Full-Scale) GPIO */
|
||||
PIOS_GPIO_Enable(0);
|
||||
SET_ACCEL_2G;
|
||||
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
|
||||
/* Magnetic sensor system */
|
||||
PIOS_I2C_Init();
|
||||
PIOS_HMC5843_Init();
|
||||
|
||||
/* Configure the HMC5843 Sensor */
|
||||
PIOS_HMC5843_ConfigTypeDef HMC5843_InitStructure;
|
||||
@ -260,6 +261,7 @@ int main()
|
||||
// Get 3 ID bytes
|
||||
strcpy ((char *)mag_data.id, "ZZZ");
|
||||
PIOS_HMC5843_ReadID(mag_data.id);
|
||||
#endif
|
||||
|
||||
/* SPI link to master */
|
||||
PIOS_SPI_Init();
|
||||
@ -310,7 +312,8 @@ int main()
|
||||
/******************* Main EKF loop ****************************/
|
||||
while (1) {
|
||||
// Alive signal
|
||||
PIOS_LED_Toggle(LED1);
|
||||
if((total_conversion_blocks % 100) == 0)
|
||||
PIOS_LED_Toggle(LED1);
|
||||
|
||||
if(calibration_pending)
|
||||
{
|
||||
@ -318,9 +321,10 @@ int main()
|
||||
calibration_pending = FALSE;
|
||||
}
|
||||
|
||||
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
|
||||
// Get magnetic readings
|
||||
PIOS_HMC5843_ReadMag(mag_data.raw.axis);
|
||||
|
||||
#endif
|
||||
// Delay for valid data
|
||||
idle_counter = 0;
|
||||
do {
|
||||
@ -387,6 +391,7 @@ int main()
|
||||
vel[0] = 0;
|
||||
vel[1] = 0;
|
||||
vel[2] = 0;
|
||||
|
||||
VelBaroCorrection(vel,altitude_data.altitude);
|
||||
// MagVelBaroCorrection(mag,vel,altitude_data.altitude); // only trust mags if outdoors
|
||||
}
|
||||
@ -496,7 +501,7 @@ void downsample_data()
|
||||
*/
|
||||
void calibrate_sensors() {
|
||||
int i;
|
||||
int16_t mag_raw[3];
|
||||
int16_t mag_raw[3] = {0,0,0};
|
||||
// local biases for noise analysis
|
||||
float accel_bias[3], gyro_bias[3], mag_bias[3];
|
||||
|
||||
@ -514,7 +519,9 @@ void calibrate_sensors() {
|
||||
accel_bias[0] += accel_data.filtered.x;
|
||||
accel_bias[1] += accel_data.filtered.y;
|
||||
accel_bias[2] += accel_data.filtered.z;
|
||||
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
|
||||
PIOS_HMC5843_ReadMag(mag_raw);
|
||||
#endif
|
||||
mag_bias[0] += mag_raw[0];
|
||||
mag_bias[1] += mag_raw[1];
|
||||
mag_bias[2] += mag_raw[2];
|
||||
@ -547,7 +554,9 @@ void calibrate_sensors() {
|
||||
accel_var[0] += (accel_data.filtered.x - accel_bias[0]) * (accel_data.filtered.x - accel_bias[0]);
|
||||
accel_var[1] += (accel_data.filtered.y - accel_bias[1]) * (accel_data.filtered.y - accel_bias[1]);
|
||||
accel_var[2] += (accel_data.filtered.z - accel_bias[2]) * (accel_data.filtered.z - accel_bias[2]);
|
||||
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
|
||||
PIOS_HMC5843_ReadMag(mag_raw);
|
||||
#endif
|
||||
mag_var[0] += (mag_raw[0] - mag_bias[0]) * (mag_raw[0] - mag_bias[0]);
|
||||
mag_var[1] += (mag_raw[1] - mag_bias[1]) * (mag_raw[1] - mag_bias[1]);
|
||||
mag_var[2] += (mag_raw[2] - mag_bias[2]) * (mag_raw[2] - mag_bias[2]);
|
||||
@ -604,7 +613,9 @@ void converge_insgps()
|
||||
rpy[1] = -atan2(accel_data.filtered.x, accel_data.filtered.z) * 180 / M_PI;
|
||||
rpy[0] = -atan2(accel_data.filtered.y, accel_data.filtered.z) * 180 / M_PI;
|
||||
// Get magnetic readings
|
||||
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
|
||||
PIOS_HMC5843_ReadMag(mag_data.raw.axis);
|
||||
#endif
|
||||
mag[0] = -mag_data.raw.axis[1];
|
||||
mag[1] = -mag_data.raw.axis[0];
|
||||
mag[2] = -mag_data.raw.axis[2];
|
||||
|
||||
@ -39,6 +39,8 @@
|
||||
#define NUMV 10 // number of measurements, v is the measurement noise vector
|
||||
#define NUMU 6 // number of deterministic inputs, U is the input vector
|
||||
|
||||
//#define COVARIANCE_PREDICTION_GENERAL
|
||||
|
||||
// Private functions
|
||||
void INSCorrection(float mag_data[3], float Pos[3], float Vel[3], float BaroAlt, uint16_t SensorsUsed);
|
||||
void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW], float Q[NUMW], float dT, float P[NUMX][NUMX]);
|
||||
@ -58,7 +60,6 @@ float P[NUMX][NUMX], X[NUMX]; // covariance matrix and sta
|
||||
float Q[NUMW], R[NUMV]; // input noise and measurement noise variances
|
||||
float K[NUMX][NUMV]; // feedback gain matrix
|
||||
|
||||
|
||||
// ************* Exposed Functions ****************
|
||||
// *************************************************
|
||||
void INSGPSInit() //pretty much just a place holder for now
|
||||
@ -151,7 +152,7 @@ void INSPrediction(float gyro_data[3], float accel_data[3], float dT)
|
||||
RungeKutta(X,U,dT);
|
||||
qmag=sqrt(X[6]*X[6] + X[7]*X[7] + X[8]*X[8] + X[9]*X[9]);
|
||||
X[6] /= qmag; X[7] /= qmag; X[8] /= qmag; X[9] /= qmag;
|
||||
CovariancePrediction(F,G,Q,dT,P);
|
||||
CovariancePrediction(F,G,Q,dT,P);
|
||||
|
||||
// Update Nav solution structure
|
||||
Nav.Pos[0] = X[0];
|
||||
@ -235,7 +236,6 @@ void INSCorrection(float mag_data[3], float Pos[3], float Vel[3], float BaroAlt,
|
||||
Nav.q[2] = X[8];
|
||||
Nav.q[3] = X[9];
|
||||
}
|
||||
|
||||
// ************* CovariancePrediction *************
|
||||
// Does the prediction step of the Kalman filter for the covariance matrix
|
||||
// Output, Pnew, overwrites P, the input covariance
|
||||
@ -243,31 +243,144 @@ void INSCorrection(float mag_data[3], float Pos[3], float Vel[3], float BaroAlt,
|
||||
// Q is the discrete time covariance of process noise
|
||||
// Q is vector of the diagonal for a square matrix with
|
||||
// dimensions equal to the number of disturbance noise variables
|
||||
// Could be much more efficient using the sparse, block structure of F and G
|
||||
// The General Method is very inefficient,not taking advantage of the sparse F and G
|
||||
// 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')]
|
||||
|
||||
dTsq = dT*dT;
|
||||
|
||||
for (i=0;i<NUMX;i++) // Calculate Dummy = (P/T +F*P)
|
||||
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];
|
||||
}
|
||||
for (i=0;i<NUMX;i++) // Calculate Pnew = Dummy/T + Dummy*F' + G*Qw*G'
|
||||
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'
|
||||
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'
|
||||
P[j][i] = P[i][j] = P[i][j]*dTsq; // Pnew = T^2*P and fill in lower triangular;
|
||||
}
|
||||
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')]
|
||||
|
||||
dTsq = dT*dT;
|
||||
|
||||
for (i=0;i<NUMX;i++) // Calculate Dummy = (P/T +F*P)
|
||||
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];
|
||||
}
|
||||
for (i=0;i<NUMX;i++) // Calculate Pnew = Dummy/T + Dummy*F' + G*Qw*G'
|
||||
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'
|
||||
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'
|
||||
P[j][i] = P[i][j] = P[i][j]*dTsq; // Pnew = T^2*P and fill in lower triangular;
|
||||
}
|
||||
}
|
||||
|
||||
#else
|
||||
|
||||
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])*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])*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])*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])*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])*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])*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[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])*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])*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])*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])*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])*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])*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[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])*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])*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])*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])*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])*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])*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[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][9]*(F[3][9]*D[9][9] + F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9]) + 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][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][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]))*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])*T + D[3][3];
|
||||
P[3][4] = P[4][3] = (F[4][9]*(F[3][9]*D[9][9] + F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9]) + 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[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[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]) + 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])*T + D[3][4];
|
||||
P[3][5] = P[5][3] = (F[5][9]*(F[3][9]*D[9][9] + F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9]) + 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[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[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]) + 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])*T + D[3][5];
|
||||
P[3][6] = P[6][3] = (F[6][9]*(F[3][9]*D[9][9] + F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9]) + F[6][10]*(F[3][9]*D[9][10] + F[3][6]*D[6][10] + F[3][7]*D[7][10] + F[3][8]*D[8][10]) + F[6][11]*(F[3][9]*D[9][11] + F[3][6]*D[6][11] + F[3][7]*D[7][11] + F[3][8]*D[8][11]) + F[6][12]*(F[3][9]*D[9][12] + F[3][6]*D[6][12] + F[3][7]*D[7][12] + F[3][8]*D[8][12]) + 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[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]))*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])*T + D[3][6];
|
||||
P[3][7] = P[7][3] = (F[7][9]*(F[3][9]*D[9][9] + F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9]) + F[7][10]*(F[3][9]*D[9][10] + F[3][6]*D[6][10] + F[3][7]*D[7][10] + F[3][8]*D[8][10]) + F[7][11]*(F[3][9]*D[9][11] + F[3][6]*D[6][11] + F[3][7]*D[7][11] + F[3][8]*D[8][11]) + F[7][12]*(F[3][9]*D[9][12] + F[3][6]*D[6][12] + F[3][7]*D[7][12] + F[3][8]*D[8][12]) + 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[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]))*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])*T + D[3][7];
|
||||
P[3][8] = P[8][3] = (F[8][9]*(F[3][9]*D[9][9] + F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9]) + F[8][10]*(F[3][9]*D[9][10] + F[3][6]*D[6][10] + F[3][7]*D[7][10] + F[3][8]*D[8][10]) + F[8][11]*(F[3][9]*D[9][11] + F[3][6]*D[6][11] + F[3][7]*D[7][11] + F[3][8]*D[8][11]) + F[8][12]*(F[3][9]*D[9][12] + F[3][6]*D[6][12] + F[3][7]*D[7][12] + F[3][8]*D[8][12]) + 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[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]))*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])*T + D[3][8];
|
||||
P[3][9] = P[9][3] = (F[9][10]*(F[3][9]*D[9][10] + F[3][6]*D[6][10] + F[3][7]*D[7][10] + F[3][8]*D[8][10]) + F[9][11]*(F[3][9]*D[9][11] + F[3][6]*D[6][11] + F[3][7]*D[7][11] + F[3][8]*D[8][11]) + F[9][12]*(F[3][9]*D[9][12] + F[3][6]*D[6][12] + F[3][7]*D[7][12] + F[3][8]*D[8][12]) + 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[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[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]))*Tsq + (F[9][6]*D[3][6] + F[9][7]*D[3][7] + F[9][8]*D[3][8] + 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][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9])*T + D[3][9];
|
||||
P[3][10] = P[10][3] = (F[3][9]*D[9][10] + F[3][6]*D[6][10] + F[3][7]*D[7][10] + F[3][8]*D[8][10])*T + D[3][10];
|
||||
P[3][11] = P[11][3] = (F[3][9]*D[9][11] + F[3][6]*D[6][11] + F[3][7]*D[7][11] + F[3][8]*D[8][11])*T + D[3][11];
|
||||
P[3][12] = P[12][3] = (F[3][9]*D[9][12] + F[3][6]*D[6][12] + F[3][7]*D[7][12] + F[3][8]*D[8][12])*T + D[3][12];
|
||||
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][9]*(F[4][9]*D[9][9] + F[4][6]*D[6][9] + F[4][7]*D[7][9] + F[4][8]*D[8][9]) + 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][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][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]))*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])*T + D[4][4];
|
||||
P[4][5] = P[5][4] = (F[5][9]*(F[4][9]*D[9][9] + F[4][6]*D[6][9] + F[4][7]*D[7][9] + F[4][8]*D[8][9]) + 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[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[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]) + 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])*T + D[4][5];
|
||||
P[4][6] = P[6][4] = (F[6][9]*(F[4][9]*D[9][9] + F[4][6]*D[6][9] + F[4][7]*D[7][9] + F[4][8]*D[8][9]) + F[6][10]*(F[4][9]*D[9][10] + F[4][6]*D[6][10] + F[4][7]*D[7][10] + F[4][8]*D[8][10]) + F[6][11]*(F[4][9]*D[9][11] + F[4][6]*D[6][11] + F[4][7]*D[7][11] + F[4][8]*D[8][11]) + F[6][12]*(F[4][9]*D[9][12] + F[4][6]*D[6][12] + F[4][7]*D[7][12] + F[4][8]*D[8][12]) + 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[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]))*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])*T + D[4][6];
|
||||
P[4][7] = P[7][4] = (F[7][9]*(F[4][9]*D[9][9] + F[4][6]*D[6][9] + F[4][7]*D[7][9] + F[4][8]*D[8][9]) + F[7][10]*(F[4][9]*D[9][10] + F[4][6]*D[6][10] + F[4][7]*D[7][10] + F[4][8]*D[8][10]) + F[7][11]*(F[4][9]*D[9][11] + F[4][6]*D[6][11] + F[4][7]*D[7][11] + F[4][8]*D[8][11]) + F[7][12]*(F[4][9]*D[9][12] + F[4][6]*D[6][12] + F[4][7]*D[7][12] + F[4][8]*D[8][12]) + 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[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]))*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])*T + D[4][7];
|
||||
P[4][8] = P[8][4] = (F[8][9]*(F[4][9]*D[9][9] + F[4][6]*D[6][9] + F[4][7]*D[7][9] + F[4][8]*D[8][9]) + F[8][10]*(F[4][9]*D[9][10] + F[4][6]*D[6][10] + F[4][7]*D[7][10] + F[4][8]*D[8][10]) + F[8][11]*(F[4][9]*D[9][11] + F[4][6]*D[6][11] + F[4][7]*D[7][11] + F[4][8]*D[8][11]) + F[8][12]*(F[4][9]*D[9][12] + F[4][6]*D[6][12] + F[4][7]*D[7][12] + F[4][8]*D[8][12]) + 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[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]))*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])*T + D[4][8];
|
||||
P[4][9] = P[9][4] = (F[9][10]*(F[4][9]*D[9][10] + F[4][6]*D[6][10] + F[4][7]*D[7][10] + F[4][8]*D[8][10]) + F[9][11]*(F[4][9]*D[9][11] + F[4][6]*D[6][11] + F[4][7]*D[7][11] + F[4][8]*D[8][11]) + F[9][12]*(F[4][9]*D[9][12] + F[4][6]*D[6][12] + F[4][7]*D[7][12] + F[4][8]*D[8][12]) + 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[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[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]))*Tsq + (F[9][6]*D[4][6] + F[9][7]*D[4][7] + F[9][8]*D[4][8] + 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][6]*D[6][9] + F[4][7]*D[7][9] + F[4][8]*D[8][9])*T + D[4][9];
|
||||
P[4][10] = P[10][4] = (F[4][9]*D[9][10] + F[4][6]*D[6][10] + F[4][7]*D[7][10] + F[4][8]*D[8][10])*T + D[4][10];
|
||||
P[4][11] = P[11][4] = (F[4][9]*D[9][11] + F[4][6]*D[6][11] + F[4][7]*D[7][11] + F[4][8]*D[8][11])*T + D[4][11];
|
||||
P[4][12] = P[12][4] = (F[4][9]*D[9][12] + F[4][6]*D[6][12] + F[4][7]*D[7][12] + F[4][8]*D[8][12])*T + D[4][12];
|
||||
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][9]*(F[5][9]*D[9][9] + F[5][6]*D[6][9] + F[5][7]*D[7][9] + F[5][8]*D[8][9]) + 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][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][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]))*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])*T + D[5][5];
|
||||
P[5][6] = P[6][5] = (F[6][9]*(F[5][9]*D[9][9] + F[5][6]*D[6][9] + F[5][7]*D[7][9] + F[5][8]*D[8][9]) + F[6][10]*(F[5][9]*D[9][10] + F[5][6]*D[6][10] + F[5][7]*D[7][10] + F[5][8]*D[8][10]) + F[6][11]*(F[5][9]*D[9][11] + F[5][6]*D[6][11] + F[5][7]*D[7][11] + F[5][8]*D[8][11]) + F[6][12]*(F[5][9]*D[9][12] + F[5][6]*D[6][12] + F[5][7]*D[7][12] + F[5][8]*D[8][12]) + 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[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]))*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])*T + D[5][6];
|
||||
P[5][7] = P[7][5] = (F[7][9]*(F[5][9]*D[9][9] + F[5][6]*D[6][9] + F[5][7]*D[7][9] + F[5][8]*D[8][9]) + F[7][10]*(F[5][9]*D[9][10] + F[5][6]*D[6][10] + F[5][7]*D[7][10] + F[5][8]*D[8][10]) + F[7][11]*(F[5][9]*D[9][11] + F[5][6]*D[6][11] + F[5][7]*D[7][11] + F[5][8]*D[8][11]) + F[7][12]*(F[5][9]*D[9][12] + F[5][6]*D[6][12] + F[5][7]*D[7][12] + F[5][8]*D[8][12]) + 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[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]))*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])*T + D[5][7];
|
||||
P[5][8] = P[8][5] = (F[8][9]*(F[5][9]*D[9][9] + F[5][6]*D[6][9] + F[5][7]*D[7][9] + F[5][8]*D[8][9]) + F[8][10]*(F[5][9]*D[9][10] + F[5][6]*D[6][10] + F[5][7]*D[7][10] + F[5][8]*D[8][10]) + F[8][11]*(F[5][9]*D[9][11] + F[5][6]*D[6][11] + F[5][7]*D[7][11] + F[5][8]*D[8][11]) + F[8][12]*(F[5][9]*D[9][12] + F[5][6]*D[6][12] + F[5][7]*D[7][12] + F[5][8]*D[8][12]) + 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[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]))*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])*T + D[5][8];
|
||||
P[5][9] = P[9][5] = (F[9][10]*(F[5][9]*D[9][10] + F[5][6]*D[6][10] + F[5][7]*D[7][10] + F[5][8]*D[8][10]) + F[9][11]*(F[5][9]*D[9][11] + F[5][6]*D[6][11] + F[5][7]*D[7][11] + F[5][8]*D[8][11]) + F[9][12]*(F[5][9]*D[9][12] + F[5][6]*D[6][12] + F[5][7]*D[7][12] + F[5][8]*D[8][12]) + 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[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[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]))*Tsq + (F[9][6]*D[5][6] + F[9][7]*D[5][7] + F[9][8]*D[5][8] + 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][6]*D[6][9] + F[5][7]*D[7][9] + F[5][8]*D[8][9])*T + D[5][9];
|
||||
P[5][10] = P[10][5] = (F[5][9]*D[9][10] + F[5][6]*D[6][10] + F[5][7]*D[7][10] + F[5][8]*D[8][10])*T + D[5][10];
|
||||
P[5][11] = P[11][5] = (F[5][9]*D[9][11] + F[5][6]*D[6][11] + F[5][7]*D[7][11] + F[5][8]*D[8][11])*T + D[5][11];
|
||||
P[5][12] = P[12][5] = (F[5][9]*D[9][12] + F[5][6]*D[6][12] + F[5][7]*D[7][12] + F[5][8]*D[8][12])*T + D[5][12];
|
||||
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][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][7]*D[7][9] + F[6][8]*D[8][9]) + F[6][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][7]*D[7][10] + F[6][8]*D[8][10]) + F[6][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][7]*D[7][11] + F[6][8]*D[8][11]) + F[6][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] + F[6][7]*D[7][12] + F[6][8]*D[8][12]) + 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]))*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][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][7]*D[7][9] + F[6][8]*D[8][9]) + F[7][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][7]*D[7][10] + F[6][8]*D[8][10]) + F[7][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][7]*D[7][11] + F[6][8]*D[8][11]) + F[7][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] + F[6][7]*D[7][12] + F[6][8]*D[8][12]) + 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]) + 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][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][7]*D[7][9] + F[6][8]*D[8][9]) + F[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][7]*D[7][10] + F[6][8]*D[8][10]) + F[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][7]*D[7][11] + F[6][8]*D[8][11]) + F[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] + F[6][7]*D[7][12] + F[6][8]*D[8][12]) + 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]) + 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][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][7]*D[7][10] + F[6][8]*D[8][10]) + F[9][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][7]*D[7][11] + F[6][8]*D[8][11]) + F[9][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] + F[6][7]*D[7][12] + F[6][8]*D[8][12]) + 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]) + G[9][0]*G[6][0]*Q[0] + G[9][1]*G[6][1]*Q[1] + G[9][2]*G[6][2]*Q[2])*Tsq + (F[9][6]*D[6][6] + F[9][7]*D[6][7] + F[9][8]*D[6][8] + F[6][9]*D[9][9] + F[9][10]*D[6][10] + F[6][10]*D[9][10] + F[9][11]*D[6][11] + F[6][11]*D[9][11] + F[9][12]*D[6][12] + F[6][12]*D[9][12] + F[6][7]*D[7][9] + F[6][8]*D[8][9])*T + D[6][9];
|
||||
P[6][10] = P[10][6] = (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][7]*D[7][10] + F[6][8]*D[8][10])*T + D[6][10];
|
||||
P[6][11] = P[11][6] = (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][7]*D[7][11] + F[6][8]*D[8][11])*T + D[6][11];
|
||||
P[6][12] = P[12][6] = (F[6][9]*D[9][12] + F[6][10]*D[10][12] + F[6][11]*D[11][12] + F[6][12]*D[12][12] + F[6][7]*D[7][12] + F[6][8]*D[8][12])*T + D[6][12];
|
||||
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][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][6]*D[6][9] + F[7][8]*D[8][9]) + F[7][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][6]*D[6][10] + F[7][8]*D[8][10]) + F[7][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][6]*D[6][11] + F[7][8]*D[8][11]) + F[7][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] + F[7][6]*D[6][12] + F[7][8]*D[8][12]) + 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]))*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][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][6]*D[6][9] + F[7][8]*D[8][9]) + F[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][6]*D[6][10] + F[7][8]*D[8][10]) + F[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][6]*D[6][11] + F[7][8]*D[8][11]) + F[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] + F[7][6]*D[6][12] + F[7][8]*D[8][12]) + 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]) + 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][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][6]*D[6][10] + F[7][8]*D[8][10]) + F[9][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][6]*D[6][11] + F[7][8]*D[8][11]) + F[9][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] + F[7][6]*D[6][12] + F[7][8]*D[8][12]) + 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]) + G[9][0]*G[7][0]*Q[0] + G[9][1]*G[7][1]*Q[1] + G[9][2]*G[7][2]*Q[2])*Tsq + (F[9][6]*D[6][7] + F[9][7]*D[7][7] + F[9][8]*D[7][8] + F[7][9]*D[9][9] + F[9][10]*D[7][10] + F[7][10]*D[9][10] + F[9][11]*D[7][11] + F[7][11]*D[9][11] + F[9][12]*D[7][12] + F[7][12]*D[9][12] + F[7][6]*D[6][9] + F[7][8]*D[8][9])*T + D[7][9];
|
||||
P[7][10] = P[10][7] = (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][6]*D[6][10] + F[7][8]*D[8][10])*T + D[7][10];
|
||||
P[7][11] = P[11][7] = (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][6]*D[6][11] + F[7][8]*D[8][11])*T + D[7][11];
|
||||
P[7][12] = P[12][7] = (F[7][9]*D[9][12] + F[7][10]*D[10][12] + F[7][11]*D[11][12] + F[7][12]*D[12][12] + F[7][6]*D[6][12] + F[7][8]*D[8][12])*T + D[7][12];
|
||||
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][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][6]*D[6][9] + F[8][7]*D[7][9]) + F[8][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][6]*D[6][10] + F[8][7]*D[7][10]) + F[8][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][6]*D[6][11] + F[8][7]*D[7][11]) + F[8][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] + F[8][6]*D[6][12] + F[8][7]*D[7][12]) + 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]))*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][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][6]*D[6][10] + F[8][7]*D[7][10]) + F[9][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][6]*D[6][11] + F[8][7]*D[7][11]) + F[9][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] + F[8][6]*D[6][12] + F[8][7]*D[7][12]) + 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]) + G[9][0]*G[8][0]*Q[0] + G[9][1]*G[8][1]*Q[1] + G[9][2]*G[8][2]*Q[2])*Tsq + (F[9][6]*D[6][8] + F[9][7]*D[7][8] + F[9][8]*D[8][8] + F[8][9]*D[9][9] + F[9][10]*D[8][10] + F[8][10]*D[9][10] + F[9][11]*D[8][11] + F[8][11]*D[9][11] + F[9][12]*D[8][12] + F[8][12]*D[9][12] + F[8][6]*D[6][9] + F[8][7]*D[7][9])*T + D[8][9];
|
||||
P[8][10] = P[10][8] = (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][6]*D[6][10] + F[8][7]*D[7][10])*T + D[8][10];
|
||||
P[8][11] = P[11][8] = (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][6]*D[6][11] + F[8][7]*D[7][11])*T + D[8][11];
|
||||
P[8][12] = P[12][8] = (F[8][9]*D[9][12] + F[8][10]*D[10][12] + F[8][11]*D[11][12] + F[8][12]*D[12][12] + F[8][6]*D[6][12] + F[8][7]*D[7][12])*T + D[8][12];
|
||||
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][10]*(F[9][10]*D[10][10] + F[9][11]*D[10][11] + F[9][12]*D[10][12] + F[9][6]*D[6][10] + F[9][7]*D[7][10] + F[9][8]*D[8][10]) + F[9][11]*(F[9][10]*D[10][11] + F[9][11]*D[11][11] + F[9][12]*D[11][12] + F[9][6]*D[6][11] + F[9][7]*D[7][11] + F[9][8]*D[8][11]) + F[9][12]*(F[9][10]*D[10][12] + F[9][11]*D[11][12] + F[9][12]*D[12][12] + F[9][6]*D[6][12] + F[9][7]*D[7][12] + F[9][8]*D[8][12]) + 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]))*Tsq + (2*F[9][10]*D[9][10] + 2*F[9][11]*D[9][11] + 2*F[9][12]*D[9][12] + 2*F[9][6]*D[6][9] + 2*F[9][7]*D[7][9] + 2*F[9][8]*D[8][9])*T + D[9][9];
|
||||
P[9][10] = P[10][9] = (F[9][10]*D[10][10] + F[9][11]*D[10][11] + F[9][12]*D[10][12] + F[9][6]*D[6][10] + F[9][7]*D[7][10] + F[9][8]*D[8][10])*T + D[9][10];
|
||||
P[9][11] = P[11][9] = (F[9][10]*D[10][11] + F[9][11]*D[11][11] + F[9][12]*D[11][12] + F[9][6]*D[6][11] + F[9][7]*D[7][11] + F[9][8]*D[8][11])*T + D[9][11];
|
||||
P[9][12] = P[12][9] = (F[9][10]*D[10][12] + F[9][11]*D[11][12] + F[9][12]*D[12][12] + F[9][6]*D[6][12] + F[9][7]*D[7][12] + F[9][8]*D[8][12])*T + D[9][12];
|
||||
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[11][11] = Q[7]*Tsq + D[11][11];
|
||||
P[11][12] = P[12][11] = D[11][12];
|
||||
P[12][12] = Q[8]*Tsq + D[12][12];
|
||||
}
|
||||
#endif
|
||||
|
||||
// ************* 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
|
||||
|
||||
@ -223,10 +223,12 @@ void PIOS_USART_aux_irq_handler(void)
|
||||
extern const struct pios_com_driver pios_usart_com_driver;
|
||||
|
||||
struct pios_com_dev pios_com_devs[] = {
|
||||
#if defined(PIOS_INCLUDE_USART)
|
||||
{
|
||||
.id = PIOS_USART_AUX,
|
||||
.driver = &pios_usart_com_driver,
|
||||
},
|
||||
#endif
|
||||
};
|
||||
|
||||
const uint8_t pios_com_num_devices = NELEMENTS(pios_com_devs);
|
||||
|
||||
@ -3,16 +3,20 @@ function analyzeRaw(device)
|
||||
downsample = 12; % relevant for knowing block size
|
||||
Fs = 512; % need to verify, close
|
||||
|
||||
s = serial(device,'Baud',115200);
|
||||
set(s,'InputBufferSize',10000);
|
||||
fopen(s);
|
||||
|
||||
dat = [];
|
||||
for i = 1:20
|
||||
i
|
||||
if(i > 5) % must flush buffer
|
||||
dat = [dat; uint8(fread(s))];
|
||||
if ischar(device)
|
||||
s = serial(device,'Baud',115200);
|
||||
set(s,'InputBufferSize',10000);
|
||||
fopen(s);
|
||||
|
||||
dat = [];
|
||||
for i = 1:20
|
||||
i
|
||||
if(i > 5) % must flush buffer
|
||||
dat = [dat; uint8(fread(s))];
|
||||
end
|
||||
end
|
||||
else
|
||||
dat = uint8(device);
|
||||
end
|
||||
|
||||
raw_framing = 0:15;
|
||||
@ -33,7 +37,7 @@ if(starts) % found raw data, process
|
||||
gyro_x = 0.007*(blocks(2,:)-1675);
|
||||
gyro_y = 0.007*(blocks(4,:)-1675);
|
||||
gyro_z = 0.007*(blocks(6,:)-1675);
|
||||
|
||||
|
||||
time = (1:length(accel_x))/512;
|
||||
|
||||
% display accels
|
||||
@ -53,7 +57,7 @@ if(starts) % found raw data, process
|
||||
title('Accel Y');
|
||||
subplot(324);
|
||||
pwelch(accel_y-mean(accel_y),[],[],[],Fs);
|
||||
|
||||
|
||||
subplot(325);
|
||||
plot(time,accel_z);
|
||||
xlim([0 1])
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user