diff --git a/EKF/airspeed_fusion.cpp b/EKF/airspeed_fusion.cpp index ae57e343c6..8a3c52c87a 100644 --- a/EKF/airspeed_fusion.cpp +++ b/EKF/airspeed_fusion.cpp @@ -73,26 +73,20 @@ void Ekf::fuseAirspeed() if (v_tas_pred > 1.0f) { // Calculate the observation jacobian // intermediate variable from algebraic optimisation - SH_TAS[0] = 1 / v_tas_pred; - SH_TAS[1] = (SH_TAS[0] * (2 * ve - 2 * vwe)) / 2.0f; - SH_TAS[2] = (SH_TAS[0] * (2 * vn - 2 * vwn)) / 2.0f; + SH_TAS[0] = 1.0f/v_tas_pred; + SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2.0f*vwe))*0.5f; + SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2.0f*vwn))*0.5f; for (uint8_t i = 0; i < _k_num_states; i++) { H_TAS[i] = 0.0f; } - H_TAS[3] = SH_TAS[2]; - H_TAS[4] = SH_TAS[1]; - H_TAS[5] = vd * SH_TAS[0]; + H_TAS[4] = SH_TAS[2]; + H_TAS[5] = SH_TAS[1]; + H_TAS[6] = vd*SH_TAS[0]; H_TAS[22] = -SH_TAS[2]; H_TAS[23] = -SH_TAS[1]; // We don't want to update the innovation variance if the calculation is ill conditioned - float _airspeed_innov_var_temp = (R_TAS + SH_TAS[2] * (P[3][3] * SH_TAS[2] + P[4][3] * SH_TAS[1] - P[22][3] * SH_TAS[2] - - P[23][3] * SH_TAS[1] + P[5][3] * vd * SH_TAS[0]) + SH_TAS[1] * (P[3][4] * SH_TAS[2] + P[4][4] * SH_TAS[1] - P[22][4] * - SH_TAS[2] - P[23][4] * SH_TAS[1] + P[5][4] * vd * SH_TAS[0]) - SH_TAS[2] * (P[3][22] * SH_TAS[2] + P[4][22] * SH_TAS[1] - - P[22][22] * SH_TAS[2] - P[23][22] * SH_TAS[1] + P[5][22] * vd * SH_TAS[0]) - SH_TAS[1] * - (P[3][23] * SH_TAS[2] + P[4][23] * SH_TAS[1] - P[22][23] * SH_TAS[2] - P[23][23] * SH_TAS[1] + P[5][23] * vd * - SH_TAS[0]) + vd * SH_TAS[0] * (P[3][5] * SH_TAS[2] + P[4][5] * SH_TAS[1] - P[22][5] * SH_TAS[2] - P[23][5] * SH_TAS[1] + - P[5][5] * vd * SH_TAS[0])); + float _airspeed_innov_var_temp = (R_TAS + SH_TAS[2]*(P[4][4]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[22][4]*SH_TAS[2] - P[23][4]*SH_TAS[1] + P[6][4]*vd*SH_TAS[0]) + SH_TAS[1]*(P[4][5]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[22][5]*SH_TAS[2] - P[23][5]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]) - SH_TAS[2]*(P[4][22]*SH_TAS[2] + P[5][22]*SH_TAS[1] - P[22][22]*SH_TAS[2] - P[23][22]*SH_TAS[1] + P[6][22]*vd*SH_TAS[0]) - SH_TAS[1]*(P[4][23]*SH_TAS[2] + P[5][23]*SH_TAS[1] - P[22][23]*SH_TAS[2] - P[23][23]*SH_TAS[1] + P[6][23]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[4][6]*SH_TAS[2] + P[5][6]*SH_TAS[1] - P[22][6]*SH_TAS[2] - P[23][6]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0])); if (_airspeed_innov_var_temp >= R_TAS) { // Check for badly conditioned calculation SK_TAS[0] = 1.0f / _airspeed_innov_var_temp; @@ -104,63 +98,30 @@ void Ekf::fuseAirspeed() SK_TAS[1] = SH_TAS[1]; - Kfusion[0] = SK_TAS[0] * (P[0][3] * SH_TAS[2] - P[0][22] * SH_TAS[2] + P[0][4] * SK_TAS[1] - P[0][23] * SK_TAS[1] + - P[0][5] * vd * SH_TAS[0]); - Kfusion[1] = SK_TAS[0] * (P[1][3] * SH_TAS[2] - P[1][22] * SH_TAS[2] + P[1][4] * SK_TAS[1] - P[1][23] * SK_TAS[1] + - P[1][5] * vd * SH_TAS[0]); - Kfusion[2] = SK_TAS[0] * (P[2][3] * SH_TAS[2] - P[2][22] * SH_TAS[2] + P[2][4] * SK_TAS[1] - P[2][23] * SK_TAS[1] + - P[2][5] * vd * SH_TAS[0]); - Kfusion[3] = SK_TAS[0] * (P[3][3] * SH_TAS[2] - P[3][22] * SH_TAS[2] + P[3][4] * SK_TAS[1] - P[3][23] * SK_TAS[1] + - P[3][5] * vd * SH_TAS[0]); - Kfusion[4] = SK_TAS[0] * (P[4][3] * SH_TAS[2] - P[4][22] * SH_TAS[2] + P[4][4] * SK_TAS[1] - P[4][23] * SK_TAS[1] + - P[4][5] * vd * SH_TAS[0]); - Kfusion[5] = SK_TAS[0] * (P[5][3] * SH_TAS[2] - P[5][22] * SH_TAS[2] + P[5][4] * SK_TAS[1] - P[5][23] * SK_TAS[1] + - P[5][5] * vd * SH_TAS[0]); - Kfusion[6] = SK_TAS[0] * (P[6][3] * SH_TAS[2] - P[6][22] * SH_TAS[2] + P[6][4] * SK_TAS[1] - P[6][23] * SK_TAS[1] + - P[6][5] * vd * SH_TAS[0]); - Kfusion[7] = SK_TAS[0] * (P[7][3] * SH_TAS[2] - P[7][22] * SH_TAS[2] + P[7][4] * SK_TAS[1] - P[7][23] * SK_TAS[1] + - P[7][5] * vd * SH_TAS[0]); - Kfusion[8] = SK_TAS[0] * (P[8][3] * SH_TAS[2] - P[8][22] * SH_TAS[2] + P[8][4] * SK_TAS[1] - P[8][23] * SK_TAS[1] + - P[8][5] * vd * SH_TAS[0]); - Kfusion[9] = SK_TAS[0] * (P[9][3] * SH_TAS[2] - P[9][22] * SH_TAS[2] + P[9][4] * SK_TAS[1] - P[9][23] * SK_TAS[1] + - P[9][5] * vd * SH_TAS[0]); - Kfusion[10] = SK_TAS[0] * (P[10][3] * SH_TAS[2] - P[10][22] * SH_TAS[2] + P[10][4] * SK_TAS[1] - P[10][23] * SK_TAS[1] + - P[10][5] * vd * SH_TAS[0]); - Kfusion[11] = SK_TAS[0] * (P[11][3] * SH_TAS[2] - P[11][22] * SH_TAS[2] + P[11][4] * SK_TAS[1] - P[11][23] * SK_TAS[1] + - P[11][5] * vd * SH_TAS[0]); - Kfusion[12] = SK_TAS[0] * (P[12][3] * SH_TAS[2] - P[12][22] * SH_TAS[2] + P[12][4] * SK_TAS[1] - P[12][23] * SK_TAS[1] + - P[12][5] * vd * SH_TAS[0]); - Kfusion[13] = SK_TAS[0] * (P[13][3] * SH_TAS[2] - P[13][22] * SH_TAS[2] + P[13][4] * SK_TAS[1] - P[13][23] * SK_TAS[1] + - P[13][5] * vd * SH_TAS[0]); - Kfusion[14] = SK_TAS[0] * (P[14][3] * SH_TAS[2] - P[14][22] * SH_TAS[2] + P[14][4] * SK_TAS[1] - P[14][23] * SK_TAS[1] + - P[14][5] * vd * SH_TAS[0]); - Kfusion[15] = SK_TAS[0] * (P[15][3] * SH_TAS[2] - P[15][22] * SH_TAS[2] + P[15][4] * SK_TAS[1] - P[15][23] * SK_TAS[1] + - P[15][5] * vd * SH_TAS[0]); - Kfusion[22] = SK_TAS[0] * (P[22][3] * SH_TAS[2] - P[22][22] * SH_TAS[2] + P[22][4] * SK_TAS[1] - P[22][23] * SK_TAS[1] + - P[22][5] * vd * SH_TAS[0]); - Kfusion[23] = SK_TAS[0] * (P[23][3] * SH_TAS[2] - P[23][22] * SH_TAS[2] + P[23][4] * SK_TAS[1] - P[23][23] * SK_TAS[1] + - P[23][5] * vd * SH_TAS[0]); - - // Only update the magnetometer states if we are airborne and using 3D mag fusion - if (_control_status.flags.mag_3D && _control_status.flags.in_air) { - Kfusion[16] = SK_TAS[0] * (P[16][3] * SH_TAS[2] - P[16][22] * SH_TAS[2] + P[16][4] * SK_TAS[1] - P[16][23] * SK_TAS[1] + - P[16][5] * vd * SH_TAS[0]); - Kfusion[17] = SK_TAS[0] * (P[17][3] * SH_TAS[2] - P[17][22] * SH_TAS[2] + P[17][4] * SK_TAS[1] - P[17][23] * SK_TAS[1] + - P[17][5] * vd * SH_TAS[0]); - Kfusion[18] = SK_TAS[0] * (P[18][3] * SH_TAS[2] - P[18][22] * SH_TAS[2] + P[18][4] * SK_TAS[1] - P[18][23] * SK_TAS[1] + - P[18][5] * vd * SH_TAS[0]); - Kfusion[19] = SK_TAS[0] * (P[19][3] * SH_TAS[2] - P[19][22] * SH_TAS[2] + P[19][4] * SK_TAS[1] - P[19][23] * SK_TAS[1] + - P[19][5] * vd * SH_TAS[0]); - Kfusion[20] = SK_TAS[0] * (P[20][3] * SH_TAS[2] - P[20][22] * SH_TAS[2] + P[20][4] * SK_TAS[1] - P[20][23] * SK_TAS[1] + - P[20][5] * vd * SH_TAS[0]); - Kfusion[21] = SK_TAS[0] * (P[21][3] * SH_TAS[2] - P[21][22] * SH_TAS[2] + P[21][4] * SK_TAS[1] - P[21][23] * SK_TAS[1] + - P[21][5] * vd * SH_TAS[0]); - - } else { - for (int i = 16; i <= 21; i++) { - Kfusion[i] = 0.0f; - } - } + Kfusion[0] = SK_TAS[0]*(P[0][4]*SH_TAS[2] - P[0][22]*SH_TAS[2] + P[0][5]*SK_TAS[1] - P[0][23]*SK_TAS[1] + P[0][6]*vd*SH_TAS[0]); + Kfusion[1] = SK_TAS[0]*(P[1][4]*SH_TAS[2] - P[1][22]*SH_TAS[2] + P[1][5]*SK_TAS[1] - P[1][23]*SK_TAS[1] + P[1][6]*vd*SH_TAS[0]); + Kfusion[2] = SK_TAS[0]*(P[2][4]*SH_TAS[2] - P[2][22]*SH_TAS[2] + P[2][5]*SK_TAS[1] - P[2][23]*SK_TAS[1] + P[2][6]*vd*SH_TAS[0]); + Kfusion[3] = SK_TAS[0]*(P[3][4]*SH_TAS[2] - P[3][22]*SH_TAS[2] + P[3][5]*SK_TAS[1] - P[3][23]*SK_TAS[1] + P[3][6]*vd*SH_TAS[0]); + Kfusion[4] = SK_TAS[0]*(P[4][4]*SH_TAS[2] - P[4][22]*SH_TAS[2] + P[4][5]*SK_TAS[1] - P[4][23]*SK_TAS[1] + P[4][6]*vd*SH_TAS[0]); + Kfusion[5] = SK_TAS[0]*(P[5][4]*SH_TAS[2] - P[5][22]*SH_TAS[2] + P[5][5]*SK_TAS[1] - P[5][23]*SK_TAS[1] + P[5][6]*vd*SH_TAS[0]); + Kfusion[6] = SK_TAS[0]*(P[6][4]*SH_TAS[2] - P[6][22]*SH_TAS[2] + P[6][5]*SK_TAS[1] - P[6][23]*SK_TAS[1] + P[6][6]*vd*SH_TAS[0]); + Kfusion[7] = SK_TAS[0]*(P[7][4]*SH_TAS[2] - P[7][22]*SH_TAS[2] + P[7][5]*SK_TAS[1] - P[7][23]*SK_TAS[1] + P[7][6]*vd*SH_TAS[0]); + Kfusion[8] = SK_TAS[0]*(P[8][4]*SH_TAS[2] - P[8][22]*SH_TAS[2] + P[8][5]*SK_TAS[1] - P[8][23]*SK_TAS[1] + P[8][6]*vd*SH_TAS[0]); + Kfusion[9] = SK_TAS[0]*(P[9][4]*SH_TAS[2] - P[9][22]*SH_TAS[2] + P[9][5]*SK_TAS[1] - P[9][23]*SK_TAS[1] + P[9][6]*vd*SH_TAS[0]); + Kfusion[10] = SK_TAS[0]*(P[10][4]*SH_TAS[2] - P[10][22]*SH_TAS[2] + P[10][5]*SK_TAS[1] - P[10][23]*SK_TAS[1] + P[10][6]*vd*SH_TAS[0]); + Kfusion[11] = SK_TAS[0]*(P[11][4]*SH_TAS[2] - P[11][22]*SH_TAS[2] + P[11][5]*SK_TAS[1] - P[11][23]*SK_TAS[1] + P[11][6]*vd*SH_TAS[0]); + Kfusion[12] = SK_TAS[0]*(P[12][4]*SH_TAS[2] - P[12][22]*SH_TAS[2] + P[12][5]*SK_TAS[1] - P[12][23]*SK_TAS[1] + P[12][6]*vd*SH_TAS[0]); + Kfusion[13] = SK_TAS[0]*(P[13][4]*SH_TAS[2] - P[13][22]*SH_TAS[2] + P[13][5]*SK_TAS[1] - P[13][23]*SK_TAS[1] + P[13][6]*vd*SH_TAS[0]); + Kfusion[14] = SK_TAS[0]*(P[14][4]*SH_TAS[2] - P[14][22]*SH_TAS[2] + P[14][5]*SK_TAS[1] - P[14][23]*SK_TAS[1] + P[14][6]*vd*SH_TAS[0]); + Kfusion[15] = SK_TAS[0]*(P[15][4]*SH_TAS[2] - P[15][22]*SH_TAS[2] + P[15][5]*SK_TAS[1] - P[15][23]*SK_TAS[1] + P[15][6]*vd*SH_TAS[0]); + Kfusion[16] = SK_TAS[0]*(P[16][4]*SH_TAS[2] - P[16][22]*SH_TAS[2] + P[16][5]*SK_TAS[1] - P[16][23]*SK_TAS[1] + P[16][6]*vd*SH_TAS[0]); + Kfusion[17] = SK_TAS[0]*(P[17][4]*SH_TAS[2] - P[17][22]*SH_TAS[2] + P[17][5]*SK_TAS[1] - P[17][23]*SK_TAS[1] + P[17][6]*vd*SH_TAS[0]); + Kfusion[18] = SK_TAS[0]*(P[18][4]*SH_TAS[2] - P[18][22]*SH_TAS[2] + P[18][5]*SK_TAS[1] - P[18][23]*SK_TAS[1] + P[18][6]*vd*SH_TAS[0]); + Kfusion[19] = SK_TAS[0]*(P[19][4]*SH_TAS[2] - P[19][22]*SH_TAS[2] + P[19][5]*SK_TAS[1] - P[19][23]*SK_TAS[1] + P[19][6]*vd*SH_TAS[0]); + Kfusion[20] = SK_TAS[0]*(P[20][4]*SH_TAS[2] - P[20][22]*SH_TAS[2] + P[20][5]*SK_TAS[1] - P[20][23]*SK_TAS[1] + P[20][6]*vd*SH_TAS[0]); + Kfusion[21] = SK_TAS[0]*(P[21][4]*SH_TAS[2] - P[21][22]*SH_TAS[2] + P[21][5]*SK_TAS[1] - P[21][23]*SK_TAS[1] + P[21][6]*vd*SH_TAS[0]); + Kfusion[22] = SK_TAS[0]*(P[22][4]*SH_TAS[2] - P[22][22]*SH_TAS[2] + P[22][5]*SK_TAS[1] - P[22][23]*SK_TAS[1] + P[22][6]*vd*SH_TAS[0]); + Kfusion[23] = SK_TAS[0]*(P[23][4]*SH_TAS[2] - P[23][22]*SH_TAS[2] + P[23][5]*SK_TAS[1] - P[23][23]*SK_TAS[1] + P[23][6]*vd*SH_TAS[0]); // calculate measurement innovation @@ -182,19 +143,6 @@ void Ekf::fuseAirspeed() // airspeed measurement sample has passed check so record it _time_last_arsp_fuse = _time_last_imu; - - // by definition the angle error state is zero at the fusion time - _state.ang_error.setZero(); - - // Fuse airspeed measurement - fuse(Kfusion, _airspeed_innov); //Why calculate angle error when it is always zero? - - // correct the nominal quaternion - Quaternion dq; - dq.from_axis_angle(_state.ang_error); - _state.quat_nominal = dq * _state.quat_nominal; - _state.quat_nominal.normalize(); - // update covariance matrix via Pnew = (I - KH)P = P - KHP for (unsigned row = 0; row < _k_num_states; row++) { for (unsigned column = 0; column < _k_num_states; column++) { // Here it will be a lot of zeros, should optimize that... @@ -210,13 +158,40 @@ void Ekf::fuseAirspeed() } } - for (unsigned row = 0; row < _k_num_states; row++) { - for (unsigned column = 0; column < _k_num_states; column++) { - P[row][column] = P[row][column] - KHP[row][column]; + // if the covariance correction will result in a negative variance, then + // the covariance marix is unhealthy and must be corrected + bool healthy = true; + _fault_status.flags.bad_airspeed = false; + for (int i = 0; i < _k_num_states; i++) { + if (P[i][i] < KHP[i][i]) { + // zero rows and columns + zeroRows(P,i,i); + zeroCols(P,i,i); + + //flag as unhealthy + healthy = false; + + // update individual measurement health status + _fault_status.flags.bad_airspeed = true; + } } - makeSymmetrical(); - limitCov(); + // only apply covariance and state corrrections if healthy + if (healthy) { + // apply the covariance corrections + for (unsigned row = 0; row < _k_num_states; row++) { + for (unsigned column = 0; column < _k_num_states; column++) { + P[row][column] = P[row][column] - KHP[row][column]; + } + } + + // correct the covariance marix for gross errors + fixCovarianceErrors(); + + // apply the state corrections + fuse(Kfusion, _airspeed_innov); + + } } } diff --git a/EKF/common.h b/EKF/common.h index 891f77d64a..3b8df55ff5 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -139,12 +139,12 @@ struct flowSample { // Bit locations for fusion_mode #define MASK_USE_GPS (1<<0) // set to true to use GPS data #define MASK_USE_OF (1<<1) // set to true to use optical flow data +#define MASK_INHIBIT_ACC_BIAS (1<<2) // set to true to inhibit estimation of accelerometer delta velocity bias // Integer definitions for mag_fusion_type #define MAG_FUSE_TYPE_AUTO 0 // The selection of either heading or 3D magnetometer fusion will be automatic #define MAG_FUSE_TYPE_HEADING 1 // Simple yaw angle fusion will always be used. This is less accurate, but less affected by earth field distortions. It should not be used for pitch angles outside the range from -60 to +60 deg #define MAG_FUSE_TYPE_3D 2 // Magnetometer 3-axis fusion will always be used. This is more accurate, but more affected by localised earth field distortions -#define MAG_FUSE_TYPE_2D 3 // A 2D fusion that uses the horizontal projection of the magnetic fields measurement will alays be used. This is less accurate, but less affected by earth field distortions. // Maximum sensor intervals in usec #define GPS_MAX_INTERVAL 5e5 @@ -169,9 +169,8 @@ struct parameters { float accel_noise; // IMU acceleration noise use for covariance prediction (m/sec/sec) // process noise - float gyro_bias_p_noise; // process noise for IMU delta angle bias prediction (rad/sec) - float accel_bias_p_noise; // process noise for IMU delta velocity bias prediction (m/sec/sec) - float gyro_scale_p_noise; // process noise for gyro scale factor prediction (N/A) + float gyro_bias_p_noise; // process noise for IMU rate gyro bias prediction (rad/sec**2) + float accel_bias_p_noise; // process noise for IMU accelerometer bias prediction (m/sec**3) float mage_p_noise; // process noise for earth magnetic field prediction (Guass/sec) float magb_p_noise; // process noise for body magnetic field prediction (Guass/sec) float wind_vel_p_noise; // process noise for wind velocity prediction (m/sec/sec) @@ -246,15 +245,14 @@ struct parameters { range_delay_ms = 5.0f; // input noise - gyro_noise = 6.0e-2f; - accel_noise = 2.5e-1f; + gyro_noise = 1.5e-2f; + accel_noise = 3.5e-1f; // process noise - gyro_bias_p_noise = 2.5e-6f; - accel_bias_p_noise = 3.0e-5f; - gyro_scale_p_noise = 3.0e-4f; - mage_p_noise = 2.5e-3f; - magb_p_noise = 5.0e-4f; + gyro_bias_p_noise = 1.0e-3f; + accel_bias_p_noise = 3.0e-3f; + mage_p_noise = 1.0e-3f; + magb_p_noise = 1.0e-4f; wind_vel_p_noise = 1.0e-1f; terrain_p_noise = 5.0f; terrain_gradient = 0.5f; @@ -275,7 +273,7 @@ struct parameters { mag_declination_deg = 0.0f; heading_innov_gate = 2.6f; mag_innov_gate = 3.0f; - mag_declination_source = 3; + mag_declination_source = 7; mag_fusion_type = 0; // airspeed fusion @@ -313,28 +311,36 @@ struct parameters { }; struct stateSample { - Vector3f ang_error; // attitude axis angle error (error state formulation) + Quaternion quat_nominal; // quaternion defining the rotaton from earth to body frame Vector3f vel; // NED velocity in earth frame in m/s Vector3f pos; // NED position in earth frame in m Vector3f gyro_bias; // gyro bias estimate in rad/s - Vector3f gyro_scale; // gyro scale estimate - float accel_z_bias; // accelerometer z axis bias estimate + Vector3f accel_bias; // accelerometer bias estimate in m/s Vector3f mag_I; // NED earth magnetic field in gauss Vector3f mag_B; // magnetometer bias estimate in body frame in gauss Vector2f wind_vel; // wind velocity in m/s - Quaternion quat_nominal; // nominal quaternion describing vehicle attitude }; -struct fault_status_t { - bool bad_mag_x: 1; // true if the fusion of the magnetometer X-axis has encountered a numerical error - bool bad_mag_y: 1; // true if the fusion of the magnetometer Y-axis has encountered a numerical error - bool bad_mag_z: 1; // true if the fusion of the magnetometer Z-axis has encountered a numerical error - bool bad_mag_hdg: 1; // true if the fusion of the magnetic heading has encountered a numerical error - bool bad_mag_decl: 1; // true if the fusion of the magnetic declination has encountered a numerical error - bool bad_airspeed: 1; // true if fusion of the airspeed has encountered a numerical error - bool bad_sideslip: 1; // true if fusion of the synthetic sideslip constraint has encountered a numerical error - bool bad_optflow_X: 1; // true if fusion of the optical flow X axis has encountered a numerical error - bool bad_optflow_Y: 1; // true if fusion of the optical flow Y axis has encountered a numerical error +union fault_status_u { + struct { + bool bad_mag_x: 1; // 0 - true if the fusion of the magnetometer X-axis has encountered a numerical error + bool bad_mag_y: 1; // 1 - true if the fusion of the magnetometer Y-axis has encountered a numerical error + bool bad_mag_z: 1; // 2 - true if the fusion of the magnetometer Z-axis has encountered a numerical error + bool bad_mag_hdg: 1; // 3 - true if the fusion of the magnetic heading has encountered a numerical error + bool bad_mag_decl: 1; // 4 - true if the fusion of the magnetic declination has encountered a numerical error + bool bad_airspeed: 1; // 5 - true if fusion of the airspeed has encountered a numerical error + bool bad_sideslip: 1; // 6 - true if fusion of the synthetic sideslip constraint has encountered a numerical error + bool bad_optflow_X: 1; // 7 - true if fusion of the optical flow X axis has encountered a numerical error + bool bad_optflow_Y: 1; // 8 - true if fusion of the optical flow Y axis has encountered a numerical error + bool bad_vel_N: 1; // 9 - true if fusion of the North velocity has encountered a numerical error + bool bad_vel_E: 1; // 10 - true if fusion of the East velocity has encountered a numerical error + bool bad_vel_D: 1; // 11 - true if fusion of the Down velocity has encountered a numerical error + bool bad_pos_N: 1; // 12 - true if fusion of the North position has encountered a numerical error + bool bad_pos_E: 1; // 13 - true if fusion of the East position has encountered a numerical error + bool bad_pos_D: 1; // 14 - true if fusion of the Down position has encountered a numerical error + } flags; + uint16_t value; + }; // publish the status of various GPS quality checks @@ -362,15 +368,14 @@ union filter_control_status_u { uint16_t gps : 1; // 2 - true if GPS measurements are being fused uint16_t opt_flow : 1; // 3 - true if optical flow measurements are being fused uint16_t mag_hdg : 1; // 4 - true if a simple magnetic yaw heading is being fused - uint16_t mag_2D : 1; // 5 - true if the horizontal projection of magnetometer data is being fused - uint16_t mag_3D : 1; // 6 - true if 3-axis magnetometer measurement are being fused - uint16_t mag_dec : 1; // 7 - true if synthetic magnetic declination measurements are being fused - uint16_t in_air : 1; // 8 - true when the vehicle is airborne - uint16_t armed : 1; // 9 - true when the vehicle motors are armed - uint16_t wind : 1; // 10 - true when wind velocity is being estimated - uint16_t baro_hgt : 1; // 11 - true when baro height is being fused as a primary height reference - uint16_t rng_hgt : 1; // 12 - true when range finder height is being fused as a primary height reference - uint16_t gps_hgt : 1; // 15 - true when range finder height is being fused as a primary height reference + uint16_t mag_3D : 1; // 5 - true if 3-axis magnetometer measurement are being fused + uint16_t mag_dec : 1; // 6 - true if synthetic magnetic declination measurements are being fused + uint16_t in_air : 1; // 7 - true when the vehicle is airborne + uint16_t armed : 1; // 8 - true when the vehicle motors are armed + uint16_t wind : 1; // 9 - true when wind velocity is being estimated + uint16_t baro_hgt : 1; // 10 - true when baro height is being fused as a primary height reference + uint16_t rng_hgt : 1; // 11 - true when range finder height is being fused as a primary height reference + uint16_t gps_hgt : 1; // 12 - true when range finder height is being fused as a primary height reference } flags; uint16_t value; }; diff --git a/EKF/control.cpp b/EKF/control.cpp index 3ad036e769..e5a1e5aa8f 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -51,13 +51,9 @@ void Ekf::controlFusionModes() // Get the magnetic declination calcMagDeclination(); - // Check for tilt convergence during initial alignment - // filter the tilt error vector using a 1 sec time constant LPF - float filt_coef = 1.0f * _imu_sample_delayed.delta_ang_dt; - _tilt_err_length_filt = filt_coef * _tilt_err_vec.norm() + (1.0f - filt_coef) * _tilt_err_length_filt; - - // Once the tilt error has reduced sufficiently, initialise the yaw and magnetic field states - if (_tilt_err_length_filt < 0.005f && !_control_status.flags.tilt_align) { + // Once the angular uncertainty has reduced sufficiently, initialise the yaw and magnetic field states + float total_angle_variance = P[0][0] + P[1][1] + P[2][2] + P[3][3]; + if (total_angle_variance < 0.002f && !_control_status.flags.tilt_align) { _control_status.flags.tilt_align = true; _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); } @@ -77,18 +73,16 @@ void Ekf::controlFusionModes() _control_status.flags.opt_flow = true; _time_last_of_fuse = _time_last_imu; - // if we are not using GPS and are in air, then we need to reset the velocity to be consistent with the optical flow reading + // if we are not using GPS then the velocity and position states and covariances need to be set if (!_control_status.flags.gps) { - // calculate the rotation matrix from body to earth frame - matrix::Dcm body_to_earth(_state.quat_nominal); - // constrain height above ground to be above minimum possible float heightAboveGndEst = fmaxf((_terrain_vpos - _state.pos(2)), _params.rng_gnd_clearance); // calculate absolute distance from focal point to centre of frame assuming a flat earth - float range = heightAboveGndEst / body_to_earth(2, 2); + float range = heightAboveGndEst / _R_to_earth(2, 2); - if (_in_air && (range - _params.rng_gnd_clearance) > 0.3f && _flow_sample_delayed.dt > 0.05f) { + if ((range - _params.rng_gnd_clearance) > 0.3f && _flow_sample_delayed.dt > 0.05f) { + // we should ahve reliable OF measurements so // calculate X and Y body relative velocities from OF measurements Vector3f vel_optflow_body; vel_optflow_body(0) = - range * _flow_sample_delayed.flowRadXYcomp(1) / _flow_sample_delayed.dt; @@ -97,14 +91,36 @@ void Ekf::controlFusionModes() // rotate from body to earth frame Vector3f vel_optflow_earth; - vel_optflow_earth = body_to_earth * vel_optflow_body; + vel_optflow_earth = _R_to_earth * vel_optflow_body; // take x and Y components _state.vel(0) = vel_optflow_earth(0); _state.vel(1) = vel_optflow_earth(1); } else { - _state.vel.setZero(); + _state.vel(0) = 0.0f; + _state.vel(1) = 0.0f; + } + + // reset the velocity covariance terms + zeroRows(P,4,5); + zeroCols(P,4,5); + + // reset the horizontal velocity variance using the optical flow noise variance + P[5][5] = P[4][4] = sq(range) * calcOptFlowMeasVar(); + + if (!_in_air) { + // we are likely starting OF for the first time so reset the horizontal position and vertical velocity states + _state.pos(0) = 0.0f; + _state.pos(1) = 0.0f; + + // reset the coresponding covariances + // we are by definition at the origin at commencement so variances are also zeroed + zeroRows(P,7,8); + zeroCols(P,7,8); + + // align the output observer to the EKF states + alignOutputFilter(); } } } @@ -295,18 +311,12 @@ void Ekf::controlFusionModes() // check if baro data is available baroSample baro_init = _baro_buffer.get_newest(); bool baro_data_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL); - // check if baro data is consistent - float baro_innov = _state.pos(2) - (_hgt_sensor_offset - baro_init.hgt + _baro_hgt_offset); - bool baro_data_consistent = sq(baro_innov) < (sq(_params.baro_noise) + P[8][8]) * sq(_params.baro_innov_gate); - // reset to baro if data is available and we have no range data + // reset to baro if we have no range data and baro data is available bool reset_to_baro = !rng_data_available && baro_data_available; - // reset to baro if data is acceptable - reset_to_baro = reset_to_baro || (baro_data_consistent && baro_data_available && !_baro_hgt_faulty); - - // reset to range data if it is available and we cannot switch to baro - bool reset_to_rng = !reset_to_baro && rng_data_available; + // reset to range data if it is available + bool reset_to_rng = rng_data_available; if (reset_to_baro) { // set height sensor health @@ -363,7 +373,6 @@ void Ekf::controlFusionModes() if (!_control_status.flags.armed) { // use heading fusion for initial startup _control_status.flags.mag_hdg = true; - _control_status.flags.mag_2D = false; _control_status.flags.mag_3D = false; } else { @@ -375,13 +384,11 @@ void Ekf::controlFusionModes() // use 3D mag fusion when airborne _control_status.flags.mag_hdg = false; - _control_status.flags.mag_2D = false; _control_status.flags.mag_3D = true; } else { // use heading fusion when on the ground _control_status.flags.mag_hdg = true; - _control_status.flags.mag_2D = false; _control_status.flags.mag_3D = false; } } @@ -389,13 +396,6 @@ void Ekf::controlFusionModes() } else if (_params.mag_fusion_type == MAG_FUSE_TYPE_HEADING) { // always use heading fusion _control_status.flags.mag_hdg = true; - _control_status.flags.mag_2D = false; - _control_status.flags.mag_3D = false; - - } else if (_params.mag_fusion_type == MAG_FUSE_TYPE_2D) { - // always use 2D mag fusion - _control_status.flags.mag_hdg = false; - _control_status.flags.mag_2D = true; _control_status.flags.mag_3D = false; } else if (_params.mag_fusion_type == MAG_FUSE_TYPE_3D) { @@ -406,13 +406,11 @@ void Ekf::controlFusionModes() // always use 3-axis mag fusion _control_status.flags.mag_hdg = false; - _control_status.flags.mag_2D = false; _control_status.flags.mag_3D = true; } else { // do no magnetometer fusion at all _control_status.flags.mag_hdg = false; - _control_status.flags.mag_2D = false; _control_status.flags.mag_3D = false; } diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 5752395781..1947505877 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -55,34 +55,39 @@ void Ekf::initialiseCovariance() // calculate average prediction time step in sec float dt = 0.001f * (float)FILTER_UPDATE_PERRIOD_MS; - // XXX use initial guess for the diagonal elements for the covariance matrix - // angle error - P[0][0] = 0.1f; - P[1][1] = 0.1f; - P[2][2] = 0.1f; + // quaternion error + P[0][0] = 0.01f; + P[1][1] = 0.01f; + P[2][2] = 0.01f; + P[3][3] = 0.01f; // velocity - P[3][3] = sq(fmaxf(_params.gps_vel_noise, 0.01f)); - P[4][4] = P[3][3]; - P[5][5] = sq(1.5f) * P[3][3]; + P[4][4] = sq(fmaxf(_params.gps_vel_noise, 0.01f)); + P[5][5] = P[4][4]; + P[6][6] = sq(1.5f) * P[4][4]; // position - P[6][6] = sq(fmaxf(_params.gps_pos_noise, 0.01f)); - P[7][7] = P[6][6]; - P[8][8] = sq(fmaxf(_params.baro_noise, 0.01f)); + P[7][7] = sq(fmaxf(_params.gps_pos_noise, 0.01f)); + P[8][8] = P[7][7]; + if (_control_status.flags.rng_hgt) { + P[9][9] = sq(fmaxf(_params.range_noise, 0.01f)); + } else if (_control_status.flags.gps_hgt) { + float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f); + float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit); + P[9][9] = sq(1.5f * math::constrain(_gps_sample_delayed.vacc, lower_limit, upper_limit)); + } else { + P[9][9] = sq(fmaxf(_params.baro_noise, 0.01f)); + } // gyro bias - P[9][9] = sq(0.035f * dt); - P[10][10] = P[9][9]; - P[11][11] = P[9][9]; + P[10][10] = sq(0.1f * dt); + P[11][11] = P[10][10]; + P[12][12] = P[10][10]; - // gyro scale - P[12][12] = sq(0.001f); - P[13][13] = P[12][12]; - P[14][14] = P[12][12]; - - // accel z bias - P[15][15] = sq(0.2f * dt); + // accel bias + P[13][13] = sq(0.2f * dt); + P[14][14] = P[13][13]; + P[15][15] = P[13][13]; // variances for optional states // these state variances are set to zero until the states are required, then they must be initialised @@ -105,16 +110,16 @@ void Ekf::initialiseCovariance() void Ekf::get_pos_var(Vector3f &pos_var) { - pos_var(0) = P[6][6]; - pos_var(1) = P[7][7]; - pos_var(2) = P[8][8]; + pos_var(0) = P[7][7]; + pos_var(1) = P[8][8]; + pos_var(2) = P[9][9]; } void Ekf::get_vel_var(Vector3f &vel_var) { - vel_var(0) = P[3][3]; - vel_var(1) = P[4][4]; - vel_var(2) = P[5][5]; + vel_var(0) = P[4][4]; + vel_var(1) = P[5][5]; + vel_var(2) = P[6][6]; } void Ekf::predictCovariance() @@ -137,33 +142,34 @@ void Ekf::predictCovariance() float day_b = _state.gyro_bias(1); float daz_b = _state.gyro_bias(2); - float dax_s = _state.gyro_scale(0); - float day_s = _state.gyro_scale(1); - float daz_s = _state.gyro_scale(2); + float dvx_b = _state.accel_bias(0); + float dvy_b = _state.accel_bias(1); + float dvz_b = _state.accel_bias(2); - float dvz_b = _state.accel_z_bias; + float dt = _imu_sample_delayed.delta_ang_dt; - float dt = _imu_sample_delayed.delta_vel_dt; - - // compute process noise + // compute noise variance for stationary processes float process_noise[_k_num_states] = {}; - float d_ang_bias_sig = dt * math::constrain(_params.gyro_bias_p_noise, 0.0f, 1e-4f); - float d_vel_bias_sig = dt * math::constrain(_params.accel_bias_p_noise, 0.0f, 1e-2f); - float d_ang_scale_sig = dt * math::constrain(_params.gyro_scale_p_noise, 0.0f, 1e-2f); - float mag_I_sig, mag_B_sig; + // convert rate of change of rate gyro bias (rad/s**2) as specified by the parameter to an expected change in delta angle (rad) since the last update + float d_ang_bias_sig = dt * dt * math::constrain(_params.gyro_bias_p_noise, 0.0f, 1.0f); + + // convert rate of change of acceerometer bias (m/s**3) as specified by the parameter to an expected change in delta velocity (m/s) since the last update + float d_vel_bias_sig = dt * dt * math::constrain(_params.accel_bias_p_noise, 0.0f, 1.0f); // Don't continue to grow the earth field variances if they are becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned + float mag_I_sig; if (_control_status.flags.mag_3D && (P[16][16] + P[17][17] + P[18][8]) < 0.1f) { - mag_I_sig = dt * math::constrain(_params.mage_p_noise, 0.0f, 1e-1f); + mag_I_sig = dt * math::constrain(_params.mage_p_noise, 0.0f, 1.0f); } else { mag_I_sig = 0.0f; } // Don't continue to grow the body field variances if they is becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned + float mag_B_sig; if (_control_status.flags.mag_3D && (P[19][19] + P[20][20] + P[21][21]) < 0.1f) { - mag_B_sig = dt * math::constrain(_params.magb_p_noise, 0.0f, 1e-1f); + mag_B_sig = dt * math::constrain(_params.magb_p_noise, 0.0f, 1.0f); } else { mag_B_sig = 0.0f; @@ -179,332 +185,250 @@ void Ekf::predictCovariance() wind_vel_sig = 0.0f; } - for (unsigned i = 0; i < 9; i++) { + // Construct the process noise variance diagonal for those states with a stationary process model + // These are kinematic states and their error growth is controlled separately by the IMU noise variances + for (unsigned i = 0; i <= 9; i++) { process_noise[i] = 0.0f; } + // delta angle bias states + process_noise[12] = process_noise[11] = process_noise[10] = sq(d_ang_bias_sig); + // delta_velocity bias states + process_noise[15] = process_noise[14] = process_noise[13] = sq(d_vel_bias_sig); + // earth frame magnetic field states + process_noise[18] = process_noise[17] = process_noise[16] = sq(mag_I_sig); + // body frame magnetic field states + process_noise[21] = process_noise[20] = process_noise[19] = sq(mag_B_sig); + // wind velocity states + process_noise[23] = process_noise[22] = sq(wind_vel_sig); - for (unsigned i = 9; i < 12; i++) { - process_noise[i] = sq(d_ang_bias_sig); - } - - for (unsigned i = 12; i < 15; i++) { - process_noise[i] = sq(d_ang_scale_sig); - } - - process_noise[15] = sq(d_vel_bias_sig); - - for (unsigned i = 16; i < 19; i++) { - process_noise[i] = sq(mag_I_sig); - } - - for (unsigned i = 19; i < 22; i++) { - process_noise[i] = sq(mag_B_sig); - } - - for (unsigned i = 22; i < 24; i++) { - process_noise[i] = sq(wind_vel_sig); - } - - - // assign input noise + // assign IMU noise variances // inputs to the system are 3 delta angles and 3 delta velocities - float daxNoise, dayNoise, dazNoise; - float dvxNoise, dvyNoise, dvzNoise; - float gyro_noise = math::constrain(_params.gyro_noise, 1e-4f, 1e-1f); - daxNoise = dayNoise = dazNoise = dt * gyro_noise; - float accel_noise = math::constrain(_params.accel_noise, 1e-2f, 1.0f); - dvxNoise = dvyNoise = dvzNoise = dt * accel_noise; + float daxVar, dayVar, dazVar; + float dvxVar, dvyVar, dvzVar; + float gyro_noise = math::constrain(_params.gyro_noise, 0.0f, 1.0f); + daxVar = dayVar = dazVar = sq(dt * gyro_noise); + float accel_noise = math::constrain(_params.accel_noise, 0.0f, 1.0f); + dvxVar = dvyVar = dvzVar = sq(dt * accel_noise); - // predict covarinace matrix + // predict the covariance // intermediate calculations - float SF[25] = {}; - SF[0] = daz_b / 2 + dazNoise / 2 - (daz * daz_s) / 2; - SF[1] = day_b / 2 + dayNoise / 2 - (day * day_s) / 2; - SF[2] = dax_b / 2 + daxNoise / 2 - (dax * dax_s) / 2; - SF[3] = q3 / 2 - (q0 * SF[0]) / 2 + (q1 * SF[1]) / 2 - (q2 * SF[2]) / 2; - SF[4] = q0 / 2 - (q1 * SF[2]) / 2 - (q2 * SF[1]) / 2 + (q3 * SF[0]) / 2; - SF[5] = q1 / 2 + (q0 * SF[2]) / 2 - (q2 * SF[0]) / 2 - (q3 * SF[1]) / 2; - SF[6] = q3 / 2 + (q0 * SF[0]) / 2 - (q1 * SF[1]) / 2 - (q2 * SF[2]) / 2; - SF[7] = q0 / 2 - (q1 * SF[2]) / 2 + (q2 * SF[1]) / 2 - (q3 * SF[0]) / 2; - SF[8] = q0 / 2 + (q1 * SF[2]) / 2 - (q2 * SF[1]) / 2 - (q3 * SF[0]) / 2; - SF[9] = q2 / 2 + (q0 * SF[1]) / 2 + (q1 * SF[0]) / 2 + (q3 * SF[2]) / 2; - SF[10] = q2 / 2 - (q0 * SF[1]) / 2 - (q1 * SF[0]) / 2 + (q3 * SF[2]) / 2; - SF[11] = q2 / 2 + (q0 * SF[1]) / 2 - (q1 * SF[0]) / 2 - (q3 * SF[2]) / 2; - SF[12] = q1 / 2 + (q0 * SF[2]) / 2 + (q2 * SF[0]) / 2 + (q3 * SF[1]) / 2; - SF[13] = q1 / 2 - (q0 * SF[2]) / 2 + (q2 * SF[0]) / 2 - (q3 * SF[1]) / 2; - SF[14] = q3 / 2 + (q0 * SF[0]) / 2 + (q1 * SF[1]) / 2 + (q2 * SF[2]) / 2; - SF[15] = - sq(q0) - sq(q1) - sq(q2) - sq(q3); - SF[16] = dvz_b - dvz + dvzNoise; - SF[17] = dvx - dvxNoise; - SF[18] = dvy - dvyNoise; - SF[19] = sq(q2); - SF[20] = SF[19] - sq(q0) + sq(q1) - sq(q3); - SF[21] = SF[19] + sq(q0) - sq(q1) - sq(q3); - SF[22] = 2 * q0 * q1 - 2 * q2 * q3; - SF[23] = SF[19] - sq(q0) - sq(q1) + sq(q3); - SF[24] = 2 * q1 * q2; + float SF[21]; + SF[0] = dvz - dvz_b; + SF[1] = dvy - dvy_b; + SF[2] = dvx - dvx_b; + SF[3] = 2*q1*SF[2] + 2*q2*SF[1] + 2*q3*SF[0]; + SF[4] = 2*q0*SF[1] - 2*q1*SF[0] + 2*q3*SF[2]; + SF[5] = 2*q0*SF[2] + 2*q2*SF[0] - 2*q3*SF[1]; + SF[6] = day/2 - day_b/2; + SF[7] = daz/2 - daz_b/2; + SF[8] = dax/2 - dax_b/2; + SF[9] = dax_b/2 - dax/2; + SF[10] = daz_b/2 - daz/2; + SF[11] = day_b/2 - day/2; + SF[12] = 2*q1*SF[1]; + SF[13] = 2*q0*SF[0]; + SF[14] = q1/2; + SF[15] = q2/2; + SF[16] = q3/2; + SF[17] = sq(q3); + SF[18] = sq(q2); + SF[19] = sq(q1); + SF[20] = sq(q0); - float SG[5] = {}; - SG[0] = - sq(q0) - sq(q1) - sq(q2) - sq(q3); + float SG[8]; + SG[0] = q0/2; SG[1] = sq(q3); SG[2] = sq(q2); SG[3] = sq(q1); SG[4] = sq(q0); + SG[5] = 2*q2*q3; + SG[6] = 2*q1*q3; + SG[7] = 2*q1*q2; - float SQ[10] = {}; - SQ[0] = - sq(dvyNoise)*(2*q0*q1 + 2*q2*q3)*(SG[1] - SG[2] + SG[3] - SG[4]) - sq(dvzNoise)*(2*q0*q1 - 2*q2*q3)*(SG[1] - SG[2] - SG[3] + SG[4]) - sq(dvxNoise)*(2*q0*q2 - 2*q1*q3)*(2*q0*q3 + 2*q1*q2); - SQ[1] = sq(dvxNoise)*(2*q0*q2 - 2*q1*q3)*(SG[1] + SG[2] - SG[3] - SG[4]) + sq(dvzNoise)*(2*q0*q2 + 2*q1*q3)*(SG[1] - SG[2] - SG[3] + SG[4]) - sq(dvyNoise)*(2*q0*q1 + 2*q2*q3)*(2*q0*q3 - 2*q1*q2); - SQ[2] = sq(dvyNoise)*(2*q0*q3 - 2*q1*q2)*(SG[1] - SG[2] + SG[3] - SG[4]) - sq(dvxNoise)*(2*q0*q3 + 2*q1*q2)*(SG[1] + SG[2] - SG[3] - SG[4]) - sq(dvzNoise)*(2*q0*q1 - 2*q2*q3)*(2*q0*q2 + 2*q1*q3); - SQ[3] = sq(SG[0]); - SQ[4] = sq(dvyNoise); - SQ[5] = sq(dvzNoise); - SQ[6] = sq(dvxNoise); - SQ[7] = 2*q2*q3; - SQ[8] = 2*q1*q3; - SQ[9] = 2*q1*q2; + float SQ[11]; + SQ[0] = dvzVar*(SG[5] - 2*q0*q1)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvyVar*(SG[5] + 2*q0*q1)*(SG[1] - SG[2] + SG[3] - SG[4]) + dvxVar*(SG[6] - 2*q0*q2)*(SG[7] + 2*q0*q3); + SQ[1] = dvzVar*(SG[6] + 2*q0*q2)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvxVar*(SG[6] - 2*q0*q2)*(SG[1] + SG[2] - SG[3] - SG[4]) + dvyVar*(SG[5] + 2*q0*q1)*(SG[7] - 2*q0*q3); + SQ[2] = dvzVar*(SG[5] - 2*q0*q1)*(SG[6] + 2*q0*q2) - dvyVar*(SG[7] - 2*q0*q3)*(SG[1] - SG[2] + SG[3] - SG[4]) - dvxVar*(SG[7] + 2*q0*q3)*(SG[1] + SG[2] - SG[3] - SG[4]); + SQ[3] = (dayVar*q1*SG[0])/2 - (dazVar*q1*SG[0])/2 - (daxVar*q2*q3)/4; + SQ[4] = (dazVar*q2*SG[0])/2 - (daxVar*q2*SG[0])/2 - (dayVar*q1*q3)/4; + SQ[5] = (daxVar*q3*SG[0])/2 - (dayVar*q3*SG[0])/2 - (dazVar*q1*q2)/4; + SQ[6] = (daxVar*q1*q2)/4 - (dazVar*q3*SG[0])/2 - (dayVar*q1*q2)/4; + SQ[7] = (dazVar*q1*q3)/4 - (daxVar*q1*q3)/4 - (dayVar*q2*SG[0])/2; + SQ[8] = (dayVar*q2*q3)/4 - (daxVar*q1*SG[0])/2 - (dazVar*q2*q3)/4; + SQ[9] = sq(SG[0]); + SQ[10] = sq(q1); - float SPP[23] = {}; - SPP[0] = SF[17]*(2*q0*q1 + 2*q2*q3) + SF[18]*(2*q0*q2 - 2*q1*q3); - SPP[1] = SF[18]*(2*q0*q2 + 2*q1*q3) + SF[16]*(SF[24] - 2*q0*q3); - SPP[2] = 2*q3*SF[8] + 2*q1*SF[11] - 2*q0*SF[14] - 2*q2*SF[13]; - SPP[3] = 2*q1*SF[7] + 2*q2*SF[6] - 2*q0*SF[12] - 2*q3*SF[10]; - SPP[4] = 2*q0*SF[6] - 2*q3*SF[7] - 2*q1*SF[10] + 2*q2*SF[12]; - SPP[5] = 2*q0*SF[8] + 2*q2*SF[11] + 2*q1*SF[13] + 2*q3*SF[14]; - SPP[6] = 2*q0*SF[7] + 2*q3*SF[6] + 2*q2*SF[10] + 2*q1*SF[12]; - SPP[7] = 2*q1*SF[3] - 2*q2*SF[4] - 2*q3*SF[5] + 2*q0*SF[9]; - SPP[8] = 2*q0*SF[5] - 2*q1*SF[4] - 2*q2*SF[3] + 2*q3*SF[9]; - SPP[9] = SF[18]*SF[20] - SF[16]*(2*q0*q1 + 2*q2*q3); - SPP[10] = SF[17]*SF[20] + SF[16]*(2*q0*q2 - 2*q1*q3); - SPP[11] = SF[17]*SF[21] - SF[18]*(SF[24] + 2*q0*q3); - SPP[12] = SF[17]*SF[22] - SF[16]*(SF[24] + 2*q0*q3); - SPP[13] = 2*q0*SF[4] + 2*q1*SF[5] + 2*q3*SF[3] + 2*q2*SF[9]; - SPP[14] = 2*q2*SF[8] - 2*q0*SF[11] - 2*q1*SF[14] + 2*q3*SF[13]; - SPP[15] = SF[18]*SF[23] + SF[17]*(SF[24] - 2*q0*q3); - SPP[16] = daz*SF[19] + daz*sq(q0) + daz*sq(q1) + daz*sq(q3); - SPP[17] = day*SF[19] + day*sq(q0) + day*sq(q1) + day*sq(q3); - SPP[18] = dax*SF[19] + dax*sq(q0) + dax*sq(q1) + dax*sq(q3); - SPP[19] = SF[16]*SF[23] - SF[17]*(2*q0*q2 + 2*q1*q3); - SPP[20] = SF[16]*SF[21] - SF[18]*SF[22]; - SPP[21] = 2*q0*q2 + 2*q1*q3; - SPP[22] = SF[15]; + float SPP[11] = {}; + SPP[0] = SF[12] + SF[13] - 2*q2*SF[2]; + SPP[1] = SF[17] - SF[18] - SF[19] + SF[20]; + SPP[2] = SF[17] - SF[18] + SF[19] - SF[20]; + SPP[3] = SF[17] + SF[18] - SF[19] - SF[20]; + SPP[4] = 2*q0*q2 - 2*q1*q3; + SPP[5] = 2*q0*q1 - 2*q2*q3; + SPP[6] = 2*q0*q3 - 2*q1*q2; + SPP[7] = 2*q0*q1 + 2*q2*q3; + SPP[8] = 2*q0*q3 + 2*q1*q2; + SPP[9] = 2*q0*q2 + 2*q1*q3; + SPP[10] = SF[16]; // covariance update - float nextP[24][24] = {}; - nextP[0][0] = SPP[5]*(P[0][0]*SPP[5] - P[1][0]*SPP[4] + P[2][0]*SPP[7] + P[9][0]*SPP[22] + P[12][0]*SPP[18]) - SPP[4]*(P[0][1]*SPP[5] - P[1][1]*SPP[4] + P[2][1]*SPP[7] + P[9][1]*SPP[22] + P[12][1]*SPP[18]) + SPP[7]*(P[0][2]*SPP[5] - P[1][2]*SPP[4] + P[2][2]*SPP[7] + P[9][2]*SPP[22] + P[12][2]*SPP[18]) + SPP[22]*(P[0][9]*SPP[5] - P[1][9]*SPP[4] + P[2][9]*SPP[7] + P[9][9]*SPP[22] + P[12][9]*SPP[18]) + SPP[18]*(P[0][12]*SPP[5] - P[1][12]*SPP[4] + P[2][12]*SPP[7] + P[9][12]*SPP[22] + P[12][12]*SPP[18]) + sq(daxNoise)*SQ[3]; - nextP[0][1] = SPP[6] * (P[0][1] * SPP[5] - P[1][1] * SPP[4] + P[2][1] * SPP[7] + P[9][1] * SPP[22] + P[12][1] * SPP[18]) - - SPP[2] * (P[0][0] * SPP[5] - P[1][0] * SPP[4] + P[2][0] * SPP[7] + P[9][0] * SPP[22] + P[12][0] * SPP[18]) - - SPP[8] * (P[0][2] * SPP[5] - P[1][2] * SPP[4] + P[2][2] * SPP[7] + P[9][2] * SPP[22] + P[12][2] * SPP[18]) + SPP[22] * - (P[0][10] * SPP[5] - P[1][10] * SPP[4] + P[2][10] * SPP[7] + P[9][10] * SPP[22] + P[12][10] * SPP[18]) + SPP[17] * - (P[0][13] * SPP[5] - P[1][13] * SPP[4] + P[2][13] * SPP[7] + P[9][13] * SPP[22] + P[12][13] * SPP[18]); - nextP[1][1] = SPP[6]*(P[1][1]*SPP[6] - P[0][1]*SPP[2] - P[2][1]*SPP[8] + P[10][1]*SPP[22] + P[13][1]*SPP[17]) - SPP[2]*(P[1][0]*SPP[6] - P[0][0]*SPP[2] - P[2][0]*SPP[8] + P[10][0]*SPP[22] + P[13][0]*SPP[17]) - SPP[8]*(P[1][2]*SPP[6] - P[0][2]*SPP[2] - P[2][2]*SPP[8] + P[10][2]*SPP[22] + P[13][2]*SPP[17]) + SPP[22]*(P[1][10]*SPP[6] - P[0][10]*SPP[2] - P[2][10]*SPP[8] + P[10][10]*SPP[22] + P[13][10]*SPP[17]) + SPP[17]*(P[1][13]*SPP[6] - P[0][13]*SPP[2] - P[2][13]*SPP[8] + P[10][13]*SPP[22] + P[13][13]*SPP[17]) + sq(dayNoise)*SQ[3]; - nextP[0][2] = SPP[14] * (P[0][0] * SPP[5] - P[1][0] * SPP[4] + P[2][0] * SPP[7] + P[9][0] * SPP[22] + P[12][0] * - SPP[18]) - SPP[3] * (P[0][1] * SPP[5] - P[1][1] * SPP[4] + P[2][1] * SPP[7] + P[9][1] * SPP[22] + P[12][1] * SPP[18]) + - SPP[13] * (P[0][2] * SPP[5] - P[1][2] * SPP[4] + P[2][2] * SPP[7] + P[9][2] * SPP[22] + P[12][2] * SPP[18]) + - SPP[22] * (P[0][11] * SPP[5] - P[1][11] * SPP[4] + P[2][11] * SPP[7] + P[9][11] * SPP[22] + P[12][11] * SPP[18]) + - SPP[16] * (P[0][14] * SPP[5] - P[1][14] * SPP[4] + P[2][14] * SPP[7] + P[9][14] * SPP[22] + P[12][14] * SPP[18]); - nextP[1][2] = SPP[14] * (P[1][0] * SPP[6] - P[0][0] * SPP[2] - P[2][0] * SPP[8] + P[10][0] * SPP[22] + P[13][0] * - SPP[17]) - SPP[3] * (P[1][1] * SPP[6] - P[0][1] * SPP[2] - P[2][1] * SPP[8] + P[10][1] * SPP[22] + P[13][1] * SPP[17]) + - SPP[13] * (P[1][2] * SPP[6] - P[0][2] * SPP[2] - P[2][2] * SPP[8] + P[10][2] * SPP[22] + P[13][2] * SPP[17]) + - SPP[22] * (P[1][11] * SPP[6] - P[0][11] * SPP[2] - P[2][11] * SPP[8] + P[10][11] * SPP[22] + P[13][11] * SPP[17]) + - SPP[16] * (P[1][14] * SPP[6] - P[0][14] * SPP[2] - P[2][14] * SPP[8] + P[10][14] * SPP[22] + P[13][14] * SPP[17]); - nextP[2][2] = SPP[14]*(P[0][0]*SPP[14] - P[1][0]*SPP[3] + P[2][0]*SPP[13] + P[11][0]*SPP[22] + P[14][0]*SPP[16]) - SPP[3]*(P[0][1]*SPP[14] - P[1][1]*SPP[3] + P[2][1]*SPP[13] + P[11][1]*SPP[22] + P[14][1]*SPP[16]) + SPP[13]*(P[0][2]*SPP[14] - P[1][2]*SPP[3] + P[2][2]*SPP[13] + P[11][2]*SPP[22] + P[14][2]*SPP[16]) + SPP[22]*(P[0][11]*SPP[14] - P[1][11]*SPP[3] + P[2][11]*SPP[13] + P[11][11]*SPP[22] + P[14][11]*SPP[16]) + SPP[16]*(P[0][14]*SPP[14] - P[1][14]*SPP[3] + P[2][14]*SPP[13] + P[11][14]*SPP[22] + P[14][14]*SPP[16]) + sq(dazNoise)*SQ[3]; - nextP[0][3] = P[0][3] * SPP[5] - P[1][3] * SPP[4] + P[2][3] * SPP[7] + P[9][3] * SPP[22] + P[12][3] * SPP[18] + - SPP[1] * (P[0][0] * SPP[5] - P[1][0] * SPP[4] + P[2][0] * SPP[7] + P[9][0] * SPP[22] + P[12][0] * SPP[18]) + SPP[19] * - (P[0][1] * SPP[5] - P[1][1] * SPP[4] + P[2][1] * SPP[7] + P[9][1] * SPP[22] + P[12][1] * SPP[18]) + SPP[15] * - (P[0][2] * SPP[5] - P[1][2] * SPP[4] + P[2][2] * SPP[7] + P[9][2] * SPP[22] + P[12][2] * SPP[18]) - SPP[21] * - (P[0][15] * SPP[5] - P[1][15] * SPP[4] + P[2][15] * SPP[7] + P[9][15] * SPP[22] + P[12][15] * SPP[18]); - nextP[1][3] = P[1][3] * SPP[6] - P[0][3] * SPP[2] - P[2][3] * SPP[8] + P[10][3] * SPP[22] + P[13][3] * SPP[17] + - SPP[1] * (P[1][0] * SPP[6] - P[0][0] * SPP[2] - P[2][0] * SPP[8] + P[10][0] * SPP[22] + P[13][0] * SPP[17]) + - SPP[19] * (P[1][1] * SPP[6] - P[0][1] * SPP[2] - P[2][1] * SPP[8] + P[10][1] * SPP[22] + P[13][1] * SPP[17]) + - SPP[15] * (P[1][2] * SPP[6] - P[0][2] * SPP[2] - P[2][2] * SPP[8] + P[10][2] * SPP[22] + P[13][2] * SPP[17]) - - SPP[21] * (P[1][15] * SPP[6] - P[0][15] * SPP[2] - P[2][15] * SPP[8] + P[10][15] * SPP[22] + P[13][15] * SPP[17]); - nextP[2][3] = P[0][3] * SPP[14] - P[1][3] * SPP[3] + P[2][3] * SPP[13] + P[11][3] * SPP[22] + P[14][3] * SPP[16] + - SPP[1] * (P[0][0] * SPP[14] - P[1][0] * SPP[3] + P[2][0] * SPP[13] + P[11][0] * SPP[22] + P[14][0] * SPP[16]) + - SPP[19] * (P[0][1] * SPP[14] - P[1][1] * SPP[3] + P[2][1] * SPP[13] + P[11][1] * SPP[22] + P[14][1] * SPP[16]) + - SPP[15] * (P[0][2] * SPP[14] - P[1][2] * SPP[3] + P[2][2] * SPP[13] + P[11][2] * SPP[22] + P[14][2] * SPP[16]) - - SPP[21] * (P[0][15] * SPP[14] - P[1][15] * SPP[3] + P[2][15] * SPP[13] + P[11][15] * SPP[22] + P[14][15] * SPP[16]); - nextP[3][3] = P[3][3] + P[0][3]*SPP[1] + P[1][3]*SPP[19] + P[2][3]*SPP[15] - P[15][3]*SPP[21] + SQ[5]*sq(SQ[8] + 2*q0*q2) + SQ[4]*sq(SQ[9] - 2*q0*q3) + SPP[1]*(P[3][0] + P[0][0]*SPP[1] + P[1][0]*SPP[19] + P[2][0]*SPP[15] - P[15][0]*SPP[21]) + SPP[19]*(P[3][1] + P[0][1]*SPP[1] + P[1][1]*SPP[19] + P[2][1]*SPP[15] - P[15][1]*SPP[21]) + SPP[15]*(P[3][2] + P[0][2]*SPP[1] + P[1][2]*SPP[19] + P[2][2]*SPP[15] - P[15][2]*SPP[21]) - SPP[21]*(P[3][15] + P[0][15]*SPP[1] + P[1][15]*SPP[19] + P[2][15]*SPP[15] - P[15][15]*SPP[21]) + SQ[6]*sq(SG[1] + SG[2] - SG[3] - SG[4]); - nextP[0][4] = P[0][4] * SPP[5] - P[1][4] * SPP[4] + P[2][4] * SPP[7] + P[9][4] * SPP[22] + P[12][4] * SPP[18] + - SF[22] * (P[0][15] * SPP[5] - P[1][15] * SPP[4] + P[2][15] * SPP[7] + P[9][15] * SPP[22] + P[12][15] * SPP[18]) + - SPP[12] * (P[0][1] * SPP[5] - P[1][1] * SPP[4] + P[2][1] * SPP[7] + P[9][1] * SPP[22] + P[12][1] * SPP[18]) + - SPP[20] * (P[0][0] * SPP[5] - P[1][0] * SPP[4] + P[2][0] * SPP[7] + P[9][0] * SPP[22] + P[12][0] * SPP[18]) + - SPP[11] * (P[0][2] * SPP[5] - P[1][2] * SPP[4] + P[2][2] * SPP[7] + P[9][2] * SPP[22] + P[12][2] * SPP[18]); - nextP[1][4] = P[1][4] * SPP[6] - P[0][4] * SPP[2] - P[2][4] * SPP[8] + P[10][4] * SPP[22] + P[13][4] * SPP[17] + - SF[22] * (P[1][15] * SPP[6] - P[0][15] * SPP[2] - P[2][15] * SPP[8] + P[10][15] * SPP[22] + P[13][15] * SPP[17]) + - SPP[12] * (P[1][1] * SPP[6] - P[0][1] * SPP[2] - P[2][1] * SPP[8] + P[10][1] * SPP[22] + P[13][1] * SPP[17]) + - SPP[20] * (P[1][0] * SPP[6] - P[0][0] * SPP[2] - P[2][0] * SPP[8] + P[10][0] * SPP[22] + P[13][0] * SPP[17]) + - SPP[11] * (P[1][2] * SPP[6] - P[0][2] * SPP[2] - P[2][2] * SPP[8] + P[10][2] * SPP[22] + P[13][2] * SPP[17]); - nextP[2][4] = P[0][4] * SPP[14] - P[1][4] * SPP[3] + P[2][4] * SPP[13] + P[11][4] * SPP[22] + P[14][4] * SPP[16] + - SF[22] * (P[0][15] * SPP[14] - P[1][15] * SPP[3] + P[2][15] * SPP[13] + P[11][15] * SPP[22] + P[14][15] * SPP[16]) + - SPP[12] * (P[0][1] * SPP[14] - P[1][1] * SPP[3] + P[2][1] * SPP[13] + P[11][1] * SPP[22] + P[14][1] * SPP[16]) + - SPP[20] * (P[0][0] * SPP[14] - P[1][0] * SPP[3] + P[2][0] * SPP[13] + P[11][0] * SPP[22] + P[14][0] * SPP[16]) + - SPP[11] * (P[0][2] * SPP[14] - P[1][2] * SPP[3] + P[2][2] * SPP[13] + P[11][2] * SPP[22] + P[14][2] * SPP[16]); - nextP[3][4] = P[3][4] + SQ[2] + P[0][4] * SPP[1] + P[1][4] * SPP[19] + P[2][4] * SPP[15] - P[15][4] * SPP[21] + - SF[22] * (P[3][15] + P[0][15] * SPP[1] + P[1][15] * SPP[19] + P[2][15] * SPP[15] - P[15][15] * SPP[21]) + SPP[12] * - (P[3][1] + P[0][1] * SPP[1] + P[1][1] * SPP[19] + P[2][1] * SPP[15] - P[15][1] * SPP[21]) + SPP[20] * - (P[3][0] + P[0][0] * SPP[1] + P[1][0] * SPP[19] + P[2][0] * SPP[15] - P[15][0] * SPP[21]) + SPP[11] * - (P[3][2] + P[0][2] * SPP[1] + P[1][2] * SPP[19] + P[2][2] * SPP[15] - P[15][2] * SPP[21]); - nextP[4][4] = P[4][4] + P[15][4]*SF[22] + P[0][4]*SPP[20] + P[1][4]*SPP[12] + P[2][4]*SPP[11] + SQ[5]*sq(SQ[7] - 2*q0*q1) + SQ[6]*sq(SQ[9] + 2*q0*q3) + SF[22]*(P[4][15] + P[15][15]*SF[22] + P[0][15]*SPP[20] + P[1][15]*SPP[12] + P[2][15]*SPP[11]) + SPP[12]*(P[4][1] + P[15][1]*SF[22] + P[0][1]*SPP[20] + P[1][1]*SPP[12] + P[2][1]*SPP[11]) + SPP[20]*(P[4][0] + P[15][0]*SF[22] + P[0][0]*SPP[20] + P[1][0]*SPP[12] + P[2][0]*SPP[11]) + SPP[11]*(P[4][2] + P[15][2]*SF[22] + P[0][2]*SPP[20] + P[1][2]*SPP[12] + P[2][2]*SPP[11]) + SQ[4]*sq(SG[1] - SG[2] + SG[3] - SG[4]); - nextP[0][5] = P[0][5] * SPP[5] - P[1][5] * SPP[4] + P[2][5] * SPP[7] + P[9][5] * SPP[22] + P[12][5] * SPP[18] + - SF[20] * (P[0][15] * SPP[5] - P[1][15] * SPP[4] + P[2][15] * SPP[7] + P[9][15] * SPP[22] + P[12][15] * SPP[18]) - - SPP[9] * (P[0][0] * SPP[5] - P[1][0] * SPP[4] + P[2][0] * SPP[7] + P[9][0] * SPP[22] + P[12][0] * SPP[18]) + SPP[0] * - (P[0][2] * SPP[5] - P[1][2] * SPP[4] + P[2][2] * SPP[7] + P[9][2] * SPP[22] + P[12][2] * SPP[18]) + SPP[10] * - (P[0][1] * SPP[5] - P[1][1] * SPP[4] + P[2][1] * SPP[7] + P[9][1] * SPP[22] + P[12][1] * SPP[18]); - nextP[1][5] = P[1][5] * SPP[6] - P[0][5] * SPP[2] - P[2][5] * SPP[8] + P[10][5] * SPP[22] + P[13][5] * SPP[17] + - SF[20] * (P[1][15] * SPP[6] - P[0][15] * SPP[2] - P[2][15] * SPP[8] + P[10][15] * SPP[22] + P[13][15] * SPP[17]) - - SPP[9] * (P[1][0] * SPP[6] - P[0][0] * SPP[2] - P[2][0] * SPP[8] + P[10][0] * SPP[22] + P[13][0] * SPP[17]) + SPP[0] * - (P[1][2] * SPP[6] - P[0][2] * SPP[2] - P[2][2] * SPP[8] + P[10][2] * SPP[22] + P[13][2] * SPP[17]) + SPP[10] * - (P[1][1] * SPP[6] - P[0][1] * SPP[2] - P[2][1] * SPP[8] + P[10][1] * SPP[22] + P[13][1] * SPP[17]); - nextP[2][5] = P[0][5] * SPP[14] - P[1][5] * SPP[3] + P[2][5] * SPP[13] + P[11][5] * SPP[22] + P[14][5] * SPP[16] + - SF[20] * (P[0][15] * SPP[14] - P[1][15] * SPP[3] + P[2][15] * SPP[13] + P[11][15] * SPP[22] + P[14][15] * SPP[16]) - - SPP[9] * (P[0][0] * SPP[14] - P[1][0] * SPP[3] + P[2][0] * SPP[13] + P[11][0] * SPP[22] + P[14][0] * SPP[16]) + - SPP[0] * (P[0][2] * SPP[14] - P[1][2] * SPP[3] + P[2][2] * SPP[13] + P[11][2] * SPP[22] + P[14][2] * SPP[16]) + - SPP[10] * (P[0][1] * SPP[14] - P[1][1] * SPP[3] + P[2][1] * SPP[13] + P[11][1] * SPP[22] + P[14][1] * SPP[16]); - nextP[3][5] = P[3][5] + SQ[1] + P[0][5] * SPP[1] + P[1][5] * SPP[19] + P[2][5] * SPP[15] - P[15][5] * SPP[21] + - SF[20] * (P[3][15] + P[0][15] * SPP[1] + P[1][15] * SPP[19] + P[2][15] * SPP[15] - P[15][15] * SPP[21]) - SPP[9] * - (P[3][0] + P[0][0] * SPP[1] + P[1][0] * SPP[19] + P[2][0] * SPP[15] - P[15][0] * SPP[21]) + SPP[0] * - (P[3][2] + P[0][2] * SPP[1] + P[1][2] * SPP[19] + P[2][2] * SPP[15] - P[15][2] * SPP[21]) + SPP[10] * - (P[3][1] + P[0][1] * SPP[1] + P[1][1] * SPP[19] + P[2][1] * SPP[15] - P[15][1] * SPP[21]); - nextP[4][5] = P[4][5] + SQ[0] + P[15][5] * SF[22] + P[0][5] * SPP[20] + P[1][5] * SPP[12] + P[2][5] * SPP[11] + - SF[20] * (P[4][15] + P[15][15] * SF[22] + P[0][15] * SPP[20] + P[1][15] * SPP[12] + P[2][15] * SPP[11]) - SPP[9] * - (P[4][0] + P[15][0] * SF[22] + P[0][0] * SPP[20] + P[1][0] * SPP[12] + P[2][0] * SPP[11]) + SPP[0] * - (P[4][2] + P[15][2] * SF[22] + P[0][2] * SPP[20] + P[1][2] * SPP[12] + P[2][2] * SPP[11]) + SPP[10] * - (P[4][1] + P[15][1] * SF[22] + P[0][1] * SPP[20] + P[1][1] * SPP[12] + P[2][1] * SPP[11]); - nextP[5][5] = P[5][5] + P[15][5]*SF[20] - P[0][5]*SPP[9] + P[1][5]*SPP[10] + P[2][5]*SPP[0] + SQ[4]*sq(SQ[7] + 2*q0*q1) + SQ[6]*sq(SQ[8] - 2*q0*q2) + SF[20]*(P[5][15] + P[15][15]*SF[20] - P[0][15]*SPP[9] + P[1][15]*SPP[10] + P[2][15]*SPP[0]) - SPP[9]*(P[5][0] + P[15][0]*SF[20] - P[0][0]*SPP[9] + P[1][0]*SPP[10] + P[2][0]*SPP[0]) + SPP[0]*(P[5][2] + P[15][2]*SF[20] - P[0][2]*SPP[9] + P[1][2]*SPP[10] + P[2][2]*SPP[0]) + SPP[10]*(P[5][1] + P[15][1]*SF[20] - P[0][1]*SPP[9] + P[1][1]*SPP[10] + P[2][1]*SPP[0]) + SQ[5]*sq(SG[1] - SG[2] - SG[3] + SG[4]); - nextP[0][6] = P[0][6] * SPP[5] - P[1][6] * SPP[4] + P[2][6] * SPP[7] + P[9][6] * SPP[22] + P[12][6] * SPP[18] + dt * - (P[0][3] * SPP[5] - P[1][3] * SPP[4] + P[2][3] * SPP[7] + P[9][3] * SPP[22] + P[12][3] * SPP[18]); - nextP[1][6] = P[1][6] * SPP[6] - P[0][6] * SPP[2] - P[2][6] * SPP[8] + P[10][6] * SPP[22] + P[13][6] * SPP[17] + dt * - (P[1][3] * SPP[6] - P[0][3] * SPP[2] - P[2][3] * SPP[8] + P[10][3] * SPP[22] + P[13][3] * SPP[17]); - nextP[2][6] = P[0][6] * SPP[14] - P[1][6] * SPP[3] + P[2][6] * SPP[13] + P[11][6] * SPP[22] + P[14][6] * SPP[16] + - dt * (P[0][3] * SPP[14] - P[1][3] * SPP[3] + P[2][3] * SPP[13] + P[11][3] * SPP[22] + P[14][3] * SPP[16]); - nextP[3][6] = P[3][6] + P[0][6] * SPP[1] + P[1][6] * SPP[19] + P[2][6] * SPP[15] - P[15][6] * SPP[21] + dt * - (P[3][3] + P[0][3] * SPP[1] + P[1][3] * SPP[19] + P[2][3] * SPP[15] - P[15][3] * SPP[21]); - nextP[4][6] = P[4][6] + P[15][6] * SF[22] + P[0][6] * SPP[20] + P[1][6] * SPP[12] + P[2][6] * SPP[11] + dt * - (P[4][3] + P[15][3] * SF[22] + P[0][3] * SPP[20] + P[1][3] * SPP[12] + P[2][3] * SPP[11]); - nextP[5][6] = P[5][6] + P[15][6] * SF[20] - P[0][6] * SPP[9] + P[1][6] * SPP[10] + P[2][6] * SPP[0] + dt * - (P[5][3] + P[15][3] * SF[20] - P[0][3] * SPP[9] + P[1][3] * SPP[10] + P[2][3] * SPP[0]); - nextP[6][6] = P[6][6] + P[3][6] * dt + dt * (P[6][3] + P[3][3] * dt); - nextP[0][7] = P[0][7] * SPP[5] - P[1][7] * SPP[4] + P[2][7] * SPP[7] + P[9][7] * SPP[22] + P[12][7] * SPP[18] + dt * - (P[0][4] * SPP[5] - P[1][4] * SPP[4] + P[2][4] * SPP[7] + P[9][4] * SPP[22] + P[12][4] * SPP[18]); - nextP[1][7] = P[1][7] * SPP[6] - P[0][7] * SPP[2] - P[2][7] * SPP[8] + P[10][7] * SPP[22] + P[13][7] * SPP[17] + dt * - (P[1][4] * SPP[6] - P[0][4] * SPP[2] - P[2][4] * SPP[8] + P[10][4] * SPP[22] + P[13][4] * SPP[17]); - nextP[2][7] = P[0][7] * SPP[14] - P[1][7] * SPP[3] + P[2][7] * SPP[13] + P[11][7] * SPP[22] + P[14][7] * SPP[16] + - dt * (P[0][4] * SPP[14] - P[1][4] * SPP[3] + P[2][4] * SPP[13] + P[11][4] * SPP[22] + P[14][4] * SPP[16]); - nextP[3][7] = P[3][7] + P[0][7] * SPP[1] + P[1][7] * SPP[19] + P[2][7] * SPP[15] - P[15][7] * SPP[21] + dt * - (P[3][4] + P[0][4] * SPP[1] + P[1][4] * SPP[19] + P[2][4] * SPP[15] - P[15][4] * SPP[21]); - nextP[4][7] = P[4][7] + P[15][7] * SF[22] + P[0][7] * SPP[20] + P[1][7] * SPP[12] + P[2][7] * SPP[11] + dt * - (P[4][4] + P[15][4] * SF[22] + P[0][4] * SPP[20] + P[1][4] * SPP[12] + P[2][4] * SPP[11]); - nextP[5][7] = P[5][7] + P[15][7] * SF[20] - P[0][7] * SPP[9] + P[1][7] * SPP[10] + P[2][7] * SPP[0] + dt * - (P[5][4] + P[15][4] * SF[20] - P[0][4] * SPP[9] + P[1][4] * SPP[10] + P[2][4] * SPP[0]); - nextP[6][7] = P[6][7] + P[3][7] * dt + dt * (P[6][4] + P[3][4] * dt); - nextP[7][7] = P[7][7] + P[4][7] * dt + dt * (P[7][4] + P[4][4] * dt); - nextP[0][8] = P[0][8] * SPP[5] - P[1][8] * SPP[4] + P[2][8] * SPP[7] + P[9][8] * SPP[22] + P[12][8] * SPP[18] + dt * - (P[0][5] * SPP[5] - P[1][5] * SPP[4] + P[2][5] * SPP[7] + P[9][5] * SPP[22] + P[12][5] * SPP[18]); - nextP[1][8] = P[1][8] * SPP[6] - P[0][8] * SPP[2] - P[2][8] * SPP[8] + P[10][8] * SPP[22] + P[13][8] * SPP[17] + dt * - (P[1][5] * SPP[6] - P[0][5] * SPP[2] - P[2][5] * SPP[8] + P[10][5] * SPP[22] + P[13][5] * SPP[17]); - nextP[2][8] = P[0][8] * SPP[14] - P[1][8] * SPP[3] + P[2][8] * SPP[13] + P[11][8] * SPP[22] + P[14][8] * SPP[16] + - dt * (P[0][5] * SPP[14] - P[1][5] * SPP[3] + P[2][5] * SPP[13] + P[11][5] * SPP[22] + P[14][5] * SPP[16]); - nextP[3][8] = P[3][8] + P[0][8] * SPP[1] + P[1][8] * SPP[19] + P[2][8] * SPP[15] - P[15][8] * SPP[21] + dt * - (P[3][5] + P[0][5] * SPP[1] + P[1][5] * SPP[19] + P[2][5] * SPP[15] - P[15][5] * SPP[21]); - nextP[4][8] = P[4][8] + P[15][8] * SF[22] + P[0][8] * SPP[20] + P[1][8] * SPP[12] + P[2][8] * SPP[11] + dt * - (P[4][5] + P[15][5] * SF[22] + P[0][5] * SPP[20] + P[1][5] * SPP[12] + P[2][5] * SPP[11]); - nextP[5][8] = P[5][8] + P[15][8] * SF[20] - P[0][8] * SPP[9] + P[1][8] * SPP[10] + P[2][8] * SPP[0] + dt * - (P[5][5] + P[15][5] * SF[20] - P[0][5] * SPP[9] + P[1][5] * SPP[10] + P[2][5] * SPP[0]); - nextP[6][8] = P[6][8] + P[3][8] * dt + dt * (P[6][5] + P[3][5] * dt); - nextP[7][8] = P[7][8] + P[4][8] * dt + dt * (P[7][5] + P[4][5] * dt); - nextP[8][8] = P[8][8] + P[5][8] * dt + dt * (P[8][5] + P[5][5] * dt); - nextP[0][9] = P[0][9] * SPP[5] - P[1][9] * SPP[4] + P[2][9] * SPP[7] + P[9][9] * SPP[22] + P[12][9] * SPP[18]; - nextP[1][9] = P[1][9] * SPP[6] - P[0][9] * SPP[2] - P[2][9] * SPP[8] + P[10][9] * SPP[22] + P[13][9] * SPP[17]; - nextP[2][9] = P[0][9] * SPP[14] - P[1][9] * SPP[3] + P[2][9] * SPP[13] + P[11][9] * SPP[22] + P[14][9] * SPP[16]; - nextP[3][9] = P[3][9] + P[0][9] * SPP[1] + P[1][9] * SPP[19] + P[2][9] * SPP[15] - P[15][9] * SPP[21]; - nextP[4][9] = P[4][9] + P[15][9] * SF[22] + P[0][9] * SPP[20] + P[1][9] * SPP[12] + P[2][9] * SPP[11]; - nextP[5][9] = P[5][9] + P[15][9] * SF[20] - P[0][9] * SPP[9] + P[1][9] * SPP[10] + P[2][9] * SPP[0]; - nextP[6][9] = P[6][9] + P[3][9] * dt; - nextP[7][9] = P[7][9] + P[4][9] * dt; - nextP[8][9] = P[8][9] + P[5][9] * dt; - nextP[9][9] = P[9][9]; - nextP[0][10] = P[0][10] * SPP[5] - P[1][10] * SPP[4] + P[2][10] * SPP[7] + P[9][10] * SPP[22] + P[12][10] * SPP[18]; - nextP[1][10] = P[1][10] * SPP[6] - P[0][10] * SPP[2] - P[2][10] * SPP[8] + P[10][10] * SPP[22] + P[13][10] * SPP[17]; - nextP[2][10] = P[0][10] * SPP[14] - P[1][10] * SPP[3] + P[2][10] * SPP[13] + P[11][10] * SPP[22] + P[14][10] * SPP[16]; - nextP[3][10] = P[3][10] + P[0][10] * SPP[1] + P[1][10] * SPP[19] + P[2][10] * SPP[15] - P[15][10] * SPP[21]; - nextP[4][10] = P[4][10] + P[15][10] * SF[22] + P[0][10] * SPP[20] + P[1][10] * SPP[12] + P[2][10] * SPP[11]; - nextP[5][10] = P[5][10] + P[15][10] * SF[20] - P[0][10] * SPP[9] + P[1][10] * SPP[10] + P[2][10] * SPP[0]; - nextP[6][10] = P[6][10] + P[3][10] * dt; - nextP[7][10] = P[7][10] + P[4][10] * dt; - nextP[8][10] = P[8][10] + P[5][10] * dt; - nextP[9][10] = P[9][10]; + float nextP[24][24]; + + // calculate variances and upper diagonal covariances for quaternion, velocity, position and gyro bias states + nextP[0][0] = P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10] + (daxVar*SQ[10])/4 + SF[9]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) + SF[11]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SF[10]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) + SF[14]*(P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10]) + SF[15]*(P[0][11] + P[1][11]*SF[9] + P[2][11]*SF[11] + P[3][11]*SF[10] + P[10][11]*SF[14] + P[11][11]*SF[15] + P[12][11]*SPP[10]) + SPP[10]*(P[0][12] + P[1][12]*SF[9] + P[2][12]*SF[11] + P[3][12]*SF[10] + P[10][12]*SF[14] + P[11][12]*SF[15] + P[12][12]*SPP[10]) + (dayVar*sq(q2))/4 + (dazVar*sq(q3))/4; + nextP[0][1] = P[0][1] + SQ[8] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10] + SF[8]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SF[7]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SF[11]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) - SF[15]*(P[0][12] + P[1][12]*SF[9] + P[2][12]*SF[11] + P[3][12]*SF[10] + P[10][12]*SF[14] + P[11][12]*SF[15] + P[12][12]*SPP[10]) + SPP[10]*(P[0][11] + P[1][11]*SF[9] + P[2][11]*SF[11] + P[3][11]*SF[10] + P[10][11]*SF[14] + P[11][11]*SF[15] + P[12][11]*SPP[10]) - (q0*(P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10]))/2; + nextP[1][1] = P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] + daxVar*SQ[9] - (P[10][1]*q0)/2 + SF[8]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SF[7]*(P[1][2] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2) + SF[11]*(P[1][3] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2) - SF[15]*(P[1][12] + P[0][12]*SF[8] + P[2][12]*SF[7] + P[3][12]*SF[11] - P[12][12]*SF[15] + P[11][12]*SPP[10] - (P[10][12]*q0)/2) + SPP[10]*(P[1][11] + P[0][11]*SF[8] + P[2][11]*SF[7] + P[3][11]*SF[11] - P[12][11]*SF[15] + P[11][11]*SPP[10] - (P[10][11]*q0)/2) + (dayVar*sq(q3))/4 + (dazVar*sq(q2))/4 - (q0*(P[1][10] + P[0][10]*SF[8] + P[2][10]*SF[7] + P[3][10]*SF[11] - P[12][10]*SF[15] + P[11][10]*SPP[10] - (P[10][10]*q0)/2))/2; + nextP[0][2] = P[0][2] + SQ[7] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10] + SF[6]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SF[10]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) + SF[8]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) + SF[14]*(P[0][12] + P[1][12]*SF[9] + P[2][12]*SF[11] + P[3][12]*SF[10] + P[10][12]*SF[14] + P[11][12]*SF[15] + P[12][12]*SPP[10]) - SPP[10]*(P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10]) - (q0*(P[0][11] + P[1][11]*SF[9] + P[2][11]*SF[11] + P[3][11]*SF[10] + P[10][11]*SF[14] + P[11][11]*SF[15] + P[12][11]*SPP[10]))/2; + nextP[1][2] = P[1][2] + SQ[5] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2 + SF[6]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SF[10]*(P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] - (P[10][1]*q0)/2) + SF[8]*(P[1][3] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2) + SF[14]*(P[1][12] + P[0][12]*SF[8] + P[2][12]*SF[7] + P[3][12]*SF[11] - P[12][12]*SF[15] + P[11][12]*SPP[10] - (P[10][12]*q0)/2) - SPP[10]*(P[1][10] + P[0][10]*SF[8] + P[2][10]*SF[7] + P[3][10]*SF[11] - P[12][10]*SF[15] + P[11][10]*SPP[10] - (P[10][10]*q0)/2) - (q0*(P[1][11] + P[0][11]*SF[8] + P[2][11]*SF[7] + P[3][11]*SF[11] - P[12][11]*SF[15] + P[11][11]*SPP[10] - (P[10][11]*q0)/2))/2; + nextP[2][2] = P[2][2] + P[0][2]*SF[6] + P[1][2]*SF[10] + P[3][2]*SF[8] + P[12][2]*SF[14] - P[10][2]*SPP[10] + dayVar*SQ[9] + (dazVar*SQ[10])/4 - (P[11][2]*q0)/2 + SF[6]*(P[2][0] + P[0][0]*SF[6] + P[1][0]*SF[10] + P[3][0]*SF[8] + P[12][0]*SF[14] - P[10][0]*SPP[10] - (P[11][0]*q0)/2) + SF[10]*(P[2][1] + P[0][1]*SF[6] + P[1][1]*SF[10] + P[3][1]*SF[8] + P[12][1]*SF[14] - P[10][1]*SPP[10] - (P[11][1]*q0)/2) + SF[8]*(P[2][3] + P[0][3]*SF[6] + P[1][3]*SF[10] + P[3][3]*SF[8] + P[12][3]*SF[14] - P[10][3]*SPP[10] - (P[11][3]*q0)/2) + SF[14]*(P[2][12] + P[0][12]*SF[6] + P[1][12]*SF[10] + P[3][12]*SF[8] + P[12][12]*SF[14] - P[10][12]*SPP[10] - (P[11][12]*q0)/2) - SPP[10]*(P[2][10] + P[0][10]*SF[6] + P[1][10]*SF[10] + P[3][10]*SF[8] + P[12][10]*SF[14] - P[10][10]*SPP[10] - (P[11][10]*q0)/2) + (daxVar*sq(q3))/4 - (q0*(P[2][11] + P[0][11]*SF[6] + P[1][11]*SF[10] + P[3][11]*SF[8] + P[12][11]*SF[14] - P[10][11]*SPP[10] - (P[11][11]*q0)/2))/2; + nextP[0][3] = P[0][3] + SQ[6] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10] + SF[7]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SF[6]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) + SF[9]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SF[15]*(P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10]) - SF[14]*(P[0][11] + P[1][11]*SF[9] + P[2][11]*SF[11] + P[3][11]*SF[10] + P[10][11]*SF[14] + P[11][11]*SF[15] + P[12][11]*SPP[10]) - (q0*(P[0][12] + P[1][12]*SF[9] + P[2][12]*SF[11] + P[3][12]*SF[10] + P[10][12]*SF[14] + P[11][12]*SF[15] + P[12][12]*SPP[10]))/2; + nextP[1][3] = P[1][3] + SQ[4] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2 + SF[7]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SF[6]*(P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] - (P[10][1]*q0)/2) + SF[9]*(P[1][2] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2) + SF[15]*(P[1][10] + P[0][10]*SF[8] + P[2][10]*SF[7] + P[3][10]*SF[11] - P[12][10]*SF[15] + P[11][10]*SPP[10] - (P[10][10]*q0)/2) - SF[14]*(P[1][11] + P[0][11]*SF[8] + P[2][11]*SF[7] + P[3][11]*SF[11] - P[12][11]*SF[15] + P[11][11]*SPP[10] - (P[10][11]*q0)/2) - (q0*(P[1][12] + P[0][12]*SF[8] + P[2][12]*SF[7] + P[3][12]*SF[11] - P[12][12]*SF[15] + P[11][12]*SPP[10] - (P[10][12]*q0)/2))/2; + nextP[2][3] = P[2][3] + SQ[3] + P[0][3]*SF[6] + P[1][3]*SF[10] + P[3][3]*SF[8] + P[12][3]*SF[14] - P[10][3]*SPP[10] - (P[11][3]*q0)/2 + SF[7]*(P[2][0] + P[0][0]*SF[6] + P[1][0]*SF[10] + P[3][0]*SF[8] + P[12][0]*SF[14] - P[10][0]*SPP[10] - (P[11][0]*q0)/2) + SF[6]*(P[2][1] + P[0][1]*SF[6] + P[1][1]*SF[10] + P[3][1]*SF[8] + P[12][1]*SF[14] - P[10][1]*SPP[10] - (P[11][1]*q0)/2) + SF[9]*(P[2][2] + P[0][2]*SF[6] + P[1][2]*SF[10] + P[3][2]*SF[8] + P[12][2]*SF[14] - P[10][2]*SPP[10] - (P[11][2]*q0)/2) + SF[15]*(P[2][10] + P[0][10]*SF[6] + P[1][10]*SF[10] + P[3][10]*SF[8] + P[12][10]*SF[14] - P[10][10]*SPP[10] - (P[11][10]*q0)/2) - SF[14]*(P[2][11] + P[0][11]*SF[6] + P[1][11]*SF[10] + P[3][11]*SF[8] + P[12][11]*SF[14] - P[10][11]*SPP[10] - (P[11][11]*q0)/2) - (q0*(P[2][12] + P[0][12]*SF[6] + P[1][12]*SF[10] + P[3][12]*SF[8] + P[12][12]*SF[14] - P[10][12]*SPP[10] - (P[11][12]*q0)/2))/2; + nextP[3][3] = P[3][3] + P[0][3]*SF[7] + P[1][3]*SF[6] + P[2][3]*SF[9] + P[10][3]*SF[15] - P[11][3]*SF[14] + (dayVar*SQ[10])/4 + dazVar*SQ[9] - (P[12][3]*q0)/2 + SF[7]*(P[3][0] + P[0][0]*SF[7] + P[1][0]*SF[6] + P[2][0]*SF[9] + P[10][0]*SF[15] - P[11][0]*SF[14] - (P[12][0]*q0)/2) + SF[6]*(P[3][1] + P[0][1]*SF[7] + P[1][1]*SF[6] + P[2][1]*SF[9] + P[10][1]*SF[15] - P[11][1]*SF[14] - (P[12][1]*q0)/2) + SF[9]*(P[3][2] + P[0][2]*SF[7] + P[1][2]*SF[6] + P[2][2]*SF[9] + P[10][2]*SF[15] - P[11][2]*SF[14] - (P[12][2]*q0)/2) + SF[15]*(P[3][10] + P[0][10]*SF[7] + P[1][10]*SF[6] + P[2][10]*SF[9] + P[10][10]*SF[15] - P[11][10]*SF[14] - (P[12][10]*q0)/2) - SF[14]*(P[3][11] + P[0][11]*SF[7] + P[1][11]*SF[6] + P[2][11]*SF[9] + P[10][11]*SF[15] - P[11][11]*SF[14] - (P[12][11]*q0)/2) + (daxVar*sq(q2))/4 - (q0*(P[3][12] + P[0][12]*SF[7] + P[1][12]*SF[6] + P[2][12]*SF[9] + P[10][12]*SF[15] - P[11][12]*SF[14] - (P[12][12]*q0)/2))/2; + nextP[0][4] = P[0][4] + P[1][4]*SF[9] + P[2][4]*SF[11] + P[3][4]*SF[10] + P[10][4]*SF[14] + P[11][4]*SF[15] + P[12][4]*SPP[10] + SF[5]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SF[3]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) - SF[4]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) + SPP[0]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SPP[3]*(P[0][13] + P[1][13]*SF[9] + P[2][13]*SF[11] + P[3][13]*SF[10] + P[10][13]*SF[14] + P[11][13]*SF[15] + P[12][13]*SPP[10]) + SPP[6]*(P[0][14] + P[1][14]*SF[9] + P[2][14]*SF[11] + P[3][14]*SF[10] + P[10][14]*SF[14] + P[11][14]*SF[15] + P[12][14]*SPP[10]) - SPP[9]*(P[0][15] + P[1][15]*SF[9] + P[2][15]*SF[11] + P[3][15]*SF[10] + P[10][15]*SF[14] + P[11][15]*SF[15] + P[12][15]*SPP[10]); + nextP[1][4] = P[1][4] + P[0][4]*SF[8] + P[2][4]*SF[7] + P[3][4]*SF[11] - P[12][4]*SF[15] + P[11][4]*SPP[10] - (P[10][4]*q0)/2 + SF[5]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SF[3]*(P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] - (P[10][1]*q0)/2) - SF[4]*(P[1][3] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2) + SPP[0]*(P[1][2] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2) + SPP[3]*(P[1][13] + P[0][13]*SF[8] + P[2][13]*SF[7] + P[3][13]*SF[11] - P[12][13]*SF[15] + P[11][13]*SPP[10] - (P[10][13]*q0)/2) + SPP[6]*(P[1][14] + P[0][14]*SF[8] + P[2][14]*SF[7] + P[3][14]*SF[11] - P[12][14]*SF[15] + P[11][14]*SPP[10] - (P[10][14]*q0)/2) - SPP[9]*(P[1][15] + P[0][15]*SF[8] + P[2][15]*SF[7] + P[3][15]*SF[11] - P[12][15]*SF[15] + P[11][15]*SPP[10] - (P[10][15]*q0)/2); + nextP[2][4] = P[2][4] + P[0][4]*SF[6] + P[1][4]*SF[10] + P[3][4]*SF[8] + P[12][4]*SF[14] - P[10][4]*SPP[10] - (P[11][4]*q0)/2 + SF[5]*(P[2][0] + P[0][0]*SF[6] + P[1][0]*SF[10] + P[3][0]*SF[8] + P[12][0]*SF[14] - P[10][0]*SPP[10] - (P[11][0]*q0)/2) + SF[3]*(P[2][1] + P[0][1]*SF[6] + P[1][1]*SF[10] + P[3][1]*SF[8] + P[12][1]*SF[14] - P[10][1]*SPP[10] - (P[11][1]*q0)/2) - SF[4]*(P[2][3] + P[0][3]*SF[6] + P[1][3]*SF[10] + P[3][3]*SF[8] + P[12][3]*SF[14] - P[10][3]*SPP[10] - (P[11][3]*q0)/2) + SPP[0]*(P[2][2] + P[0][2]*SF[6] + P[1][2]*SF[10] + P[3][2]*SF[8] + P[12][2]*SF[14] - P[10][2]*SPP[10] - (P[11][2]*q0)/2) + SPP[3]*(P[2][13] + P[0][13]*SF[6] + P[1][13]*SF[10] + P[3][13]*SF[8] + P[12][13]*SF[14] - P[10][13]*SPP[10] - (P[11][13]*q0)/2) + SPP[6]*(P[2][14] + P[0][14]*SF[6] + P[1][14]*SF[10] + P[3][14]*SF[8] + P[12][14]*SF[14] - P[10][14]*SPP[10] - (P[11][14]*q0)/2) - SPP[9]*(P[2][15] + P[0][15]*SF[6] + P[1][15]*SF[10] + P[3][15]*SF[8] + P[12][15]*SF[14] - P[10][15]*SPP[10] - (P[11][15]*q0)/2); + nextP[3][4] = P[3][4] + P[0][4]*SF[7] + P[1][4]*SF[6] + P[2][4]*SF[9] + P[10][4]*SF[15] - P[11][4]*SF[14] - (P[12][4]*q0)/2 + SF[5]*(P[3][0] + P[0][0]*SF[7] + P[1][0]*SF[6] + P[2][0]*SF[9] + P[10][0]*SF[15] - P[11][0]*SF[14] - (P[12][0]*q0)/2) + SF[3]*(P[3][1] + P[0][1]*SF[7] + P[1][1]*SF[6] + P[2][1]*SF[9] + P[10][1]*SF[15] - P[11][1]*SF[14] - (P[12][1]*q0)/2) - SF[4]*(P[3][3] + P[0][3]*SF[7] + P[1][3]*SF[6] + P[2][3]*SF[9] + P[10][3]*SF[15] - P[11][3]*SF[14] - (P[12][3]*q0)/2) + SPP[0]*(P[3][2] + P[0][2]*SF[7] + P[1][2]*SF[6] + P[2][2]*SF[9] + P[10][2]*SF[15] - P[11][2]*SF[14] - (P[12][2]*q0)/2) + SPP[3]*(P[3][13] + P[0][13]*SF[7] + P[1][13]*SF[6] + P[2][13]*SF[9] + P[10][13]*SF[15] - P[11][13]*SF[14] - (P[12][13]*q0)/2) + SPP[6]*(P[3][14] + P[0][14]*SF[7] + P[1][14]*SF[6] + P[2][14]*SF[9] + P[10][14]*SF[15] - P[11][14]*SF[14] - (P[12][14]*q0)/2) - SPP[9]*(P[3][15] + P[0][15]*SF[7] + P[1][15]*SF[6] + P[2][15]*SF[9] + P[10][15]*SF[15] - P[11][15]*SF[14] - (P[12][15]*q0)/2); + nextP[4][4] = P[4][4] + P[0][4]*SF[5] + P[1][4]*SF[3] - P[3][4]*SF[4] + P[2][4]*SPP[0] + P[13][4]*SPP[3] + P[14][4]*SPP[6] - P[15][4]*SPP[9] + dvyVar*sq(SG[7] - 2*q0*q3) + dvzVar*sq(SG[6] + 2*q0*q2) + SF[5]*(P[4][0] + P[0][0]*SF[5] + P[1][0]*SF[3] - P[3][0]*SF[4] + P[2][0]*SPP[0] + P[13][0]*SPP[3] + P[14][0]*SPP[6] - P[15][0]*SPP[9]) + SF[3]*(P[4][1] + P[0][1]*SF[5] + P[1][1]*SF[3] - P[3][1]*SF[4] + P[2][1]*SPP[0] + P[13][1]*SPP[3] + P[14][1]*SPP[6] - P[15][1]*SPP[9]) - SF[4]*(P[4][3] + P[0][3]*SF[5] + P[1][3]*SF[3] - P[3][3]*SF[4] + P[2][3]*SPP[0] + P[13][3]*SPP[3] + P[14][3]*SPP[6] - P[15][3]*SPP[9]) + SPP[0]*(P[4][2] + P[0][2]*SF[5] + P[1][2]*SF[3] - P[3][2]*SF[4] + P[2][2]*SPP[0] + P[13][2]*SPP[3] + P[14][2]*SPP[6] - P[15][2]*SPP[9]) + SPP[3]*(P[4][13] + P[0][13]*SF[5] + P[1][13]*SF[3] - P[3][13]*SF[4] + P[2][13]*SPP[0] + P[13][13]*SPP[3] + P[14][13]*SPP[6] - P[15][13]*SPP[9]) + SPP[6]*(P[4][14] + P[0][14]*SF[5] + P[1][14]*SF[3] - P[3][14]*SF[4] + P[2][14]*SPP[0] + P[13][14]*SPP[3] + P[14][14]*SPP[6] - P[15][14]*SPP[9]) - SPP[9]*(P[4][15] + P[0][15]*SF[5] + P[1][15]*SF[3] - P[3][15]*SF[4] + P[2][15]*SPP[0] + P[13][15]*SPP[3] + P[14][15]*SPP[6] - P[15][15]*SPP[9]) + dvxVar*sq(SG[1] + SG[2] - SG[3] - SG[4]); + nextP[0][5] = P[0][5] + P[1][5]*SF[9] + P[2][5]*SF[11] + P[3][5]*SF[10] + P[10][5]*SF[14] + P[11][5]*SF[15] + P[12][5]*SPP[10] + SF[4]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SF[3]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SF[5]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) - SPP[0]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) - SPP[8]*(P[0][13] + P[1][13]*SF[9] + P[2][13]*SF[11] + P[3][13]*SF[10] + P[10][13]*SF[14] + P[11][13]*SF[15] + P[12][13]*SPP[10]) + SPP[2]*(P[0][14] + P[1][14]*SF[9] + P[2][14]*SF[11] + P[3][14]*SF[10] + P[10][14]*SF[14] + P[11][14]*SF[15] + P[12][14]*SPP[10]) + SPP[5]*(P[0][15] + P[1][15]*SF[9] + P[2][15]*SF[11] + P[3][15]*SF[10] + P[10][15]*SF[14] + P[11][15]*SF[15] + P[12][15]*SPP[10]); + nextP[1][5] = P[1][5] + P[0][5]*SF[8] + P[2][5]*SF[7] + P[3][5]*SF[11] - P[12][5]*SF[15] + P[11][5]*SPP[10] - (P[10][5]*q0)/2 + SF[4]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SF[3]*(P[1][2] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2) + SF[5]*(P[1][3] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2) - SPP[0]*(P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] - (P[10][1]*q0)/2) - SPP[8]*(P[1][13] + P[0][13]*SF[8] + P[2][13]*SF[7] + P[3][13]*SF[11] - P[12][13]*SF[15] + P[11][13]*SPP[10] - (P[10][13]*q0)/2) + SPP[2]*(P[1][14] + P[0][14]*SF[8] + P[2][14]*SF[7] + P[3][14]*SF[11] - P[12][14]*SF[15] + P[11][14]*SPP[10] - (P[10][14]*q0)/2) + SPP[5]*(P[1][15] + P[0][15]*SF[8] + P[2][15]*SF[7] + P[3][15]*SF[11] - P[12][15]*SF[15] + P[11][15]*SPP[10] - (P[10][15]*q0)/2); + nextP[2][5] = P[2][5] + P[0][5]*SF[6] + P[1][5]*SF[10] + P[3][5]*SF[8] + P[12][5]*SF[14] - P[10][5]*SPP[10] - (P[11][5]*q0)/2 + SF[4]*(P[2][0] + P[0][0]*SF[6] + P[1][0]*SF[10] + P[3][0]*SF[8] + P[12][0]*SF[14] - P[10][0]*SPP[10] - (P[11][0]*q0)/2) + SF[3]*(P[2][2] + P[0][2]*SF[6] + P[1][2]*SF[10] + P[3][2]*SF[8] + P[12][2]*SF[14] - P[10][2]*SPP[10] - (P[11][2]*q0)/2) + SF[5]*(P[2][3] + P[0][3]*SF[6] + P[1][3]*SF[10] + P[3][3]*SF[8] + P[12][3]*SF[14] - P[10][3]*SPP[10] - (P[11][3]*q0)/2) - SPP[0]*(P[2][1] + P[0][1]*SF[6] + P[1][1]*SF[10] + P[3][1]*SF[8] + P[12][1]*SF[14] - P[10][1]*SPP[10] - (P[11][1]*q0)/2) - SPP[8]*(P[2][13] + P[0][13]*SF[6] + P[1][13]*SF[10] + P[3][13]*SF[8] + P[12][13]*SF[14] - P[10][13]*SPP[10] - (P[11][13]*q0)/2) + SPP[2]*(P[2][14] + P[0][14]*SF[6] + P[1][14]*SF[10] + P[3][14]*SF[8] + P[12][14]*SF[14] - P[10][14]*SPP[10] - (P[11][14]*q0)/2) + SPP[5]*(P[2][15] + P[0][15]*SF[6] + P[1][15]*SF[10] + P[3][15]*SF[8] + P[12][15]*SF[14] - P[10][15]*SPP[10] - (P[11][15]*q0)/2); + nextP[3][5] = P[3][5] + P[0][5]*SF[7] + P[1][5]*SF[6] + P[2][5]*SF[9] + P[10][5]*SF[15] - P[11][5]*SF[14] - (P[12][5]*q0)/2 + SF[4]*(P[3][0] + P[0][0]*SF[7] + P[1][0]*SF[6] + P[2][0]*SF[9] + P[10][0]*SF[15] - P[11][0]*SF[14] - (P[12][0]*q0)/2) + SF[3]*(P[3][2] + P[0][2]*SF[7] + P[1][2]*SF[6] + P[2][2]*SF[9] + P[10][2]*SF[15] - P[11][2]*SF[14] - (P[12][2]*q0)/2) + SF[5]*(P[3][3] + P[0][3]*SF[7] + P[1][3]*SF[6] + P[2][3]*SF[9] + P[10][3]*SF[15] - P[11][3]*SF[14] - (P[12][3]*q0)/2) - SPP[0]*(P[3][1] + P[0][1]*SF[7] + P[1][1]*SF[6] + P[2][1]*SF[9] + P[10][1]*SF[15] - P[11][1]*SF[14] - (P[12][1]*q0)/2) - SPP[8]*(P[3][13] + P[0][13]*SF[7] + P[1][13]*SF[6] + P[2][13]*SF[9] + P[10][13]*SF[15] - P[11][13]*SF[14] - (P[12][13]*q0)/2) + SPP[2]*(P[3][14] + P[0][14]*SF[7] + P[1][14]*SF[6] + P[2][14]*SF[9] + P[10][14]*SF[15] - P[11][14]*SF[14] - (P[12][14]*q0)/2) + SPP[5]*(P[3][15] + P[0][15]*SF[7] + P[1][15]*SF[6] + P[2][15]*SF[9] + P[10][15]*SF[15] - P[11][15]*SF[14] - (P[12][15]*q0)/2); + nextP[4][5] = P[4][5] + SQ[2] + P[0][5]*SF[5] + P[1][5]*SF[3] - P[3][5]*SF[4] + P[2][5]*SPP[0] + P[13][5]*SPP[3] + P[14][5]*SPP[6] - P[15][5]*SPP[9] + SF[4]*(P[4][0] + P[0][0]*SF[5] + P[1][0]*SF[3] - P[3][0]*SF[4] + P[2][0]*SPP[0] + P[13][0]*SPP[3] + P[14][0]*SPP[6] - P[15][0]*SPP[9]) + SF[3]*(P[4][2] + P[0][2]*SF[5] + P[1][2]*SF[3] - P[3][2]*SF[4] + P[2][2]*SPP[0] + P[13][2]*SPP[3] + P[14][2]*SPP[6] - P[15][2]*SPP[9]) + SF[5]*(P[4][3] + P[0][3]*SF[5] + P[1][3]*SF[3] - P[3][3]*SF[4] + P[2][3]*SPP[0] + P[13][3]*SPP[3] + P[14][3]*SPP[6] - P[15][3]*SPP[9]) - SPP[0]*(P[4][1] + P[0][1]*SF[5] + P[1][1]*SF[3] - P[3][1]*SF[4] + P[2][1]*SPP[0] + P[13][1]*SPP[3] + P[14][1]*SPP[6] - P[15][1]*SPP[9]) - SPP[8]*(P[4][13] + P[0][13]*SF[5] + P[1][13]*SF[3] - P[3][13]*SF[4] + P[2][13]*SPP[0] + P[13][13]*SPP[3] + P[14][13]*SPP[6] - P[15][13]*SPP[9]) + SPP[2]*(P[4][14] + P[0][14]*SF[5] + P[1][14]*SF[3] - P[3][14]*SF[4] + P[2][14]*SPP[0] + P[13][14]*SPP[3] + P[14][14]*SPP[6] - P[15][14]*SPP[9]) + SPP[5]*(P[4][15] + P[0][15]*SF[5] + P[1][15]*SF[3] - P[3][15]*SF[4] + P[2][15]*SPP[0] + P[13][15]*SPP[3] + P[14][15]*SPP[6] - P[15][15]*SPP[9]); + nextP[5][5] = P[5][5] + P[0][5]*SF[4] + P[2][5]*SF[3] + P[3][5]*SF[5] - P[1][5]*SPP[0] - P[13][5]*SPP[8] + P[14][5]*SPP[2] + P[15][5]*SPP[5] + dvxVar*sq(SG[7] + 2*q0*q3) + dvzVar*sq(SG[5] - 2*q0*q1) + SF[4]*(P[5][0] + P[0][0]*SF[4] + P[2][0]*SF[3] + P[3][0]*SF[5] - P[1][0]*SPP[0] - P[13][0]*SPP[8] + P[14][0]*SPP[2] + P[15][0]*SPP[5]) + SF[3]*(P[5][2] + P[0][2]*SF[4] + P[2][2]*SF[3] + P[3][2]*SF[5] - P[1][2]*SPP[0] - P[13][2]*SPP[8] + P[14][2]*SPP[2] + P[15][2]*SPP[5]) + SF[5]*(P[5][3] + P[0][3]*SF[4] + P[2][3]*SF[3] + P[3][3]*SF[5] - P[1][3]*SPP[0] - P[13][3]*SPP[8] + P[14][3]*SPP[2] + P[15][3]*SPP[5]) - SPP[0]*(P[5][1] + P[0][1]*SF[4] + P[2][1]*SF[3] + P[3][1]*SF[5] - P[1][1]*SPP[0] - P[13][1]*SPP[8] + P[14][1]*SPP[2] + P[15][1]*SPP[5]) - SPP[8]*(P[5][13] + P[0][13]*SF[4] + P[2][13]*SF[3] + P[3][13]*SF[5] - P[1][13]*SPP[0] - P[13][13]*SPP[8] + P[14][13]*SPP[2] + P[15][13]*SPP[5]) + SPP[2]*(P[5][14] + P[0][14]*SF[4] + P[2][14]*SF[3] + P[3][14]*SF[5] - P[1][14]*SPP[0] - P[13][14]*SPP[8] + P[14][14]*SPP[2] + P[15][14]*SPP[5]) + SPP[5]*(P[5][15] + P[0][15]*SF[4] + P[2][15]*SF[3] + P[3][15]*SF[5] - P[1][15]*SPP[0] - P[13][15]*SPP[8] + P[14][15]*SPP[2] + P[15][15]*SPP[5]) + dvyVar*sq(SG[1] - SG[2] + SG[3] - SG[4]); + nextP[0][6] = P[0][6] + P[1][6]*SF[9] + P[2][6]*SF[11] + P[3][6]*SF[10] + P[10][6]*SF[14] + P[11][6]*SF[15] + P[12][6]*SPP[10] + SF[4]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) - SF[5]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SF[3]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) + SPP[0]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SPP[4]*(P[0][13] + P[1][13]*SF[9] + P[2][13]*SF[11] + P[3][13]*SF[10] + P[10][13]*SF[14] + P[11][13]*SF[15] + P[12][13]*SPP[10]) - SPP[7]*(P[0][14] + P[1][14]*SF[9] + P[2][14]*SF[11] + P[3][14]*SF[10] + P[10][14]*SF[14] + P[11][14]*SF[15] + P[12][14]*SPP[10]) - SPP[1]*(P[0][15] + P[1][15]*SF[9] + P[2][15]*SF[11] + P[3][15]*SF[10] + P[10][15]*SF[14] + P[11][15]*SF[15] + P[12][15]*SPP[10]); + nextP[1][6] = P[1][6] + P[0][6]*SF[8] + P[2][6]*SF[7] + P[3][6]*SF[11] - P[12][6]*SF[15] + P[11][6]*SPP[10] - (P[10][6]*q0)/2 + SF[4]*(P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] - (P[10][1]*q0)/2) - SF[5]*(P[1][2] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2) + SF[3]*(P[1][3] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2) + SPP[0]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SPP[4]*(P[1][13] + P[0][13]*SF[8] + P[2][13]*SF[7] + P[3][13]*SF[11] - P[12][13]*SF[15] + P[11][13]*SPP[10] - (P[10][13]*q0)/2) - SPP[7]*(P[1][14] + P[0][14]*SF[8] + P[2][14]*SF[7] + P[3][14]*SF[11] - P[12][14]*SF[15] + P[11][14]*SPP[10] - (P[10][14]*q0)/2) - SPP[1]*(P[1][15] + P[0][15]*SF[8] + P[2][15]*SF[7] + P[3][15]*SF[11] - P[12][15]*SF[15] + P[11][15]*SPP[10] - (P[10][15]*q0)/2); + nextP[2][6] = P[2][6] + P[0][6]*SF[6] + P[1][6]*SF[10] + P[3][6]*SF[8] + P[12][6]*SF[14] - P[10][6]*SPP[10] - (P[11][6]*q0)/2 + SF[4]*(P[2][1] + P[0][1]*SF[6] + P[1][1]*SF[10] + P[3][1]*SF[8] + P[12][1]*SF[14] - P[10][1]*SPP[10] - (P[11][1]*q0)/2) - SF[5]*(P[2][2] + P[0][2]*SF[6] + P[1][2]*SF[10] + P[3][2]*SF[8] + P[12][2]*SF[14] - P[10][2]*SPP[10] - (P[11][2]*q0)/2) + SF[3]*(P[2][3] + P[0][3]*SF[6] + P[1][3]*SF[10] + P[3][3]*SF[8] + P[12][3]*SF[14] - P[10][3]*SPP[10] - (P[11][3]*q0)/2) + SPP[0]*(P[2][0] + P[0][0]*SF[6] + P[1][0]*SF[10] + P[3][0]*SF[8] + P[12][0]*SF[14] - P[10][0]*SPP[10] - (P[11][0]*q0)/2) + SPP[4]*(P[2][13] + P[0][13]*SF[6] + P[1][13]*SF[10] + P[3][13]*SF[8] + P[12][13]*SF[14] - P[10][13]*SPP[10] - (P[11][13]*q0)/2) - SPP[7]*(P[2][14] + P[0][14]*SF[6] + P[1][14]*SF[10] + P[3][14]*SF[8] + P[12][14]*SF[14] - P[10][14]*SPP[10] - (P[11][14]*q0)/2) - SPP[1]*(P[2][15] + P[0][15]*SF[6] + P[1][15]*SF[10] + P[3][15]*SF[8] + P[12][15]*SF[14] - P[10][15]*SPP[10] - (P[11][15]*q0)/2); + nextP[3][6] = P[3][6] + P[0][6]*SF[7] + P[1][6]*SF[6] + P[2][6]*SF[9] + P[10][6]*SF[15] - P[11][6]*SF[14] - (P[12][6]*q0)/2 + SF[4]*(P[3][1] + P[0][1]*SF[7] + P[1][1]*SF[6] + P[2][1]*SF[9] + P[10][1]*SF[15] - P[11][1]*SF[14] - (P[12][1]*q0)/2) - SF[5]*(P[3][2] + P[0][2]*SF[7] + P[1][2]*SF[6] + P[2][2]*SF[9] + P[10][2]*SF[15] - P[11][2]*SF[14] - (P[12][2]*q0)/2) + SF[3]*(P[3][3] + P[0][3]*SF[7] + P[1][3]*SF[6] + P[2][3]*SF[9] + P[10][3]*SF[15] - P[11][3]*SF[14] - (P[12][3]*q0)/2) + SPP[0]*(P[3][0] + P[0][0]*SF[7] + P[1][0]*SF[6] + P[2][0]*SF[9] + P[10][0]*SF[15] - P[11][0]*SF[14] - (P[12][0]*q0)/2) + SPP[4]*(P[3][13] + P[0][13]*SF[7] + P[1][13]*SF[6] + P[2][13]*SF[9] + P[10][13]*SF[15] - P[11][13]*SF[14] - (P[12][13]*q0)/2) - SPP[7]*(P[3][14] + P[0][14]*SF[7] + P[1][14]*SF[6] + P[2][14]*SF[9] + P[10][14]*SF[15] - P[11][14]*SF[14] - (P[12][14]*q0)/2) - SPP[1]*(P[3][15] + P[0][15]*SF[7] + P[1][15]*SF[6] + P[2][15]*SF[9] + P[10][15]*SF[15] - P[11][15]*SF[14] - (P[12][15]*q0)/2); + nextP[4][6] = P[4][6] + SQ[1] + P[0][6]*SF[5] + P[1][6]*SF[3] - P[3][6]*SF[4] + P[2][6]*SPP[0] + P[13][6]*SPP[3] + P[14][6]*SPP[6] - P[15][6]*SPP[9] + SF[4]*(P[4][1] + P[0][1]*SF[5] + P[1][1]*SF[3] - P[3][1]*SF[4] + P[2][1]*SPP[0] + P[13][1]*SPP[3] + P[14][1]*SPP[6] - P[15][1]*SPP[9]) - SF[5]*(P[4][2] + P[0][2]*SF[5] + P[1][2]*SF[3] - P[3][2]*SF[4] + P[2][2]*SPP[0] + P[13][2]*SPP[3] + P[14][2]*SPP[6] - P[15][2]*SPP[9]) + SF[3]*(P[4][3] + P[0][3]*SF[5] + P[1][3]*SF[3] - P[3][3]*SF[4] + P[2][3]*SPP[0] + P[13][3]*SPP[3] + P[14][3]*SPP[6] - P[15][3]*SPP[9]) + SPP[0]*(P[4][0] + P[0][0]*SF[5] + P[1][0]*SF[3] - P[3][0]*SF[4] + P[2][0]*SPP[0] + P[13][0]*SPP[3] + P[14][0]*SPP[6] - P[15][0]*SPP[9]) + SPP[4]*(P[4][13] + P[0][13]*SF[5] + P[1][13]*SF[3] - P[3][13]*SF[4] + P[2][13]*SPP[0] + P[13][13]*SPP[3] + P[14][13]*SPP[6] - P[15][13]*SPP[9]) - SPP[7]*(P[4][14] + P[0][14]*SF[5] + P[1][14]*SF[3] - P[3][14]*SF[4] + P[2][14]*SPP[0] + P[13][14]*SPP[3] + P[14][14]*SPP[6] - P[15][14]*SPP[9]) - SPP[1]*(P[4][15] + P[0][15]*SF[5] + P[1][15]*SF[3] - P[3][15]*SF[4] + P[2][15]*SPP[0] + P[13][15]*SPP[3] + P[14][15]*SPP[6] - P[15][15]*SPP[9]); + nextP[5][6] = P[5][6] + SQ[0] + P[0][6]*SF[4] + P[2][6]*SF[3] + P[3][6]*SF[5] - P[1][6]*SPP[0] - P[13][6]*SPP[8] + P[14][6]*SPP[2] + P[15][6]*SPP[5] + SF[4]*(P[5][1] + P[0][1]*SF[4] + P[2][1]*SF[3] + P[3][1]*SF[5] - P[1][1]*SPP[0] - P[13][1]*SPP[8] + P[14][1]*SPP[2] + P[15][1]*SPP[5]) - SF[5]*(P[5][2] + P[0][2]*SF[4] + P[2][2]*SF[3] + P[3][2]*SF[5] - P[1][2]*SPP[0] - P[13][2]*SPP[8] + P[14][2]*SPP[2] + P[15][2]*SPP[5]) + SF[3]*(P[5][3] + P[0][3]*SF[4] + P[2][3]*SF[3] + P[3][3]*SF[5] - P[1][3]*SPP[0] - P[13][3]*SPP[8] + P[14][3]*SPP[2] + P[15][3]*SPP[5]) + SPP[0]*(P[5][0] + P[0][0]*SF[4] + P[2][0]*SF[3] + P[3][0]*SF[5] - P[1][0]*SPP[0] - P[13][0]*SPP[8] + P[14][0]*SPP[2] + P[15][0]*SPP[5]) + SPP[4]*(P[5][13] + P[0][13]*SF[4] + P[2][13]*SF[3] + P[3][13]*SF[5] - P[1][13]*SPP[0] - P[13][13]*SPP[8] + P[14][13]*SPP[2] + P[15][13]*SPP[5]) - SPP[7]*(P[5][14] + P[0][14]*SF[4] + P[2][14]*SF[3] + P[3][14]*SF[5] - P[1][14]*SPP[0] - P[13][14]*SPP[8] + P[14][14]*SPP[2] + P[15][14]*SPP[5]) - SPP[1]*(P[5][15] + P[0][15]*SF[4] + P[2][15]*SF[3] + P[3][15]*SF[5] - P[1][15]*SPP[0] - P[13][15]*SPP[8] + P[14][15]*SPP[2] + P[15][15]*SPP[5]); + nextP[6][6] = P[6][6] + P[1][6]*SF[4] - P[2][6]*SF[5] + P[3][6]*SF[3] + P[0][6]*SPP[0] + P[13][6]*SPP[4] - P[14][6]*SPP[7] - P[15][6]*SPP[1] + dvxVar*sq(SG[6] - 2*q0*q2) + dvyVar*sq(SG[5] + 2*q0*q1) + SF[4]*(P[6][1] + P[1][1]*SF[4] - P[2][1]*SF[5] + P[3][1]*SF[3] + P[0][1]*SPP[0] + P[13][1]*SPP[4] - P[14][1]*SPP[7] - P[15][1]*SPP[1]) - SF[5]*(P[6][2] + P[1][2]*SF[4] - P[2][2]*SF[5] + P[3][2]*SF[3] + P[0][2]*SPP[0] + P[13][2]*SPP[4] - P[14][2]*SPP[7] - P[15][2]*SPP[1]) + SF[3]*(P[6][3] + P[1][3]*SF[4] - P[2][3]*SF[5] + P[3][3]*SF[3] + P[0][3]*SPP[0] + P[13][3]*SPP[4] - P[14][3]*SPP[7] - P[15][3]*SPP[1]) + SPP[0]*(P[6][0] + P[1][0]*SF[4] - P[2][0]*SF[5] + P[3][0]*SF[3] + P[0][0]*SPP[0] + P[13][0]*SPP[4] - P[14][0]*SPP[7] - P[15][0]*SPP[1]) + SPP[4]*(P[6][13] + P[1][13]*SF[4] - P[2][13]*SF[5] + P[3][13]*SF[3] + P[0][13]*SPP[0] + P[13][13]*SPP[4] - P[14][13]*SPP[7] - P[15][13]*SPP[1]) - SPP[7]*(P[6][14] + P[1][14]*SF[4] - P[2][14]*SF[5] + P[3][14]*SF[3] + P[0][14]*SPP[0] + P[13][14]*SPP[4] - P[14][14]*SPP[7] - P[15][14]*SPP[1]) - SPP[1]*(P[6][15] + P[1][15]*SF[4] - P[2][15]*SF[5] + P[3][15]*SF[3] + P[0][15]*SPP[0] + P[13][15]*SPP[4] - P[14][15]*SPP[7] - P[15][15]*SPP[1]) + dvzVar*sq(SG[1] - SG[2] - SG[3] + SG[4]); + nextP[0][7] = P[0][7] + P[1][7]*SF[9] + P[2][7]*SF[11] + P[3][7]*SF[10] + P[10][7]*SF[14] + P[11][7]*SF[15] + P[12][7]*SPP[10] + dt*(P[0][4] + P[1][4]*SF[9] + P[2][4]*SF[11] + P[3][4]*SF[10] + P[10][4]*SF[14] + P[11][4]*SF[15] + P[12][4]*SPP[10]); + nextP[1][7] = P[1][7] + P[0][7]*SF[8] + P[2][7]*SF[7] + P[3][7]*SF[11] - P[12][7]*SF[15] + P[11][7]*SPP[10] - (P[10][7]*q0)/2 + dt*(P[1][4] + P[0][4]*SF[8] + P[2][4]*SF[7] + P[3][4]*SF[11] - P[12][4]*SF[15] + P[11][4]*SPP[10] - (P[10][4]*q0)/2); + nextP[2][7] = P[2][7] + P[0][7]*SF[6] + P[1][7]*SF[10] + P[3][7]*SF[8] + P[12][7]*SF[14] - P[10][7]*SPP[10] - (P[11][7]*q0)/2 + dt*(P[2][4] + P[0][4]*SF[6] + P[1][4]*SF[10] + P[3][4]*SF[8] + P[12][4]*SF[14] - P[10][4]*SPP[10] - (P[11][4]*q0)/2); + nextP[3][7] = P[3][7] + P[0][7]*SF[7] + P[1][7]*SF[6] + P[2][7]*SF[9] + P[10][7]*SF[15] - P[11][7]*SF[14] - (P[12][7]*q0)/2 + dt*(P[3][4] + P[0][4]*SF[7] + P[1][4]*SF[6] + P[2][4]*SF[9] + P[10][4]*SF[15] - P[11][4]*SF[14] - (P[12][4]*q0)/2); + nextP[4][7] = P[4][7] + P[0][7]*SF[5] + P[1][7]*SF[3] - P[3][7]*SF[4] + P[2][7]*SPP[0] + P[13][7]*SPP[3] + P[14][7]*SPP[6] - P[15][7]*SPP[9] + dt*(P[4][4] + P[0][4]*SF[5] + P[1][4]*SF[3] - P[3][4]*SF[4] + P[2][4]*SPP[0] + P[13][4]*SPP[3] + P[14][4]*SPP[6] - P[15][4]*SPP[9]); + nextP[5][7] = P[5][7] + P[0][7]*SF[4] + P[2][7]*SF[3] + P[3][7]*SF[5] - P[1][7]*SPP[0] - P[13][7]*SPP[8] + P[14][7]*SPP[2] + P[15][7]*SPP[5] + dt*(P[5][4] + P[0][4]*SF[4] + P[2][4]*SF[3] + P[3][4]*SF[5] - P[1][4]*SPP[0] - P[13][4]*SPP[8] + P[14][4]*SPP[2] + P[15][4]*SPP[5]); + nextP[6][7] = P[6][7] + P[1][7]*SF[4] - P[2][7]*SF[5] + P[3][7]*SF[3] + P[0][7]*SPP[0] + P[13][7]*SPP[4] - P[14][7]*SPP[7] - P[15][7]*SPP[1] + dt*(P[6][4] + P[1][4]*SF[4] - P[2][4]*SF[5] + P[3][4]*SF[3] + P[0][4]*SPP[0] + P[13][4]*SPP[4] - P[14][4]*SPP[7] - P[15][4]*SPP[1]); + nextP[7][7] = P[7][7] + P[4][7]*dt + dt*(P[7][4] + P[4][4]*dt); + nextP[0][8] = P[0][8] + P[1][8]*SF[9] + P[2][8]*SF[11] + P[3][8]*SF[10] + P[10][8]*SF[14] + P[11][8]*SF[15] + P[12][8]*SPP[10] + dt*(P[0][5] + P[1][5]*SF[9] + P[2][5]*SF[11] + P[3][5]*SF[10] + P[10][5]*SF[14] + P[11][5]*SF[15] + P[12][5]*SPP[10]); + nextP[1][8] = P[1][8] + P[0][8]*SF[8] + P[2][8]*SF[7] + P[3][8]*SF[11] - P[12][8]*SF[15] + P[11][8]*SPP[10] - (P[10][8]*q0)/2 + dt*(P[1][5] + P[0][5]*SF[8] + P[2][5]*SF[7] + P[3][5]*SF[11] - P[12][5]*SF[15] + P[11][5]*SPP[10] - (P[10][5]*q0)/2); + nextP[2][8] = P[2][8] + P[0][8]*SF[6] + P[1][8]*SF[10] + P[3][8]*SF[8] + P[12][8]*SF[14] - P[10][8]*SPP[10] - (P[11][8]*q0)/2 + dt*(P[2][5] + P[0][5]*SF[6] + P[1][5]*SF[10] + P[3][5]*SF[8] + P[12][5]*SF[14] - P[10][5]*SPP[10] - (P[11][5]*q0)/2); + nextP[3][8] = P[3][8] + P[0][8]*SF[7] + P[1][8]*SF[6] + P[2][8]*SF[9] + P[10][8]*SF[15] - P[11][8]*SF[14] - (P[12][8]*q0)/2 + dt*(P[3][5] + P[0][5]*SF[7] + P[1][5]*SF[6] + P[2][5]*SF[9] + P[10][5]*SF[15] - P[11][5]*SF[14] - (P[12][5]*q0)/2); + nextP[4][8] = P[4][8] + P[0][8]*SF[5] + P[1][8]*SF[3] - P[3][8]*SF[4] + P[2][8]*SPP[0] + P[13][8]*SPP[3] + P[14][8]*SPP[6] - P[15][8]*SPP[9] + dt*(P[4][5] + P[0][5]*SF[5] + P[1][5]*SF[3] - P[3][5]*SF[4] + P[2][5]*SPP[0] + P[13][5]*SPP[3] + P[14][5]*SPP[6] - P[15][5]*SPP[9]); + nextP[5][8] = P[5][8] + P[0][8]*SF[4] + P[2][8]*SF[3] + P[3][8]*SF[5] - P[1][8]*SPP[0] - P[13][8]*SPP[8] + P[14][8]*SPP[2] + P[15][8]*SPP[5] + dt*(P[5][5] + P[0][5]*SF[4] + P[2][5]*SF[3] + P[3][5]*SF[5] - P[1][5]*SPP[0] - P[13][5]*SPP[8] + P[14][5]*SPP[2] + P[15][5]*SPP[5]); + nextP[6][8] = P[6][8] + P[1][8]*SF[4] - P[2][8]*SF[5] + P[3][8]*SF[3] + P[0][8]*SPP[0] + P[13][8]*SPP[4] - P[14][8]*SPP[7] - P[15][8]*SPP[1] + dt*(P[6][5] + P[1][5]*SF[4] - P[2][5]*SF[5] + P[3][5]*SF[3] + P[0][5]*SPP[0] + P[13][5]*SPP[4] - P[14][5]*SPP[7] - P[15][5]*SPP[1]); + nextP[7][8] = P[7][8] + P[4][8]*dt + dt*(P[7][5] + P[4][5]*dt); + nextP[8][8] = P[8][8] + P[5][8]*dt + dt*(P[8][5] + P[5][5]*dt); + nextP[0][9] = P[0][9] + P[1][9]*SF[9] + P[2][9]*SF[11] + P[3][9]*SF[10] + P[10][9]*SF[14] + P[11][9]*SF[15] + P[12][9]*SPP[10] + dt*(P[0][6] + P[1][6]*SF[9] + P[2][6]*SF[11] + P[3][6]*SF[10] + P[10][6]*SF[14] + P[11][6]*SF[15] + P[12][6]*SPP[10]); + nextP[1][9] = P[1][9] + P[0][9]*SF[8] + P[2][9]*SF[7] + P[3][9]*SF[11] - P[12][9]*SF[15] + P[11][9]*SPP[10] - (P[10][9]*q0)/2 + dt*(P[1][6] + P[0][6]*SF[8] + P[2][6]*SF[7] + P[3][6]*SF[11] - P[12][6]*SF[15] + P[11][6]*SPP[10] - (P[10][6]*q0)/2); + nextP[2][9] = P[2][9] + P[0][9]*SF[6] + P[1][9]*SF[10] + P[3][9]*SF[8] + P[12][9]*SF[14] - P[10][9]*SPP[10] - (P[11][9]*q0)/2 + dt*(P[2][6] + P[0][6]*SF[6] + P[1][6]*SF[10] + P[3][6]*SF[8] + P[12][6]*SF[14] - P[10][6]*SPP[10] - (P[11][6]*q0)/2); + nextP[3][9] = P[3][9] + P[0][9]*SF[7] + P[1][9]*SF[6] + P[2][9]*SF[9] + P[10][9]*SF[15] - P[11][9]*SF[14] - (P[12][9]*q0)/2 + dt*(P[3][6] + P[0][6]*SF[7] + P[1][6]*SF[6] + P[2][6]*SF[9] + P[10][6]*SF[15] - P[11][6]*SF[14] - (P[12][6]*q0)/2); + nextP[4][9] = P[4][9] + P[0][9]*SF[5] + P[1][9]*SF[3] - P[3][9]*SF[4] + P[2][9]*SPP[0] + P[13][9]*SPP[3] + P[14][9]*SPP[6] - P[15][9]*SPP[9] + dt*(P[4][6] + P[0][6]*SF[5] + P[1][6]*SF[3] - P[3][6]*SF[4] + P[2][6]*SPP[0] + P[13][6]*SPP[3] + P[14][6]*SPP[6] - P[15][6]*SPP[9]); + nextP[5][9] = P[5][9] + P[0][9]*SF[4] + P[2][9]*SF[3] + P[3][9]*SF[5] - P[1][9]*SPP[0] - P[13][9]*SPP[8] + P[14][9]*SPP[2] + P[15][9]*SPP[5] + dt*(P[5][6] + P[0][6]*SF[4] + P[2][6]*SF[3] + P[3][6]*SF[5] - P[1][6]*SPP[0] - P[13][6]*SPP[8] + P[14][6]*SPP[2] + P[15][6]*SPP[5]); + nextP[6][9] = P[6][9] + P[1][9]*SF[4] - P[2][9]*SF[5] + P[3][9]*SF[3] + P[0][9]*SPP[0] + P[13][9]*SPP[4] - P[14][9]*SPP[7] - P[15][9]*SPP[1] + dt*(P[6][6] + P[1][6]*SF[4] - P[2][6]*SF[5] + P[3][6]*SF[3] + P[0][6]*SPP[0] + P[13][6]*SPP[4] - P[14][6]*SPP[7] - P[15][6]*SPP[1]); + nextP[7][9] = P[7][9] + P[4][9]*dt + dt*(P[7][6] + P[4][6]*dt); + nextP[8][9] = P[8][9] + P[5][9]*dt + dt*(P[8][6] + P[5][6]*dt); + nextP[9][9] = P[9][9] + P[6][9]*dt + dt*(P[9][6] + P[6][6]*dt); + nextP[0][10] = P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10]; + nextP[1][10] = P[1][10] + P[0][10]*SF[8] + P[2][10]*SF[7] + P[3][10]*SF[11] - P[12][10]*SF[15] + P[11][10]*SPP[10] - (P[10][10]*q0)/2; + nextP[2][10] = P[2][10] + P[0][10]*SF[6] + P[1][10]*SF[10] + P[3][10]*SF[8] + P[12][10]*SF[14] - P[10][10]*SPP[10] - (P[11][10]*q0)/2; + nextP[3][10] = P[3][10] + P[0][10]*SF[7] + P[1][10]*SF[6] + P[2][10]*SF[9] + P[10][10]*SF[15] - P[11][10]*SF[14] - (P[12][10]*q0)/2; + nextP[4][10] = P[4][10] + P[0][10]*SF[5] + P[1][10]*SF[3] - P[3][10]*SF[4] + P[2][10]*SPP[0] + P[13][10]*SPP[3] + P[14][10]*SPP[6] - P[15][10]*SPP[9]; + nextP[5][10] = P[5][10] + P[0][10]*SF[4] + P[2][10]*SF[3] + P[3][10]*SF[5] - P[1][10]*SPP[0] - P[13][10]*SPP[8] + P[14][10]*SPP[2] + P[15][10]*SPP[5]; + nextP[6][10] = P[6][10] + P[1][10]*SF[4] - P[2][10]*SF[5] + P[3][10]*SF[3] + P[0][10]*SPP[0] + P[13][10]*SPP[4] - P[14][10]*SPP[7] - P[15][10]*SPP[1]; + nextP[7][10] = P[7][10] + P[4][10]*dt; + nextP[8][10] = P[8][10] + P[5][10]*dt; + nextP[9][10] = P[9][10] + P[6][10]*dt; nextP[10][10] = P[10][10]; - nextP[0][11] = P[0][11] * SPP[5] - P[1][11] * SPP[4] + P[2][11] * SPP[7] + P[9][11] * SPP[22] + P[12][11] * SPP[18]; - nextP[1][11] = P[1][11] * SPP[6] - P[0][11] * SPP[2] - P[2][11] * SPP[8] + P[10][11] * SPP[22] + P[13][11] * SPP[17]; - nextP[2][11] = P[0][11] * SPP[14] - P[1][11] * SPP[3] + P[2][11] * SPP[13] + P[11][11] * SPP[22] + P[14][11] * SPP[16]; - nextP[3][11] = P[3][11] + P[0][11] * SPP[1] + P[1][11] * SPP[19] + P[2][11] * SPP[15] - P[15][11] * SPP[21]; - nextP[4][11] = P[4][11] + P[15][11] * SF[22] + P[0][11] * SPP[20] + P[1][11] * SPP[12] + P[2][11] * SPP[11]; - nextP[5][11] = P[5][11] + P[15][11] * SF[20] - P[0][11] * SPP[9] + P[1][11] * SPP[10] + P[2][11] * SPP[0]; - nextP[6][11] = P[6][11] + P[3][11] * dt; - nextP[7][11] = P[7][11] + P[4][11] * dt; - nextP[8][11] = P[8][11] + P[5][11] * dt; - nextP[9][11] = P[9][11]; + nextP[0][11] = P[0][11] + P[1][11]*SF[9] + P[2][11]*SF[11] + P[3][11]*SF[10] + P[10][11]*SF[14] + P[11][11]*SF[15] + P[12][11]*SPP[10]; + nextP[1][11] = P[1][11] + P[0][11]*SF[8] + P[2][11]*SF[7] + P[3][11]*SF[11] - P[12][11]*SF[15] + P[11][11]*SPP[10] - (P[10][11]*q0)/2; + nextP[2][11] = P[2][11] + P[0][11]*SF[6] + P[1][11]*SF[10] + P[3][11]*SF[8] + P[12][11]*SF[14] - P[10][11]*SPP[10] - (P[11][11]*q0)/2; + nextP[3][11] = P[3][11] + P[0][11]*SF[7] + P[1][11]*SF[6] + P[2][11]*SF[9] + P[10][11]*SF[15] - P[11][11]*SF[14] - (P[12][11]*q0)/2; + nextP[4][11] = P[4][11] + P[0][11]*SF[5] + P[1][11]*SF[3] - P[3][11]*SF[4] + P[2][11]*SPP[0] + P[13][11]*SPP[3] + P[14][11]*SPP[6] - P[15][11]*SPP[9]; + nextP[5][11] = P[5][11] + P[0][11]*SF[4] + P[2][11]*SF[3] + P[3][11]*SF[5] - P[1][11]*SPP[0] - P[13][11]*SPP[8] + P[14][11]*SPP[2] + P[15][11]*SPP[5]; + nextP[6][11] = P[6][11] + P[1][11]*SF[4] - P[2][11]*SF[5] + P[3][11]*SF[3] + P[0][11]*SPP[0] + P[13][11]*SPP[4] - P[14][11]*SPP[7] - P[15][11]*SPP[1]; + nextP[7][11] = P[7][11] + P[4][11]*dt; + nextP[8][11] = P[8][11] + P[5][11]*dt; + nextP[9][11] = P[9][11] + P[6][11]*dt; nextP[10][11] = P[10][11]; nextP[11][11] = P[11][11]; - nextP[0][12] = P[0][12] * SPP[5] - P[1][12] * SPP[4] + P[2][12] * SPP[7] + P[9][12] * SPP[22] + P[12][12] * SPP[18]; - nextP[1][12] = P[1][12] * SPP[6] - P[0][12] * SPP[2] - P[2][12] * SPP[8] + P[10][12] * SPP[22] + P[13][12] * SPP[17]; - nextP[2][12] = P[0][12] * SPP[14] - P[1][12] * SPP[3] + P[2][12] * SPP[13] + P[11][12] * SPP[22] + P[14][12] * SPP[16]; - nextP[3][12] = P[3][12] + P[0][12] * SPP[1] + P[1][12] * SPP[19] + P[2][12] * SPP[15] - P[15][12] * SPP[21]; - nextP[4][12] = P[4][12] + P[15][12] * SF[22] + P[0][12] * SPP[20] + P[1][12] * SPP[12] + P[2][12] * SPP[11]; - nextP[5][12] = P[5][12] + P[15][12] * SF[20] - P[0][12] * SPP[9] + P[1][12] * SPP[10] + P[2][12] * SPP[0]; - nextP[6][12] = P[6][12] + P[3][12] * dt; - nextP[7][12] = P[7][12] + P[4][12] * dt; - nextP[8][12] = P[8][12] + P[5][12] * dt; - nextP[9][12] = P[9][12]; + nextP[0][12] = P[0][12] + P[1][12]*SF[9] + P[2][12]*SF[11] + P[3][12]*SF[10] + P[10][12]*SF[14] + P[11][12]*SF[15] + P[12][12]*SPP[10]; + nextP[1][12] = P[1][12] + P[0][12]*SF[8] + P[2][12]*SF[7] + P[3][12]*SF[11] - P[12][12]*SF[15] + P[11][12]*SPP[10] - (P[10][12]*q0)/2; + nextP[2][12] = P[2][12] + P[0][12]*SF[6] + P[1][12]*SF[10] + P[3][12]*SF[8] + P[12][12]*SF[14] - P[10][12]*SPP[10] - (P[11][12]*q0)/2; + nextP[3][12] = P[3][12] + P[0][12]*SF[7] + P[1][12]*SF[6] + P[2][12]*SF[9] + P[10][12]*SF[15] - P[11][12]*SF[14] - (P[12][12]*q0)/2; + nextP[4][12] = P[4][12] + P[0][12]*SF[5] + P[1][12]*SF[3] - P[3][12]*SF[4] + P[2][12]*SPP[0] + P[13][12]*SPP[3] + P[14][12]*SPP[6] - P[15][12]*SPP[9]; + nextP[5][12] = P[5][12] + P[0][12]*SF[4] + P[2][12]*SF[3] + P[3][12]*SF[5] - P[1][12]*SPP[0] - P[13][12]*SPP[8] + P[14][12]*SPP[2] + P[15][12]*SPP[5]; + nextP[6][12] = P[6][12] + P[1][12]*SF[4] - P[2][12]*SF[5] + P[3][12]*SF[3] + P[0][12]*SPP[0] + P[13][12]*SPP[4] - P[14][12]*SPP[7] - P[15][12]*SPP[1]; + nextP[7][12] = P[7][12] + P[4][12]*dt; + nextP[8][12] = P[8][12] + P[5][12]*dt; + nextP[9][12] = P[9][12] + P[6][12]*dt; nextP[10][12] = P[10][12]; nextP[11][12] = P[11][12]; nextP[12][12] = P[12][12]; - nextP[0][13] = P[0][13] * SPP[5] - P[1][13] * SPP[4] + P[2][13] * SPP[7] + P[9][13] * SPP[22] + P[12][13] * SPP[18]; - nextP[1][13] = P[1][13] * SPP[6] - P[0][13] * SPP[2] - P[2][13] * SPP[8] + P[10][13] * SPP[22] + P[13][13] * SPP[17]; - nextP[2][13] = P[0][13] * SPP[14] - P[1][13] * SPP[3] + P[2][13] * SPP[13] + P[11][13] * SPP[22] + P[14][13] * SPP[16]; - nextP[3][13] = P[3][13] + P[0][13] * SPP[1] + P[1][13] * SPP[19] + P[2][13] * SPP[15] - P[15][13] * SPP[21]; - nextP[4][13] = P[4][13] + P[15][13] * SF[22] + P[0][13] * SPP[20] + P[1][13] * SPP[12] + P[2][13] * SPP[11]; - nextP[5][13] = P[5][13] + P[15][13] * SF[20] - P[0][13] * SPP[9] + P[1][13] * SPP[10] + P[2][13] * SPP[0]; - nextP[6][13] = P[6][13] + P[3][13] * dt; - nextP[7][13] = P[7][13] + P[4][13] * dt; - nextP[8][13] = P[8][13] + P[5][13] * dt; - nextP[9][13] = P[9][13]; - nextP[10][13] = P[10][13]; - nextP[11][13] = P[11][13]; - nextP[12][13] = P[12][13]; - nextP[13][13] = P[13][13]; - nextP[0][14] = P[0][14] * SPP[5] - P[1][14] * SPP[4] + P[2][14] * SPP[7] + P[9][14] * SPP[22] + P[12][14] * SPP[18]; - nextP[1][14] = P[1][14] * SPP[6] - P[0][14] * SPP[2] - P[2][14] * SPP[8] + P[10][14] * SPP[22] + P[13][14] * SPP[17]; - nextP[2][14] = P[0][14] * SPP[14] - P[1][14] * SPP[3] + P[2][14] * SPP[13] + P[11][14] * SPP[22] + P[14][14] * SPP[16]; - nextP[3][14] = P[3][14] + P[0][14] * SPP[1] + P[1][14] * SPP[19] + P[2][14] * SPP[15] - P[15][14] * SPP[21]; - nextP[4][14] = P[4][14] + P[15][14] * SF[22] + P[0][14] * SPP[20] + P[1][14] * SPP[12] + P[2][14] * SPP[11]; - nextP[5][14] = P[5][14] + P[15][14] * SF[20] - P[0][14] * SPP[9] + P[1][14] * SPP[10] + P[2][14] * SPP[0]; - nextP[6][14] = P[6][14] + P[3][14] * dt; - nextP[7][14] = P[7][14] + P[4][14] * dt; - nextP[8][14] = P[8][14] + P[5][14] * dt; - nextP[9][14] = P[9][14]; - nextP[10][14] = P[10][14]; - nextP[11][14] = P[11][14]; - nextP[12][14] = P[12][14]; - nextP[13][14] = P[13][14]; - nextP[14][14] = P[14][14]; - nextP[0][15] = P[0][15] * SPP[5] - P[1][15] * SPP[4] + P[2][15] * SPP[7] + P[9][15] * SPP[22] + P[12][15] * SPP[18]; - nextP[1][15] = P[1][15] * SPP[6] - P[0][15] * SPP[2] - P[2][15] * SPP[8] + P[10][15] * SPP[22] + P[13][15] * SPP[17]; - nextP[2][15] = P[0][15] * SPP[14] - P[1][15] * SPP[3] + P[2][15] * SPP[13] + P[11][15] * SPP[22] + P[14][15] * SPP[16]; - nextP[3][15] = P[3][15] + P[0][15] * SPP[1] + P[1][15] * SPP[19] + P[2][15] * SPP[15] - P[15][15] * SPP[21]; - nextP[4][15] = P[4][15] + P[15][15] * SF[22] + P[0][15] * SPP[20] + P[1][15] * SPP[12] + P[2][15] * SPP[11]; - nextP[5][15] = P[5][15] + P[15][15] * SF[20] - P[0][15] * SPP[9] + P[1][15] * SPP[10] + P[2][15] * SPP[0]; - nextP[6][15] = P[6][15] + P[3][15] * dt; - nextP[7][15] = P[7][15] + P[4][15] * dt; - nextP[8][15] = P[8][15] + P[5][15] * dt; - nextP[9][15] = P[9][15]; - nextP[10][15] = P[10][15]; - nextP[11][15] = P[11][15]; - nextP[12][15] = P[12][15]; - nextP[13][15] = P[13][15]; - nextP[14][15] = P[14][15]; - nextP[15][15] = P[15][15]; + + // add process noise that is not from the IMU + for (unsigned i = 0; i <= 12; i++) { + nextP[i][i] += process_noise[i]; + } + + // Don't calculate these covariance terms if IMU delta vlocity bias estimation is inhibited + if (!(_params.fusion_mode & MASK_INHIBIT_ACC_BIAS)) { + + // calculate variances and upper diagonal covariances for IMU delta velocity bias states + nextP[0][13] = P[0][13] + P[1][13]*SF[9] + P[2][13]*SF[11] + P[3][13]*SF[10] + P[10][13]*SF[14] + P[11][13]*SF[15] + P[12][13]*SPP[10]; + nextP[1][13] = P[1][13] + P[0][13]*SF[8] + P[2][13]*SF[7] + P[3][13]*SF[11] - P[12][13]*SF[15] + P[11][13]*SPP[10] - (P[10][13]*q0)/2; + nextP[2][13] = P[2][13] + P[0][13]*SF[6] + P[1][13]*SF[10] + P[3][13]*SF[8] + P[12][13]*SF[14] - P[10][13]*SPP[10] - (P[11][13]*q0)/2; + nextP[3][13] = P[3][13] + P[0][13]*SF[7] + P[1][13]*SF[6] + P[2][13]*SF[9] + P[10][13]*SF[15] - P[11][13]*SF[14] - (P[12][13]*q0)/2; + nextP[4][13] = P[4][13] + P[0][13]*SF[5] + P[1][13]*SF[3] - P[3][13]*SF[4] + P[2][13]*SPP[0] + P[13][13]*SPP[3] + P[14][13]*SPP[6] - P[15][13]*SPP[9]; + nextP[5][13] = P[5][13] + P[0][13]*SF[4] + P[2][13]*SF[3] + P[3][13]*SF[5] - P[1][13]*SPP[0] - P[13][13]*SPP[8] + P[14][13]*SPP[2] + P[15][13]*SPP[5]; + nextP[6][13] = P[6][13] + P[1][13]*SF[4] - P[2][13]*SF[5] + P[3][13]*SF[3] + P[0][13]*SPP[0] + P[13][13]*SPP[4] - P[14][13]*SPP[7] - P[15][13]*SPP[1]; + nextP[7][13] = P[7][13] + P[4][13]*dt; + nextP[8][13] = P[8][13] + P[5][13]*dt; + nextP[9][13] = P[9][13] + P[6][13]*dt; + nextP[10][13] = P[10][13]; + nextP[11][13] = P[11][13]; + nextP[12][13] = P[12][13]; + nextP[13][13] = P[13][13]; + nextP[0][14] = P[0][14] + P[1][14]*SF[9] + P[2][14]*SF[11] + P[3][14]*SF[10] + P[10][14]*SF[14] + P[11][14]*SF[15] + P[12][14]*SPP[10]; + nextP[1][14] = P[1][14] + P[0][14]*SF[8] + P[2][14]*SF[7] + P[3][14]*SF[11] - P[12][14]*SF[15] + P[11][14]*SPP[10] - (P[10][14]*q0)/2; + nextP[2][14] = P[2][14] + P[0][14]*SF[6] + P[1][14]*SF[10] + P[3][14]*SF[8] + P[12][14]*SF[14] - P[10][14]*SPP[10] - (P[11][14]*q0)/2; + nextP[3][14] = P[3][14] + P[0][14]*SF[7] + P[1][14]*SF[6] + P[2][14]*SF[9] + P[10][14]*SF[15] - P[11][14]*SF[14] - (P[12][14]*q0)/2; + nextP[4][14] = P[4][14] + P[0][14]*SF[5] + P[1][14]*SF[3] - P[3][14]*SF[4] + P[2][14]*SPP[0] + P[13][14]*SPP[3] + P[14][14]*SPP[6] - P[15][14]*SPP[9]; + nextP[5][14] = P[5][14] + P[0][14]*SF[4] + P[2][14]*SF[3] + P[3][14]*SF[5] - P[1][14]*SPP[0] - P[13][14]*SPP[8] + P[14][14]*SPP[2] + P[15][14]*SPP[5]; + nextP[6][14] = P[6][14] + P[1][14]*SF[4] - P[2][14]*SF[5] + P[3][14]*SF[3] + P[0][14]*SPP[0] + P[13][14]*SPP[4] - P[14][14]*SPP[7] - P[15][14]*SPP[1]; + nextP[7][14] = P[7][14] + P[4][14]*dt; + nextP[8][14] = P[8][14] + P[5][14]*dt; + nextP[9][14] = P[9][14] + P[6][14]*dt; + nextP[10][14] = P[10][14]; + nextP[11][14] = P[11][14]; + nextP[12][14] = P[12][14]; + nextP[13][14] = P[13][14]; + nextP[14][14] = P[14][14]; + nextP[0][15] = P[0][15] + P[1][15]*SF[9] + P[2][15]*SF[11] + P[3][15]*SF[10] + P[10][15]*SF[14] + P[11][15]*SF[15] + P[12][15]*SPP[10]; + nextP[1][15] = P[1][15] + P[0][15]*SF[8] + P[2][15]*SF[7] + P[3][15]*SF[11] - P[12][15]*SF[15] + P[11][15]*SPP[10] - (P[10][15]*q0)/2; + nextP[2][15] = P[2][15] + P[0][15]*SF[6] + P[1][15]*SF[10] + P[3][15]*SF[8] + P[12][15]*SF[14] - P[10][15]*SPP[10] - (P[11][15]*q0)/2; + nextP[3][15] = P[3][15] + P[0][15]*SF[7] + P[1][15]*SF[6] + P[2][15]*SF[9] + P[10][15]*SF[15] - P[11][15]*SF[14] - (P[12][15]*q0)/2; + nextP[4][15] = P[4][15] + P[0][15]*SF[5] + P[1][15]*SF[3] - P[3][15]*SF[4] + P[2][15]*SPP[0] + P[13][15]*SPP[3] + P[14][15]*SPP[6] - P[15][15]*SPP[9]; + nextP[5][15] = P[5][15] + P[0][15]*SF[4] + P[2][15]*SF[3] + P[3][15]*SF[5] - P[1][15]*SPP[0] - P[13][15]*SPP[8] + P[14][15]*SPP[2] + P[15][15]*SPP[5]; + nextP[6][15] = P[6][15] + P[1][15]*SF[4] - P[2][15]*SF[5] + P[3][15]*SF[3] + P[0][15]*SPP[0] + P[13][15]*SPP[4] - P[14][15]*SPP[7] - P[15][15]*SPP[1]; + nextP[7][15] = P[7][15] + P[4][15]*dt; + nextP[8][15] = P[8][15] + P[5][15]*dt; + nextP[9][15] = P[9][15] + P[6][15]*dt; + nextP[10][15] = P[10][15]; + nextP[11][15] = P[11][15]; + nextP[12][15] = P[12][15]; + nextP[13][15] = P[13][15]; + nextP[14][15] = P[14][15]; + nextP[15][15] = P[15][15]; + + // add process noise that is not from the IMU + for (unsigned i = 13; i <= 15; i++) { + nextP[i][i] += process_noise[i]; + } + + } // Don't do covariance prediction on magnetic field states unless we are using 3-axis fusion if (_control_status.flags.mag_3D) { @@ -515,16 +439,17 @@ void Ekf::predictCovariance() } } - nextP[0][16] = P[0][16] * SPP[5] - P[1][16] * SPP[4] + P[2][16] * SPP[7] + P[9][16] * SPP[22] + P[12][16] * SPP[18]; - nextP[1][16] = P[1][16] * SPP[6] - P[0][16] * SPP[2] - P[2][16] * SPP[8] + P[10][16] * SPP[22] + P[13][16] * SPP[17]; - nextP[2][16] = P[0][16] * SPP[14] - P[1][16] * SPP[3] + P[2][16] * SPP[13] + P[11][16] * SPP[22] + P[14][16] * SPP[16]; - nextP[3][16] = P[3][16] + P[0][16] * SPP[1] + P[1][16] * SPP[19] + P[2][16] * SPP[15] - P[15][16] * SPP[21]; - nextP[4][16] = P[4][16] + P[15][16] * SF[22] + P[0][16] * SPP[20] + P[1][16] * SPP[12] + P[2][16] * SPP[11]; - nextP[5][16] = P[5][16] + P[15][16] * SF[20] - P[0][16] * SPP[9] + P[1][16] * SPP[10] + P[2][16] * SPP[0]; - nextP[6][16] = P[6][16] + P[3][16] * dt; - nextP[7][16] = P[7][16] + P[4][16] * dt; - nextP[8][16] = P[8][16] + P[5][16] * dt; - nextP[9][16] = P[9][16]; + // calculate variances and upper diagonal covariances for earth and body magnetic field states + nextP[0][16] = P[0][16] + P[1][16]*SF[9] + P[2][16]*SF[11] + P[3][16]*SF[10] + P[10][16]*SF[14] + P[11][16]*SF[15] + P[12][16]*SPP[10]; + nextP[1][16] = P[1][16] + P[0][16]*SF[8] + P[2][16]*SF[7] + P[3][16]*SF[11] - P[12][16]*SF[15] + P[11][16]*SPP[10] - (P[10][16]*q0)/2; + nextP[2][16] = P[2][16] + P[0][16]*SF[6] + P[1][16]*SF[10] + P[3][16]*SF[8] + P[12][16]*SF[14] - P[10][16]*SPP[10] - (P[11][16]*q0)/2; + nextP[3][16] = P[3][16] + P[0][16]*SF[7] + P[1][16]*SF[6] + P[2][16]*SF[9] + P[10][16]*SF[15] - P[11][16]*SF[14] - (P[12][16]*q0)/2; + nextP[4][16] = P[4][16] + P[0][16]*SF[5] + P[1][16]*SF[3] - P[3][16]*SF[4] + P[2][16]*SPP[0] + P[13][16]*SPP[3] + P[14][16]*SPP[6] - P[15][16]*SPP[9]; + nextP[5][16] = P[5][16] + P[0][16]*SF[4] + P[2][16]*SF[3] + P[3][16]*SF[5] - P[1][16]*SPP[0] - P[13][16]*SPP[8] + P[14][16]*SPP[2] + P[15][16]*SPP[5]; + nextP[6][16] = P[6][16] + P[1][16]*SF[4] - P[2][16]*SF[5] + P[3][16]*SF[3] + P[0][16]*SPP[0] + P[13][16]*SPP[4] - P[14][16]*SPP[7] - P[15][16]*SPP[1]; + nextP[7][16] = P[7][16] + P[4][16]*dt; + nextP[8][16] = P[8][16] + P[5][16]*dt; + nextP[9][16] = P[9][16] + P[6][16]*dt; nextP[10][16] = P[10][16]; nextP[11][16] = P[11][16]; nextP[12][16] = P[12][16]; @@ -532,17 +457,16 @@ void Ekf::predictCovariance() nextP[14][16] = P[14][16]; nextP[15][16] = P[15][16]; nextP[16][16] = P[16][16]; - - nextP[0][17] = P[0][17] * SPP[5] - P[1][17] * SPP[4] + P[2][17] * SPP[7] + P[9][17] * SPP[22] + P[12][17] * SPP[18]; - nextP[1][17] = P[1][17] * SPP[6] - P[0][17] * SPP[2] - P[2][17] * SPP[8] + P[10][17] * SPP[22] + P[13][17] * SPP[17]; - nextP[2][17] = P[0][17] * SPP[14] - P[1][17] * SPP[3] + P[2][17] * SPP[13] + P[11][17] * SPP[22] + P[14][17] * SPP[16]; - nextP[3][17] = P[3][17] + P[0][17] * SPP[1] + P[1][17] * SPP[19] + P[2][17] * SPP[15] - P[15][17] * SPP[21]; - nextP[4][17] = P[4][17] + P[15][17] * SF[22] + P[0][17] * SPP[20] + P[1][17] * SPP[12] + P[2][17] * SPP[11]; - nextP[5][17] = P[5][17] + P[15][17] * SF[20] - P[0][17] * SPP[9] + P[1][17] * SPP[10] + P[2][17] * SPP[0]; - nextP[6][17] = P[6][17] + P[3][17] * dt; - nextP[7][17] = P[7][17] + P[4][17] * dt; - nextP[8][17] = P[8][17] + P[5][17] * dt; - nextP[9][17] = P[9][17]; + nextP[0][17] = P[0][17] + P[1][17]*SF[9] + P[2][17]*SF[11] + P[3][17]*SF[10] + P[10][17]*SF[14] + P[11][17]*SF[15] + P[12][17]*SPP[10]; + nextP[1][17] = P[1][17] + P[0][17]*SF[8] + P[2][17]*SF[7] + P[3][17]*SF[11] - P[12][17]*SF[15] + P[11][17]*SPP[10] - (P[10][17]*q0)/2; + nextP[2][17] = P[2][17] + P[0][17]*SF[6] + P[1][17]*SF[10] + P[3][17]*SF[8] + P[12][17]*SF[14] - P[10][17]*SPP[10] - (P[11][17]*q0)/2; + nextP[3][17] = P[3][17] + P[0][17]*SF[7] + P[1][17]*SF[6] + P[2][17]*SF[9] + P[10][17]*SF[15] - P[11][17]*SF[14] - (P[12][17]*q0)/2; + nextP[4][17] = P[4][17] + P[0][17]*SF[5] + P[1][17]*SF[3] - P[3][17]*SF[4] + P[2][17]*SPP[0] + P[13][17]*SPP[3] + P[14][17]*SPP[6] - P[15][17]*SPP[9]; + nextP[5][17] = P[5][17] + P[0][17]*SF[4] + P[2][17]*SF[3] + P[3][17]*SF[5] - P[1][17]*SPP[0] - P[13][17]*SPP[8] + P[14][17]*SPP[2] + P[15][17]*SPP[5]; + nextP[6][17] = P[6][17] + P[1][17]*SF[4] - P[2][17]*SF[5] + P[3][17]*SF[3] + P[0][17]*SPP[0] + P[13][17]*SPP[4] - P[14][17]*SPP[7] - P[15][17]*SPP[1]; + nextP[7][17] = P[7][17] + P[4][17]*dt; + nextP[8][17] = P[8][17] + P[5][17]*dt; + nextP[9][17] = P[9][17] + P[6][17]*dt; nextP[10][17] = P[10][17]; nextP[11][17] = P[11][17]; nextP[12][17] = P[12][17]; @@ -551,17 +475,16 @@ void Ekf::predictCovariance() nextP[15][17] = P[15][17]; nextP[16][17] = P[16][17]; nextP[17][17] = P[17][17]; - - nextP[0][18] = P[0][18] * SPP[5] - P[1][18] * SPP[4] + P[2][18] * SPP[7] + P[9][18] * SPP[22] + P[12][18] * SPP[18]; - nextP[1][18] = P[1][18] * SPP[6] - P[0][18] * SPP[2] - P[2][18] * SPP[8] + P[10][18] * SPP[22] + P[13][18] * SPP[17]; - nextP[2][18] = P[0][18] * SPP[14] - P[1][18] * SPP[3] + P[2][18] * SPP[13] + P[11][18] * SPP[22] + P[14][18] * SPP[16]; - nextP[3][18] = P[3][18] + P[0][18] * SPP[1] + P[1][18] * SPP[19] + P[2][18] * SPP[15] - P[15][18] * SPP[21]; - nextP[4][18] = P[4][18] + P[15][18] * SF[22] + P[0][18] * SPP[20] + P[1][18] * SPP[12] + P[2][18] * SPP[11]; - nextP[5][18] = P[5][18] + P[15][18] * SF[20] - P[0][18] * SPP[9] + P[1][18] * SPP[10] + P[2][18] * SPP[0]; - nextP[6][18] = P[6][18] + P[3][18] * dt; - nextP[7][18] = P[7][18] + P[4][18] * dt; - nextP[8][18] = P[8][18] + P[5][18] * dt; - nextP[9][18] = P[9][18]; + nextP[0][18] = P[0][18] + P[1][18]*SF[9] + P[2][18]*SF[11] + P[3][18]*SF[10] + P[10][18]*SF[14] + P[11][18]*SF[15] + P[12][18]*SPP[10]; + nextP[1][18] = P[1][18] + P[0][18]*SF[8] + P[2][18]*SF[7] + P[3][18]*SF[11] - P[12][18]*SF[15] + P[11][18]*SPP[10] - (P[10][18]*q0)/2; + nextP[2][18] = P[2][18] + P[0][18]*SF[6] + P[1][18]*SF[10] + P[3][18]*SF[8] + P[12][18]*SF[14] - P[10][18]*SPP[10] - (P[11][18]*q0)/2; + nextP[3][18] = P[3][18] + P[0][18]*SF[7] + P[1][18]*SF[6] + P[2][18]*SF[9] + P[10][18]*SF[15] - P[11][18]*SF[14] - (P[12][18]*q0)/2; + nextP[4][18] = P[4][18] + P[0][18]*SF[5] + P[1][18]*SF[3] - P[3][18]*SF[4] + P[2][18]*SPP[0] + P[13][18]*SPP[3] + P[14][18]*SPP[6] - P[15][18]*SPP[9]; + nextP[5][18] = P[5][18] + P[0][18]*SF[4] + P[2][18]*SF[3] + P[3][18]*SF[5] - P[1][18]*SPP[0] - P[13][18]*SPP[8] + P[14][18]*SPP[2] + P[15][18]*SPP[5]; + nextP[6][18] = P[6][18] + P[1][18]*SF[4] - P[2][18]*SF[5] + P[3][18]*SF[3] + P[0][18]*SPP[0] + P[13][18]*SPP[4] - P[14][18]*SPP[7] - P[15][18]*SPP[1]; + nextP[7][18] = P[7][18] + P[4][18]*dt; + nextP[8][18] = P[8][18] + P[5][18]*dt; + nextP[9][18] = P[9][18] + P[6][18]*dt; nextP[10][18] = P[10][18]; nextP[11][18] = P[11][18]; nextP[12][18] = P[12][18]; @@ -571,17 +494,16 @@ void Ekf::predictCovariance() nextP[16][18] = P[16][18]; nextP[17][18] = P[17][18]; nextP[18][18] = P[18][18]; - - nextP[0][19] = P[0][19] * SPP[5] - P[1][19] * SPP[4] + P[2][19] * SPP[7] + P[9][19] * SPP[22] + P[12][19] * SPP[18]; - nextP[1][19] = P[1][19] * SPP[6] - P[0][19] * SPP[2] - P[2][19] * SPP[8] + P[10][19] * SPP[22] + P[13][19] * SPP[17]; - nextP[2][19] = P[0][19] * SPP[14] - P[1][19] * SPP[3] + P[2][19] * SPP[13] + P[11][19] * SPP[22] + P[14][19] * SPP[16]; - nextP[3][19] = P[3][19] + P[0][19] * SPP[1] + P[1][19] * SPP[19] + P[2][19] * SPP[15] - P[15][19] * SPP[21]; - nextP[4][19] = P[4][19] + P[15][19] * SF[22] + P[0][19] * SPP[20] + P[1][19] * SPP[12] + P[2][19] * SPP[11]; - nextP[5][19] = P[5][19] + P[15][19] * SF[20] - P[0][19] * SPP[9] + P[1][19] * SPP[10] + P[2][19] * SPP[0]; - nextP[6][19] = P[6][19] + P[3][19] * dt; - nextP[7][19] = P[7][19] + P[4][19] * dt; - nextP[8][19] = P[8][19] + P[5][19] * dt; - nextP[9][19] = P[9][19]; + nextP[0][19] = P[0][19] + P[1][19]*SF[9] + P[2][19]*SF[11] + P[3][19]*SF[10] + P[10][19]*SF[14] + P[11][19]*SF[15] + P[12][19]*SPP[10]; + nextP[1][19] = P[1][19] + P[0][19]*SF[8] + P[2][19]*SF[7] + P[3][19]*SF[11] - P[12][19]*SF[15] + P[11][19]*SPP[10] - (P[10][19]*q0)/2; + nextP[2][19] = P[2][19] + P[0][19]*SF[6] + P[1][19]*SF[10] + P[3][19]*SF[8] + P[12][19]*SF[14] - P[10][19]*SPP[10] - (P[11][19]*q0)/2; + nextP[3][19] = P[3][19] + P[0][19]*SF[7] + P[1][19]*SF[6] + P[2][19]*SF[9] + P[10][19]*SF[15] - P[11][19]*SF[14] - (P[12][19]*q0)/2; + nextP[4][19] = P[4][19] + P[0][19]*SF[5] + P[1][19]*SF[3] - P[3][19]*SF[4] + P[2][19]*SPP[0] + P[13][19]*SPP[3] + P[14][19]*SPP[6] - P[15][19]*SPP[9]; + nextP[5][19] = P[5][19] + P[0][19]*SF[4] + P[2][19]*SF[3] + P[3][19]*SF[5] - P[1][19]*SPP[0] - P[13][19]*SPP[8] + P[14][19]*SPP[2] + P[15][19]*SPP[5]; + nextP[6][19] = P[6][19] + P[1][19]*SF[4] - P[2][19]*SF[5] + P[3][19]*SF[3] + P[0][19]*SPP[0] + P[13][19]*SPP[4] - P[14][19]*SPP[7] - P[15][19]*SPP[1]; + nextP[7][19] = P[7][19] + P[4][19]*dt; + nextP[8][19] = P[8][19] + P[5][19]*dt; + nextP[9][19] = P[9][19] + P[6][19]*dt; nextP[10][19] = P[10][19]; nextP[11][19] = P[11][19]; nextP[12][19] = P[12][19]; @@ -592,17 +514,16 @@ void Ekf::predictCovariance() nextP[17][19] = P[17][19]; nextP[18][19] = P[18][19]; nextP[19][19] = P[19][19]; - - nextP[0][20] = P[0][20] * SPP[5] - P[1][20] * SPP[4] + P[2][20] * SPP[7] + P[9][20] * SPP[22] + P[12][20] * SPP[18]; - nextP[1][20] = P[1][20] * SPP[6] - P[0][20] * SPP[2] - P[2][20] * SPP[8] + P[10][20] * SPP[22] + P[13][20] * SPP[17]; - nextP[2][20] = P[0][20] * SPP[14] - P[1][20] * SPP[3] + P[2][20] * SPP[13] + P[11][20] * SPP[22] + P[14][20] * SPP[16]; - nextP[3][20] = P[3][20] + P[0][20] * SPP[1] + P[1][20] * SPP[19] + P[2][20] * SPP[15] - P[15][20] * SPP[21]; - nextP[4][20] = P[4][20] + P[15][20] * SF[22] + P[0][20] * SPP[20] + P[1][20] * SPP[12] + P[2][20] * SPP[11]; - nextP[5][20] = P[5][20] + P[15][20] * SF[20] - P[0][20] * SPP[9] + P[1][20] * SPP[10] + P[2][20] * SPP[0]; - nextP[6][20] = P[6][20] + P[3][20] * dt; - nextP[7][20] = P[7][20] + P[4][20] * dt; - nextP[8][20] = P[8][20] + P[5][20] * dt; - nextP[9][20] = P[9][20]; + nextP[0][20] = P[0][20] + P[1][20]*SF[9] + P[2][20]*SF[11] + P[3][20]*SF[10] + P[10][20]*SF[14] + P[11][20]*SF[15] + P[12][20]*SPP[10]; + nextP[1][20] = P[1][20] + P[0][20]*SF[8] + P[2][20]*SF[7] + P[3][20]*SF[11] - P[12][20]*SF[15] + P[11][20]*SPP[10] - (P[10][20]*q0)/2; + nextP[2][20] = P[2][20] + P[0][20]*SF[6] + P[1][20]*SF[10] + P[3][20]*SF[8] + P[12][20]*SF[14] - P[10][20]*SPP[10] - (P[11][20]*q0)/2; + nextP[3][20] = P[3][20] + P[0][20]*SF[7] + P[1][20]*SF[6] + P[2][20]*SF[9] + P[10][20]*SF[15] - P[11][20]*SF[14] - (P[12][20]*q0)/2; + nextP[4][20] = P[4][20] + P[0][20]*SF[5] + P[1][20]*SF[3] - P[3][20]*SF[4] + P[2][20]*SPP[0] + P[13][20]*SPP[3] + P[14][20]*SPP[6] - P[15][20]*SPP[9]; + nextP[5][20] = P[5][20] + P[0][20]*SF[4] + P[2][20]*SF[3] + P[3][20]*SF[5] - P[1][20]*SPP[0] - P[13][20]*SPP[8] + P[14][20]*SPP[2] + P[15][20]*SPP[5]; + nextP[6][20] = P[6][20] + P[1][20]*SF[4] - P[2][20]*SF[5] + P[3][20]*SF[3] + P[0][20]*SPP[0] + P[13][20]*SPP[4] - P[14][20]*SPP[7] - P[15][20]*SPP[1]; + nextP[7][20] = P[7][20] + P[4][20]*dt; + nextP[8][20] = P[8][20] + P[5][20]*dt; + nextP[9][20] = P[9][20] + P[6][20]*dt; nextP[10][20] = P[10][20]; nextP[11][20] = P[11][20]; nextP[12][20] = P[12][20]; @@ -614,17 +535,16 @@ void Ekf::predictCovariance() nextP[18][20] = P[18][20]; nextP[19][20] = P[19][20]; nextP[20][20] = P[20][20]; - - nextP[0][21] = P[0][21] * SPP[5] - P[1][21] * SPP[4] + P[2][21] * SPP[7] + P[9][21] * SPP[22] + P[12][21] * SPP[18]; - nextP[1][21] = P[1][21] * SPP[6] - P[0][21] * SPP[2] - P[2][21] * SPP[8] + P[10][21] * SPP[22] + P[13][21] * SPP[17]; - nextP[2][21] = P[0][21] * SPP[14] - P[1][21] * SPP[3] + P[2][21] * SPP[13] + P[11][21] * SPP[22] + P[14][21] * SPP[16]; - nextP[3][21] = P[3][21] + P[0][21] * SPP[1] + P[1][21] * SPP[19] + P[2][21] * SPP[15] - P[15][21] * SPP[21]; - nextP[4][21] = P[4][21] + P[15][21] * SF[22] + P[0][21] * SPP[20] + P[1][21] * SPP[12] + P[2][21] * SPP[11]; - nextP[5][21] = P[5][21] + P[15][21] * SF[20] - P[0][21] * SPP[9] + P[1][21] * SPP[10] + P[2][21] * SPP[0]; - nextP[6][21] = P[6][21] + P[3][21] * dt; - nextP[7][21] = P[7][21] + P[4][21] * dt; - nextP[8][21] = P[8][21] + P[5][21] * dt; - nextP[9][21] = P[9][21]; + nextP[0][21] = P[0][21] + P[1][21]*SF[9] + P[2][21]*SF[11] + P[3][21]*SF[10] + P[10][21]*SF[14] + P[11][21]*SF[15] + P[12][21]*SPP[10]; + nextP[1][21] = P[1][21] + P[0][21]*SF[8] + P[2][21]*SF[7] + P[3][21]*SF[11] - P[12][21]*SF[15] + P[11][21]*SPP[10] - (P[10][21]*q0)/2; + nextP[2][21] = P[2][21] + P[0][21]*SF[6] + P[1][21]*SF[10] + P[3][21]*SF[8] + P[12][21]*SF[14] - P[10][21]*SPP[10] - (P[11][21]*q0)/2; + nextP[3][21] = P[3][21] + P[0][21]*SF[7] + P[1][21]*SF[6] + P[2][21]*SF[9] + P[10][21]*SF[15] - P[11][21]*SF[14] - (P[12][21]*q0)/2; + nextP[4][21] = P[4][21] + P[0][21]*SF[5] + P[1][21]*SF[3] - P[3][21]*SF[4] + P[2][21]*SPP[0] + P[13][21]*SPP[3] + P[14][21]*SPP[6] - P[15][21]*SPP[9]; + nextP[5][21] = P[5][21] + P[0][21]*SF[4] + P[2][21]*SF[3] + P[3][21]*SF[5] - P[1][21]*SPP[0] - P[13][21]*SPP[8] + P[14][21]*SPP[2] + P[15][21]*SPP[5]; + nextP[6][21] = P[6][21] + P[1][21]*SF[4] - P[2][21]*SF[5] + P[3][21]*SF[3] + P[0][21]*SPP[0] + P[13][21]*SPP[4] - P[14][21]*SPP[7] - P[15][21]*SPP[1]; + nextP[7][21] = P[7][21] + P[4][21]*dt; + nextP[8][21] = P[8][21] + P[5][21]*dt; + nextP[9][21] = P[9][21] + P[6][21]*dt; nextP[10][21] = P[10][21]; nextP[11][21] = P[11][21]; nextP[12][21] = P[12][21]; @@ -637,6 +557,12 @@ void Ekf::predictCovariance() nextP[19][21] = P[19][21]; nextP[20][21] = P[20][21]; nextP[21][21] = P[21][21]; + + // add process noise that is not from the IMU + for (unsigned i = 16; i <= 21; i++) { + nextP[i][i] += process_noise[i]; + } + } // Don't do covariance prediction on wind states unless we are using them @@ -686,16 +612,17 @@ void Ekf::predictCovariance() } } - nextP[0][22] = P[0][22] * SPP[5] - P[1][22] * SPP[4] + P[2][22] * SPP[7] + P[9][22] * SPP[22] + P[12][22] * SPP[18]; - nextP[1][22] = P[1][22] * SPP[6] - P[0][22] * SPP[2] - P[2][22] * SPP[8] + P[10][22] * SPP[22] + P[13][22] * SPP[17]; - nextP[2][22] = P[0][22] * SPP[14] - P[1][22] * SPP[3] + P[2][22] * SPP[13] + P[11][22] * SPP[22] + P[14][22] * SPP[16]; - nextP[3][22] = P[3][22] + P[0][22] * SPP[1] + P[1][22] * SPP[19] + P[2][22] * SPP[15] - P[15][22] * SPP[21]; - nextP[4][22] = P[4][22] + P[15][22] * SF[22] + P[0][22] * SPP[20] + P[1][22] * SPP[12] + P[2][22] * SPP[11]; - nextP[5][22] = P[5][22] + P[15][22] * SF[20] - P[0][22] * SPP[9] + P[1][22] * SPP[10] + P[2][22] * SPP[0]; - nextP[6][22] = P[6][22] + P[3][22] * dt; - nextP[7][22] = P[7][22] + P[4][22] * dt; - nextP[8][22] = P[8][22] + P[5][22] * dt; - nextP[9][22] = P[9][22]; + // calculate variances and upper diagonal covariances for wind states + nextP[0][22] = P[0][22] + P[1][22]*SF[9] + P[2][22]*SF[11] + P[3][22]*SF[10] + P[10][22]*SF[14] + P[11][22]*SF[15] + P[12][22]*SPP[10]; + nextP[1][22] = P[1][22] + P[0][22]*SF[8] + P[2][22]*SF[7] + P[3][22]*SF[11] - P[12][22]*SF[15] + P[11][22]*SPP[10] - (P[10][22]*q0)/2; + nextP[2][22] = P[2][22] + P[0][22]*SF[6] + P[1][22]*SF[10] + P[3][22]*SF[8] + P[12][22]*SF[14] - P[10][22]*SPP[10] - (P[11][22]*q0)/2; + nextP[3][22] = P[3][22] + P[0][22]*SF[7] + P[1][22]*SF[6] + P[2][22]*SF[9] + P[10][22]*SF[15] - P[11][22]*SF[14] - (P[12][22]*q0)/2; + nextP[4][22] = P[4][22] + P[0][22]*SF[5] + P[1][22]*SF[3] - P[3][22]*SF[4] + P[2][22]*SPP[0] + P[13][22]*SPP[3] + P[14][22]*SPP[6] - P[15][22]*SPP[9]; + nextP[5][22] = P[5][22] + P[0][22]*SF[4] + P[2][22]*SF[3] + P[3][22]*SF[5] - P[1][22]*SPP[0] - P[13][22]*SPP[8] + P[14][22]*SPP[2] + P[15][22]*SPP[5]; + nextP[6][22] = P[6][22] + P[1][22]*SF[4] - P[2][22]*SF[5] + P[3][22]*SF[3] + P[0][22]*SPP[0] + P[13][22]*SPP[4] - P[14][22]*SPP[7] - P[15][22]*SPP[1]; + nextP[7][22] = P[7][22] + P[4][22]*dt; + nextP[8][22] = P[8][22] + P[5][22]*dt; + nextP[9][22] = P[9][22] + P[6][22]*dt; nextP[10][22] = P[10][22]; nextP[11][22] = P[11][22]; nextP[12][22] = P[12][22]; @@ -709,17 +636,16 @@ void Ekf::predictCovariance() nextP[20][22] = P[20][22]; nextP[21][22] = P[21][22]; nextP[22][22] = P[22][22]; - - nextP[0][23] = P[0][23] * SPP[5] - P[1][23] * SPP[4] + P[2][23] * SPP[7] + P[9][23] * SPP[22] + P[12][23] * SPP[18]; - nextP[1][23] = P[1][23] * SPP[6] - P[0][23] * SPP[2] - P[2][23] * SPP[8] + P[10][23] * SPP[22] + P[13][23] * SPP[17]; - nextP[2][23] = P[0][23] * SPP[14] - P[1][23] * SPP[3] + P[2][23] * SPP[13] + P[11][23] * SPP[22] + P[14][23] * SPP[16]; - nextP[3][23] = P[3][23] + P[0][23] * SPP[1] + P[1][23] * SPP[19] + P[2][23] * SPP[15] - P[15][23] * SPP[21]; - nextP[4][23] = P[4][23] + P[15][23] * SF[22] + P[0][23] * SPP[20] + P[1][23] * SPP[12] + P[2][23] * SPP[11]; - nextP[5][23] = P[5][23] + P[15][23] * SF[20] - P[0][23] * SPP[9] + P[1][23] * SPP[10] + P[2][23] * SPP[0]; - nextP[6][23] = P[6][23] + P[3][23] * dt; - nextP[7][23] = P[7][23] + P[4][23] * dt; - nextP[8][23] = P[8][23] + P[5][23] * dt; - nextP[9][23] = P[9][23]; + nextP[0][23] = P[0][23] + P[1][23]*SF[9] + P[2][23]*SF[11] + P[3][23]*SF[10] + P[10][23]*SF[14] + P[11][23]*SF[15] + P[12][23]*SPP[10]; + nextP[1][23] = P[1][23] + P[0][23]*SF[8] + P[2][23]*SF[7] + P[3][23]*SF[11] - P[12][23]*SF[15] + P[11][23]*SPP[10] - (P[10][23]*q0)/2; + nextP[2][23] = P[2][23] + P[0][23]*SF[6] + P[1][23]*SF[10] + P[3][23]*SF[8] + P[12][23]*SF[14] - P[10][23]*SPP[10] - (P[11][23]*q0)/2; + nextP[3][23] = P[3][23] + P[0][23]*SF[7] + P[1][23]*SF[6] + P[2][23]*SF[9] + P[10][23]*SF[15] - P[11][23]*SF[14] - (P[12][23]*q0)/2; + nextP[4][23] = P[4][23] + P[0][23]*SF[5] + P[1][23]*SF[3] - P[3][23]*SF[4] + P[2][23]*SPP[0] + P[13][23]*SPP[3] + P[14][23]*SPP[6] - P[15][23]*SPP[9]; + nextP[5][23] = P[5][23] + P[0][23]*SF[4] + P[2][23]*SF[3] + P[3][23]*SF[5] - P[1][23]*SPP[0] - P[13][23]*SPP[8] + P[14][23]*SPP[2] + P[15][23]*SPP[5]; + nextP[6][23] = P[6][23] + P[1][23]*SF[4] - P[2][23]*SF[5] + P[3][23]*SF[3] + P[0][23]*SPP[0] + P[13][23]*SPP[4] - P[14][23]*SPP[7] - P[15][23]*SPP[1]; + nextP[7][23] = P[7][23] + P[4][23]*dt; + nextP[8][23] = P[8][23] + P[5][23]*dt; + nextP[9][23] = P[9][23] + P[6][23]*dt; nextP[10][23] = P[10][23]; nextP[11][23] = P[11][23]; nextP[12][23] = P[12][23]; @@ -735,17 +661,17 @@ void Ekf::predictCovariance() nextP[22][23] = P[22][23]; nextP[23][23] = P[23][23]; - } + // add process noise that is not from the IMU + for (unsigned i = 22; i <= 23; i++) { + nextP[i][i] += process_noise[i]; + } - // add process noise - for (unsigned i = 0; i < _k_num_states; i++) { - nextP[i][i] += process_noise[i]; } // stop position covariance growth if our total position variance reaches 100m // this can happen if we lose gps for some time - if ((P[6][6] + P[7][7]) > 1e4f) { - for (uint8_t i = 6; i < 8; i++) { + if ((P[7][7] + P[8][8]) > 1e4f) { + for (uint8_t i = 7; i <= 8; i++) { for (uint8_t j = 0; j < _k_num_states; j++) { nextP[i][j] = P[i][j]; nextP[j][i] = P[j][i]; @@ -756,84 +682,103 @@ void Ekf::predictCovariance() // covariance matrix is symmetrical, so copy upper half to lower half for (unsigned row = 1; row < _k_num_states; row++) { for (unsigned column = 0 ; column < row; column++) { - nextP[row][column] = nextP[column][row]; + P[row][column] = P[column][row] = nextP[column][row]; } } // copy variances (diagonals) for (unsigned i = 0; i < _k_num_states; i++) { - if(!_control_status.flags.wind && i > 21) { - continue; - } P[i][i] = nextP[i][i]; } - // force symmetry - for (unsigned row = 1; row < _k_num_states; row++) { - for (unsigned column = 0; column < row; column++) { - P[row][column] = 0.5f * (nextP[row][column] + nextP[column][row]); - P[column][row] = P[row][column]; - } - } + // fix gross errors in the covariance matrix and ensure rows and + // columns for un-used states are zero + fixCovarianceErrors(); - limitCov(); } -void Ekf::limitCov() +void Ekf::fixCovarianceErrors() { + // NOTE: This limiting is a last resort and should not be relied on + // TODO: Split covariance prediction into separate F*P*transpose(F) and Q contributions + // and set corresponding entries in Q to zero when states exceed 50% of the limit // Covariance diagonal limits. Use same values for states which // belong to the same group (e.g. vel_x, vel_y, vel_z) - float P_lim[9] = {}; - P_lim[0] = 1.0f; // angle error max var - P_lim[1] = 1000.0f; // velocity max var - P_lim[2] = 1000000.0f; // positiion max var - P_lim[3] = 0.001f; // gyro bias max var - P_lim[4] = 0.01f; // gyro scale max var - P_lim[5] = 0.1f; // delta velocity z bias max var - P_lim[6] = 0.1f; // earth mag field max var - P_lim[7] = 0.1f; // body mag field max var - P_lim[8] = 1000.0f; // wind max var + float P_lim[8] = {}; + P_lim[0] = 1.0f; // quaternion max var + P_lim[1] = 1e6f; // velocity max var + P_lim[2] = 1e6f; // positiion max var + P_lim[3] = 1.0f; // gyro bias max var + P_lim[4] = 1.0f; // delta velocity z bias max var + P_lim[5] = 1.0f; // earth mag field max var + P_lim[6] = 1.0f; // body mag field max var + P_lim[7] = 1e6f; // wind max var - for (int i = 0; i < 3; i++) { - - math::constrain(P[i][i], 0.0f, P_lim[0]); + for (int i = 0; i <= 3; i++) { + // quaternion states + P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[0]); } - for (int i = 3; i < 6; i++) { - - math::constrain(P[i][i], 0.0f, P_lim[1]); + for (int i = 4; i <= 6; i++) { + // NED velocity states + P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[1]); } - for (int i = 6; i < 9; i++) { - - - math::constrain(P[i][i], 0.0f, P_lim[2]); + for (int i = 7; i <= 9; i++) { + // NED position states + P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[2]); } - for (int i = 9; i < 12; i++) { - - - math::constrain(P[i][i], 0.0f, P_lim[3]); + for (int i = 10; i <= 12; i++) { + // gyro bias states + P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[3]); } - for (int i = 12; i < 15; i++) { + // force symmetry on the quaternion, velocity and positon state covariances + makeSymmetrical(P,0,12); + // the following states are optional and are deactivaed when not required + // by ensuring the corresponding covariance matrix values are kept at zero - math::constrain(P[i][i], 0.0f, P_lim[4]); + // accelerometer bias states + if ((_params.fusion_mode & MASK_INHIBIT_ACC_BIAS)) { + zeroRows(P,13,15); + zeroCols(P,13,15); + } else { + // constrain variances + for (int i = 13; i <= 15; i++) { + P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[4]); + } + // force symmetry + makeSymmetrical(P,13,15); } - - math::constrain(P[15][15], 0.0f, P_lim[5]); - - for (int i = 16; i < 19; i++) { - math::constrain(P[i][i], 0.0f, P_lim[6]); + // magnetic field states + if (!_control_status.flags.mag_3D) { + zeroRows(P,16,21); + zeroCols(P,16,21); + } else { + // constrain variances + for (int i = 16; i <= 18; i++) { + P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[5]); + } + for (int i = 19; i <= 21; i++) { + P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[6]); + } + // force symmetry + makeSymmetrical(P,16,21); } - for (int i = 19; i < 22; i++) { - math::constrain(P[i][i], 0.0f, P_lim[7]); - } - - for (int i = 22; i < 24; i++) { - math::constrain(P[i][i], 0.0f, P_lim[8]); + // wind velocity states + if (!_control_status.flags.wind) { + zeroRows(P,22,23); + zeroCols(P,22,23); + } else { + // constrain variances + for (int i = 22; i <= 23; i++) { + P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[7]); + } + // force symmetry + makeSymmetrical(P,22,23); } } diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index f6e75002a3..db1356f470 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -73,6 +73,7 @@ Ekf::Ekf(): _time_last_of_fuse(0), _time_last_arsp_fuse(0), _last_disarmed_posD(0.0f), + _last_dt_overrun(0.0f), _airspeed_innov(0.0f), _airspeed_innov_var(0.0f), _heading_innov(0.0f), @@ -138,14 +139,10 @@ Ekf::~Ekf() bool Ekf::init(uint64_t timestamp) { bool ret = initialise_interface(timestamp); - _state.ang_error.setZero(); _state.vel.setZero(); _state.pos.setZero(); _state.gyro_bias.setZero(); - _state.gyro_scale(0) = 1.0f; - _state.gyro_scale(1) = 1.0f; - _state.gyro_scale(2) = 1.0f; - _state.accel_z_bias = 0.0f; + _state.accel_bias.setZero(); _state.mag_I.setZero(); _state.mag_B.setZero(); _state.wind_vel.setZero(); @@ -182,6 +179,8 @@ bool Ekf::init(uint64_t timestamp) _control_status.value = 0; _control_status_prev.value = 0; + _dt_ekf_avg = 0.001f * (float)(FILTER_UPDATE_PERRIOD_MS); + return ret; } @@ -225,9 +224,6 @@ bool Ekf::update() fuseDeclination(); } - } else if (_control_status.flags.mag_2D && _control_status.flags.yaw_align) { - fuseMag2D(); - } else if (_control_status.flags.mag_hdg && _control_status.flags.yaw_align) { // fusion of a Euler yaw angle from either a 321 or 312 rotation sequence fuseHeading(); @@ -267,14 +263,18 @@ bool Ekf::update() } // determine if baro data has fallen behind the fuson time horizon and fuse it in the main filter if enabled + uint64_t last_baro_time_us = _baro_sample_delayed.time_us; if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) { if (_control_status.flags.baro_hgt) { _fuse_height = true; } else { // calculate a filtered offset between the baro origin and local NED origin if we are not using the baro as a height reference - float offset_error = _state.pos(2) + _baro_sample_delayed.hgt - _hgt_sensor_offset - _baro_hgt_offset; - _baro_hgt_offset += 0.02f * math::constrain(offset_error, -5.0f, 5.0f); + float local_time_step = 1e-6f*(float)(_baro_sample_delayed.time_us - last_baro_time_us); + local_time_step = math::constrain(local_time_step,0.0f,1.0f); + last_baro_time_us = _baro_sample_delayed.time_us; + float offset_rate_correction = 0.1f * (_baro_sample_delayed.hgt - _hgt_sensor_offset) + _state.pos(2) - _baro_hgt_offset; + _baro_hgt_offset += local_time_step * math::constrain(offset_rate_correction, -0.1f, 0.1f); } } @@ -446,12 +446,10 @@ bool Ekf::initialiseFilter(void) _gps_alt_ref = 0.0f; // Zero all of the states - _state.ang_error.setZero(); _state.vel.setZero(); _state.pos.setZero(); _state.gyro_bias.setZero(); - _state.gyro_scale(0) = _state.gyro_scale(1) = _state.gyro_scale(2) = 1.0f; - _state.accel_z_bias = 0.0f; + _state.accel_bias.setZero(); _state.mag_I.setZero(); _state.mag_B.setZero(); _state.wind_vel.setZero(); @@ -477,9 +475,6 @@ bool Ekf::initialiseFilter(void) // update transformation matrix from body to world frame _R_to_earth = quat_to_invrotmat(_state.quat_nominal); - // initialise the filtered alignment error estimate to a larger starting value - _tilt_err_length_filt = 1.0f; - // calculate the averaged magnetometer reading Vector3f mag_init = _mag_filt_state; @@ -520,18 +515,12 @@ void Ekf::predictState() } } - // correct delta angles for bias offsets and scale factors - Vector3f corrected_delta_ang; - corrected_delta_ang(0) = _imu_sample_delayed.delta_ang(0) * _state.gyro_scale(0) - _state.gyro_bias(0); - corrected_delta_ang(1) = _imu_sample_delayed.delta_ang(1) * _state.gyro_scale(1) - _state.gyro_bias(1); - corrected_delta_ang(2) = _imu_sample_delayed.delta_ang(2) * _state.gyro_scale(2) - _state.gyro_bias(2); - - // correct delta velocity for bias offsets - Vector3f corrected_delta_vel = _imu_sample_delayed.delta_vel; - corrected_delta_vel(2) -= _state.accel_z_bias; + // apply imu bias corrections + Vector3f corrected_delta_ang = _imu_sample_delayed.delta_ang - _state.gyro_bias; + Vector3f corrected_delta_vel = _imu_sample_delayed.delta_vel - _state.accel_bias; // correct delta angles for earth rotation rate - corrected_delta_ang -= _R_to_earth.transpose() * _earth_rate_NED * _imu_sample_delayed.delta_ang_dt; + corrected_delta_ang -= -_R_to_earth.transpose() * _earth_rate_NED * _imu_sample_delayed.delta_ang_dt; // convert the delta angle to a delta quaternion Quaternion dq; @@ -560,30 +549,52 @@ void Ekf::predictState() _R_to_earth = quat_to_invrotmat(_state.quat_nominal); constrainStates(); + + // calculate an average filter update time + float input = 0.5f*(_imu_sample_delayed.delta_vel_dt + _imu_sample_delayed.delta_ang_dt); + + // filter and limit input between -50% and +100% of nominal value + input = math::constrain(input,0.0005f * (float)(FILTER_UPDATE_PERRIOD_MS),0.002f * (float)(FILTER_UPDATE_PERRIOD_MS)); + _dt_ekf_avg = 0.99f*_dt_ekf_avg + 0.005f*(_imu_sample_delayed.delta_vel_dt + _imu_sample_delayed.delta_ang_dt); } bool Ekf::collect_imu(imuSample &imu) { + // accumulate and downsample IMU data across a period FILTER_UPDATE_PERRIOD_MS long + + // copy imu data to local variables _imu_sample_new.delta_ang = imu.delta_ang; _imu_sample_new.delta_vel = imu.delta_vel; _imu_sample_new.delta_ang_dt = imu.delta_ang_dt; _imu_sample_new.delta_vel_dt = imu.delta_vel_dt; - _imu_sample_new.time_us = imu.time_us; + _imu_sample_new.time_us = imu.time_us; + // accumulate the time deltas _imu_down_sampled.delta_ang_dt += imu.delta_ang_dt; _imu_down_sampled.delta_vel_dt += imu.delta_vel_dt; + // use a quaternion to accumulate delta angle data + // this quaternion represents the rotation from the start to end of the accumulation period Quaternion delta_q; delta_q.rotate(imu.delta_ang); _q_down_sampled = _q_down_sampled * delta_q; _q_down_sampled.normalize(); + // rotate the accumulated delta velocity data forward each time so it is always in the updated rotation frame matrix::Dcm delta_R(delta_q.inversed()); _imu_down_sampled.delta_vel = delta_R * _imu_down_sampled.delta_vel; - _imu_down_sampled.delta_vel += imu.delta_vel; - if ((_dt_imu_avg * _imu_ticks >= (float)(FILTER_UPDATE_PERRIOD_MS) / 1000) || - _dt_imu_avg * _imu_ticks >= 0.02f) { + // accumulate the most recent delta velocity data at the updated rotation frame + // assume effective sample time is halfway between the previous and current rotation frame + _imu_down_sampled.delta_vel += (_imu_sample_new.delta_vel + delta_R * _imu_sample_new.delta_vel) * 0.5f; + + // if the target time delta between filter prediction steps has been exceeded + // write the accumulated IMU data to the ring buffer + float target_dt = (float)(FILTER_UPDATE_PERRIOD_MS) / 1000; + if (_imu_down_sampled.delta_ang_dt >= target_dt - _last_dt_overrun) { + + // store the amount we have over-run the target update rate by + _last_dt_overrun = _imu_down_sampled.delta_ang_dt - target_dt; imu.delta_ang = _q_down_sampled.to_axis_angle(); imu.delta_vel = _imu_down_sampled.delta_vel; @@ -620,13 +631,13 @@ void Ekf::calculateOutputStates() // correct delta angles for bias offsets and scale factors Vector3f delta_angle; - delta_angle(0) = _imu_sample_new.delta_ang(0) * _state.gyro_scale(0) - _state.gyro_bias(0); - delta_angle(1) = _imu_sample_new.delta_ang(1) * _state.gyro_scale(1) - _state.gyro_bias(1); - delta_angle(2) = _imu_sample_new.delta_ang(2) * _state.gyro_scale(2) - _state.gyro_bias(2); + float dt_scale_correction = _dt_imu_avg/_dt_ekf_avg; + delta_angle(0) = _imu_sample_new.delta_ang(0) - _state.gyro_bias(0)*dt_scale_correction; + delta_angle(1) = _imu_sample_new.delta_ang(1) - _state.gyro_bias(1)*dt_scale_correction; + delta_angle(2) = _imu_sample_new.delta_ang(2) - _state.gyro_bias(2)*dt_scale_correction; // correct delta velocity for bias offsets - Vector3f delta_vel = _imu_sample_new.delta_vel; - delta_vel(2) -= _state.accel_z_bias; + Vector3f delta_vel = _imu_sample_new.delta_vel - _state.accel_bias*dt_scale_correction; // Apply corrections to the delta angle required to track the quaternion states at the EKF fusion time horizon delta_angle += _delta_angle_corr; diff --git a/EKF/ekf.h b/EKF/ekf.h index 754786161e..c22df787ad 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -121,7 +121,8 @@ public: // return the estimated terrain vertical position relative to the NED origin bool get_terrain_vert_pos(float *ret); - void get_accel_bias(float *bias) {*bias = _state.accel_z_bias;} + // get the accerometer bias in m/s/s + void get_accel_bias(float bias[3]); // get GPS check status void get_gps_check_status(uint16_t *_gps_check_fail_status); @@ -135,6 +136,8 @@ private: const float _k_earth_rate = 0.000072921f; const float _gravity_mss = 9.80665f; + float _dt_ekf_avg; // average update rate of the ekf + stateSample _state; // state struct of the ekf running at the delayed time horizon bool _filter_initialised; // true when the EKF sttes and covariances been initialised @@ -156,6 +159,7 @@ private: uint64_t _time_last_arsp_fuse; // time the last fusion of airspeed measurements were performed (usec) Vector2f _last_known_posNE; // last known local NE position vector (m) float _last_disarmed_posD; // vertical position recorded at arming (m) + float _last_dt_overrun; // the amount of time the last IMU collection over-ran the target set by FILTER_UPDATE_PERRIOD_MS (sec) Vector3f _earth_rate_NED; // earth rotation vector (NED) in rad/s @@ -177,9 +181,6 @@ private: float _heading_innov; // heading measurement innovation float _heading_innov_var; // heading measurement innovation variance - Vector3f _tilt_err_vec; // Vector of the most recent attitude error correction from velocity and position fusion - float _tilt_err_length_filt; // filtered length of _tilt_err_vec - // optical flow processing float _flow_innov[2]; // flow measurement innovation float _flow_innov_var[2]; // flow innovation variance @@ -265,9 +266,6 @@ private: // fuse the first euler angle from either a 321 or 312 rotation sequence as the observation (currently measures yaw using the magnetometer) void fuseHeading(); - // fuse projecton of magnetometer onto horizontal plane - void fuseMag2D(); - // fuse magnetometer declination measurement void fuseDeclination(); @@ -309,13 +307,14 @@ private: // reset height state of the ekf void resetHeight(); - void makeCovSymetrical(); + // modify output filter to match the the EKF state at the fusion time horizon + void alignOutputFilter(); // limit the diagonal of the covariance matrix - void limitCov(); + void fixCovarianceErrors(); - // make ekf covariance matrix symmetric - void makeSymmetrical(); + // make ekf covariance matrix symmetric between a nominated state indexe range + void makeSymmetrical(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last); // constrain the ekf states void constrainStates(); @@ -348,4 +347,7 @@ private: // zero the specified range of columns in the state covariance matrix void zeroCols(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last); + // calculate the measurement variance for the optical flow sensor + float calcOptFlowMeasVar(); + }; diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 12b10ab952..d1d6d85c63 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -108,11 +108,11 @@ void Ekf::resetHeight() _state.pos(2) = new_pos_down; // reset the associated covariance values - zeroRows(P, 8, 8); - zeroCols(P, 8, 8); + zeroRows(P, 9, 9); + zeroCols(P, 9, 9); // the state variance is the same as the observation - P[8][8] = sq(_params.range_noise); + P[9][9] = sq(_params.range_noise); vert_pos_reset = true; @@ -132,11 +132,11 @@ void Ekf::resetHeight() _state.pos(2) = _hgt_sensor_offset - baro_newest.hgt + _baro_hgt_offset; // reset the associated covariance values - zeroRows(P, 8, 8); - zeroCols(P, 8, 8); + zeroRows(P, 9, 9); + zeroCols(P, 9, 9); - // the state variance is th esame as the observation - P[8][8] = sq(_params.baro_noise); + // the state variance is the same as the observation + P[9][9] = sq(_params.baro_noise); vert_pos_reset = true; @@ -150,11 +150,11 @@ void Ekf::resetHeight() _state.pos(2) = _hgt_sensor_offset - gps_newest.hgt + _gps_alt_ref; // reset the associated covarince values - zeroRows(P, 8, 8); - zeroCols(P, 8, 8); + zeroRows(P, 9, 9); + zeroCols(P, 9, 9); // the state variance is the same as the observation - P[8][8] = sq(gps_newest.hacc); + P[9][9] = sq(gps_newest.hacc); vert_pos_reset = true; @@ -168,8 +168,8 @@ void Ekf::resetHeight() } // reset the vertical velocity covariance values - zeroRows(P, 5, 5); - zeroCols(P, 5, 5); + zeroRows(P, 6, 6); + zeroCols(P, 6, 6); // reset the vertical velocity state if (_control_status.flags.gps && (_time_last_imu - gps_newest.time_us < 2 * GPS_MAX_INTERVAL)) { @@ -177,7 +177,7 @@ void Ekf::resetHeight() _state.vel(2) = gps_newest.vel(2); // the state variance is the same as the observation - P[5][5] = sq(1.5f * gps_newest.sacc); + P[6][6] = sq(1.5f * gps_newest.sacc); } else { // we don't know what the vertical velocity is, so set it to zero @@ -185,7 +185,7 @@ void Ekf::resetHeight() // Set the variance to a value large enough to allow the state to converge quickly // that does not destabilise the filter - P[5][5] = fminf(sq(_state.vel(2)),100.0f); + P[6][6] = 10.0f; } vert_vel_reset = true; @@ -225,6 +225,31 @@ void Ekf::resetHeight() } +// align output filter states to match EKF states at the fusion time horizon +void Ekf::alignOutputFilter() +{ + // calculate the quaternion delta between the output and EKF quaternions at the EKF fusion time horizon + Quaternion quat_inv = _state.quat_nominal.inversed(); + Quaternion q_delta = _output_sample_delayed.quat_nominal * quat_inv; + q_delta.normalize(); + + // calculate the velocity and posiiton deltas between the output and EKF at the EKF fusion time horizon + Vector3f vel_delta = _state.vel - _output_sample_delayed.vel; + Vector3f pos_delta = _state.pos - _output_sample_delayed.pos; + + // loop through the output filter state history and add the deltas + outputSample output_states; + unsigned output_length = _output_buffer.get_length(); + for (unsigned i=0; i < output_length; i++) { + output_states = _output_buffer.get_from_index(i); + output_states.quat_nominal *= q_delta; + output_states.quat_nominal.normalize(); + output_states.vel += vel_delta; + output_states.pos += pos_delta; + _output_buffer.push_to_index(i,output_states); + } +} + // Reset heading and magnetic field states bool Ekf::resetMagHeading(Vector3f &mag_init) { @@ -248,11 +273,11 @@ bool Ekf::resetMagHeading(Vector3f &mag_init) // we don't change the output attitude to avoid jumps _state.quat_nominal = Quaternion(euler_init); - // reset the angle error variances because the yaw angle could have changed by a significant amount + // reset the quaternion variances because the yaw angle could have changed by a significant amount // by setting them to zero we avoid 'kicks' in angle when 3-D fusion starts and the imu process noise // will grow them again. - zeroRows(P, 0, 2); - zeroCols(P, 0, 2); + zeroRows(P, 0, 3); + zeroCols(P, 0, 3); // calculate initial earth magnetic field states matrix::Dcm R_to_earth(euler_init); @@ -292,21 +317,21 @@ void Ekf::calcMagDeclination() } // This function forces the covariance matrix to be symmetric -void Ekf::makeSymmetrical() +void Ekf::makeSymmetrical(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last) { - for (unsigned row = 0; row < _k_num_states; row++) { + for (unsigned row = first; row <= last; row++) { for (unsigned column = 0; column < row; column++) { - float tmp = (P[row][column] + P[column][row]) / 2; - P[row][column] = tmp; - P[column][row] = tmp; + float tmp = (cov_mat[row][column] + cov_mat[column][row]) / 2; + cov_mat[row][column] = tmp; + cov_mat[column][row] = tmp; } } } void Ekf::constrainStates() { - for (int i = 0; i < 3; i++) { - _state.ang_error(i) = math::constrain(_state.ang_error(i), -1.0f, 1.0f); + for (int i = 0; i < 4; i++) { + _state.quat_nominal(i) = math::constrain(_state.quat_nominal(i), -1.0f, 1.0f); } for (int i = 0; i < 3; i++) { @@ -322,11 +347,9 @@ void Ekf::constrainStates() } for (int i = 0; i < 3; i++) { - _state.gyro_scale(i) = math::constrain(_state.gyro_scale(i), 0.95f, 1.05f); + _state.accel_bias(i) = math::constrain(_state.accel_bias(i), -1.0f * _dt_imu_avg, 1.0f * _dt_imu_avg); } - _state.accel_z_bias = math::constrain(_state.accel_z_bias, -1.0f * _dt_imu_avg, 1.0f * _dt_imu_avg); - for (int i = 0; i < 3; i++) { _state.mag_I(i) = math::constrain(_state.mag_I(i), -1.0f, 1.0f); } @@ -407,28 +430,26 @@ void Ekf::get_gps_check_status(uint16_t *val) // get the state vector at the delayed time horizon void Ekf::get_state_delayed(float *state) { - for (int i = 0; i < 3; i++) { - state[i] = _state.ang_error(i); + for (int i = 0; i < 4; i++) { + state[i] = _state.quat_nominal(i); } for (int i = 0; i < 3; i++) { - state[i + 3] = _state.vel(i); + state[i + 4] = _state.vel(i); } for (int i = 0; i < 3; i++) { - state[i + 6] = _state.pos(i); + state[i + 7] = _state.pos(i); } for (int i = 0; i < 3; i++) { - state[i + 9] = _state.gyro_bias(i); + state[i + 10] = _state.gyro_bias(i); } for (int i = 0; i < 3; i++) { - state[i + 12] = _state.gyro_scale(i); + state[i + 13] = _state.accel_bias(i); } - state[15] = _state.accel_z_bias; - for (int i = 0; i < 3; i++) { state[i + 16] = _state.mag_I(i); } @@ -442,6 +463,16 @@ void Ekf::get_state_delayed(float *state) } } +// get the accelerometer bias +void Ekf::get_accel_bias(float bias[3]) +{ + float temp[3]; + temp[0] = _state.accel_bias(0) /_dt_ekf_avg; + temp[1] = _state.accel_bias(1) /_dt_ekf_avg; + temp[2] = _state.accel_bias(2) /_dt_ekf_avg; + memcpy(bias, temp, 3 * sizeof(float)); +} + // get the diagonal elements of the covariance matrix void Ekf::get_covariances(float *covariances) { @@ -463,8 +494,8 @@ void Ekf::get_ekf_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_reckoning) { // report absolute accuracy taking into account the uncertainty in location of the origin // TODO we a need a way to allow for baro drift error - float temp1 = sqrtf(P[6][6] + P[7][7] + sq(_gps_origin_eph)); - float temp2 = sqrtf(P[8][8] + sq(_gps_origin_epv)); + float temp1 = sqrtf(P[7][7] + P[8][8] + sq(_gps_origin_eph)); + float temp2 = sqrtf(P[9][9] + sq(_gps_origin_epv)); memcpy(ekf_eph, &temp1, sizeof(float)); memcpy(ekf_epv, &temp2, sizeof(float)); @@ -476,28 +507,27 @@ void Ekf::get_ekf_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_reckoning) // fuse measurement void Ekf::fuse(float *K, float innovation) { + for (unsigned i = 0; i < 4; i++) { + _state.quat_nominal(i) = _state.quat_nominal(i) - K[i] * innovation; + } + _state.quat_nominal.normalize(); + for (unsigned i = 0; i < 3; i++) { - _state.ang_error(i) = _state.ang_error(i) - K[i] * innovation; + _state.vel(i) = _state.vel(i) - K[i + 4] * innovation; } for (unsigned i = 0; i < 3; i++) { - _state.vel(i) = _state.vel(i) - K[i + 3] * innovation; + _state.pos(i) = _state.pos(i) - K[i + 7] * innovation; } for (unsigned i = 0; i < 3; i++) { - _state.pos(i) = _state.pos(i) - K[i + 6] * innovation; + _state.gyro_bias(i) = _state.gyro_bias(i) - K[i + 10] * innovation; } for (unsigned i = 0; i < 3; i++) { - _state.gyro_bias(i) = _state.gyro_bias(i) - K[i + 9] * innovation; + _state.accel_bias(i) = _state.accel_bias(i) - K[i + 13] * innovation; } - for (unsigned i = 0; i < 3; i++) { - _state.gyro_scale(i) = _state.gyro_scale(i) - K[i + 12] * innovation; - } - - _state.accel_z_bias -= K[15] * innovation; - for (unsigned i = 0; i < 3; i++) { _state.mag_I(i) = _state.mag_I(i) - K[i + 16] * innovation; } diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index a4dfcee8c8..162cfaba21 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -346,7 +346,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) _time_last_airspeed = 0; _time_last_optflow = 0; - memset(&_fault_status, 0, sizeof(_fault_status)); + memset(&_fault_status.flags, 0, sizeof(_fault_status.flags)); return true; } diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index a917fa8456..5806d17f87 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -209,7 +209,7 @@ public: *val = _mag_declination_to_save_deg; } - virtual void get_accel_bias(float *bias) = 0; + virtual void get_accel_bias(float bias[3]) = 0; // get EKF mode status void get_control_mode(uint16_t *val) @@ -217,6 +217,12 @@ public: *val = _control_status.value; } + // get EKF internal fault status + void get_filter_fault_status(uint16_t *val) + { + *val = _fault_status.value; + } + // get GPS check status virtual void get_gps_check_status(uint16_t *val) = 0; @@ -286,7 +292,7 @@ protected: uint64_t _time_last_airspeed; // timestamp of last airspeed measurement in microseconds uint64_t _time_last_optflow; - fault_status_t _fault_status; + fault_status_u _fault_status; // allocate data buffers and intialise interface variables bool initialise_interface(uint64_t timestamp); diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 22625289bd..88c133deae 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -55,20 +55,20 @@ void Ekf::fuseMag() float magD = _state.mag_I(2); // XYZ Measurement uncertainty. Need to consider timing errors for fast rotations - float R_MAG = fmaxf(_params.mag_noise, 1.0e-3f); + float R_MAG = fmaxf(_params.mag_noise, 0.0f); R_MAG = R_MAG * R_MAG; // intermediate variables from algebraic optimisation float SH_MAG[9]; - SH_MAG[0] = sq(q0) - sq(q1) + sq(q2) - sq(q3); - SH_MAG[1] = sq(q0) + sq(q1) - sq(q2) - sq(q3); - SH_MAG[2] = sq(q0) - sq(q1) - sq(q2) + sq(q3); - SH_MAG[3] = 2 * q0 * q1 + 2 * q2 * q3; - SH_MAG[4] = 2 * q0 * q3 + 2 * q1 * q2; - SH_MAG[5] = 2 * q0 * q2 + 2 * q1 * q3; - SH_MAG[6] = magE * (2 * q0 * q1 - 2 * q2 * q3); - SH_MAG[7] = 2 * q1 * q3 - 2 * q0 * q2; - SH_MAG[8] = 2 * q0 * q3; + SH_MAG[0] = 2*magD*q3 + 2*magE*q2 + 2*magN*q1; + SH_MAG[1] = 2*magD*q0 - 2*magE*q1 + 2*magN*q2; + SH_MAG[2] = 2*magD*q1 + 2*magE*q0 - 2*magN*q3; + SH_MAG[3] = sq(q3); + SH_MAG[4] = sq(q2); + SH_MAG[5] = sq(q1); + SH_MAG[6] = sq(q0); + SH_MAG[7] = 2*magN*q0; + SH_MAG[8] = 2*magE*q3; // rotate magnetometer earth field state into body frame matrix::Dcm R_to_body(_state.quat_nominal); @@ -88,41 +88,28 @@ void Ekf::fuseMag() // Calculate observation Jacobians and kalman gains for each magentoemter axis // X Axis - H_MAG[0][1] = SH_MAG[6] - magD * SH_MAG[2] - magN * SH_MAG[5]; - H_MAG[0][2] = magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2); - H_MAG[0][16] = SH_MAG[1]; - H_MAG[0][17] = SH_MAG[4]; - H_MAG[0][18] = SH_MAG[7]; - H_MAG[0][19] = 1; + H_MAG[0][0] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; + H_MAG[0][1] = SH_MAG[0]; + H_MAG[0][2] = -SH_MAG[1]; + H_MAG[0][3] = SH_MAG[2]; + H_MAG[0][16] = SH_MAG[5] - SH_MAG[4] - SH_MAG[3] + SH_MAG[6]; + H_MAG[0][17] = 2*q0*q3 + 2*q1*q2; + H_MAG[0][18] = 2*q1*q3 - 2*q0*q2; + H_MAG[0][19] = 1.0f; // intermediate variables - float SK_MX[4] = {}; + float SK_MX[5] = {}; // innovation variance - _mag_innov_var[0] = (P[19][19] + R_MAG - P[1][19] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[16][19] * - SH_MAG[1] - + P[17][19] * SH_MAG[4] + P[18][19] * SH_MAG[7] + P[2][19] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * - (SH_MAG[8] - 2 * q1 * q2)) - (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) * (P[19][1] - P[1][1] * - (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[16][1] * SH_MAG[1] + P[17][1] * SH_MAG[4] + P[18][1] * SH_MAG[7] + - P[2][1] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2))) + SH_MAG[1] * - (P[19][16] - P[1][16] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[16][16] * SH_MAG[1] + P[17][16] * - SH_MAG[4] + P[18][16] * SH_MAG[7] + P[2][16] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * - (SH_MAG[8] - 2 * q1 * q2))) + SH_MAG[4] * (P[19][17] - P[1][17] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + - P[16][17] * SH_MAG[1] + P[17][17] * SH_MAG[4] + P[18][17] * SH_MAG[7] + P[2][17] * (magE * SH_MAG[0] + magD * SH_MAG[3] - - magN * (SH_MAG[8] - 2 * q1 * q2))) + SH_MAG[7] * (P[19][18] - P[1][18] * (magD * SH_MAG[2] - SH_MAG[6] + magN * - SH_MAG[5]) + P[16][18] * SH_MAG[1] + P[17][18] * SH_MAG[4] + P[18][18] * SH_MAG[7] + P[2][18] * - (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2))) + (magE * SH_MAG[0] + magD * SH_MAG[3] - - magN * (SH_MAG[8] - 2 * q1 * q2)) * (P[19][2] - P[1][2] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[16][2] * - SH_MAG[1] + P[17][2] * SH_MAG[4] + P[18][2] * SH_MAG[7] + P[2][2] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * - (SH_MAG[8] - 2 * q1 * q2)))); + _mag_innov_var[0] = (P[19][19] + R_MAG + P[1][19]*SH_MAG[0] - P[2][19]*SH_MAG[1] + P[3][19]*SH_MAG[2] - P[16][19]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + (2*q0*q3 + 2*q1*q2)*(P[19][17] + P[1][17]*SH_MAG[0] - P[2][17]*SH_MAG[1] + P[3][17]*SH_MAG[2] - P[16][17]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][17]*(2*q0*q3 + 2*q1*q2) - P[18][17]*(2*q0*q2 - 2*q1*q3) + P[0][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q2 - 2*q1*q3)*(P[19][18] + P[1][18]*SH_MAG[0] - P[2][18]*SH_MAG[1] + P[3][18]*SH_MAG[2] - P[16][18]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][18]*(2*q0*q3 + 2*q1*q2) - P[18][18]*(2*q0*q2 - 2*q1*q3) + P[0][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[19][0] + P[1][0]*SH_MAG[0] - P[2][0]*SH_MAG[1] + P[3][0]*SH_MAG[2] - P[16][0]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][0]*(2*q0*q3 + 2*q1*q2) - P[18][0]*(2*q0*q2 - 2*q1*q3) + P[0][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[17][19]*(2*q0*q3 + 2*q1*q2) - P[18][19]*(2*q0*q2 - 2*q1*q3) + SH_MAG[0]*(P[19][1] + P[1][1]*SH_MAG[0] - P[2][1]*SH_MAG[1] + P[3][1]*SH_MAG[2] - P[16][1]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][1]*(2*q0*q3 + 2*q1*q2) - P[18][1]*(2*q0*q2 - 2*q1*q3) + P[0][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - SH_MAG[1]*(P[19][2] + P[1][2]*SH_MAG[0] - P[2][2]*SH_MAG[1] + P[3][2]*SH_MAG[2] - P[16][2]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][2]*(2*q0*q3 + 2*q1*q2) - P[18][2]*(2*q0*q2 - 2*q1*q3) + P[0][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[2]*(P[19][3] + P[1][3]*SH_MAG[0] - P[2][3]*SH_MAG[1] + P[3][3]*SH_MAG[2] - P[16][3]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][3]*(2*q0*q3 + 2*q1*q2) - P[18][3]*(2*q0*q2 - 2*q1*q3) + P[0][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P[19][16] + P[1][16]*SH_MAG[0] - P[2][16]*SH_MAG[1] + P[3][16]*SH_MAG[2] - P[16][16]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][16]*(2*q0*q3 + 2*q1*q2) - P[18][16]*(2*q0*q2 - 2*q1*q3) + P[0][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[0][19]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); // check for a badly conditioned covariance matrix if (_mag_innov_var[0] >= R_MAG) { // the innovation variance contribution from the state covariances is non-negative - no fault - _fault_status.bad_mag_x = false; + _fault_status.flags.bad_mag_x = false; } else { // the innovation variance contribution from the state covariances is negtive which means the covariance matrix is badly conditioned - _fault_status.bad_mag_x = true; + _fault_status.flags.bad_mag_x = true; // we need to reinitialise the covariance matrix and abort this fusion step initialiseCovariance(); return; @@ -131,40 +118,27 @@ void Ekf::fuseMag() // Y axis - H_MAG[1][0] = magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]; - H_MAG[1][2] = - magE * SH_MAG[4] - magD * SH_MAG[7] - magN * SH_MAG[1]; - H_MAG[1][16] = 2 * q1 * q2 - SH_MAG[8]; - H_MAG[1][17] = SH_MAG[0]; - H_MAG[1][18] = SH_MAG[3]; - H_MAG[1][20] = 1; + H_MAG[1][0] = SH_MAG[2]; + H_MAG[1][1] = SH_MAG[1]; + H_MAG[1][2] = SH_MAG[0]; + H_MAG[1][3] = 2*magD*q2 - SH_MAG[8] - SH_MAG[7]; + H_MAG[1][16] = 2*q1*q2 - 2*q0*q3; + H_MAG[1][17] = SH_MAG[4] - SH_MAG[3] - SH_MAG[5] + SH_MAG[6]; + H_MAG[1][18] = 2*q0*q1 + 2*q2*q3; + H_MAG[1][20] = 1.0f; // intermediate variables - note SK_MY[0] is 1/(innovation variance) - float SK_MY[4]; - _mag_innov_var[1] = (P[20][20] + R_MAG + P[0][20] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[17][20] * - SH_MAG[0] - + P[18][20] * SH_MAG[3] - (SH_MAG[8] - 2 * q1 * q2) * (P[20][16] + P[0][16] * (magD * SH_MAG[2] - SH_MAG[6] + magN * - SH_MAG[5]) + P[17][16] * SH_MAG[0] + P[18][16] * SH_MAG[3] - P[2][16] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * - SH_MAG[1]) - P[16][16] * (SH_MAG[8] - 2 * q1 * q2)) - P[2][20] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * - SH_MAG[1]) + (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) * (P[20][0] + P[0][0] * - (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[17][0] * SH_MAG[0] + P[18][0] * SH_MAG[3] - P[2][0] * - (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[16][0] * (SH_MAG[8] - 2 * q1 * q2)) + SH_MAG[0] * - (P[20][17] + P[0][17] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[17][17] * SH_MAG[0] + P[18][17] * - SH_MAG[3] - P[2][17] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[16][17] * - (SH_MAG[8] - 2 * q1 * q2)) + SH_MAG[3] * (P[20][18] + P[0][18] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + - P[17][18] * SH_MAG[0] + P[18][18] * SH_MAG[3] - P[2][18] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - - P[16][18] * (SH_MAG[8] - 2 * q1 * q2)) - P[16][20] * (SH_MAG[8] - 2 * q1 * q2) - (magE * SH_MAG[4] + magD * SH_MAG[7] + - magN * SH_MAG[1]) * (P[20][2] + P[0][2] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[17][2] * SH_MAG[0] + - P[18][2] * SH_MAG[3] - P[2][2] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[16][2] * - (SH_MAG[8] - 2 * q1 * q2))); + float SK_MY[5]; + _mag_innov_var[1] = (P[20][20] + R_MAG + P[0][20]*SH_MAG[2] + P[1][20]*SH_MAG[1] + P[2][20]*SH_MAG[0] - P[17][20]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - (2*q0*q3 - 2*q1*q2)*(P[20][16] + P[0][16]*SH_MAG[2] + P[1][16]*SH_MAG[1] + P[2][16]*SH_MAG[0] - P[17][16]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][16]*(2*q0*q3 - 2*q1*q2) + P[18][16]*(2*q0*q1 + 2*q2*q3) - P[3][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (2*q0*q1 + 2*q2*q3)*(P[20][18] + P[0][18]*SH_MAG[2] + P[1][18]*SH_MAG[1] + P[2][18]*SH_MAG[0] - P[17][18]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][18]*(2*q0*q3 - 2*q1*q2) + P[18][18]*(2*q0*q1 + 2*q2*q3) - P[3][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[20][3] + P[0][3]*SH_MAG[2] + P[1][3]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[17][3]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][3]*(2*q0*q3 - 2*q1*q2) + P[18][3]*(2*q0*q1 + 2*q2*q3) - P[3][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[16][20]*(2*q0*q3 - 2*q1*q2) + P[18][20]*(2*q0*q1 + 2*q2*q3) + SH_MAG[2]*(P[20][0] + P[0][0]*SH_MAG[2] + P[1][0]*SH_MAG[1] + P[2][0]*SH_MAG[0] - P[17][0]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][0]*(2*q0*q3 - 2*q1*q2) + P[18][0]*(2*q0*q1 + 2*q2*q3) - P[3][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[20][1] + P[0][1]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[2][1]*SH_MAG[0] - P[17][1]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][1]*(2*q0*q3 - 2*q1*q2) + P[18][1]*(2*q0*q1 + 2*q2*q3) - P[3][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[20][2] + P[0][2]*SH_MAG[2] + P[1][2]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[17][2]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][2]*(2*q0*q3 - 2*q1*q2) + P[18][2]*(2*q0*q1 + 2*q2*q3) - P[3][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6])*(P[20][17] + P[0][17]*SH_MAG[2] + P[1][17]*SH_MAG[1] + P[2][17]*SH_MAG[0] - P[17][17]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][17]*(2*q0*q3 - 2*q1*q2) + P[18][17]*(2*q0*q1 + 2*q2*q3) - P[3][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[3][20]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); // check for a badly conditioned covariance matrix if (_mag_innov_var[1] >= R_MAG) { // the innovation variance contribution from the state covariances is non-negative - no fault - _fault_status.bad_mag_y = false; + _fault_status.flags.bad_mag_y = false; } else { // the innovation variance contribution from the state covariances is negtive which means the covariance matrix is badly conditioned - _fault_status.bad_mag_y = true; + _fault_status.flags.bad_mag_y = true; // we need to reinitialise the covariance matrix and abort this fusion step initialiseCovariance(); return; @@ -173,41 +147,27 @@ void Ekf::fuseMag() // Z axis - H_MAG[2][0] = magN * (SH_MAG[8] - 2 * q1 * q2) - magD * SH_MAG[3] - magE * SH_MAG[0]; - H_MAG[2][1] = magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]; - H_MAG[2][16] = SH_MAG[5]; - H_MAG[2][17] = 2 * q2 * q3 - 2 * q0 * q1; - H_MAG[2][18] = SH_MAG[2]; - H_MAG[2][21] = 1; + H_MAG[2][0] = SH_MAG[1]; + H_MAG[2][1] = -SH_MAG[2]; + H_MAG[2][2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; + H_MAG[2][3] = SH_MAG[0]; + H_MAG[2][16] = 2*q0*q2 + 2*q1*q3; + H_MAG[2][17] = 2*q2*q3 - 2*q0*q1; + H_MAG[2][18] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]; + H_MAG[2][21] = 1.0f; // intermediate variables - float SK_MZ[4]; - _mag_innov_var[2] = (P[21][21] + R_MAG + P[16][21] * SH_MAG[5] + P[18][21] * SH_MAG[2] - (2 * q0 * q1 - 2 * q2 * q3) * - (P[21][17] + P[16][17] * SH_MAG[5] + P[18][17] * SH_MAG[2] - P[0][17] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * - (SH_MAG[8] - 2 * q1 * q2)) + P[1][17] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[17][17] * - (2 * q0 * q1 - 2 * q2 * q3)) - P[0][21] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * - (SH_MAG[8] - 2 * q1 * q2)) + P[1][21] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) + SH_MAG[5] * - (P[21][16] + P[16][16] * SH_MAG[5] + P[18][16] * SH_MAG[2] - P[0][16] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * - (SH_MAG[8] - 2 * q1 * q2)) + P[1][16] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[17][16] * - (2 * q0 * q1 - 2 * q2 * q3)) + SH_MAG[2] * (P[21][18] + P[16][18] * SH_MAG[5] + P[18][18] * SH_MAG[2] - P[0][18] * - (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2)) + P[1][18] * - (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[17][18] * (2 * q0 * q1 - 2 * q2 * q3)) - - (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2)) * (P[21][0] + P[16][0] * SH_MAG[5] + P[18][0] * - SH_MAG[2] - P[0][0] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2)) + P[1][0] * - (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[17][0] * (2 * q0 * q1 - 2 * q2 * q3)) - P[17][21] * - (2 * q0 * q1 - 2 * q2 * q3) + (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) * - (P[21][1] + P[16][1] * SH_MAG[5] + P[18][1] * SH_MAG[2] - P[0][1] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * - (SH_MAG[8] - 2 * q1 * q2)) + P[1][1] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[17][1] * - (2 * q0 * q1 - 2 * q2 * q3))); + float SK_MZ[5]; + _mag_innov_var[2] = (P[21][21] + R_MAG + P[0][21]*SH_MAG[1] - P[1][21]*SH_MAG[2] + P[3][21]*SH_MAG[0] + P[18][21]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + (2*q0*q2 + 2*q1*q3)*(P[21][16] + P[0][16]*SH_MAG[1] - P[1][16]*SH_MAG[2] + P[3][16]*SH_MAG[0] + P[18][16]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][16]*(2*q0*q2 + 2*q1*q3) - P[17][16]*(2*q0*q1 - 2*q2*q3) + P[2][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q1 - 2*q2*q3)*(P[21][17] + P[0][17]*SH_MAG[1] - P[1][17]*SH_MAG[2] + P[3][17]*SH_MAG[0] + P[18][17]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][17]*(2*q0*q2 + 2*q1*q3) - P[17][17]*(2*q0*q1 - 2*q2*q3) + P[2][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[21][2] + P[0][2]*SH_MAG[1] - P[1][2]*SH_MAG[2] + P[3][2]*SH_MAG[0] + P[18][2]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][2]*(2*q0*q2 + 2*q1*q3) - P[17][2]*(2*q0*q1 - 2*q2*q3) + P[2][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[16][21]*(2*q0*q2 + 2*q1*q3) - P[17][21]*(2*q0*q1 - 2*q2*q3) + SH_MAG[1]*(P[21][0] + P[0][0]*SH_MAG[1] - P[1][0]*SH_MAG[2] + P[3][0]*SH_MAG[0] + P[18][0]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][0]*(2*q0*q2 + 2*q1*q3) - P[17][0]*(2*q0*q1 - 2*q2*q3) + P[2][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - SH_MAG[2]*(P[21][1] + P[0][1]*SH_MAG[1] - P[1][1]*SH_MAG[2] + P[3][1]*SH_MAG[0] + P[18][1]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][1]*(2*q0*q2 + 2*q1*q3) - P[17][1]*(2*q0*q1 - 2*q2*q3) + P[2][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[21][3] + P[0][3]*SH_MAG[1] - P[1][3]*SH_MAG[2] + P[3][3]*SH_MAG[0] + P[18][3]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][3]*(2*q0*q2 + 2*q1*q3) - P[17][3]*(2*q0*q1 - 2*q2*q3) + P[2][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6])*(P[21][18] + P[0][18]*SH_MAG[1] - P[1][18]*SH_MAG[2] + P[3][18]*SH_MAG[0] + P[18][18]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][18]*(2*q0*q2 + 2*q1*q3) - P[17][18]*(2*q0*q1 - 2*q2*q3) + P[2][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[2][21]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); // check for a badly conditioned covariance matrix if (_mag_innov_var[2] >= R_MAG) { // the innovation variance contribution from the state covariances is non-negative - no fault - _fault_status.bad_mag_z = false; + _fault_status.flags.bad_mag_z = false; } else { // the innovation variance contribution from the state covariances is negtive which means the covariance matrix is badly conditioned - _fault_status.bad_mag_z = true; + _fault_status.flags.bad_mag_z = true; // we need to reinitialise the covariance matrix and abort this fusion step initialiseCovariance(); return; @@ -236,215 +196,111 @@ void Ekf::fuseMag() if (index == 0) { // Calculate X axis Kalman gains SK_MX[0] = 1.0f / _mag_innov_var[0]; - SK_MX[1] = magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2); - SK_MX[2] = magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]; - SK_MX[3] = SH_MAG[7]; + SK_MX[1] = SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]; + SK_MX[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; + SK_MX[3] = 2*q0*q2 - 2*q1*q3; + SK_MX[4] = 2*q0*q3 + 2*q1*q2; - Kfusion[0] = SK_MX[0] * (P[0][19] + P[0][16] * SH_MAG[1] + P[0][17] * SH_MAG[4] - P[0][1] * SK_MX[2] + P[0][2] * - SK_MX[1] + P[0][18] * SK_MX[3]); - Kfusion[1] = SK_MX[0] * (P[1][19] + P[1][16] * SH_MAG[1] + P[1][17] * SH_MAG[4] - P[1][1] * SK_MX[2] + P[1][2] * - SK_MX[1] + P[1][18] * SK_MX[3]); - Kfusion[2] = SK_MX[0] * (P[2][19] + P[2][16] * SH_MAG[1] + P[2][17] * SH_MAG[4] - P[2][1] * SK_MX[2] + P[2][2] * - SK_MX[1] + P[2][18] * SK_MX[3]); - Kfusion[3] = SK_MX[0] * (P[3][19] + P[3][16] * SH_MAG[1] + P[3][17] * SH_MAG[4] - P[3][1] * SK_MX[2] + P[3][2] * - SK_MX[1] + P[3][18] * SK_MX[3]); - Kfusion[4] = SK_MX[0] * (P[4][19] + P[4][16] * SH_MAG[1] + P[4][17] * SH_MAG[4] - P[4][1] * SK_MX[2] + P[4][2] * - SK_MX[1] + P[4][18] * SK_MX[3]); - Kfusion[5] = SK_MX[0] * (P[5][19] + P[5][16] * SH_MAG[1] + P[5][17] * SH_MAG[4] - P[5][1] * SK_MX[2] + P[5][2] * - SK_MX[1] + P[5][18] * SK_MX[3]); - Kfusion[6] = SK_MX[0] * (P[6][19] + P[6][16] * SH_MAG[1] + P[6][17] * SH_MAG[4] - P[6][1] * SK_MX[2] + P[6][2] * - SK_MX[1] + P[6][18] * SK_MX[3]); - Kfusion[7] = SK_MX[0] * (P[7][19] + P[7][16] * SH_MAG[1] + P[7][17] * SH_MAG[4] - P[7][1] * SK_MX[2] + P[7][2] * - SK_MX[1] + P[7][18] * SK_MX[3]); - Kfusion[8] = SK_MX[0] * (P[8][19] + P[8][16] * SH_MAG[1] + P[8][17] * SH_MAG[4] - P[8][1] * SK_MX[2] + P[8][2] * - SK_MX[1] + P[8][18] * SK_MX[3]); - Kfusion[9] = SK_MX[0] * (P[9][19] + P[9][16] * SH_MAG[1] + P[9][17] * SH_MAG[4] - P[9][1] * SK_MX[2] + P[9][2] * - SK_MX[1] + P[9][18] * SK_MX[3]); - Kfusion[10] = SK_MX[0] * (P[10][19] + P[10][16] * SH_MAG[1] + P[10][17] * SH_MAG[4] - P[10][1] * SK_MX[2] + P[10][2] * - SK_MX[1] + P[10][18] * SK_MX[3]); - Kfusion[11] = SK_MX[0] * (P[11][19] + P[11][16] * SH_MAG[1] + P[11][17] * SH_MAG[4] - P[11][1] * SK_MX[2] + P[11][2] * - SK_MX[1] + P[11][18] * SK_MX[3]); - Kfusion[12] = SK_MX[0] * (P[12][19] + P[12][16] * SH_MAG[1] + P[12][17] * SH_MAG[4] - P[12][1] * SK_MX[2] + P[12][2] * - SK_MX[1] + P[12][18] * SK_MX[3]); - Kfusion[13] = SK_MX[0] * (P[13][19] + P[13][16] * SH_MAG[1] + P[13][17] * SH_MAG[4] - P[13][1] * SK_MX[2] + P[13][2] * - SK_MX[1] + P[13][18] * SK_MX[3]); - Kfusion[14] = SK_MX[0] * (P[14][19] + P[14][16] * SH_MAG[1] + P[14][17] * SH_MAG[4] - P[14][1] * SK_MX[2] + P[14][2] * - SK_MX[1] + P[14][18] * SK_MX[3]); - Kfusion[15] = SK_MX[0] * (P[15][19] + P[15][16] * SH_MAG[1] + P[15][17] * SH_MAG[4] - P[15][1] * SK_MX[2] + P[15][2] * - SK_MX[1] + P[15][18] * SK_MX[3]); - Kfusion[16] = SK_MX[0] * (P[16][19] + P[16][16] * SH_MAG[1] + P[16][17] * SH_MAG[4] - P[16][1] * SK_MX[2] + P[16][2] * - SK_MX[1] + P[16][18] * SK_MX[3]); - Kfusion[17] = SK_MX[0] * (P[17][19] + P[17][16] * SH_MAG[1] + P[17][17] * SH_MAG[4] - P[17][1] * SK_MX[2] + P[17][2] * - SK_MX[1] + P[17][18] * SK_MX[3]); - Kfusion[18] = SK_MX[0] * (P[18][19] + P[18][16] * SH_MAG[1] + P[18][17] * SH_MAG[4] - P[18][1] * SK_MX[2] + P[18][2] * - SK_MX[1] + P[18][18] * SK_MX[3]); - Kfusion[19] = SK_MX[0] * (P[19][19] + P[19][16] * SH_MAG[1] + P[19][17] * SH_MAG[4] - P[19][1] * SK_MX[2] + P[19][2] * - SK_MX[1] + P[19][18] * SK_MX[3]); - Kfusion[20] = SK_MX[0] * (P[20][19] + P[20][16] * SH_MAG[1] + P[20][17] * SH_MAG[4] - P[20][1] * SK_MX[2] + P[20][2] * - SK_MX[1] + P[20][18] * SK_MX[3]); - Kfusion[21] = SK_MX[0] * (P[21][19] + P[21][16] * SH_MAG[1] + P[21][17] * SH_MAG[4] - P[21][1] * SK_MX[2] + P[21][2] * - SK_MX[1] + P[21][18] * SK_MX[3]); - - // Don't update wind states unless we are doing wind estimation - if (_control_status.flags.wind) { - Kfusion[22] = SK_MX[0] * (P[22][19] + P[22][16] * SH_MAG[1] + P[22][17] * SH_MAG[4] - P[22][1] * SK_MX[2] + P[22][2] * - SK_MX[1] + P[22][18] * SK_MX[3]); - Kfusion[23] = SK_MX[0] * (P[23][19] + P[23][16] * SH_MAG[1] + P[23][17] * SH_MAG[4] - P[23][1] * SK_MX[2] + P[23][2] * - SK_MX[1] + P[23][18] * SK_MX[3]); - - } else { - Kfusion[22] = 0.0f; - Kfusion[23] = 0.0f; - } + Kfusion[0] = SK_MX[0]*(P[0][19] + P[0][1]*SH_MAG[0] - P[0][2]*SH_MAG[1] + P[0][3]*SH_MAG[2] + P[0][0]*SK_MX[2] - P[0][16]*SK_MX[1] + P[0][17]*SK_MX[4] - P[0][18]*SK_MX[3]); + Kfusion[1] = SK_MX[0]*(P[1][19] + P[1][1]*SH_MAG[0] - P[1][2]*SH_MAG[1] + P[1][3]*SH_MAG[2] + P[1][0]*SK_MX[2] - P[1][16]*SK_MX[1] + P[1][17]*SK_MX[4] - P[1][18]*SK_MX[3]); + Kfusion[2] = SK_MX[0]*(P[2][19] + P[2][1]*SH_MAG[0] - P[2][2]*SH_MAG[1] + P[2][3]*SH_MAG[2] + P[2][0]*SK_MX[2] - P[2][16]*SK_MX[1] + P[2][17]*SK_MX[4] - P[2][18]*SK_MX[3]); + Kfusion[3] = SK_MX[0]*(P[3][19] + P[3][1]*SH_MAG[0] - P[3][2]*SH_MAG[1] + P[3][3]*SH_MAG[2] + P[3][0]*SK_MX[2] - P[3][16]*SK_MX[1] + P[3][17]*SK_MX[4] - P[3][18]*SK_MX[3]); + Kfusion[4] = SK_MX[0]*(P[4][19] + P[4][1]*SH_MAG[0] - P[4][2]*SH_MAG[1] + P[4][3]*SH_MAG[2] + P[4][0]*SK_MX[2] - P[4][16]*SK_MX[1] + P[4][17]*SK_MX[4] - P[4][18]*SK_MX[3]); + Kfusion[5] = SK_MX[0]*(P[5][19] + P[5][1]*SH_MAG[0] - P[5][2]*SH_MAG[1] + P[5][3]*SH_MAG[2] + P[5][0]*SK_MX[2] - P[5][16]*SK_MX[1] + P[5][17]*SK_MX[4] - P[5][18]*SK_MX[3]); + Kfusion[6] = SK_MX[0]*(P[6][19] + P[6][1]*SH_MAG[0] - P[6][2]*SH_MAG[1] + P[6][3]*SH_MAG[2] + P[6][0]*SK_MX[2] - P[6][16]*SK_MX[1] + P[6][17]*SK_MX[4] - P[6][18]*SK_MX[3]); + Kfusion[7] = SK_MX[0]*(P[7][19] + P[7][1]*SH_MAG[0] - P[7][2]*SH_MAG[1] + P[7][3]*SH_MAG[2] + P[7][0]*SK_MX[2] - P[7][16]*SK_MX[1] + P[7][17]*SK_MX[4] - P[7][18]*SK_MX[3]); + Kfusion[8] = SK_MX[0]*(P[8][19] + P[8][1]*SH_MAG[0] - P[8][2]*SH_MAG[1] + P[8][3]*SH_MAG[2] + P[8][0]*SK_MX[2] - P[8][16]*SK_MX[1] + P[8][17]*SK_MX[4] - P[8][18]*SK_MX[3]); + Kfusion[9] = SK_MX[0]*(P[9][19] + P[9][1]*SH_MAG[0] - P[9][2]*SH_MAG[1] + P[9][3]*SH_MAG[2] + P[9][0]*SK_MX[2] - P[9][16]*SK_MX[1] + P[9][17]*SK_MX[4] - P[9][18]*SK_MX[3]); + Kfusion[10] = SK_MX[0]*(P[10][19] + P[10][1]*SH_MAG[0] - P[10][2]*SH_MAG[1] + P[10][3]*SH_MAG[2] + P[10][0]*SK_MX[2] - P[10][16]*SK_MX[1] + P[10][17]*SK_MX[4] - P[10][18]*SK_MX[3]); + Kfusion[11] = SK_MX[0]*(P[11][19] + P[11][1]*SH_MAG[0] - P[11][2]*SH_MAG[1] + P[11][3]*SH_MAG[2] + P[11][0]*SK_MX[2] - P[11][16]*SK_MX[1] + P[11][17]*SK_MX[4] - P[11][18]*SK_MX[3]); + Kfusion[12] = SK_MX[0]*(P[12][19] + P[12][1]*SH_MAG[0] - P[12][2]*SH_MAG[1] + P[12][3]*SH_MAG[2] + P[12][0]*SK_MX[2] - P[12][16]*SK_MX[1] + P[12][17]*SK_MX[4] - P[12][18]*SK_MX[3]); + Kfusion[13] = SK_MX[0]*(P[13][19] + P[13][1]*SH_MAG[0] - P[13][2]*SH_MAG[1] + P[13][3]*SH_MAG[2] + P[13][0]*SK_MX[2] - P[13][16]*SK_MX[1] + P[13][17]*SK_MX[4] - P[13][18]*SK_MX[3]); + Kfusion[14] = SK_MX[0]*(P[14][19] + P[14][1]*SH_MAG[0] - P[14][2]*SH_MAG[1] + P[14][3]*SH_MAG[2] + P[14][0]*SK_MX[2] - P[14][16]*SK_MX[1] + P[14][17]*SK_MX[4] - P[14][18]*SK_MX[3]); + Kfusion[15] = SK_MX[0]*(P[15][19] + P[15][1]*SH_MAG[0] - P[15][2]*SH_MAG[1] + P[15][3]*SH_MAG[2] + P[15][0]*SK_MX[2] - P[15][16]*SK_MX[1] + P[15][17]*SK_MX[4] - P[15][18]*SK_MX[3]); + Kfusion[16] = SK_MX[0]*(P[16][19] + P[16][1]*SH_MAG[0] - P[16][2]*SH_MAG[1] + P[16][3]*SH_MAG[2] + P[16][0]*SK_MX[2] - P[16][16]*SK_MX[1] + P[16][17]*SK_MX[4] - P[16][18]*SK_MX[3]); + Kfusion[17] = SK_MX[0]*(P[17][19] + P[17][1]*SH_MAG[0] - P[17][2]*SH_MAG[1] + P[17][3]*SH_MAG[2] + P[17][0]*SK_MX[2] - P[17][16]*SK_MX[1] + P[17][17]*SK_MX[4] - P[17][18]*SK_MX[3]); + Kfusion[18] = SK_MX[0]*(P[18][19] + P[18][1]*SH_MAG[0] - P[18][2]*SH_MAG[1] + P[18][3]*SH_MAG[2] + P[18][0]*SK_MX[2] - P[18][16]*SK_MX[1] + P[18][17]*SK_MX[4] - P[18][18]*SK_MX[3]); + Kfusion[19] = SK_MX[0]*(P[19][19] + P[19][1]*SH_MAG[0] - P[19][2]*SH_MAG[1] + P[19][3]*SH_MAG[2] + P[19][0]*SK_MX[2] - P[19][16]*SK_MX[1] + P[19][17]*SK_MX[4] - P[19][18]*SK_MX[3]); + Kfusion[20] = SK_MX[0]*(P[20][19] + P[20][1]*SH_MAG[0] - P[20][2]*SH_MAG[1] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[2] - P[20][16]*SK_MX[1] + P[20][17]*SK_MX[4] - P[20][18]*SK_MX[3]); + Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][1]*SH_MAG[0] - P[21][2]*SH_MAG[1] + P[21][3]*SH_MAG[2] + P[21][0]*SK_MX[2] - P[21][16]*SK_MX[1] + P[21][17]*SK_MX[4] - P[21][18]*SK_MX[3]); + Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] - P[22][2]*SH_MAG[1] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[4] - P[22][18]*SK_MX[3]); + Kfusion[23] = SK_MX[0]*(P[23][19] + P[23][1]*SH_MAG[0] - P[23][2]*SH_MAG[1] + P[23][3]*SH_MAG[2] + P[23][0]*SK_MX[2] - P[23][16]*SK_MX[1] + P[23][17]*SK_MX[4] - P[23][18]*SK_MX[3]); } else if (index == 1) { // Calculate Y axis Kalman gains SK_MY[0] = 1.0f / _mag_innov_var[1]; - SK_MY[1] = magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]; - SK_MY[2] = magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]; - SK_MY[3] = SH_MAG[8] - 2 * q1 * q2; + SK_MY[1] = SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]; + SK_MY[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; + SK_MY[3] = 2*q0*q3 - 2*q1*q2; + SK_MY[4] = 2*q0*q1 + 2*q2*q3; - Kfusion[0] = SK_MY[0] * (P[0][20] + P[0][17] * SH_MAG[0] + P[0][18] * SH_MAG[3] + P[0][0] * SK_MY[2] - P[0][2] * - SK_MY[1] - P[0][16] * SK_MY[3]); - Kfusion[1] = SK_MY[0] * (P[1][20] + P[1][17] * SH_MAG[0] + P[1][18] * SH_MAG[3] + P[1][0] * SK_MY[2] - P[1][2] * - SK_MY[1] - P[1][16] * SK_MY[3]); - Kfusion[2] = SK_MY[0] * (P[2][20] + P[2][17] * SH_MAG[0] + P[2][18] * SH_MAG[3] + P[2][0] * SK_MY[2] - P[2][2] * - SK_MY[1] - P[2][16] * SK_MY[3]); - Kfusion[3] = SK_MY[0] * (P[3][20] + P[3][17] * SH_MAG[0] + P[3][18] * SH_MAG[3] + P[3][0] * SK_MY[2] - P[3][2] * - SK_MY[1] - P[3][16] * SK_MY[3]); - Kfusion[4] = SK_MY[0] * (P[4][20] + P[4][17] * SH_MAG[0] + P[4][18] * SH_MAG[3] + P[4][0] * SK_MY[2] - P[4][2] * - SK_MY[1] - P[4][16] * SK_MY[3]); - Kfusion[5] = SK_MY[0] * (P[5][20] + P[5][17] * SH_MAG[0] + P[5][18] * SH_MAG[3] + P[5][0] * SK_MY[2] - P[5][2] * - SK_MY[1] - P[5][16] * SK_MY[3]); - Kfusion[6] = SK_MY[0] * (P[6][20] + P[6][17] * SH_MAG[0] + P[6][18] * SH_MAG[3] + P[6][0] * SK_MY[2] - P[6][2] * - SK_MY[1] - P[6][16] * SK_MY[3]); - Kfusion[7] = SK_MY[0] * (P[7][20] + P[7][17] * SH_MAG[0] + P[7][18] * SH_MAG[3] + P[7][0] * SK_MY[2] - P[7][2] * - SK_MY[1] - P[7][16] * SK_MY[3]); - Kfusion[8] = SK_MY[0] * (P[8][20] + P[8][17] * SH_MAG[0] + P[8][18] * SH_MAG[3] + P[8][0] * SK_MY[2] - P[8][2] * - SK_MY[1] - P[8][16] * SK_MY[3]); - Kfusion[9] = SK_MY[0] * (P[9][20] + P[9][17] * SH_MAG[0] + P[9][18] * SH_MAG[3] + P[9][0] * SK_MY[2] - P[9][2] * - SK_MY[1] - P[9][16] * SK_MY[3]); - Kfusion[10] = SK_MY[0] * (P[10][20] + P[10][17] * SH_MAG[0] + P[10][18] * SH_MAG[3] + P[10][0] * SK_MY[2] - P[10][2] * - SK_MY[1] - P[10][16] * SK_MY[3]); - Kfusion[11] = SK_MY[0] * (P[11][20] + P[11][17] * SH_MAG[0] + P[11][18] * SH_MAG[3] + P[11][0] * SK_MY[2] - P[11][2] * - SK_MY[1] - P[11][16] * SK_MY[3]); - Kfusion[12] = SK_MY[0] * (P[12][20] + P[12][17] * SH_MAG[0] + P[12][18] * SH_MAG[3] + P[12][0] * SK_MY[2] - P[12][2] * - SK_MY[1] - P[12][16] * SK_MY[3]); - Kfusion[13] = SK_MY[0] * (P[13][20] + P[13][17] * SH_MAG[0] + P[13][18] * SH_MAG[3] + P[13][0] * SK_MY[2] - P[13][2] * - SK_MY[1] - P[13][16] * SK_MY[3]); - Kfusion[14] = SK_MY[0] * (P[14][20] + P[14][17] * SH_MAG[0] + P[14][18] * SH_MAG[3] + P[14][0] * SK_MY[2] - P[14][2] * - SK_MY[1] - P[14][16] * SK_MY[3]); - Kfusion[15] = SK_MY[0] * (P[15][20] + P[15][17] * SH_MAG[0] + P[15][18] * SH_MAG[3] + P[15][0] * SK_MY[2] - P[15][2] * - SK_MY[1] - P[15][16] * SK_MY[3]); - Kfusion[16] = SK_MY[0] * (P[16][20] + P[16][17] * SH_MAG[0] + P[16][18] * SH_MAG[3] + P[16][0] * SK_MY[2] - P[16][2] * - SK_MY[1] - P[16][16] * SK_MY[3]); - Kfusion[17] = SK_MY[0] * (P[17][20] + P[17][17] * SH_MAG[0] + P[17][18] * SH_MAG[3] + P[17][0] * SK_MY[2] - P[17][2] * - SK_MY[1] - P[17][16] * SK_MY[3]); - Kfusion[18] = SK_MY[0] * (P[18][20] + P[18][17] * SH_MAG[0] + P[18][18] * SH_MAG[3] + P[18][0] * SK_MY[2] - P[18][2] * - SK_MY[1] - P[18][16] * SK_MY[3]); - Kfusion[19] = SK_MY[0] * (P[19][20] + P[19][17] * SH_MAG[0] + P[19][18] * SH_MAG[3] + P[19][0] * SK_MY[2] - P[19][2] * - SK_MY[1] - P[19][16] * SK_MY[3]); - Kfusion[20] = SK_MY[0] * (P[20][20] + P[20][17] * SH_MAG[0] + P[20][18] * SH_MAG[3] + P[20][0] * SK_MY[2] - P[20][2] * - SK_MY[1] - P[20][16] * SK_MY[3]); - Kfusion[21] = SK_MY[0] * (P[21][20] + P[21][17] * SH_MAG[0] + P[21][18] * SH_MAG[3] + P[21][0] * SK_MY[2] - P[21][2] * - SK_MY[1] - P[21][16] * SK_MY[3]); - - // Don't update wind states unless we are doing wind estimation - if (_control_status.flags.wind) { - Kfusion[22] = SK_MY[0] * (P[22][20] + P[22][17] * SH_MAG[0] + P[22][18] * SH_MAG[3] + P[22][0] * SK_MY[2] - P[22][2] * - SK_MY[1] - P[22][16] * SK_MY[3]); - Kfusion[23] = SK_MY[0] * (P[23][20] + P[23][17] * SH_MAG[0] + P[23][18] * SH_MAG[3] + P[23][0] * SK_MY[2] - P[23][2] * - SK_MY[1] - P[23][16] * SK_MY[3]); - - } else { - Kfusion[22] = 0.0f; - Kfusion[23] = 0.0f; - } + Kfusion[0] = SK_MY[0]*(P[0][20] + P[0][0]*SH_MAG[2] + P[0][1]*SH_MAG[1] + P[0][2]*SH_MAG[0] - P[0][3]*SK_MY[2] - P[0][17]*SK_MY[1] - P[0][16]*SK_MY[3] + P[0][18]*SK_MY[4]); + Kfusion[1] = SK_MY[0]*(P[1][20] + P[1][0]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[1][2]*SH_MAG[0] - P[1][3]*SK_MY[2] - P[1][17]*SK_MY[1] - P[1][16]*SK_MY[3] + P[1][18]*SK_MY[4]); + Kfusion[2] = SK_MY[0]*(P[2][20] + P[2][0]*SH_MAG[2] + P[2][1]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[2][3]*SK_MY[2] - P[2][17]*SK_MY[1] - P[2][16]*SK_MY[3] + P[2][18]*SK_MY[4]); + Kfusion[3] = SK_MY[0]*(P[3][20] + P[3][0]*SH_MAG[2] + P[3][1]*SH_MAG[1] + P[3][2]*SH_MAG[0] - P[3][3]*SK_MY[2] - P[3][17]*SK_MY[1] - P[3][16]*SK_MY[3] + P[3][18]*SK_MY[4]); + Kfusion[4] = SK_MY[0]*(P[4][20] + P[4][0]*SH_MAG[2] + P[4][1]*SH_MAG[1] + P[4][2]*SH_MAG[0] - P[4][3]*SK_MY[2] - P[4][17]*SK_MY[1] - P[4][16]*SK_MY[3] + P[4][18]*SK_MY[4]); + Kfusion[5] = SK_MY[0]*(P[5][20] + P[5][0]*SH_MAG[2] + P[5][1]*SH_MAG[1] + P[5][2]*SH_MAG[0] - P[5][3]*SK_MY[2] - P[5][17]*SK_MY[1] - P[5][16]*SK_MY[3] + P[5][18]*SK_MY[4]); + Kfusion[6] = SK_MY[0]*(P[6][20] + P[6][0]*SH_MAG[2] + P[6][1]*SH_MAG[1] + P[6][2]*SH_MAG[0] - P[6][3]*SK_MY[2] - P[6][17]*SK_MY[1] - P[6][16]*SK_MY[3] + P[6][18]*SK_MY[4]); + Kfusion[7] = SK_MY[0]*(P[7][20] + P[7][0]*SH_MAG[2] + P[7][1]*SH_MAG[1] + P[7][2]*SH_MAG[0] - P[7][3]*SK_MY[2] - P[7][17]*SK_MY[1] - P[7][16]*SK_MY[3] + P[7][18]*SK_MY[4]); + Kfusion[8] = SK_MY[0]*(P[8][20] + P[8][0]*SH_MAG[2] + P[8][1]*SH_MAG[1] + P[8][2]*SH_MAG[0] - P[8][3]*SK_MY[2] - P[8][17]*SK_MY[1] - P[8][16]*SK_MY[3] + P[8][18]*SK_MY[4]); + Kfusion[9] = SK_MY[0]*(P[9][20] + P[9][0]*SH_MAG[2] + P[9][1]*SH_MAG[1] + P[9][2]*SH_MAG[0] - P[9][3]*SK_MY[2] - P[9][17]*SK_MY[1] - P[9][16]*SK_MY[3] + P[9][18]*SK_MY[4]); + Kfusion[10] = SK_MY[0]*(P[10][20] + P[10][0]*SH_MAG[2] + P[10][1]*SH_MAG[1] + P[10][2]*SH_MAG[0] - P[10][3]*SK_MY[2] - P[10][17]*SK_MY[1] - P[10][16]*SK_MY[3] + P[10][18]*SK_MY[4]); + Kfusion[11] = SK_MY[0]*(P[11][20] + P[11][0]*SH_MAG[2] + P[11][1]*SH_MAG[1] + P[11][2]*SH_MAG[0] - P[11][3]*SK_MY[2] - P[11][17]*SK_MY[1] - P[11][16]*SK_MY[3] + P[11][18]*SK_MY[4]); + Kfusion[12] = SK_MY[0]*(P[12][20] + P[12][0]*SH_MAG[2] + P[12][1]*SH_MAG[1] + P[12][2]*SH_MAG[0] - P[12][3]*SK_MY[2] - P[12][17]*SK_MY[1] - P[12][16]*SK_MY[3] + P[12][18]*SK_MY[4]); + Kfusion[13] = SK_MY[0]*(P[13][20] + P[13][0]*SH_MAG[2] + P[13][1]*SH_MAG[1] + P[13][2]*SH_MAG[0] - P[13][3]*SK_MY[2] - P[13][17]*SK_MY[1] - P[13][16]*SK_MY[3] + P[13][18]*SK_MY[4]); + Kfusion[14] = SK_MY[0]*(P[14][20] + P[14][0]*SH_MAG[2] + P[14][1]*SH_MAG[1] + P[14][2]*SH_MAG[0] - P[14][3]*SK_MY[2] - P[14][17]*SK_MY[1] - P[14][16]*SK_MY[3] + P[14][18]*SK_MY[4]); + Kfusion[15] = SK_MY[0]*(P[15][20] + P[15][0]*SH_MAG[2] + P[15][1]*SH_MAG[1] + P[15][2]*SH_MAG[0] - P[15][3]*SK_MY[2] - P[15][17]*SK_MY[1] - P[15][16]*SK_MY[3] + P[15][18]*SK_MY[4]); + Kfusion[16] = SK_MY[0]*(P[16][20] + P[16][0]*SH_MAG[2] + P[16][1]*SH_MAG[1] + P[16][2]*SH_MAG[0] - P[16][3]*SK_MY[2] - P[16][17]*SK_MY[1] - P[16][16]*SK_MY[3] + P[16][18]*SK_MY[4]); + Kfusion[17] = SK_MY[0]*(P[17][20] + P[17][0]*SH_MAG[2] + P[17][1]*SH_MAG[1] + P[17][2]*SH_MAG[0] - P[17][3]*SK_MY[2] - P[17][17]*SK_MY[1] - P[17][16]*SK_MY[3] + P[17][18]*SK_MY[4]); + Kfusion[18] = SK_MY[0]*(P[18][20] + P[18][0]*SH_MAG[2] + P[18][1]*SH_MAG[1] + P[18][2]*SH_MAG[0] - P[18][3]*SK_MY[2] - P[18][17]*SK_MY[1] - P[18][16]*SK_MY[3] + P[18][18]*SK_MY[4]); + Kfusion[19] = SK_MY[0]*(P[19][20] + P[19][0]*SH_MAG[2] + P[19][1]*SH_MAG[1] + P[19][2]*SH_MAG[0] - P[19][3]*SK_MY[2] - P[19][17]*SK_MY[1] - P[19][16]*SK_MY[3] + P[19][18]*SK_MY[4]); + Kfusion[20] = SK_MY[0]*(P[20][20] + P[20][0]*SH_MAG[2] + P[20][1]*SH_MAG[1] + P[20][2]*SH_MAG[0] - P[20][3]*SK_MY[2] - P[20][17]*SK_MY[1] - P[20][16]*SK_MY[3] + P[20][18]*SK_MY[4]); + Kfusion[21] = SK_MY[0]*(P[21][20] + P[21][0]*SH_MAG[2] + P[21][1]*SH_MAG[1] + P[21][2]*SH_MAG[0] - P[21][3]*SK_MY[2] - P[21][17]*SK_MY[1] - P[21][16]*SK_MY[3] + P[21][18]*SK_MY[4]); + Kfusion[22] = SK_MY[0]*(P[22][20] + P[22][0]*SH_MAG[2] + P[22][1]*SH_MAG[1] + P[22][2]*SH_MAG[0] - P[22][3]*SK_MY[2] - P[22][17]*SK_MY[1] - P[22][16]*SK_MY[3] + P[22][18]*SK_MY[4]); + Kfusion[23] = SK_MY[0]*(P[23][20] + P[23][0]*SH_MAG[2] + P[23][1]*SH_MAG[1] + P[23][2]*SH_MAG[0] - P[23][3]*SK_MY[2] - P[23][17]*SK_MY[1] - P[23][16]*SK_MY[3] + P[23][18]*SK_MY[4]); } else if (index == 2) { // Calculate Z axis Kalman gains SK_MZ[0] = 1.0f / _mag_innov_var[2]; - SK_MZ[1] = magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2); - SK_MZ[2] = magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]; - SK_MZ[3] = 2 * q0 * q1 - 2 * q2 * q3; + SK_MZ[1] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]; + SK_MZ[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; + SK_MZ[3] = 2*q0*q1 - 2*q2*q3; + SK_MZ[4] = 2*q0*q2 + 2*q1*q3; - Kfusion[0] = SK_MZ[0] * (P[0][21] + P[0][18] * SH_MAG[2] + P[0][16] * SH_MAG[5] - P[0][0] * SK_MZ[1] + P[0][1] * - SK_MZ[2] - P[0][17] * SK_MZ[3]); - Kfusion[1] = SK_MZ[0] * (P[1][21] + P[1][18] * SH_MAG[2] + P[1][16] * SH_MAG[5] - P[1][0] * SK_MZ[1] + P[1][1] * - SK_MZ[2] - P[1][17] * SK_MZ[3]); - Kfusion[2] = SK_MZ[0] * (P[2][21] + P[2][18] * SH_MAG[2] + P[2][16] * SH_MAG[5] - P[2][0] * SK_MZ[1] + P[2][1] * - SK_MZ[2] - P[2][17] * SK_MZ[3]); - Kfusion[3] = SK_MZ[0] * (P[3][21] + P[3][18] * SH_MAG[2] + P[3][16] * SH_MAG[5] - P[3][0] * SK_MZ[1] + P[3][1] * - SK_MZ[2] - P[3][17] * SK_MZ[3]); - Kfusion[4] = SK_MZ[0] * (P[4][21] + P[4][18] * SH_MAG[2] + P[4][16] * SH_MAG[5] - P[4][0] * SK_MZ[1] + P[4][1] * - SK_MZ[2] - P[4][17] * SK_MZ[3]); - Kfusion[5] = SK_MZ[0] * (P[5][21] + P[5][18] * SH_MAG[2] + P[5][16] * SH_MAG[5] - P[5][0] * SK_MZ[1] + P[5][1] * - SK_MZ[2] - P[5][17] * SK_MZ[3]); - Kfusion[6] = SK_MZ[0] * (P[6][21] + P[6][18] * SH_MAG[2] + P[6][16] * SH_MAG[5] - P[6][0] * SK_MZ[1] + P[6][1] * - SK_MZ[2] - P[6][17] * SK_MZ[3]); - Kfusion[7] = SK_MZ[0] * (P[7][21] + P[7][18] * SH_MAG[2] + P[7][16] * SH_MAG[5] - P[7][0] * SK_MZ[1] + P[7][1] * - SK_MZ[2] - P[7][17] * SK_MZ[3]); - Kfusion[8] = SK_MZ[0] * (P[8][21] + P[8][18] * SH_MAG[2] + P[8][16] * SH_MAG[5] - P[8][0] * SK_MZ[1] + P[8][1] * - SK_MZ[2] - P[8][17] * SK_MZ[3]); - Kfusion[9] = SK_MZ[0] * (P[9][21] + P[9][18] * SH_MAG[2] + P[9][16] * SH_MAG[5] - P[9][0] * SK_MZ[1] + P[9][1] * - SK_MZ[2] - P[9][17] * SK_MZ[3]); - Kfusion[10] = SK_MZ[0] * (P[10][21] + P[10][18] * SH_MAG[2] + P[10][16] * SH_MAG[5] - P[10][0] * SK_MZ[1] + P[10][1] * - SK_MZ[2] - P[10][17] * SK_MZ[3]); - Kfusion[11] = SK_MZ[0] * (P[11][21] + P[11][18] * SH_MAG[2] + P[11][16] * SH_MAG[5] - P[11][0] * SK_MZ[1] + P[11][1] * - SK_MZ[2] - P[11][17] * SK_MZ[3]); - Kfusion[12] = SK_MZ[0] * (P[12][21] + P[12][18] * SH_MAG[2] + P[12][16] * SH_MAG[5] - P[12][0] * SK_MZ[1] + P[12][1] * - SK_MZ[2] - P[12][17] * SK_MZ[3]); - Kfusion[13] = SK_MZ[0] * (P[13][21] + P[13][18] * SH_MAG[2] + P[13][16] * SH_MAG[5] - P[13][0] * SK_MZ[1] + P[13][1] * - SK_MZ[2] - P[13][17] * SK_MZ[3]); - Kfusion[14] = SK_MZ[0] * (P[14][21] + P[14][18] * SH_MAG[2] + P[14][16] * SH_MAG[5] - P[14][0] * SK_MZ[1] + P[14][1] * - SK_MZ[2] - P[14][17] * SK_MZ[3]); - Kfusion[15] = SK_MZ[0] * (P[15][21] + P[15][18] * SH_MAG[2] + P[15][16] * SH_MAG[5] - P[15][0] * SK_MZ[1] + P[15][1] * - SK_MZ[2] - P[15][17] * SK_MZ[3]); - Kfusion[16] = SK_MZ[0] * (P[16][21] + P[16][18] * SH_MAG[2] + P[16][16] * SH_MAG[5] - P[16][0] * SK_MZ[1] + P[16][1] * - SK_MZ[2] - P[16][17] * SK_MZ[3]); - Kfusion[17] = SK_MZ[0] * (P[17][21] + P[17][18] * SH_MAG[2] + P[17][16] * SH_MAG[5] - P[17][0] * SK_MZ[1] + P[17][1] * - SK_MZ[2] - P[17][17] * SK_MZ[3]); - Kfusion[18] = SK_MZ[0] * (P[18][21] + P[18][18] * SH_MAG[2] + P[18][16] * SH_MAG[5] - P[18][0] * SK_MZ[1] + P[18][1] * - SK_MZ[2] - P[18][17] * SK_MZ[3]); - Kfusion[19] = SK_MZ[0] * (P[19][21] + P[19][18] * SH_MAG[2] + P[19][16] * SH_MAG[5] - P[19][0] * SK_MZ[1] + P[19][1] * - SK_MZ[2] - P[19][17] * SK_MZ[3]); - Kfusion[20] = SK_MZ[0] * (P[20][21] + P[20][18] * SH_MAG[2] + P[20][16] * SH_MAG[5] - P[20][0] * SK_MZ[1] + P[20][1] * - SK_MZ[2] - P[20][17] * SK_MZ[3]); - Kfusion[21] = SK_MZ[0] * (P[21][21] + P[21][18] * SH_MAG[2] + P[21][16] * SH_MAG[5] - P[21][0] * SK_MZ[1] + P[21][1] * - SK_MZ[2] - P[21][17] * SK_MZ[3]); - - // Don't update wind states unless we are doing wind estimation - if (_control_status.flags.wind) { - Kfusion[22] = SK_MZ[0] * (P[22][21] + P[22][18] * SH_MAG[2] + P[22][16] * SH_MAG[5] - P[22][0] * SK_MZ[1] + P[22][1] * - SK_MZ[2] - P[22][17] * SK_MZ[3]); - Kfusion[23] = SK_MZ[0] * (P[23][21] + P[23][18] * SH_MAG[2] + P[23][16] * SH_MAG[5] - P[23][0] * SK_MZ[1] + P[23][1] * - SK_MZ[2] - P[23][17] * SK_MZ[3]); - - } else { - Kfusion[22] = 0.0f; - Kfusion[23] = 0.0f; - } + Kfusion[0] = SK_MZ[0]*(P[0][21] + P[0][0]*SH_MAG[1] - P[0][1]*SH_MAG[2] + P[0][3]*SH_MAG[0] + P[0][2]*SK_MZ[2] + P[0][18]*SK_MZ[1] + P[0][16]*SK_MZ[4] - P[0][17]*SK_MZ[3]); + Kfusion[1] = SK_MZ[0]*(P[1][21] + P[1][0]*SH_MAG[1] - P[1][1]*SH_MAG[2] + P[1][3]*SH_MAG[0] + P[1][2]*SK_MZ[2] + P[1][18]*SK_MZ[1] + P[1][16]*SK_MZ[4] - P[1][17]*SK_MZ[3]); + Kfusion[2] = SK_MZ[0]*(P[2][21] + P[2][0]*SH_MAG[1] - P[2][1]*SH_MAG[2] + P[2][3]*SH_MAG[0] + P[2][2]*SK_MZ[2] + P[2][18]*SK_MZ[1] + P[2][16]*SK_MZ[4] - P[2][17]*SK_MZ[3]); + Kfusion[3] = SK_MZ[0]*(P[3][21] + P[3][0]*SH_MAG[1] - P[3][1]*SH_MAG[2] + P[3][3]*SH_MAG[0] + P[3][2]*SK_MZ[2] + P[3][18]*SK_MZ[1] + P[3][16]*SK_MZ[4] - P[3][17]*SK_MZ[3]); + Kfusion[4] = SK_MZ[0]*(P[4][21] + P[4][0]*SH_MAG[1] - P[4][1]*SH_MAG[2] + P[4][3]*SH_MAG[0] + P[4][2]*SK_MZ[2] + P[4][18]*SK_MZ[1] + P[4][16]*SK_MZ[4] - P[4][17]*SK_MZ[3]); + Kfusion[5] = SK_MZ[0]*(P[5][21] + P[5][0]*SH_MAG[1] - P[5][1]*SH_MAG[2] + P[5][3]*SH_MAG[0] + P[5][2]*SK_MZ[2] + P[5][18]*SK_MZ[1] + P[5][16]*SK_MZ[4] - P[5][17]*SK_MZ[3]); + Kfusion[6] = SK_MZ[0]*(P[6][21] + P[6][0]*SH_MAG[1] - P[6][1]*SH_MAG[2] + P[6][3]*SH_MAG[0] + P[6][2]*SK_MZ[2] + P[6][18]*SK_MZ[1] + P[6][16]*SK_MZ[4] - P[6][17]*SK_MZ[3]); + Kfusion[7] = SK_MZ[0]*(P[7][21] + P[7][0]*SH_MAG[1] - P[7][1]*SH_MAG[2] + P[7][3]*SH_MAG[0] + P[7][2]*SK_MZ[2] + P[7][18]*SK_MZ[1] + P[7][16]*SK_MZ[4] - P[7][17]*SK_MZ[3]); + Kfusion[8] = SK_MZ[0]*(P[8][21] + P[8][0]*SH_MAG[1] - P[8][1]*SH_MAG[2] + P[8][3]*SH_MAG[0] + P[8][2]*SK_MZ[2] + P[8][18]*SK_MZ[1] + P[8][16]*SK_MZ[4] - P[8][17]*SK_MZ[3]); + Kfusion[9] = SK_MZ[0]*(P[9][21] + P[9][0]*SH_MAG[1] - P[9][1]*SH_MAG[2] + P[9][3]*SH_MAG[0] + P[9][2]*SK_MZ[2] + P[9][18]*SK_MZ[1] + P[9][16]*SK_MZ[4] - P[9][17]*SK_MZ[3]); + Kfusion[10] = SK_MZ[0]*(P[10][21] + P[10][0]*SH_MAG[1] - P[10][1]*SH_MAG[2] + P[10][3]*SH_MAG[0] + P[10][2]*SK_MZ[2] + P[10][18]*SK_MZ[1] + P[10][16]*SK_MZ[4] - P[10][17]*SK_MZ[3]); + Kfusion[11] = SK_MZ[0]*(P[11][21] + P[11][0]*SH_MAG[1] - P[11][1]*SH_MAG[2] + P[11][3]*SH_MAG[0] + P[11][2]*SK_MZ[2] + P[11][18]*SK_MZ[1] + P[11][16]*SK_MZ[4] - P[11][17]*SK_MZ[3]); + Kfusion[12] = SK_MZ[0]*(P[12][21] + P[12][0]*SH_MAG[1] - P[12][1]*SH_MAG[2] + P[12][3]*SH_MAG[0] + P[12][2]*SK_MZ[2] + P[12][18]*SK_MZ[1] + P[12][16]*SK_MZ[4] - P[12][17]*SK_MZ[3]); + Kfusion[13] = SK_MZ[0]*(P[13][21] + P[13][0]*SH_MAG[1] - P[13][1]*SH_MAG[2] + P[13][3]*SH_MAG[0] + P[13][2]*SK_MZ[2] + P[13][18]*SK_MZ[1] + P[13][16]*SK_MZ[4] - P[13][17]*SK_MZ[3]); + Kfusion[14] = SK_MZ[0]*(P[14][21] + P[14][0]*SH_MAG[1] - P[14][1]*SH_MAG[2] + P[14][3]*SH_MAG[0] + P[14][2]*SK_MZ[2] + P[14][18]*SK_MZ[1] + P[14][16]*SK_MZ[4] - P[14][17]*SK_MZ[3]); + Kfusion[15] = SK_MZ[0]*(P[15][21] + P[15][0]*SH_MAG[1] - P[15][1]*SH_MAG[2] + P[15][3]*SH_MAG[0] + P[15][2]*SK_MZ[2] + P[15][18]*SK_MZ[1] + P[15][16]*SK_MZ[4] - P[15][17]*SK_MZ[3]); + Kfusion[16] = SK_MZ[0]*(P[16][21] + P[16][0]*SH_MAG[1] - P[16][1]*SH_MAG[2] + P[16][3]*SH_MAG[0] + P[16][2]*SK_MZ[2] + P[16][18]*SK_MZ[1] + P[16][16]*SK_MZ[4] - P[16][17]*SK_MZ[3]); + Kfusion[17] = SK_MZ[0]*(P[17][21] + P[17][0]*SH_MAG[1] - P[17][1]*SH_MAG[2] + P[17][3]*SH_MAG[0] + P[17][2]*SK_MZ[2] + P[17][18]*SK_MZ[1] + P[17][16]*SK_MZ[4] - P[17][17]*SK_MZ[3]); + Kfusion[18] = SK_MZ[0]*(P[18][21] + P[18][0]*SH_MAG[1] - P[18][1]*SH_MAG[2] + P[18][3]*SH_MAG[0] + P[18][2]*SK_MZ[2] + P[18][18]*SK_MZ[1] + P[18][16]*SK_MZ[4] - P[18][17]*SK_MZ[3]); + Kfusion[19] = SK_MZ[0]*(P[19][21] + P[19][0]*SH_MAG[1] - P[19][1]*SH_MAG[2] + P[19][3]*SH_MAG[0] + P[19][2]*SK_MZ[2] + P[19][18]*SK_MZ[1] + P[19][16]*SK_MZ[4] - P[19][17]*SK_MZ[3]); + Kfusion[20] = SK_MZ[0]*(P[20][21] + P[20][0]*SH_MAG[1] - P[20][1]*SH_MAG[2] + P[20][3]*SH_MAG[0] + P[20][2]*SK_MZ[2] + P[20][18]*SK_MZ[1] + P[20][16]*SK_MZ[4] - P[20][17]*SK_MZ[3]); + Kfusion[21] = SK_MZ[0]*(P[21][21] + P[21][0]*SH_MAG[1] - P[21][1]*SH_MAG[2] + P[21][3]*SH_MAG[0] + P[21][2]*SK_MZ[2] + P[21][18]*SK_MZ[1] + P[21][16]*SK_MZ[4] - P[21][17]*SK_MZ[3]); + Kfusion[22] = SK_MZ[0]*(P[22][21] + P[22][0]*SH_MAG[1] - P[22][1]*SH_MAG[2] + P[22][3]*SH_MAG[0] + P[22][2]*SK_MZ[2] + P[22][18]*SK_MZ[1] + P[22][16]*SK_MZ[4] - P[22][17]*SK_MZ[3]); + Kfusion[23] = SK_MZ[0]*(P[23][21] + P[23][0]*SH_MAG[1] - P[23][1]*SH_MAG[2] + P[23][3]*SH_MAG[0] + P[23][2]*SK_MZ[2] + P[23][18]*SK_MZ[1] + P[23][16]*SK_MZ[4] - P[23][17]*SK_MZ[3]); } else { return; } - // by definition our error state is zero at the time of fusion - _state.ang_error.setZero(); - - fuse(Kfusion, _mag_innov[index]); - - Quaternion q_correction; - q_correction.from_axis_angle(_state.ang_error); - _state.quat_nominal = q_correction * _state.quat_nominal; - _state.quat_nominal.normalize(); - _state.ang_error.setZero(); - // apply covariance correction via P_new = (I -K*H)*P // first calculate expression for KHP // then calculate P - KHP for (unsigned row = 0; row < _k_num_states; row++) { - for (unsigned column = 0; column <= 2; column++) { + for (unsigned column = 0; column <= 3; column++) { KH[row][column] = Kfusion[row] * H_MAG[index][column]; } @@ -459,6 +315,7 @@ void Ekf::fuseMag() float tmp = KH[row][0] * P[0][column]; tmp += KH[row][1] * P[1][column]; tmp += KH[row][2] * P[2][column]; + tmp += KH[row][3] * P[3][column]; tmp += KH[row][16] * P[16][column]; tmp += KH[row][17] * P[17][column]; tmp += KH[row][18] * P[18][column]; @@ -469,14 +326,48 @@ void Ekf::fuseMag() } } - for (unsigned row = 0; row < _k_num_states; row++) { - for (unsigned column = 0; column < _k_num_states; column++) { - P[row][column] -= KHP[row][column]; + // if the covariance correction will result in a negative variance, then + // the covariance marix is unhealthy and must be corrected + bool healthy = true; + _fault_status.flags.bad_mag_x = false; + _fault_status.flags.bad_mag_y = false; + _fault_status.flags.bad_mag_z = false; + for (int i = 0; i < _k_num_states; i++) { + if (P[i][i] < KHP[i][i]) { + // zero rows and columns + zeroRows(P,i,i); + zeroCols(P,i,i); + + //flag as unhealthy + healthy = false; + + // update individual measurement health status + if (index == 0) { + _fault_status.flags.bad_mag_x = true; + } else if (index == 1) { + _fault_status.flags.bad_mag_y = true; + } else if (index == 2) { + _fault_status.flags.bad_mag_z = true; + } } } - makeSymmetrical(); - limitCov(); + // only apply covariance and state corrrections if healthy + if (healthy) { + // apply the covariance corrections + for (unsigned row = 0; row < _k_num_states; row++) { + for (unsigned column = 0; column < _k_num_states; column++) { + P[row][column] = P[row][column] - KHP[row][column]; + } + } + + // correct the covariance marix for gross errors + fixCovarianceErrors(); + + // apply the state corrections + fuse(Kfusion, _mag_innov[index]); + + } } } @@ -492,46 +383,40 @@ void Ekf::fuseHeading() R_YAW = R_YAW * R_YAW; float predicted_hdg; - float H_YAW[3]; + float H_YAW[4]; matrix::Vector3f mag_earth_pred; // determine if a 321 or 312 Euler sequence is best if (fabsf(_R_to_earth(2, 0)) < fabsf(_R_to_earth(2, 1))) { // calculate observation jacobian when we are observing the first rotation in a 321 sequence - float t2 = q0 * q0; - float t3 = q1 * q1; - float t4 = q2 * q2; - float t5 = q3 * q3; - float t6 = t2 + t3 - t4 - t5; - float t7 = q0 * q3 * 2.0f; - float t8 = q1 * q2 * 2.0f; - float t9 = t7 + t8; - float t10 = sq(t6); - - if (t10 > 1e-6f) { - t10 = 1.0f / t10; - + float t9 = q0*q3; + float t10 = q1*q2; + float t2 = t9+t10; + float t3 = q0*q0; + float t4 = q1*q1; + float t5 = q2*q2; + float t6 = q3*q3; + float t7 = t3+t4-t5-t6; + float t8 = t7*t7; + if (t8 > 1e-6f) { + t8 = 1.0f/t8; } else { return; } - - float t11 = t9 * t9; - float t12 = t10 * t11; - float t13 = t12 + 1.0f; + float t11 = t2*t2; + float t12 = t8*t11*4.0f; + float t13 = t12+1.0f; float t14; - - if (fabsf(t13) > 1e-3f) { - t14 = 1.0f / t13; - + if (fabsf(t13) > 1e-6f) { + t14 = 1.0f/t13; } else { return; } - float t15 = 1.0f / t6; - - H_YAW[0] = 0.0f; - H_YAW[1] = t14 * (t15 * (q0 * q1 * 2.0f - q2 * q3 * 2.0f) + t9 * t10 * (q0 * q2 * 2.0f + q1 * q3 * 2.0f)); - H_YAW[2] = t14 * (t15 * (t2 - t3 + t4 - t5) + t9 * t10 * (t7 - t8)); + H_YAW[0] = t8*t14*(q3*t3-q3*t4+q3*t5+q3*t6+q0*q1*q2*2.0f)*-2.0f; + H_YAW[1] = t8*t14*(-q2*t3+q2*t4+q2*t5+q2*t6+q0*q1*q3*2.0f)*-2.0f; + H_YAW[2] = t8*t14*(q1*t3+q1*t4+q1*t5-q1*t6+q0*q2*q3*2.0f)*2.0f; + H_YAW[3] = t8*t14*(q0*t3+q0*t4-q0*t5+q0*t6+q1*q2*q3*2.0f)*2.0f; // rotate the magnetometer measurement into earth frame matrix::Euler euler321(_state.quat_nominal); @@ -546,40 +431,34 @@ void Ekf::fuseHeading() } else { // calculate observaton jacobian when we are observing a rotation in a 312 sequence - float t2 = q0 * q0; - float t3 = q1 * q1; - float t4 = q2 * q2; - float t5 = q3 * q3; - float t6 = t2 - t3 + t4 - t5; - float t7 = q0 * q3 * 2.0f; - float t10 = q1 * q2 * 2.0f; - float t8 = t7 - t10; - float t9 = sq(t6); - - if (t9 > 1e-6f) { - t9 = 1.0f / t9; - + float t9 = q0*q3; + float t10 = q1*q2; + float t2 = t9-t10; + float t3 = q0*q0; + float t4 = q1*q1; + float t5 = q2*q2; + float t6 = q3*q3; + float t7 = t3-t4+t5-t6; + float t8 = t7*t7; + if (t8 > 1e-6f) { + t8 = 1.0f/t8; } else { return; } - - float t11 = t8 * t8; - float t12 = t9 * t11; - float t13 = t12 + 1.0f; + float t11 = t2*t2; + float t12 = t8*t11*4.0f; + float t13 = t12+1.0f; float t14; - - if (fabsf(t13) > 1e-3f) { - t14 = 1.0f / t13; - + if (fabsf(t13) > 1e-6f) { + t14 = 1.0f/t13; } else { return; } - float t15 = 1.0f / t6; - - H_YAW[0] = -t14 * (t15 * (q0 * q2 * 2.0f + q1 * q3 * 2.0f) - t8 * t9 * (q0 * q1 * 2.0f - q2 * q3 * 2.0f)); - H_YAW[1] = 0.0f; - H_YAW[2] = t14 * (t15 * (t2 + t3 - t4 - t5) + t8 * t9 * (t7 + t10)); + H_YAW[0] = t8*t14*(q3*t3+q3*t4-q3*t5+q3*t6-q0*q1*q2*2.0f)*-2.0f; + H_YAW[1] = t8*t14*(q2*t3+q2*t4+q2*t5-q2*t6-q0*q1*q3*2.0f)*-2.0f; + H_YAW[2] = t8*t14*(-q1*t3+q1*t4+q1*t5+q1*t6-q0*q2*q3*2.0f)*2.0f; + H_YAW[3] = t8*t14*(q0*t3-q0*t4+q0*t5+q0*t6-q1*q2*q3*2.0f)*2.0f; // Calculate the 312 sequence euler angles that rotate from earth to body frame // See http://www.atacolorado.com/eulersequences.doc @@ -618,13 +497,13 @@ void Ekf::fuseHeading() // Calculate innovation variance and Kalman gains, taking advantage of the fact that only the first 3 elements in H are non zero // calculate the innovaton variance - float PH[3]; + float PH[4]; _heading_innov_var = R_YAW; - for (unsigned row = 0; row <= 2; row++) { + for (unsigned row = 0; row <= 3; row++) { PH[row] = 0.0f; - for (uint8_t col = 0; col <= 2; col++) { + for (uint8_t col = 0; col <= 3; col++) { PH[row] += P[row][col] * H_YAW[col]; } @@ -636,12 +515,12 @@ void Ekf::fuseHeading() // check if the innovation variance calculation is badly conditioned if (_heading_innov_var >= R_YAW) { // the innovation variance contribution from the state covariances is not negative, no fault - _fault_status.bad_mag_hdg = false; + _fault_status.flags.bad_mag_hdg = false; heading_innov_var_inv = 1.0f / _heading_innov_var; } else { // the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned - _fault_status.bad_mag_hdg = true; + _fault_status.flags.bad_mag_hdg = true; // we reinitialise the covariance matrix and abort this fusion step initialiseCovariance(); @@ -655,7 +534,7 @@ void Ekf::fuseHeading() for (uint8_t row = 0; row <= 15; row++) { Kfusion[row] = 0.0f; - for (uint8_t col = 0; col <= 2; col++) { + for (uint8_t col = 0; col <= 3; col++) { Kfusion[row] += P[row][col] * H_YAW[col]; } @@ -666,7 +545,7 @@ void Ekf::fuseHeading() for (uint8_t row = 22; row <= 23; row++) { Kfusion[row] = 0.0f; - for (uint8_t col = 0; col <= 2; col++) { + for (uint8_t col = 0; col <= 3; col++) { Kfusion[row] += P[row][col] * H_YAW[col]; } @@ -709,21 +588,11 @@ void Ekf::fuseHeading() _mag_healthy = true; } - // zero the attitude error states and use the kalman gain vector and innovation to update the states - _state.ang_error.setZero(); - fuse(Kfusion, _heading_innov); - - // correct the nominal quaternion - Quaternion dq; - dq.from_axis_angle(_state.ang_error); - _state.quat_nominal = dq * _state.quat_nominal; - _state.quat_nominal.normalize(); - // apply covariance correction via P_new = (I -K*H)*P // first calculate expression for KHP // then calculate P - KHP for (unsigned row = 0; row < _k_num_states; row++) { - for (unsigned column = 0; column <= 2; column++) { + for (unsigned column = 0; column <= 3; column++) { KH[row][column] = Kfusion[row] * H_YAW[column]; } } @@ -733,18 +602,46 @@ void Ekf::fuseHeading() float tmp = KH[row][0] * P[0][column]; tmp += KH[row][1] * P[1][column]; tmp += KH[row][2] * P[2][column]; + tmp += KH[row][3] * P[3][column]; KHP[row][column] = tmp; } } - for (unsigned row = 0; row < _k_num_states; row++) { - for (unsigned column = 0; column < _k_num_states; column++) { - P[row][column] -= KHP[row][column]; + // if the covariance correction will result in a negative variance, then + // the covariance marix is unhealthy and must be corrected + bool healthy = true; + _fault_status.flags.bad_mag_hdg = false; + for (int i = 0; i < _k_num_states; i++) { + if (P[i][i] < KHP[i][i]) { + // zero rows and columns + zeroRows(P,i,i); + zeroCols(P,i,i); + + //flag as unhealthy + healthy = false; + + // update individual measurement health status + _fault_status.flags.bad_mag_hdg = true; + } } - makeSymmetrical(); - limitCov(); + // only apply covariance and state corrrections if healthy + if (healthy) { + // apply the covariance corrections + for (unsigned row = 0; row < _k_num_states; row++) { + for (unsigned column = 0; column < _k_num_states; column++) { + P[row][column] = P[row][column] - KHP[row][column]; + } + } + + // correct the covariance marix for gross errors + fixCovarianceErrors(); + + // apply the state corrections + fuse(Kfusion, _heading_innov); + + } } void Ekf::fuseDeclination() @@ -756,100 +653,76 @@ void Ekf::fuseDeclination() float R_DECL = sq(0.5f); // Calculate intermediate variables + float t2 = magE*magE; + float t3 = magN*magN; + float t4 = t2+t3; // if the horizontal magnetic field is too small, this calculation will be badly conditioned - if (magN < 0.001f) { + if (t4 < 1e-4f) { return; } - - float t2 = magE * magE; - float t3 = magN * magN; - float t4 = t2 + t3; - float t5 = 1.0f / t4; - float t22 = magE * t5; - float t23 = magN * t5; - float t6 = P[16][16] * t22; - float t13 = P[17][16] * t23; - float t7 = t6 - t13; - float t8 = t22 * t7; - float t9 = P[16][17] * t22; - float t14 = P[17][17] * t23; - float t10 = t9 - t14; - float t15 = t23 * t10; - float t11 = R_DECL + t8 - t15; // innovation variance - - // check the innovation variance calculation for a badly conditioned covariance matrix - if (t11 >= R_DECL) { - // the innovation variance contribution from the state covariances is not negative, no fault - _fault_status.bad_mag_decl = false; - + float t5 = P[16][16]*t2; + float t6 = P[17][17]*t3; + float t7 = t2*t2; + float t8 = R_DECL*t7; + float t9 = t3*t3; + float t10 = R_DECL*t9; + float t11 = R_DECL*t2*t3*2.0f; + float t14 = P[16][17]*magE*magN; + float t15 = P[17][16]*magE*magN; + float t12 = t5+t6+t8+t10+t11-t14-t15; + float t13; + if (fabsf(t12) > 1e-6f) { + t13 = 1.0f / t12; + } else { + return; + } + float t18 = magE*magE; + float t19 = magN*magN; + float t20 = t18+t19; + float t21; + if (fabsf(t20) > 1e-6f) { + t21 = 1.0f/t20; } else { - // the innovation variance contribution from the state covariances is negtive which means the covariance matrix is badly conditioned - _fault_status.bad_mag_decl = true; - - // we reinitialise the covariance matrix and abort this fusion step - initialiseCovariance(); return; } - - float t12 = 1.0f / t11; // Calculate the observation Jacobian // Note only 2 terms are non-zero which can be used in matrix operations for calculation of Kalman gains and covariance update to significantly reduce cost float H_DECL[24] = {}; - H_DECL[16] = -magE * t5; - H_DECL[17] = magN * t5; + H_DECL[16] = -magE*t21; + H_DECL[17] = magN*t21; // Calculate the Kalman gains float Kfusion[_k_num_states] = {}; - Kfusion[0] = -t12 * (P[0][16] * t22 - P[0][17] * t23); - Kfusion[1] = -t12 * (P[1][16] * t22 - P[1][17] * t23); - Kfusion[2] = -t12 * (P[2][16] * t22 - P[2][17] * t23); - Kfusion[3] = -t12 * (P[3][16] * t22 - P[3][17] * t23); - Kfusion[4] = -t12 * (P[4][16] * t22 - P[4][17] * t23); - Kfusion[5] = -t12 * (P[5][16] * t22 - P[5][17] * t23); - Kfusion[6] = -t12 * (P[6][16] * t22 - P[6][17] * t23); - Kfusion[7] = -t12 * (P[7][16] * t22 - P[7][17] * t23); - Kfusion[8] = -t12 * (P[8][16] * t22 - P[8][17] * t23); - Kfusion[9] = -t12 * (P[9][16] * t22 - P[9][17] * t23); - Kfusion[10] = -t12 * (P[10][16] * t22 - P[10][17] * t23); - Kfusion[11] = -t12 * (P[11][16] * t22 - P[11][17] * t23); - Kfusion[12] = -t12 * (P[12][16] * t22 - P[12][17] * t23); - Kfusion[13] = -t12 * (P[13][16] * t22 - P[13][17] * t23); - Kfusion[14] = -t12 * (P[14][16] * t22 - P[14][17] * t23); - Kfusion[15] = -t12 * (P[15][16] * t22 - P[15][17] * t23); - - // We only do declination fusion when we are using all the field states, so no logic required here - Kfusion[16] = -t12 * (t6 - P[16][17] * t23); - Kfusion[17] = t12 * (t14 - P[17][16] * t22); - Kfusion[18] = -t12 * (P[18][16] * t22 - P[18][17] * t23); - Kfusion[19] = -t12 * (P[19][16] * t22 - P[19][17] * t23); - Kfusion[20] = -t12 * (P[20][16] * t22 - P[20][17] * t23); - Kfusion[21] = -t12 * (P[21][16] * t22 - P[21][17] * t23); - - // Don't update wind states unless we are doing wind estimation - if (_control_status.flags.wind) { - Kfusion[22] = -t12 * (P[22][16] * t22 - P[22][17] * t23); - Kfusion[23] = -t12 * (P[23][16] * t22 - P[23][17] * t23); - - } else { - Kfusion[22] = 0.0f; - Kfusion[23] = 0.0f; - } + Kfusion[0] = -t4*t13*(P[0][16]*magE-P[0][17]*magN); + Kfusion[1] = -t4*t13*(P[1][16]*magE-P[1][17]*magN); + Kfusion[2] = -t4*t13*(P[2][16]*magE-P[2][17]*magN); + Kfusion[3] = -t4*t13*(P[3][16]*magE-P[3][17]*magN); + Kfusion[4] = -t4*t13*(P[4][16]*magE-P[4][17]*magN); + Kfusion[5] = -t4*t13*(P[5][16]*magE-P[5][17]*magN); + Kfusion[6] = -t4*t13*(P[6][16]*magE-P[6][17]*magN); + Kfusion[7] = -t4*t13*(P[7][16]*magE-P[7][17]*magN); + Kfusion[8] = -t4*t13*(P[8][16]*magE-P[8][17]*magN); + Kfusion[9] = -t4*t13*(P[9][16]*magE-P[9][17]*magN); + Kfusion[10] = -t4*t13*(P[10][16]*magE-P[10][17]*magN); + Kfusion[11] = -t4*t13*(P[11][16]*magE-P[11][17]*magN); + Kfusion[12] = -t4*t13*(P[12][16]*magE-P[12][17]*magN); + Kfusion[13] = -t4*t13*(P[13][16]*magE-P[13][17]*magN); + Kfusion[14] = -t4*t13*(P[14][16]*magE-P[14][17]*magN); + Kfusion[15] = -t4*t13*(P[15][16]*magE-P[15][17]*magN); + Kfusion[16] = -t4*t13*(P[16][16]*magE-P[16][17]*magN); + Kfusion[17] = -t4*t13*(P[17][16]*magE-P[17][17]*magN); + Kfusion[18] = -t4*t13*(P[18][16]*magE-P[18][17]*magN); + Kfusion[19] = -t4*t13*(P[19][16]*magE-P[19][17]*magN); + Kfusion[20] = -t4*t13*(P[20][16]*magE-P[20][17]*magN); + Kfusion[21] = -t4*t13*(P[21][16]*magE-P[21][17]*magN); + Kfusion[22] = -t4*t13*(P[22][16]*magE-P[22][17]*magN); + Kfusion[23] = -t4*t13*(P[23][16]*magE-P[23][17]*magN); // calculate innovation and constrain - float innovation = atanf(magE / magN) - _mag_declination; + float innovation = atan2f(magE , magN) - _mag_declination; innovation = math::constrain(innovation, -0.5f, 0.5f); - // zero attitude error states and perform the state correction - _state.ang_error.setZero(); - fuse(Kfusion, innovation); - - // use the attitude error estimate to correct the quaternion - Quaternion dq; - dq.from_axis_angle(_state.ang_error); - _state.quat_nominal = dq * _state.quat_nominal; - _state.quat_nominal.normalize(); - // apply covariance correction via P_new = (I -K*H)*P // first calculate expression for KHP // then calculate P - KHP @@ -868,209 +741,39 @@ void Ekf::fuseDeclination() } } - for (unsigned row = 0; row < _k_num_states; row++) { - for (unsigned column = 0; column < _k_num_states; column++) { - P[row][column] -= KHP[row][column]; + // if the covariance correction will result in a negative variance, then + // the covariance marix is unhealthy and must be corrected + bool healthy = true; + _fault_status.flags.bad_mag_decl = false; + for (int i = 0; i < _k_num_states; i++) { + if (P[i][i] < KHP[i][i]) { + // zero rows and columns + zeroRows(P,i,i); + zeroCols(P,i,i); + + //flag as unhealthy + healthy = false; + + // update individual measurement health status + _fault_status.flags.bad_mag_decl = true; + } } - // force the covariance matrix to be symmetrical and don't allow the variances to be negative. - makeSymmetrical(); - limitCov(); -} - -void Ekf::fuseMag2D() -{ - // assign intermediate state variables - float q0 = _state.quat_nominal(0); - float q1 = _state.quat_nominal(1); - float q2 = _state.quat_nominal(2); - float q3 = _state.quat_nominal(3); - - float magX = _mag_sample_delayed.mag(0); - float magY = _mag_sample_delayed.mag(1); - float magZ = _mag_sample_delayed.mag(2); - - float R_DECL = fmaxf(_params.mag_heading_noise, 1.0e-2f); - R_DECL = R_DECL * R_DECL; - - // calculate intermediate variables for observation jacobian - float t2 = q0 * q0; - float t3 = q1 * q1; - float t4 = q2 * q2; - float t5 = q3 * q3; - float t6 = q0 * q3 * 2.0f; - float t8 = t2 - t3 + t4 - t5; - float t9 = q0 * q1 * 2.0f; - float t10 = q2 * q3 * 2.0f; - float t11 = t9 - t10; - float t14 = q1 * q2 * 2.0f; - float t21 = magY * t8; - float t22 = t6 + t14; - float t23 = magX * t22; - float t24 = magZ * t11; - float t7 = t21 + t23 - t24; - float t12 = t2 + t3 - t4 - t5; - float t13 = magX * t12; - float t15 = q0 * q2 * 2.0f; - float t16 = q1 * q3 * 2.0f; - float t17 = t15 + t16; - float t18 = magZ * t17; - float t19 = t6 - t14; - float t25 = magY * t19; - float t20 = t13 + t18 - t25; - - if (fabsf(t20) < 1e-6f) { - return; - } - - float t26 = 1.0f / (t20 * t20); - float t27 = t7 * t7; - float t28 = t26 * t27; - float t29 = t28 + 1.0f; - - if (fabsf(t29) < 1e-12f) { - return; - } - - float t30 = 1.0f / t29; - - if (fabsf(t20) < 1e-12f) { - return; - } - - float t31 = 1.0f / t20; - - // calculate observation jacobian - float H_DECL[3] = {}; - H_DECL[0] = -t30 * (t31 * (magZ * t8 + magY * t11) + t7 * t26 * (magY * t17 + magZ * t19)); - H_DECL[1] = t30 * (t31 * (magX * t11 + magZ * t22) - t7 * t26 * (magZ * t12 - magX * t17)); - H_DECL[2] = t30 * (t31 * (magX * t8 - magY * t22) + t7 * t26 * (magY * t12 + magX * t19)); - - // rotate the magnetometer measurement into earth frame - matrix::Dcm R_to_earth(_state.quat_nominal); - matrix::Vector3f mag_earth_pred = R_to_earth * _mag_sample_delayed.mag; - - // check if there is enough magnetic field length to use and exit if too small - float magLength2 = sq(mag_earth_pred(0) + mag_earth_pred(1)); - - if (magLength2 < sq(_params.mag_noise)) { - return; - } - - // Adjust the measurement variance upwards if thehorizontal strength to magnetometer noise ratio make the value unrealistic - R_DECL = fmaxf(R_DECL, sq(_params.mag_noise) / magLength2); - - // Calculate the innovation, using the declination angle of the projection onto the horizontal as the measurement - _heading_innov = atan2f(mag_earth_pred(1), mag_earth_pred(0)) - _mag_declination; - - // wrap the innovation to the interval between +-pi - _heading_innov = matrix::wrap_pi(_heading_innov); - - // Calculate innovation variance and Kalman gains, taking advantage of the fact that only the first 3 elements in H are non zero - float PH[3]; - _heading_innov_var = R_DECL; - - for (unsigned row = 0; row <= 2; row++) { - PH[row] = 0.0f; - - for (unsigned col = 0; col <= 2; col++) { - PH[row] += P[row][col] * H_DECL[col]; - } - - _heading_innov_var += H_DECL[row] * PH[row]; - } - - float varInnovInv; - - if (_heading_innov_var >= R_DECL) { - // the innovation variance contribution from the state covariances is not negative, no fault - _fault_status.bad_mag_hdg = false; - - } else { - // the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned - _fault_status.bad_mag_hdg = true; - - // we reinitialise the covariance matrix and abort this fusion step - initialiseCovariance(); - return; - } - - // innovation test ratio - _yaw_test_ratio = sq(_heading_innov) / (sq(math::max(_params.heading_innov_gate, 1.0f)) * _heading_innov_var); - - // set the magnetometer unhealthy if the test fails - if (_yaw_test_ratio > 1.0f) { - _mag_healthy = false; - - // if we are in air we don't want to fuse the measurement - // we allow to use it when on the ground because the large innovation could be caused - // by interference or a large initial gyro bias - if (_control_status.flags.in_air) { - return; - - } else { - // constrain the innovation to the maximum set by the gate - float gate_limit = sqrtf((sq(math::max(_params.heading_innov_gate, 1.0f)) * _heading_innov_var)); - _heading_innov = math::constrain(_heading_innov, -gate_limit, gate_limit); - } - - } else { - _mag_healthy = true; - } - - varInnovInv = 1.0f / _heading_innov_var; - - // calculate the Kalman gains - float Kfusion[24] = {}; - - for (unsigned row = 0; row < 16; row++) { - Kfusion[row] = 0.0f; - - for (unsigned col = 0; col <= 2; col++) { - Kfusion[row] += P[row][col] * H_DECL[col]; - } - - Kfusion[row] *= varInnovInv; - } - - // by definition our error state is zero at the time of fusion - _state.ang_error.setZero(); - - // correct the states - fuse(Kfusion, _heading_innov); - - // correct the quaternon using the attitude error estimate - Quaternion q_correction; - q_correction.from_axis_angle(_state.ang_error); - _state.quat_nominal = q_correction * _state.quat_nominal; - _state.quat_nominal.normalize(); - _state.ang_error.setZero(); - - // apply covariance correction via P_new = (I -K*H)*P - // first calculate expression for KHP - // then calculate P - KHP - for (unsigned row = 0; row < _k_num_states; row++) { - for (unsigned column = 0; column <= 2; column++) { - KH[row][column] = Kfusion[row] * H_DECL[column]; - } - } - - for (unsigned row = 0; row < _k_num_states; row++) { - for (unsigned column = 0; column < _k_num_states; column++) { - float tmp = KH[row][0] * P[0][column]; - tmp += KH[row][1] * P[1][column]; - tmp += KH[row][2] * P[2][column]; - KHP[row][column] = tmp; - } - } - - for (unsigned row = 0; row < _k_num_states; row++) { - for (unsigned column = 0; column < _k_num_states; column++) { - P[row][column] -= KHP[row][column]; - } - } - - makeSymmetrical(); - limitCov(); + // only apply covariance and state corrrections if healthy + if (healthy) { + // apply the covariance corrections + for (unsigned row = 0; row < _k_num_states; row++) { + for (unsigned column = 0; column < _k_num_states; column++) { + P[row][column] = P[row][column] - KHP[row][column]; + } + } + + // correct the covariance marix for gross errors + fixCovarianceErrors(); + + // apply the state corrections + fuse(Kfusion, innovation); + + } } diff --git a/EKF/optflow_fusion.cpp b/EKF/optflow_fusion.cpp index 09f325b8f4..747d0b29c5 100644 --- a/EKF/optflow_fusion.cpp +++ b/EKF/optflow_fusion.cpp @@ -59,23 +59,8 @@ void Ekf::fuseOptFlow() float ve = _state.vel(1); float vd = _state.vel(2); - // calculate the observation noise variance - scaling noise linearly across flow quality range - float R_LOS_best = fmaxf(_params.flow_noise, 0.05f); - float R_LOS_worst = fmaxf(_params.flow_noise_qual_min, 0.05f); - - // calculate a weighting that varies between 1 when flow quality is best and 0 when flow quality is worst - float weighting = (255.0f - (float)_params.flow_qual_min); - - if (weighting >= 1.0f) { - weighting = math::constrain(((float)_flow_sample_delayed.quality - (float)_params.flow_qual_min) / weighting, 0.0f, - 1.0f); - - } else { - weighting = 0.0f; - } - - // take the weighted average of the observation noie for the best and wort flow quality - float R_LOS = sq(R_LOS_best * weighting + R_LOS_worst * (1.0f - weighting)); + // calculate the optical flow observation variance + float R_LOS = calcOptFlowMeasVar(); float H_LOS[2][24] = {}; // Optical flow observation Jacobians float Kfusion[24][2] = {}; // Optical flow Kalman gains @@ -126,92 +111,111 @@ void Ekf::fuseOptFlow() // calculate X axis observation Jacobian float t2 = 1.0f / range; - float t3 = q0 * q0; - float t4 = q1 * q1; - float t5 = q2 * q2; - float t6 = q3 * q3; - float t7 = q0 * q2 * 2.0f; - float t8 = q1 * q3 * 2.0f; - float t9 = q0 * q3 * 2.0f; - float t10 = q1 * q2 * 2.0f; - float t11 = q0 * q1 * 2.0f; - H_LOS[0][0] = t2 * (vn * (t7 + t8) + vd * (t3 - t4 - t5 + t6) - ve * (t11 - q2 * q3 * 2.0f)); - H_LOS[0][2] = -t2 * (ve * (t9 + t10) - vd * (t7 - t8) + vn * (t3 + t4 - t5 - t6)); - H_LOS[0][3] = -t2 * (t9 - t10); - H_LOS[0][4] = t2 * (t3 - t4 + t5 - t6); - H_LOS[0][5] = t2 * (t11 + q2 * q3 * 2.0f); - - // calculate intermediate variables for the X observaton innovatoin variance and klmna gains - t2 = 1.0f / range; - t3 = q0 * q1 * 2.0f; - t4 = q2 * q3 * 2.0f; - t5 = q0 * q0; - t6 = q1 * q1; - t7 = q2 * q2; - t8 = q3 * q3; - t9 = q0 * q2 * 2.0f; - t10 = q1 * q3 * 2.0f; - t11 = q0 * q3 * 2.0f; - float t12 = q1 * q2 * 2.0f; - float t13 = t11 - t12; - float t14 = t3 + t4; - float t15 = t5 - t6 - t7 + t8; - float t16 = t15 * vd; - float t17 = t3 - t4; - float t18 = t9 + t10; - float t19 = t18 * vn; - float t28 = t17 * ve; - float t20 = t16 + t19 - t28; - float t21 = t5 + t6 - t7 - t8; - float t22 = t21 * vn; - float t23 = t9 - t10; - float t24 = t11 + t12; - float t25 = t24 * ve; - float t29 = t23 * vd; - float t26 = t22 + t25 - t29; - float t27 = t5 - t6 + t7 - t8; - float t30 = P[0][0] * t2 * t20; - float t31 = P[5][3] * t2 * t14; - float t32 = P[0][3] * t2 * t20; - float t33 = P[4][3] * t2 * t27; - float t56 = P[3][3] * t2 * t13; - float t57 = P[2][3] * t2 * t26; - float t34 = t31 + t32 + t33 - t56 - t57; - float t35 = P[5][5] * t2 * t14; - float t36 = P[0][5] * t2 * t20; - float t37 = P[4][5] * t2 * t27; - float t59 = P[3][5] * t2 * t13; - float t60 = P[2][5] * t2 * t26; - float t38 = t35 + t36 + t37 - t59 - t60; - float t39 = t2 * t14 * t38; - float t40 = P[5][0] * t2 * t14; - float t41 = P[4][0] * t2 * t27; - float t61 = P[3][0] * t2 * t13; - float t62 = P[2][0] * t2 * t26; - float t42 = t30 + t40 + t41 - t61 - t62; - float t43 = t2 * t20 * t42; - float t44 = P[5][2] * t2 * t14; - float t45 = P[0][2] * t2 * t20; - float t46 = P[4][2] * t2 * t27; - float t55 = P[2][2] * t2 * t26; - float t63 = P[3][2] * t2 * t13; - float t47 = t44 + t45 + t46 - t55 - t63; - float t48 = P[5][4] * t2 * t14; - float t49 = P[0][4] * t2 * t20; - float t50 = P[4][4] * t2 * t27; - float t65 = P[3][4] * t2 * t13; - float t66 = P[2][4] * t2 * t26; - float t51 = t48 + t49 + t50 - t65 - t66; - float t52 = t2 * t27 * t51; - float t58 = t2 * t13 * t34; - float t64 = t2 * t26 * t47; - float t53 = R_LOS + t39 + t43 + t52 - t58 - t64; - float t54; + H_LOS[0][0] = t2*(q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f); + H_LOS[0][1] = t2*(q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f); + H_LOS[0][2] = t2*(q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f); + H_LOS[0][3] = -t2*(q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f); + H_LOS[0][4] = -t2*(q0*q3*2.0f-q1*q2*2.0f); + H_LOS[0][5] = t2*(q0*q0-q1*q1+q2*q2-q3*q3); + H_LOS[0][6] = t2*(q0*q1*2.0f+q2*q3*2.0f); + // calculate intermediate variables for the X observaton innovatoin variance and Kalman gains + float t3 = q1*vd*2.0f; + float t4 = q0*ve*2.0f; + float t11 = q3*vn*2.0f; + float t5 = t3+t4-t11; + float t6 = q0*q3*2.0f; + float t29 = q1*q2*2.0f; + float t7 = t6-t29; + float t8 = q0*q1*2.0f; + float t9 = q2*q3*2.0f; + float t10 = t8+t9; + float t12 = P[0][0]*t2*t5; + float t13 = q0*vd*2.0f; + float t14 = q2*vn*2.0f; + float t28 = q1*ve*2.0f; + float t15 = t13+t14-t28; + float t16 = q3*vd*2.0f; + float t17 = q2*ve*2.0f; + float t18 = q1*vn*2.0f; + float t19 = t16+t17+t18; + float t20 = q3*ve*2.0f; + float t21 = q0*vn*2.0f; + float t30 = q2*vd*2.0f; + float t22 = t20+t21-t30; + float t23 = q0*q0; + float t24 = q1*q1; + float t25 = q2*q2; + float t26 = q3*q3; + float t27 = t23-t24+t25-t26; + float t31 = P[1][1]*t2*t15; + float t32 = P[6][0]*t2*t10; + float t33 = P[1][0]*t2*t15; + float t34 = P[2][0]*t2*t19; + float t35 = P[5][0]*t2*t27; + float t79 = P[4][0]*t2*t7; + float t80 = P[3][0]*t2*t22; + float t36 = t12+t32+t33+t34+t35-t79-t80; + float t37 = t2*t5*t36; + float t38 = P[6][1]*t2*t10; + float t39 = P[0][1]*t2*t5; + float t40 = P[2][1]*t2*t19; + float t41 = P[5][1]*t2*t27; + float t81 = P[4][1]*t2*t7; + float t82 = P[3][1]*t2*t22; + float t42 = t31+t38+t39+t40+t41-t81-t82; + float t43 = t2*t15*t42; + float t44 = P[6][2]*t2*t10; + float t45 = P[0][2]*t2*t5; + float t46 = P[1][2]*t2*t15; + float t47 = P[2][2]*t2*t19; + float t48 = P[5][2]*t2*t27; + float t83 = P[4][2]*t2*t7; + float t84 = P[3][2]*t2*t22; + float t49 = t44+t45+t46+t47+t48-t83-t84; + float t50 = t2*t19*t49; + float t51 = P[6][3]*t2*t10; + float t52 = P[0][3]*t2*t5; + float t53 = P[1][3]*t2*t15; + float t54 = P[2][3]*t2*t19; + float t55 = P[5][3]*t2*t27; + float t85 = P[4][3]*t2*t7; + float t86 = P[3][3]*t2*t22; + float t56 = t51+t52+t53+t54+t55-t85-t86; + float t57 = P[6][5]*t2*t10; + float t58 = P[0][5]*t2*t5; + float t59 = P[1][5]*t2*t15; + float t60 = P[2][5]*t2*t19; + float t61 = P[5][5]*t2*t27; + float t88 = P[4][5]*t2*t7; + float t89 = P[3][5]*t2*t22; + float t62 = t57+t58+t59+t60+t61-t88-t89; + float t63 = t2*t27*t62; + float t64 = P[6][4]*t2*t10; + float t65 = P[0][4]*t2*t5; + float t66 = P[1][4]*t2*t15; + float t67 = P[2][4]*t2*t19; + float t68 = P[5][4]*t2*t27; + float t90 = P[4][4]*t2*t7; + float t91 = P[3][4]*t2*t22; + float t69 = t64+t65+t66+t67+t68-t90-t91; + float t70 = P[6][6]*t2*t10; + float t71 = P[0][6]*t2*t5; + float t72 = P[1][6]*t2*t15; + float t73 = P[2][6]*t2*t19; + float t74 = P[5][6]*t2*t27; + float t93 = P[4][6]*t2*t7; + float t94 = P[3][6]*t2*t22; + float t75 = t70+t71+t72+t73+t74-t93-t94; + float t76 = t2*t10*t75; + float t87 = t2*t22*t56; + float t92 = t2*t7*t69; + float t77 = R_LOS+t37+t43+t50+t63+t76-t87-t92; + float t78; // calculate innovation variance for X axis observation and protect against a badly conditioned calculation - if (t53 >= R_LOS) { - t54 = 1.0f / t53; - _flow_innov_var[0] = t53; + if (t77 >= R_LOS) { + t78 = 1.0f / t77; + _flow_innov_var[0] = t77; } else { // we need to reinitialise the covariance matrix and abort this fusion step @@ -220,56 +224,30 @@ void Ekf::fuseOptFlow() } // calculate Kalman gains for X-axis observation - Kfusion[0][0] = t54 * (t30 - P[0][3] * t2 * (t11 - q1 * q2 * 2.0f) + P[0][5] * t2 * t14 - P[0][2] * t2 * t26 + P[0][4] * - t2 * t27); - Kfusion[1][0] = t54 * (-P[1][3] * t2 * t13 + P[1][5] * t2 * t14 + P[1][0] * t2 * t20 - P[1][2] * t2 * t26 + P[1][4] * t2 - * t27); - Kfusion[2][0] = t54 * (-t55 - P[2][3] * t2 * t13 + P[2][5] * t2 * t14 + P[2][0] * t2 * t20 + P[2][4] * t2 * t27); - Kfusion[3][0] = t54 * (-t56 + P[3][5] * t2 * t14 + P[3][0] * t2 * t20 - P[3][2] * t2 * t26 + P[3][4] * t2 * t27); - Kfusion[4][0] = t54 * (t50 - P[4][3] * t2 * t13 + P[4][5] * t2 * t14 + P[4][0] * t2 * t20 - P[4][2] * t2 * t26); - Kfusion[5][0] = t54 * (t35 - P[5][3] * t2 * t13 + P[5][0] * t2 * t20 - P[5][2] * t2 * t26 + P[5][4] * t2 * t27); - Kfusion[6][0] = t54 * (-P[6][3] * t2 * t13 + P[6][5] * t2 * t14 + P[6][0] * t2 * t20 - P[6][2] * t2 * t26 + P[6][4] * t2 - * t27); - Kfusion[7][0] = t54 * (-P[7][3] * t2 * t13 + P[7][5] * t2 * t14 + P[7][0] * t2 * t20 - P[7][2] * t2 * t26 + P[7][4] * t2 - * t27); - Kfusion[8][0] = t54 * (-P[8][3] * t2 * t13 + P[8][5] * t2 * t14 + P[8][0] * t2 * t20 - P[8][2] * t2 * t26 + P[8][4] * t2 - * t27); - Kfusion[9][0] = t54 * (-P[9][3] * t2 * t13 + P[9][5] * t2 * t14 + P[9][0] * t2 * t20 - P[9][2] * t2 * t26 + P[9][4] * t2 - * t27); - Kfusion[10][0] = t54 * (-P[10][3] * t2 * t13 + P[10][5] * t2 * t14 + P[10][0] * t2 * t20 - P[10][2] * t2 * t26 + - P[10][4] * t2 * t27); - Kfusion[11][0] = t54 * (-P[11][3] * t2 * t13 + P[11][5] * t2 * t14 + P[11][0] * t2 * t20 - P[11][2] * t2 * t26 + - P[11][4] * t2 * t27); - Kfusion[12][0] = t54 * (-P[12][3] * t2 * t13 + P[12][5] * t2 * t14 + P[12][0] * t2 * t20 - P[12][2] * t2 * t26 + - P[12][4] * t2 * t27); - Kfusion[13][0] = t54 * (-P[13][3] * t2 * t13 + P[13][5] * t2 * t14 + P[13][0] * t2 * t20 - P[13][2] * t2 * t26 + - P[13][4] * t2 * t27); - Kfusion[14][0] = t54 * (-P[14][3] * t2 * t13 + P[14][5] * t2 * t14 + P[14][0] * t2 * t20 - P[14][2] * t2 * t26 + - P[14][4] * t2 * t27); - Kfusion[15][0] = t54 * (-P[15][3] * t2 * t13 + P[15][5] * t2 * t14 + P[15][0] * t2 * t20 - P[15][2] * t2 * t26 + - P[15][4] * t2 * t27); - - if (_control_status.flags.mag_3D) { - Kfusion[16][0] = t54 * (-P[16][3] * t2 * t13 + P[16][5] * t2 * t14 + P[16][0] * t2 * t20 - P[16][2] * t2 * t26 + - P[16][4] * t2 * t27); - Kfusion[17][0] = t54 * (-P[17][3] * t2 * t13 + P[17][5] * t2 * t14 + P[17][0] * t2 * t20 - P[17][2] * t2 * t26 + - P[17][4] * t2 * t27); - Kfusion[18][0] = t54 * (-P[18][3] * t2 * t13 + P[18][5] * t2 * t14 + P[18][0] * t2 * t20 - P[18][2] * t2 * t26 + - P[18][4] * t2 * t27); - Kfusion[19][0] = t54 * (-P[19][3] * t2 * t13 + P[19][5] * t2 * t14 + P[19][0] * t2 * t20 - P[19][2] * t2 * t26 + - P[19][4] * t2 * t27); - Kfusion[20][0] = t54 * (-P[20][3] * t2 * t13 + P[20][5] * t2 * t14 + P[20][0] * t2 * t20 - P[20][2] * t2 * t26 + - P[20][4] * t2 * t27); - Kfusion[21][0] = t54 * (-P[21][3] * t2 * t13 + P[21][5] * t2 * t14 + P[21][0] * t2 * t20 - P[21][2] * t2 * t26 + - P[21][4] * t2 * t27); - } - - if (_control_status.flags.wind) { - Kfusion[22][0] = t54 * (-P[22][3] * t2 * t13 + P[22][5] * t2 * t14 + P[22][0] * t2 * t20 - P[22][2] * t2 * t26 + - P[22][4] * t2 * t27); - Kfusion[23][0] = t54 * (-P[23][3] * t2 * t13 + P[23][5] * t2 * t14 + P[23][0] * t2 * t20 - P[23][2] * t2 * t26 + - P[23][4] * t2 * t27); - } + Kfusion[0][0] = t78*(t12-P[0][4]*t2*t7+P[0][1]*t2*t15+P[0][6]*t2*t10+P[0][2]*t2*t19-P[0][3]*t2*t22+P[0][5]*t2*t27); + Kfusion[1][0] = t78*(t31+P[1][0]*t2*t5-P[1][4]*t2*t7+P[1][6]*t2*t10+P[1][2]*t2*t19-P[1][3]*t2*t22+P[1][5]*t2*t27); + Kfusion[2][0] = t78*(t47+P[2][0]*t2*t5-P[2][4]*t2*t7+P[2][1]*t2*t15+P[2][6]*t2*t10-P[2][3]*t2*t22+P[2][5]*t2*t27); + Kfusion[3][0] = t78*(-t86+P[3][0]*t2*t5-P[3][4]*t2*t7+P[3][1]*t2*t15+P[3][6]*t2*t10+P[3][2]*t2*t19+P[3][5]*t2*t27); + Kfusion[4][0] = t78*(-t90+P[4][0]*t2*t5+P[4][1]*t2*t15+P[4][6]*t2*t10+P[4][2]*t2*t19-P[4][3]*t2*t22+P[4][5]*t2*t27); + Kfusion[5][0] = t78*(t61+P[5][0]*t2*t5-P[5][4]*t2*t7+P[5][1]*t2*t15+P[5][6]*t2*t10+P[5][2]*t2*t19-P[5][3]*t2*t22); + Kfusion[6][0] = t78*(t70+P[6][0]*t2*t5-P[6][4]*t2*t7+P[6][1]*t2*t15+P[6][2]*t2*t19-P[6][3]*t2*t22+P[6][5]*t2*t27); + Kfusion[7][0] = t78*(P[7][0]*t2*t5-P[7][4]*t2*t7+P[7][1]*t2*t15+P[7][6]*t2*t10+P[7][2]*t2*t19-P[7][3]*t2*t22+P[7][5]*t2*t27); + Kfusion[8][0] = t78*(P[8][0]*t2*t5-P[8][4]*t2*t7+P[8][1]*t2*t15+P[8][6]*t2*t10+P[8][2]*t2*t19-P[8][3]*t2*t22+P[8][5]*t2*t27); + Kfusion[9][0] = t78*(P[9][0]*t2*t5-P[9][4]*t2*t7+P[9][1]*t2*t15+P[9][6]*t2*t10+P[9][2]*t2*t19-P[9][3]*t2*t22+P[9][5]*t2*t27); + Kfusion[10][0] = t78*(P[10][0]*t2*t5-P[10][4]*t2*t7+P[10][1]*t2*t15+P[10][6]*t2*t10+P[10][2]*t2*t19-P[10][3]*t2*t22+P[10][5]*t2*t27); + Kfusion[11][0] = t78*(P[11][0]*t2*t5-P[11][4]*t2*t7+P[11][1]*t2*t15+P[11][6]*t2*t10+P[11][2]*t2*t19-P[11][3]*t2*t22+P[11][5]*t2*t27); + Kfusion[12][0] = t78*(P[12][0]*t2*t5-P[12][4]*t2*t7+P[12][1]*t2*t15+P[12][6]*t2*t10+P[12][2]*t2*t19-P[12][3]*t2*t22+P[12][5]*t2*t27); + Kfusion[13][0] = t78*(P[13][0]*t2*t5-P[13][4]*t2*t7+P[13][1]*t2*t15+P[13][6]*t2*t10+P[13][2]*t2*t19-P[13][3]*t2*t22+P[13][5]*t2*t27); + Kfusion[14][0] = t78*(P[14][0]*t2*t5-P[14][4]*t2*t7+P[14][1]*t2*t15+P[14][6]*t2*t10+P[14][2]*t2*t19-P[14][3]*t2*t22+P[14][5]*t2*t27); + Kfusion[15][0] = t78*(P[15][0]*t2*t5-P[15][4]*t2*t7+P[15][1]*t2*t15+P[15][6]*t2*t10+P[15][2]*t2*t19-P[15][3]*t2*t22+P[15][5]*t2*t27); + Kfusion[16][0] = t78*(P[16][0]*t2*t5-P[16][4]*t2*t7+P[16][1]*t2*t15+P[16][6]*t2*t10+P[16][2]*t2*t19-P[16][3]*t2*t22+P[16][5]*t2*t27); + Kfusion[17][0] = t78*(P[17][0]*t2*t5-P[17][4]*t2*t7+P[17][1]*t2*t15+P[17][6]*t2*t10+P[17][2]*t2*t19-P[17][3]*t2*t22+P[17][5]*t2*t27); + Kfusion[18][0] = t78*(P[18][0]*t2*t5-P[18][4]*t2*t7+P[18][1]*t2*t15+P[18][6]*t2*t10+P[18][2]*t2*t19-P[18][3]*t2*t22+P[18][5]*t2*t27); + Kfusion[19][0] = t78*(P[19][0]*t2*t5-P[19][4]*t2*t7+P[19][1]*t2*t15+P[19][6]*t2*t10+P[19][2]*t2*t19-P[19][3]*t2*t22+P[19][5]*t2*t27); + Kfusion[20][0] = t78*(P[20][0]*t2*t5-P[20][4]*t2*t7+P[20][1]*t2*t15+P[20][6]*t2*t10+P[20][2]*t2*t19-P[20][3]*t2*t22+P[20][5]*t2*t27); + Kfusion[21][0] = t78*(P[21][0]*t2*t5-P[21][4]*t2*t7+P[21][1]*t2*t15+P[21][6]*t2*t10+P[21][2]*t2*t19-P[21][3]*t2*t22+P[21][5]*t2*t27); + Kfusion[22][0] = t78*(P[22][0]*t2*t5-P[22][4]*t2*t7+P[22][1]*t2*t15+P[22][6]*t2*t10+P[22][2]*t2*t19-P[22][3]*t2*t22+P[22][5]*t2*t27); + Kfusion[23][0] = t78*(P[23][0]*t2*t5-P[23][4]*t2*t7+P[23][1]*t2*t15+P[23][6]*t2*t10+P[23][2]*t2*t19-P[23][3]*t2*t22+P[23][5]*t2*t27); // run innovation consistency checks optflow_test_ratio[0] = sq(_flow_innov[0]) / (sq(math::max(_params.flow_innov_gate, 1.0f)) * _flow_innov_var[0]); @@ -278,91 +256,111 @@ void Ekf::fuseOptFlow() // calculate Y axis observation Jacobian float t2 = 1.0f / range; - float t3 = q0 * q0; - float t4 = q1 * q1; - float t5 = q2 * q2; - float t6 = q3 * q3; - float t7 = q0 * q1 * 2.0f; - float t8 = q0 * q3 * 2.0f; - float t9 = q0 * q2 * 2.0f; - float t10 = q1 * q3 * 2.0f; - H_LOS[1][1] = t2 * (vn * (t9 + t10) + vd * (t3 - t4 - t5 + t6) - ve * (t7 - q2 * q3 * 2.0f)); - H_LOS[1][2] = -t2 * (ve * (t3 - t4 + t5 - t6) + vd * (t7 + q2 * q3 * 2.0f) - vn * (t8 - q1 * q2 * 2.0f)); - H_LOS[1][3] = -t2 * (t3 + t4 - t5 - t6); - H_LOS[1][4] = -t2 * (t8 + q1 * q2 * 2.0f); - H_LOS[1][5] = t2 * (t9 - t10); + H_LOS[1][0] = -t2*(q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f); + H_LOS[1][1] = -t2*(q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f); + H_LOS[1][2] = t2*(q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f); + H_LOS[1][3] = -t2*(q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f); + H_LOS[1][4] = -t2*(q0*q0+q1*q1-q2*q2-q3*q3); + H_LOS[1][5] = -t2*(q0*q3*2.0f+q1*q2*2.0f); + H_LOS[1][6] = t2*(q0*q2*2.0f-q1*q3*2.0f); - // calculate intermediate variables for the X observaton innovatoin variance and klmna gains - t2 = 1.0f / range; - t3 = q0 * q2 * 2.0f; - t4 = q0 * q0; - t5 = q1 * q1; - t6 = q2 * q2; - t7 = q3 * q3; - t8 = q0 * q1 * 2.0f; - t9 = q0 * q3 * 2.0f; - t10 = q1 * q2 * 2.0f; - float t11 = t9 + t10; - float t12 = q1 * q3 * 2.0f; - float t13 = t4 - t5 - t6 + t7; - float t14 = t13 * vd; - float t15 = q2 * q3 * 2.0f; - float t16 = t3 + t12; - float t17 = t16 * vn; - float t18 = t4 - t5 + t6 - t7; - float t19 = t18 * ve; - float t20 = t8 + t15; - float t21 = t20 * vd; - float t22 = t9 - t10; - float t28 = t22 * vn; - float t23 = t19 + t21 - t28; - float t24 = t4 + t5 - t6 - t7; - float t25 = t3 - t12; - float t26 = t8 - t15; - float t29 = t26 * ve; - float t27 = t14 + t17 - t29; - float t30 = P[4][4] * t2 * t11; - float t31 = P[2][4] * t2 * t23; - float t32 = P[3][4] * t2 * t24; - float t56 = P[5][4] * t2 * t25; - float t57 = P[1][4] * t2 * t27; - float t33 = t30 + t31 + t32 - t56 - t57; - float t34 = t2 * t11 * t33; - float t35 = P[4][5] * t2 * t11; - float t36 = P[2][5] * t2 * t23; - float t37 = P[3][5] * t2 * t24; - float t58 = P[5][5] * t2 * t25; - float t59 = P[1][5] * t2 * t27; - float t38 = t35 + t36 + t37 - t58 - t59; - float t39 = P[4][1] * t2 * t11; - float t40 = P[2][1] * t2 * t23; - float t41 = P[3][1] * t2 * t24; - float t55 = P[1][1] * t2 * t27; - float t61 = P[5][1] * t2 * t25; - float t42 = t39 + t40 + t41 - t55 - t61; - float t43 = P[4][2] * t2 * t11; - float t44 = P[2][2] * t2 * t23; - float t45 = P[3][2] * t2 * t24; - float t63 = P[5][2] * t2 * t25; - float t64 = P[1][2] * t2 * t27; - float t46 = t43 + t44 + t45 - t63 - t64; - float t47 = t2 * t23 * t46; - float t48 = P[4][3] * t2 * t11; - float t49 = P[2][3] * t2 * t23; - float t50 = P[3][3] * t2 * t24; - float t65 = P[5][3] * t2 * t25; - float t66 = P[1][3] * t2 * t27; - float t51 = t48 + t49 + t50 - t65 - t66; - float t52 = t2 * t24 * t51; - float t60 = t2 * t25 * t38; - float t62 = t2 * t27 * t42; - float t53 = R_LOS + t34 + t47 + t52 - t60 - t62; - float t54; - - // calculate innovation variance for X axis observation and protect against a badly conditioned calculation - if (t53 >= R_LOS) { - t54 = 1.0f / t53; - _flow_innov_var[1] = t53; + // calculate intermediate variables for the Y observaton innovatoin variance and Kalman gains + float t3 = q3*ve*2.0f; + float t4 = q0*vn*2.0f; + float t11 = q2*vd*2.0f; + float t5 = t3+t4-t11; + float t6 = q0*q3*2.0f; + float t7 = q1*q2*2.0f; + float t8 = t6+t7; + float t9 = q0*q2*2.0f; + float t28 = q1*q3*2.0f; + float t10 = t9-t28; + float t12 = P[0][0]*t2*t5; + float t13 = q3*vd*2.0f; + float t14 = q2*ve*2.0f; + float t15 = q1*vn*2.0f; + float t16 = t13+t14+t15; + float t17 = q0*vd*2.0f; + float t18 = q2*vn*2.0f; + float t29 = q1*ve*2.0f; + float t19 = t17+t18-t29; + float t20 = q1*vd*2.0f; + float t21 = q0*ve*2.0f; + float t30 = q3*vn*2.0f; + float t22 = t20+t21-t30; + float t23 = q0*q0; + float t24 = q1*q1; + float t25 = q2*q2; + float t26 = q3*q3; + float t27 = t23+t24-t25-t26; + float t31 = P[1][1]*t2*t16; + float t32 = P[5][0]*t2*t8; + float t33 = P[1][0]*t2*t16; + float t34 = P[3][0]*t2*t22; + float t35 = P[4][0]*t2*t27; + float t80 = P[6][0]*t2*t10; + float t81 = P[2][0]*t2*t19; + float t36 = t12+t32+t33+t34+t35-t80-t81; + float t37 = t2*t5*t36; + float t38 = P[5][1]*t2*t8; + float t39 = P[0][1]*t2*t5; + float t40 = P[3][1]*t2*t22; + float t41 = P[4][1]*t2*t27; + float t82 = P[6][1]*t2*t10; + float t83 = P[2][1]*t2*t19; + float t42 = t31+t38+t39+t40+t41-t82-t83; + float t43 = t2*t16*t42; + float t44 = P[5][2]*t2*t8; + float t45 = P[0][2]*t2*t5; + float t46 = P[1][2]*t2*t16; + float t47 = P[3][2]*t2*t22; + float t48 = P[4][2]*t2*t27; + float t79 = P[2][2]*t2*t19; + float t84 = P[6][2]*t2*t10; + float t49 = t44+t45+t46+t47+t48-t79-t84; + float t50 = P[5][3]*t2*t8; + float t51 = P[0][3]*t2*t5; + float t52 = P[1][3]*t2*t16; + float t53 = P[3][3]*t2*t22; + float t54 = P[4][3]*t2*t27; + float t86 = P[6][3]*t2*t10; + float t87 = P[2][3]*t2*t19; + float t55 = t50+t51+t52+t53+t54-t86-t87; + float t56 = t2*t22*t55; + float t57 = P[5][4]*t2*t8; + float t58 = P[0][4]*t2*t5; + float t59 = P[1][4]*t2*t16; + float t60 = P[3][4]*t2*t22; + float t61 = P[4][4]*t2*t27; + float t88 = P[6][4]*t2*t10; + float t89 = P[2][4]*t2*t19; + float t62 = t57+t58+t59+t60+t61-t88-t89; + float t63 = t2*t27*t62; + float t64 = P[5][5]*t2*t8; + float t65 = P[0][5]*t2*t5; + float t66 = P[1][5]*t2*t16; + float t67 = P[3][5]*t2*t22; + float t68 = P[4][5]*t2*t27; + float t90 = P[6][5]*t2*t10; + float t91 = P[2][5]*t2*t19; + float t69 = t64+t65+t66+t67+t68-t90-t91; + float t70 = t2*t8*t69; + float t71 = P[5][6]*t2*t8; + float t72 = P[0][6]*t2*t5; + float t73 = P[1][6]*t2*t16; + float t74 = P[3][6]*t2*t22; + float t75 = P[4][6]*t2*t27; + float t92 = P[6][6]*t2*t10; + float t93 = P[2][6]*t2*t19; + float t76 = t71+t72+t73+t74+t75-t92-t93; + float t85 = t2*t19*t49; + float t94 = t2*t10*t76; + float t77 = R_LOS+t37+t43+t56+t63+t70-t85-t94; + float t78; + // calculate innovation variance for Y axis observation and protect against a badly conditioned calculation + if (t77 >= R_LOS) { + t78 = 1.0f / t77; + _flow_innov_var[1] = t77; } else { // we need to reinitialise the covariance matrix and abort this fusion step @@ -370,56 +368,31 @@ void Ekf::fuseOptFlow() return; } - // calculate Kalman gains for X-axis observation - Kfusion[0][1] = -t54 * (P[0][4] * t2 * t11 + P[0][2] * t2 * t23 + P[0][3] * t2 * t24 - P[0][1] * t2 * t27 - P[0][5] * t2 - * t25); - Kfusion[1][1] = -t54 * (-t55 + P[1][4] * t2 * t11 + P[1][2] * t2 * t23 + P[1][3] * t2 * t24 - P[1][5] * t2 * t25); - Kfusion[2][1] = -t54 * (t44 + P[2][4] * t2 * t11 + P[2][3] * t2 * t24 - P[2][1] * t2 * t27 - P[2][5] * t2 * t25); - Kfusion[3][1] = -t54 * (t50 + P[3][4] * t2 * t11 + P[3][2] * t2 * t23 - P[3][1] * t2 * t27 - P[3][5] * t2 * t25); - Kfusion[4][1] = -t54 * (t30 + P[4][2] * t2 * t23 + P[4][3] * t2 * t24 - P[4][1] * t2 * t27 - P[4][5] * t2 * t25); - Kfusion[5][1] = -t54 * (-t58 + P[5][4] * t2 * t11 + P[5][2] * t2 * t23 + P[5][3] * t2 * t24 - P[5][1] * t2 * t27); - Kfusion[6][1] = -t54 * (P[6][4] * t2 * t11 + P[6][2] * t2 * t23 + P[6][3] * t2 * t24 - P[6][1] * t2 * t27 - P[6][5] * t2 - * t25); - Kfusion[7][1] = -t54 * (P[7][4] * t2 * t11 + P[7][2] * t2 * t23 + P[7][3] * t2 * t24 - P[7][1] * t2 * t27 - P[7][5] * t2 - * t25); - Kfusion[8][1] = -t54 * (P[8][4] * t2 * t11 + P[8][2] * t2 * t23 + P[8][3] * t2 * t24 - P[8][1] * t2 * t27 - P[8][5] * t2 - * t25); - Kfusion[9][1] = -t54 * (P[9][4] * t2 * t11 + P[9][2] * t2 * t23 + P[9][3] * t2 * t24 - P[9][1] * t2 * t27 - P[9][5] * t2 - * t25); - Kfusion[10][1] = -t54 * (P[10][4] * t2 * t11 + P[10][2] * t2 * t23 + P[10][3] * t2 * t24 - P[10][1] * t2 * t27 - - P[10][5] * t2 * t25); - Kfusion[11][1] = -t54 * (P[11][4] * t2 * t11 + P[11][2] * t2 * t23 + P[11][3] * t2 * t24 - P[11][1] * t2 * t27 - - P[11][5] * t2 * t25); - Kfusion[12][1] = -t54 * (P[12][4] * t2 * t11 + P[12][2] * t2 * t23 + P[12][3] * t2 * t24 - P[12][1] * t2 * t27 - - P[12][5] * t2 * t25); - Kfusion[13][1] = -t54 * (P[13][4] * t2 * t11 + P[13][2] * t2 * t23 + P[13][3] * t2 * t24 - P[13][1] * t2 * t27 - - P[13][5] * t2 * t25); - Kfusion[14][1] = -t54 * (P[14][4] * t2 * t11 + P[14][2] * t2 * t23 + P[14][3] * t2 * t24 - P[14][1] * t2 * t27 - - P[14][5] * t2 * t25); - Kfusion[15][1] = -t54 * (P[15][4] * t2 * t11 + P[15][2] * t2 * t23 + P[15][3] * t2 * t24 - P[15][1] * t2 * t27 - - P[15][5] * t2 * t25); - - if (_control_status.flags.mag_3D) { - Kfusion[16][1] = -t54 * (P[16][4] * t2 * t11 + P[16][2] * t2 * t23 + P[16][3] * t2 * t24 - P[16][1] * t2 * t27 - - P[16][5] * t2 * t25); - Kfusion[17][1] = -t54 * (P[17][4] * t2 * t11 + P[17][2] * t2 * t23 + P[17][3] * t2 * t24 - P[17][1] * t2 * t27 - - P[17][5] * t2 * t25); - Kfusion[18][1] = -t54 * (P[18][4] * t2 * t11 + P[18][2] * t2 * t23 + P[18][3] * t2 * t24 - P[18][1] * t2 * t27 - - P[18][5] * t2 * t25); - Kfusion[19][1] = -t54 * (P[19][4] * t2 * t11 + P[19][2] * t2 * t23 + P[19][3] * t2 * t24 - P[19][1] * t2 * t27 - - P[19][5] * t2 * t25); - Kfusion[20][1] = -t54 * (P[20][4] * t2 * t11 + P[20][2] * t2 * t23 + P[20][3] * t2 * t24 - P[20][1] * t2 * t27 - - P[20][5] * t2 * t25); - Kfusion[21][1] = -t54 * (P[21][4] * t2 * t11 + P[21][2] * t2 * t23 + P[21][3] * t2 * t24 - P[21][1] * t2 * t27 - - P[21][5] * t2 * t25); - } - - if (_control_status.flags.wind) { - Kfusion[22][1] = -t54 * (P[22][4] * t2 * t11 + P[22][2] * t2 * t23 + P[22][3] * t2 * t24 - P[22][1] * t2 * t27 - - P[22][5] * t2 * t25); - Kfusion[23][1] = -t54 * (P[23][4] * t2 * t11 + P[23][2] * t2 * t23 + P[23][3] * t2 * t24 - P[23][1] * t2 * t27 - - P[23][5] * t2 * t25); - } + // calculate Kalman gains for Y-axis observation + Kfusion[0][1] = -t78*(t12+P[0][5]*t2*t8-P[0][6]*t2*t10+P[0][1]*t2*t16-P[0][2]*t2*t19+P[0][3]*t2*t22+P[0][4]*t2*t27); + Kfusion[1][1] = -t78*(t31+P[1][0]*t2*t5+P[1][5]*t2*t8-P[1][6]*t2*t10-P[1][2]*t2*t19+P[1][3]*t2*t22+P[1][4]*t2*t27); + Kfusion[2][1] = -t78*(-t79+P[2][0]*t2*t5+P[2][5]*t2*t8-P[2][6]*t2*t10+P[2][1]*t2*t16+P[2][3]*t2*t22+P[2][4]*t2*t27); + Kfusion[3][1] = -t78*(t53+P[3][0]*t2*t5+P[3][5]*t2*t8-P[3][6]*t2*t10+P[3][1]*t2*t16-P[3][2]*t2*t19+P[3][4]*t2*t27); + Kfusion[4][1] = -t78*(t61+P[4][0]*t2*t5+P[4][5]*t2*t8-P[4][6]*t2*t10+P[4][1]*t2*t16-P[4][2]*t2*t19+P[4][3]*t2*t22); + Kfusion[5][1] = -t78*(t64+P[5][0]*t2*t5-P[5][6]*t2*t10+P[5][1]*t2*t16-P[5][2]*t2*t19+P[5][3]*t2*t22+P[5][4]*t2*t27); + Kfusion[6][1] = -t78*(-t92+P[6][0]*t2*t5+P[6][5]*t2*t8+P[6][1]*t2*t16-P[6][2]*t2*t19+P[6][3]*t2*t22+P[6][4]*t2*t27); + Kfusion[7][1] = -t78*(P[7][0]*t2*t5+P[7][5]*t2*t8-P[7][6]*t2*t10+P[7][1]*t2*t16-P[7][2]*t2*t19+P[7][3]*t2*t22+P[7][4]*t2*t27); + Kfusion[8][1] = -t78*(P[8][0]*t2*t5+P[8][5]*t2*t8-P[8][6]*t2*t10+P[8][1]*t2*t16-P[8][2]*t2*t19+P[8][3]*t2*t22+P[8][4]*t2*t27); + Kfusion[9][1] = -t78*(P[9][0]*t2*t5+P[9][5]*t2*t8-P[9][6]*t2*t10+P[9][1]*t2*t16-P[9][2]*t2*t19+P[9][3]*t2*t22+P[9][4]*t2*t27); + Kfusion[10][1] = -t78*(P[10][0]*t2*t5+P[10][5]*t2*t8-P[10][6]*t2*t10+P[10][1]*t2*t16-P[10][2]*t2*t19+P[10][3]*t2*t22+P[10][4]*t2*t27); + Kfusion[11][1] = -t78*(P[11][0]*t2*t5+P[11][5]*t2*t8-P[11][6]*t2*t10+P[11][1]*t2*t16-P[11][2]*t2*t19+P[11][3]*t2*t22+P[11][4]*t2*t27); + Kfusion[12][1] = -t78*(P[12][0]*t2*t5+P[12][5]*t2*t8-P[12][6]*t2*t10+P[12][1]*t2*t16-P[12][2]*t2*t19+P[12][3]*t2*t22+P[12][4]*t2*t27); + Kfusion[13][1] = -t78*(P[13][0]*t2*t5+P[13][5]*t2*t8-P[13][6]*t2*t10+P[13][1]*t2*t16-P[13][2]*t2*t19+P[13][3]*t2*t22+P[13][4]*t2*t27); + Kfusion[14][1] = -t78*(P[14][0]*t2*t5+P[14][5]*t2*t8-P[14][6]*t2*t10+P[14][1]*t2*t16-P[14][2]*t2*t19+P[14][3]*t2*t22+P[14][4]*t2*t27); + Kfusion[15][1] = -t78*(P[15][0]*t2*t5+P[15][5]*t2*t8-P[15][6]*t2*t10+P[15][1]*t2*t16-P[15][2]*t2*t19+P[15][3]*t2*t22+P[15][4]*t2*t27); + Kfusion[16][1] = -t78*(P[16][0]*t2*t5+P[16][5]*t2*t8-P[16][6]*t2*t10+P[16][1]*t2*t16-P[16][2]*t2*t19+P[16][3]*t2*t22+P[16][4]*t2*t27); + Kfusion[17][1] = -t78*(P[17][0]*t2*t5+P[17][5]*t2*t8-P[17][6]*t2*t10+P[17][1]*t2*t16-P[17][2]*t2*t19+P[17][3]*t2*t22+P[17][4]*t2*t27); + Kfusion[18][1] = -t78*(P[18][0]*t2*t5+P[18][5]*t2*t8-P[18][6]*t2*t10+P[18][1]*t2*t16-P[18][2]*t2*t19+P[18][3]*t2*t22+P[18][4]*t2*t27); + Kfusion[19][1] = -t78*(P[19][0]*t2*t5+P[19][5]*t2*t8-P[19][6]*t2*t10+P[19][1]*t2*t16-P[19][2]*t2*t19+P[19][3]*t2*t22+P[19][4]*t2*t27); + Kfusion[20][1] = -t78*(P[20][0]*t2*t5+P[20][5]*t2*t8-P[20][6]*t2*t10+P[20][1]*t2*t16-P[20][2]*t2*t19+P[20][3]*t2*t22+P[20][4]*t2*t27); + Kfusion[21][1] = -t78*(P[21][0]*t2*t5+P[21][5]*t2*t8-P[21][6]*t2*t10+P[21][1]*t2*t16-P[21][2]*t2*t19+P[21][3]*t2*t22+P[21][4]*t2*t27); + Kfusion[22][1] = -t78*(P[22][0]*t2*t5+P[22][5]*t2*t8-P[22][6]*t2*t10+P[22][1]*t2*t16-P[22][2]*t2*t19+P[22][3]*t2*t22+P[22][4]*t2*t27); + Kfusion[23][1] = -t78*(P[23][0]*t2*t5+P[23][5]*t2*t8-P[23][6]*t2*t10+P[23][1]*t2*t16-P[23][2]*t2*t19+P[23][3]*t2*t22+P[23][4]*t2*t27); // run innovation consistency check optflow_test_ratio[1] = sq(_flow_innov[1]) / (sq(math::max(_params.flow_innov_gate, 1.0f)) * _flow_innov_var[1]); @@ -436,9 +409,6 @@ void Ekf::fuseOptFlow() for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) { - // by definition our error state is zero at the time of fusion - _state.ang_error.setZero(); - // copy the Kalman gain vector for the axis we are fusing float gain[24]; @@ -446,23 +416,11 @@ void Ekf::fuseOptFlow() gain[row] = Kfusion[row][obs_index]; } - // Update the state vector - fuse(gain, _flow_innov[obs_index]); - - // correct the quaternion using the attitude error state - Quaternion q_correction; - q_correction.from_axis_angle(_state.ang_error); - _state.quat_nominal = q_correction * _state.quat_nominal; - _state.quat_nominal.normalize(); - - // reset attitude error to zero after the correction has been applied - _state.ang_error.setZero(); - // apply covariance correction via P_new = (I -K*H)*P // first calculate expression for KHP // then calculate P - KHP for (unsigned row = 0; row < _k_num_states; row++) { - for (unsigned column = 0; column <= 5; column++) { + for (unsigned column = 0; column <= 6; column++) { KH[row][column] = gain[row] * H_LOS[obs_index][column]; } } @@ -475,20 +433,52 @@ void Ekf::fuseOptFlow() tmp += KH[row][3] * P[3][column]; tmp += KH[row][4] * P[4][column]; tmp += KH[row][5] * P[5][column]; + tmp += KH[row][6] * P[6][column]; KHP[row][column] = tmp; } } - for (unsigned row = 0; row < _k_num_states; row++) { - for (unsigned column = 0; column < _k_num_states; column++) { - P[row][column] -= KHP[row][column]; + // if the covariance correction will result in a negative variance, then + // the covariance marix is unhealthy and must be corrected + bool healthy = true; + _fault_status.flags.bad_optflow_X = false; + _fault_status.flags.bad_optflow_Y = false; + for (int i = 0; i < _k_num_states; i++) { + if (P[i][i] < KHP[i][i]) { + // zero rows and columns + zeroRows(P,i,i); + zeroCols(P,i,i); + + //flag as unhealthy + healthy = false; + + // update individual measurement health status + if (obs_index == 0) { + _fault_status.flags.bad_optflow_X = true; + } else if (obs_index == 1) { + _fault_status.flags.bad_optflow_Y = true; + } } } - _time_last_of_fuse = _time_last_imu; - _gps_check_fail_status.value = 0; - makeSymmetrical(); - limitCov(); + // only apply covariance and state corrrections if healthy + if (healthy) { + // apply the covariance corrections + for (unsigned row = 0; row < _k_num_states; row++) { + for (unsigned column = 0; column < _k_num_states; column++) { + P[row][column] = P[row][column] - KHP[row][column]; + } + } + + // correct the covariance marix for gross errors + fixCovarianceErrors(); + + // apply the state corrections + fuse(gain, _flow_innov[obs_index]); + + _time_last_of_fuse = _time_last_imu; + _gps_check_fail_status.value = 0; + } } } @@ -543,3 +533,27 @@ void Ekf::calcOptFlowBias() _delta_time_of = 0.0f; } } + +// calculate the measurement variance for the optical flow sensor (rad/sec)^2 +float Ekf::calcOptFlowMeasVar() +{ + // calculate the observation noise variance - scaling noise linearly across flow quality range + float R_LOS_best = fmaxf(_params.flow_noise, 0.05f); + float R_LOS_worst = fmaxf(_params.flow_noise_qual_min, 0.05f); + + // calculate a weighting that varies between 1 when flow quality is best and 0 when flow quality is worst + float weighting = (255.0f - (float)_params.flow_qual_min); + + if (weighting >= 1.0f) { + weighting = math::constrain(((float)_flow_sample_delayed.quality - (float)_params.flow_qual_min) / weighting, 0.0f, + 1.0f); + + } else { + weighting = 0.0f; + } + + // take the weighted average of the observation noie for the best and wort flow quality + float R_LOS = sq(R_LOS_best * weighting + R_LOS_worst * (1.0f - weighting)); + + return R_LOS; +} diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index 75165a1cf5..433b6eeb1a 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -153,7 +153,7 @@ void Ekf::fuseVelPosHeight() for (unsigned obs_index = 0; obs_index < 6; obs_index++) { if (fuse_map[obs_index]) { // compute the innovation variance SK = HPH + R - unsigned state_index = obs_index + 3; // we start with vx and this is the 4. state + unsigned state_index = obs_index + 4; // we start with vx and this is the 4. state _vel_pos_innov_var[obs_index] = P[state_index][state_index] + R[obs_index]; // Compute the ratio of innovation to gate size _vel_pos_test_ratio[obs_index] = sq(_vel_pos_innov[obs_index]) / (sq(gate_size[obs_index]) * @@ -163,26 +163,25 @@ void Ekf::fuseVelPosHeight() // check position, velocity and height innovations // treat 3D velocity, 2D position and height as separate sensors - // always pass position checks if using synthetic position measurements + // always pass position checks if using synthetic position measurements or yet to complete tilt alignment + // always pass height checks if yet to complete tilt alignment bool vel_check_pass = (_vel_pos_test_ratio[0] <= 1.0f) && (_vel_pos_test_ratio[1] <= 1.0f) && (_vel_pos_test_ratio[2] <= 1.0f); innov_check_pass_map[2] = innov_check_pass_map[1] = innov_check_pass_map[0] = vel_check_pass; bool using_synthetic_measurements = !_control_status.flags.gps && !_control_status.flags.opt_flow; bool pos_check_pass = ((_vel_pos_test_ratio[3] <= 1.0f) && (_vel_pos_test_ratio[4] <= 1.0f)) - || using_synthetic_measurements; + || using_synthetic_measurements || !_control_status.flags.tilt_align; innov_check_pass_map[4] = innov_check_pass_map[3] = pos_check_pass; - innov_check_pass_map[5] = (_vel_pos_test_ratio[5] <= 1.0f); + innov_check_pass_map[5] = (_vel_pos_test_ratio[5] <= 1.0f) || !_control_status.flags.tilt_align; // record the successful velocity fusion time if (vel_check_pass && _fuse_hor_vel) { _time_last_vel_fuse = _time_last_imu; - _tilt_err_vec.setZero(); } // record the successful position fusion time if (pos_check_pass && _fuse_pos) { _time_last_pos_fuse = _time_last_imu; - _tilt_err_vec.setZero(); } // record the successful height fusion time @@ -196,55 +195,13 @@ void Ekf::fuseVelPosHeight() continue; } - unsigned state_index = obs_index + 3; // we start with vx and this is the 4. state + unsigned state_index = obs_index + 4; // we start with vx and this is the 4. state // calculate kalman gain K = PHS, where S = 1/innovation variance - for (int row = 0; row <= 15; row++) { + for (int row = 0; row < _k_num_states; row++) { Kfusion[row] = P[row][state_index] / _vel_pos_innov_var[obs_index]; } - // only update magnetic field states if we are fusing 3-axis observations - if (_control_status.flags.mag_3D) { - for (int row = 16; row <= 21; row++) { - Kfusion[row] = P[row][state_index] / _vel_pos_innov_var[obs_index]; - } - - } else { - for (int row = 16; row <= 21; row++) { - Kfusion[row] = 0.0f; - } - } - - // only update wind states if we are doing wind estimation - if (_control_status.flags.wind) { - for (int row = 22; row <= 23; row++) { - Kfusion[row] = P[row][state_index] / _vel_pos_innov_var[obs_index]; - } - - } else { - for (int row = 22; row <= 23; row++) { - Kfusion[row] = 0.0f; - } - } - - // sum the attitude error from velocity and position fusion only - // used as a metric for convergence monitoring - if (obs_index != 5) { - _tilt_err_vec += _state.ang_error; - } - - // by definition the angle error state is zero at the fusion time - _state.ang_error.setZero(); - - // fuse the observation - fuse(Kfusion, _vel_pos_innov[obs_index]); - - // correct the nominal quaternion - Quaternion dq; - dq.from_axis_angle(_state.ang_error); - _state.quat_nominal = dq * _state.quat_nominal; - _state.quat_nominal.normalize(); - // update covarinace matrix via Pnew = (I - KH)P for (unsigned row = 0; row < _k_num_states; row++) { for (unsigned column = 0; column < _k_num_states; column++) { @@ -252,13 +209,64 @@ void Ekf::fuseVelPosHeight() } } - for (unsigned row = 0; row < _k_num_states; row++) { - for (unsigned column = 0; column < _k_num_states; column++) { - P[row][column] = P[row][column] - KHP[row][column]; + // if the covariance correction will result in a negative variance, then + // the covariance marix is unhealthy and must be corrected + bool healthy = true; + for (int i = 0; i < _k_num_states; i++) { + if (P[i][i] < KHP[i][i]) { + // zero rows and columns + zeroRows(P,i,i); + zeroCols(P,i,i); + + //flag as unhealthy + healthy = false; + + // update individual measurement health status + if (obs_index == 0) { + _fault_status.flags.bad_vel_N = true; + } else if (obs_index == 1) { + _fault_status.flags.bad_vel_E = true; + } else if (obs_index == 2) { + _fault_status.flags.bad_vel_D = true; + } else if (obs_index == 3) { + _fault_status.flags.bad_pos_N = true; + } else if (obs_index == 4) { + _fault_status.flags.bad_pos_E = true; + } else if (obs_index == 5) { + _fault_status.flags.bad_pos_D = true; + } + } else { + // update individual measurement health status + if (obs_index == 0) { + _fault_status.flags.bad_vel_N = false; + } else if (obs_index == 1) { + _fault_status.flags.bad_vel_E = false; + } else if (obs_index == 2) { + _fault_status.flags.bad_vel_D = false; + } else if (obs_index == 3) { + _fault_status.flags.bad_pos_N = false; + } else if (obs_index == 4) { + _fault_status.flags.bad_pos_E = false; + } else if (obs_index == 5) { + _fault_status.flags.bad_pos_D = false; + } } } - makeSymmetrical(); - limitCov(); + // only apply covariance and state corrrections if healthy + if (healthy) { + // apply the covariance corrections + for (unsigned row = 0; row < _k_num_states; row++) { + for (unsigned column = 0; column < _k_num_states; column++) { + P[row][column] = P[row][column] - KHP[row][column]; + } + } + + // correct the covariance marix for gross errors + fixCovarianceErrors(); + + // apply the state corrections + fuse(Kfusion, _vel_pos_innov[obs_index]); + } } } diff --git a/matlab/generated/Inertial Nav EKF/Airspeed Fusion.txt b/matlab/generated/Inertial Nav EKF/Airspeed Fusion.txt index c84d841b0d..8cc6b0c703 100644 --- a/matlab/generated/Inertial Nav EKF/Airspeed Fusion.txt +++ b/matlab/generated/Inertial Nav EKF/Airspeed Fusion.txt @@ -4,15 +4,15 @@ // intermediate variable from algebraic optimisation float SH_TAS[3]; -SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd))); -SH_TAS[1] = (SH_TAS[0]*(2*ve - 2*vwe))/2; -SH_TAS[2] = (SH_TAS[0]*(2*vn - 2*vwn))/2; +SH_TAS[0] = 1.0f/v_tas_pred; +SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2.0f*vwe))*0.5f; +SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2.0f*vwn))*0.5f; // observation jacobian float H_TAS[24]; -H_TAS[3] = SH_TAS[2]; -H_TAS[4] = SH_TAS[1]; -H_TAS[5] = vd*SH_TAS[0]; +H_TAS[4] = SH_TAS[2]; +H_TAS[5] = SH_TAS[1]; +H_TAS[6] = vd*SH_TAS[0]; H_TAS[22] = -SH_TAS[2]; H_TAS[23] = -SH_TAS[1]; @@ -20,31 +20,31 @@ H_TAS[23] = -SH_TAS[1]; // intermediate variables - note SK_TAS[0] is 1/(innovation variance) float SK_TAS[2]; -SK_TAS[0] = 1/(R_TAS + SH_TAS[2]*(P[3][3]*SH_TAS[2] + P[4][3]*SH_TAS[1] - P[22][3]*SH_TAS[2] - P[23][3]*SH_TAS[1] + P[5][3]*vd*SH_TAS[0]) + SH_TAS[1]*(P[3][4]*SH_TAS[2] + P[4][4]*SH_TAS[1] - P[22][4]*SH_TAS[2] - P[23][4]*SH_TAS[1] + P[5][4]*vd*SH_TAS[0]) - SH_TAS[2]*(P[3][22]*SH_TAS[2] + P[4][22]*SH_TAS[1] - P[22][22]*SH_TAS[2] - P[23][22]*SH_TAS[1] + P[5][22]*vd*SH_TAS[0]) - SH_TAS[1]*(P[3][23]*SH_TAS[2] + P[4][23]*SH_TAS[1] - P[22][23]*SH_TAS[2] - P[23][23]*SH_TAS[1] + P[5][23]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[3][5]*SH_TAS[2] + P[4][5]*SH_TAS[1] - P[22][5]*SH_TAS[2] - P[23][5]*SH_TAS[1] + P[5][5]*vd*SH_TAS[0])); +SK_TAS[0] = 1/(R_TAS + SH_TAS[2]*(P[4][4]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[22][4]*SH_TAS[2] - P[23][4]*SH_TAS[1] + P[6][4]*vd*SH_TAS[0]) + SH_TAS[1]*(P[4][5]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[22][5]*SH_TAS[2] - P[23][5]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]) - SH_TAS[2]*(P[4][22]*SH_TAS[2] + P[5][22]*SH_TAS[1] - P[22][22]*SH_TAS[2] - P[23][22]*SH_TAS[1] + P[6][22]*vd*SH_TAS[0]) - SH_TAS[1]*(P[4][23]*SH_TAS[2] + P[5][23]*SH_TAS[1] - P[22][23]*SH_TAS[2] - P[23][23]*SH_TAS[1] + P[6][23]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[4][6]*SH_TAS[2] + P[5][6]*SH_TAS[1] - P[22][6]*SH_TAS[2] - P[23][6]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0])); SK_TAS[1] = SH_TAS[1]; float Kfusion[24]; -Kfusion[0] = SK_TAS[0]*(P[0][3]*SH_TAS[2] - P[0][22]*SH_TAS[2] + P[0][4]*SK_TAS[1] - P[0][23]*SK_TAS[1] + P[0][5]*vd*SH_TAS[0]); -Kfusion[1] = SK_TAS[0]*(P[1][3]*SH_TAS[2] - P[1][22]*SH_TAS[2] + P[1][4]*SK_TAS[1] - P[1][23]*SK_TAS[1] + P[1][5]*vd*SH_TAS[0]); -Kfusion[2] = SK_TAS[0]*(P[2][3]*SH_TAS[2] - P[2][22]*SH_TAS[2] + P[2][4]*SK_TAS[1] - P[2][23]*SK_TAS[1] + P[2][5]*vd*SH_TAS[0]); -Kfusion[3] = SK_TAS[0]*(P[3][3]*SH_TAS[2] - P[3][22]*SH_TAS[2] + P[3][4]*SK_TAS[1] - P[3][23]*SK_TAS[1] + P[3][5]*vd*SH_TAS[0]); -Kfusion[4] = SK_TAS[0]*(P[4][3]*SH_TAS[2] - P[4][22]*SH_TAS[2] + P[4][4]*SK_TAS[1] - P[4][23]*SK_TAS[1] + P[4][5]*vd*SH_TAS[0]); -Kfusion[5] = SK_TAS[0]*(P[5][3]*SH_TAS[2] - P[5][22]*SH_TAS[2] + P[5][4]*SK_TAS[1] - P[5][23]*SK_TAS[1] + P[5][5]*vd*SH_TAS[0]); -Kfusion[6] = SK_TAS[0]*(P[6][3]*SH_TAS[2] - P[6][22]*SH_TAS[2] + P[6][4]*SK_TAS[1] - P[6][23]*SK_TAS[1] + P[6][5]*vd*SH_TAS[0]); -Kfusion[7] = SK_TAS[0]*(P[7][3]*SH_TAS[2] - P[7][22]*SH_TAS[2] + P[7][4]*SK_TAS[1] - P[7][23]*SK_TAS[1] + P[7][5]*vd*SH_TAS[0]); -Kfusion[8] = SK_TAS[0]*(P[8][3]*SH_TAS[2] - P[8][22]*SH_TAS[2] + P[8][4]*SK_TAS[1] - P[8][23]*SK_TAS[1] + P[8][5]*vd*SH_TAS[0]); -Kfusion[9] = SK_TAS[0]*(P[9][3]*SH_TAS[2] - P[9][22]*SH_TAS[2] + P[9][4]*SK_TAS[1] - P[9][23]*SK_TAS[1] + P[9][5]*vd*SH_TAS[0]); -Kfusion[10] = SK_TAS[0]*(P[10][3]*SH_TAS[2] - P[10][22]*SH_TAS[2] + P[10][4]*SK_TAS[1] - P[10][23]*SK_TAS[1] + P[10][5]*vd*SH_TAS[0]); -Kfusion[11] = SK_TAS[0]*(P[11][3]*SH_TAS[2] - P[11][22]*SH_TAS[2] + P[11][4]*SK_TAS[1] - P[11][23]*SK_TAS[1] + P[11][5]*vd*SH_TAS[0]); -Kfusion[12] = SK_TAS[0]*(P[12][3]*SH_TAS[2] - P[12][22]*SH_TAS[2] + P[12][4]*SK_TAS[1] - P[12][23]*SK_TAS[1] + P[12][5]*vd*SH_TAS[0]); -Kfusion[13] = SK_TAS[0]*(P[13][3]*SH_TAS[2] - P[13][22]*SH_TAS[2] + P[13][4]*SK_TAS[1] - P[13][23]*SK_TAS[1] + P[13][5]*vd*SH_TAS[0]); -Kfusion[14] = SK_TAS[0]*(P[14][3]*SH_TAS[2] - P[14][22]*SH_TAS[2] + P[14][4]*SK_TAS[1] - P[14][23]*SK_TAS[1] + P[14][5]*vd*SH_TAS[0]); -Kfusion[15] = SK_TAS[0]*(P[15][3]*SH_TAS[2] - P[15][22]*SH_TAS[2] + P[15][4]*SK_TAS[1] - P[15][23]*SK_TAS[1] + P[15][5]*vd*SH_TAS[0]); -Kfusion[16] = SK_TAS[0]*(P[16][3]*SH_TAS[2] - P[16][22]*SH_TAS[2] + P[16][4]*SK_TAS[1] - P[16][23]*SK_TAS[1] + P[16][5]*vd*SH_TAS[0]); -Kfusion[17] = SK_TAS[0]*(P[17][3]*SH_TAS[2] - P[17][22]*SH_TAS[2] + P[17][4]*SK_TAS[1] - P[17][23]*SK_TAS[1] + P[17][5]*vd*SH_TAS[0]); -Kfusion[18] = SK_TAS[0]*(P[18][3]*SH_TAS[2] - P[18][22]*SH_TAS[2] + P[18][4]*SK_TAS[1] - P[18][23]*SK_TAS[1] + P[18][5]*vd*SH_TAS[0]); -Kfusion[19] = SK_TAS[0]*(P[19][3]*SH_TAS[2] - P[19][22]*SH_TAS[2] + P[19][4]*SK_TAS[1] - P[19][23]*SK_TAS[1] + P[19][5]*vd*SH_TAS[0]); -Kfusion[20] = SK_TAS[0]*(P[20][3]*SH_TAS[2] - P[20][22]*SH_TAS[2] + P[20][4]*SK_TAS[1] - P[20][23]*SK_TAS[1] + P[20][5]*vd*SH_TAS[0]); -Kfusion[21] = SK_TAS[0]*(P[21][3]*SH_TAS[2] - P[21][22]*SH_TAS[2] + P[21][4]*SK_TAS[1] - P[21][23]*SK_TAS[1] + P[21][5]*vd*SH_TAS[0]); -Kfusion[22] = SK_TAS[0]*(P[22][3]*SH_TAS[2] - P[22][22]*SH_TAS[2] + P[22][4]*SK_TAS[1] - P[22][23]*SK_TAS[1] + P[22][5]*vd*SH_TAS[0]); -Kfusion[23] = SK_TAS[0]*(P[23][3]*SH_TAS[2] - P[23][22]*SH_TAS[2] + P[23][4]*SK_TAS[1] - P[23][23]*SK_TAS[1] + P[23][5]*vd*SH_TAS[0]); \ No newline at end of file +Kfusion[0] = SK_TAS[0]*(P[0][4]*SH_TAS[2] - P[0][22]*SH_TAS[2] + P[0][5]*SK_TAS[1] - P[0][23]*SK_TAS[1] + P[0][6]*vd*SH_TAS[0]); +Kfusion[1] = SK_TAS[0]*(P[1][4]*SH_TAS[2] - P[1][22]*SH_TAS[2] + P[1][5]*SK_TAS[1] - P[1][23]*SK_TAS[1] + P[1][6]*vd*SH_TAS[0]); +Kfusion[2] = SK_TAS[0]*(P[2][4]*SH_TAS[2] - P[2][22]*SH_TAS[2] + P[2][5]*SK_TAS[1] - P[2][23]*SK_TAS[1] + P[2][6]*vd*SH_TAS[0]); +Kfusion[3] = SK_TAS[0]*(P[3][4]*SH_TAS[2] - P[3][22]*SH_TAS[2] + P[3][5]*SK_TAS[1] - P[3][23]*SK_TAS[1] + P[3][6]*vd*SH_TAS[0]); +Kfusion[4] = SK_TAS[0]*(P[4][4]*SH_TAS[2] - P[4][22]*SH_TAS[2] + P[4][5]*SK_TAS[1] - P[4][23]*SK_TAS[1] + P[4][6]*vd*SH_TAS[0]); +Kfusion[5] = SK_TAS[0]*(P[5][4]*SH_TAS[2] - P[5][22]*SH_TAS[2] + P[5][5]*SK_TAS[1] - P[5][23]*SK_TAS[1] + P[5][6]*vd*SH_TAS[0]); +Kfusion[6] = SK_TAS[0]*(P[6][4]*SH_TAS[2] - P[6][22]*SH_TAS[2] + P[6][5]*SK_TAS[1] - P[6][23]*SK_TAS[1] + P[6][6]*vd*SH_TAS[0]); +Kfusion[7] = SK_TAS[0]*(P[7][4]*SH_TAS[2] - P[7][22]*SH_TAS[2] + P[7][5]*SK_TAS[1] - P[7][23]*SK_TAS[1] + P[7][6]*vd*SH_TAS[0]); +Kfusion[8] = SK_TAS[0]*(P[8][4]*SH_TAS[2] - P[8][22]*SH_TAS[2] + P[8][5]*SK_TAS[1] - P[8][23]*SK_TAS[1] + P[8][6]*vd*SH_TAS[0]); +Kfusion[9] = SK_TAS[0]*(P[9][4]*SH_TAS[2] - P[9][22]*SH_TAS[2] + P[9][5]*SK_TAS[1] - P[9][23]*SK_TAS[1] + P[9][6]*vd*SH_TAS[0]); +Kfusion[10] = SK_TAS[0]*(P[10][4]*SH_TAS[2] - P[10][22]*SH_TAS[2] + P[10][5]*SK_TAS[1] - P[10][23]*SK_TAS[1] + P[10][6]*vd*SH_TAS[0]); +Kfusion[11] = SK_TAS[0]*(P[11][4]*SH_TAS[2] - P[11][22]*SH_TAS[2] + P[11][5]*SK_TAS[1] - P[11][23]*SK_TAS[1] + P[11][6]*vd*SH_TAS[0]); +Kfusion[12] = SK_TAS[0]*(P[12][4]*SH_TAS[2] - P[12][22]*SH_TAS[2] + P[12][5]*SK_TAS[1] - P[12][23]*SK_TAS[1] + P[12][6]*vd*SH_TAS[0]); +Kfusion[13] = SK_TAS[0]*(P[13][4]*SH_TAS[2] - P[13][22]*SH_TAS[2] + P[13][5]*SK_TAS[1] - P[13][23]*SK_TAS[1] + P[13][6]*vd*SH_TAS[0]); +Kfusion[14] = SK_TAS[0]*(P[14][4]*SH_TAS[2] - P[14][22]*SH_TAS[2] + P[14][5]*SK_TAS[1] - P[14][23]*SK_TAS[1] + P[14][6]*vd*SH_TAS[0]); +Kfusion[15] = SK_TAS[0]*(P[15][4]*SH_TAS[2] - P[15][22]*SH_TAS[2] + P[15][5]*SK_TAS[1] - P[15][23]*SK_TAS[1] + P[15][6]*vd*SH_TAS[0]); +Kfusion[16] = SK_TAS[0]*(P[16][4]*SH_TAS[2] - P[16][22]*SH_TAS[2] + P[16][5]*SK_TAS[1] - P[16][23]*SK_TAS[1] + P[16][6]*vd*SH_TAS[0]); +Kfusion[17] = SK_TAS[0]*(P[17][4]*SH_TAS[2] - P[17][22]*SH_TAS[2] + P[17][5]*SK_TAS[1] - P[17][23]*SK_TAS[1] + P[17][6]*vd*SH_TAS[0]); +Kfusion[18] = SK_TAS[0]*(P[18][4]*SH_TAS[2] - P[18][22]*SH_TAS[2] + P[18][5]*SK_TAS[1] - P[18][23]*SK_TAS[1] + P[18][6]*vd*SH_TAS[0]); +Kfusion[19] = SK_TAS[0]*(P[19][4]*SH_TAS[2] - P[19][22]*SH_TAS[2] + P[19][5]*SK_TAS[1] - P[19][23]*SK_TAS[1] + P[19][6]*vd*SH_TAS[0]); +Kfusion[20] = SK_TAS[0]*(P[20][4]*SH_TAS[2] - P[20][22]*SH_TAS[2] + P[20][5]*SK_TAS[1] - P[20][23]*SK_TAS[1] + P[20][6]*vd*SH_TAS[0]); +Kfusion[21] = SK_TAS[0]*(P[21][4]*SH_TAS[2] - P[21][22]*SH_TAS[2] + P[21][5]*SK_TAS[1] - P[21][23]*SK_TAS[1] + P[21][6]*vd*SH_TAS[0]); +Kfusion[22] = SK_TAS[0]*(P[22][4]*SH_TAS[2] - P[22][22]*SH_TAS[2] + P[22][5]*SK_TAS[1] - P[22][23]*SK_TAS[1] + P[22][6]*vd*SH_TAS[0]); +Kfusion[23] = SK_TAS[0]*(P[23][4]*SH_TAS[2] - P[23][22]*SH_TAS[2] + P[23][5]*SK_TAS[1] - P[23][23]*SK_TAS[1] + P[23][6]*vd*SH_TAS[0]); diff --git a/matlab/generated/Inertial Nav EKF/Covariance Prediction.txt b/matlab/generated/Inertial Nav EKF/Covariance Prediction.txt index 15b34f496c..348ab0827f 100644 --- a/matlab/generated/Inertial Nav EKF/Covariance Prediction.txt +++ b/matlab/generated/Inertial Nav EKF/Covariance Prediction.txt @@ -1,228 +1,216 @@ // Auto code for covariance prediction // Intermediate expressions obtained from algebraic optimisation -float SF[25]; -SF[0] = daz_b/2 + dazNoise/2 - (daz*daz_s)/2; -SF[1] = day_b/2 + dayNoise/2 - (day*day_s)/2; -SF[2] = dax_b/2 + daxNoise/2 - (dax*dax_s)/2; -SF[3] = q3/2 - (q0*SF[0])/2 + (q1*SF[1])/2 - (q2*SF[2])/2; -SF[4] = q0/2 - (q1*SF[2])/2 - (q2*SF[1])/2 + (q3*SF[0])/2; -SF[5] = q1/2 + (q0*SF[2])/2 - (q2*SF[0])/2 - (q3*SF[1])/2; -SF[6] = q3/2 + (q0*SF[0])/2 - (q1*SF[1])/2 - (q2*SF[2])/2; -SF[7] = q0/2 - (q1*SF[2])/2 + (q2*SF[1])/2 - (q3*SF[0])/2; -SF[8] = q0/2 + (q1*SF[2])/2 - (q2*SF[1])/2 - (q3*SF[0])/2; -SF[9] = q2/2 + (q0*SF[1])/2 + (q1*SF[0])/2 + (q3*SF[2])/2; -SF[10] = q2/2 - (q0*SF[1])/2 - (q1*SF[0])/2 + (q3*SF[2])/2; -SF[11] = q2/2 + (q0*SF[1])/2 - (q1*SF[0])/2 - (q3*SF[2])/2; -SF[12] = q1/2 + (q0*SF[2])/2 + (q2*SF[0])/2 + (q3*SF[1])/2; -SF[13] = q1/2 - (q0*SF[2])/2 + (q2*SF[0])/2 - (q3*SF[1])/2; -SF[14] = q3/2 + (q0*SF[0])/2 + (q1*SF[1])/2 + (q2*SF[2])/2; -SF[15] = - sq(q0) - sq(q1) - sq(q2) - sq(q3); -SF[16] = dvz_b - dvz + dvzNoise; -SF[17] = dvx - dvxNoise; -SF[18] = dvy - dvyNoise; -SF[19] = sq(q2); -SF[20] = SF[19] - sq(q0) + sq(q1) - sq(q3); -SF[21] = SF[19] + sq(q0) - sq(q1) - sq(q3); -SF[22] = 2*q0*q1 - 2*q2*q3; -SF[23] = SF[19] - sq(q0) - sq(q1) + sq(q3); -SF[24] = 2*q1*q2; +float SF[21] = {}; +SF[0] = dvz - dvz_b; +SF[1] = dvy - dvy_b; +SF[2] = dvx - dvx_b; +SF[3] = 2.0f*q1*SF[2] + 2.0f*q2*SF[1] + 2.0f*q3*SF[0]; +SF[4] = 2.0f*q0*SF[1] - 2.0f*q1*SF[0] + 2.0f*q3*SF[2]; +SF[5] = 2.0f*q0*SF[2] + 2.0f*q2*SF[0] - 2.0f*q3*SF[1]; +SF[6] = day*0.5f - day_b*0.5f; +SF[7] = daz*0.5f - daz_b*0.5f; +SF[8] = dax*0.5f - dax_b*0.5f; +SF[9] = dax_b*0.5f - dax*0.5f; +SF[10] = daz_b*0.5f - daz*0.5f; +SF[11] = day_b*0.5f - day*0.5f; +SF[12] = 2.0f*q1*SF[1]; +SF[13] = 2.0f*q0*SF[0]; +SF[14] = q1*0.5f; +SF[15] = q2*0.5f; +SF[16] = q3*0.5f; +SF[17] = sq(q3); +SF[18] = sq(q2); +SF[19] = sq(q1); +SF[20] = sq(q0); -float SG[5]; -SG[0] = - sq(q0) - sq(q1) - sq(q2) - sq(q3); +float SG[8] = {}; +SG[0] = q0*0.5f; SG[1] = sq(q3); SG[2] = sq(q2); SG[3] = sq(q1); SG[4] = sq(q0); +SG[5] = 2.0f*q2*q3; +SG[6] = 2.0f*q1*q3; +SG[7] = 2.0f*q1*q2; -float SQ[10]; -SQ[0] = - sq(dvyNoise)*(2*q0*q1 + 2*q2*q3)*(SG[1] - SG[2] + SG[3] - SG[4]) - sq(dvzNoise)*(2*q0*q1 - 2*q2*q3)*(SG[1] - SG[2] - SG[3] + SG[4]) - sq(dvxNoise)*(2*q0*q2 - 2*q1*q3)*(2*q0*q3 + 2*q1*q2); -SQ[1] = sq(dvxNoise)*(2*q0*q2 - 2*q1*q3)*(SG[1] + SG[2] - SG[3] - SG[4]) + sq(dvzNoise)*(2*q0*q2 + 2*q1*q3)*(SG[1] - SG[2] - SG[3] + SG[4]) - sq(dvyNoise)*(2*q0*q1 + 2*q2*q3)*(2*q0*q3 - 2*q1*q2); -SQ[2] = sq(dvyNoise)*(2*q0*q3 - 2*q1*q2)*(SG[1] - SG[2] + SG[3] - SG[4]) - sq(dvxNoise)*(2*q0*q3 + 2*q1*q2)*(SG[1] + SG[2] - SG[3] - SG[4]) - sq(dvzNoise)*(2*q0*q1 - 2*q2*q3)*(2*q0*q2 + 2*q1*q3); -SQ[3] = sq(SG[0]); -SQ[4] = sq(dvyNoise); -SQ[5] = sq(dvzNoise); -SQ[6] = sq(dvxNoise); -SQ[7] = 2*q2*q3; -SQ[8] = 2*q1*q3; -SQ[9] = 2*q1*q2; +float SQ[11] = {}; +SQ[0] = dvzVar*(SG[5] - 2.0f*q0*q1)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvyVar*(SG[5] + 2.0f*q0*q1)*(SG[1] - SG[2] + SG[3] - SG[4]) + dvxVar*(SG[6] - 2.0f*q0*q2)*(SG[7] + 2.0f*q0*q3); +SQ[1] = dvzVar*(SG[6] + 2.0f*q0*q2)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvxVar*(SG[6] - 2.0f*q0*q2)*(SG[1] + SG[2] - SG[3] - SG[4]) + dvyVar*(SG[5] + 2.0f*q0*q1)*(SG[7] - 2.0f*q0*q3); +SQ[2] = dvzVar*(SG[5] - 2.0f*q0*q1)*(SG[6] + 2.0f*q0*q2) - dvyVar*(SG[7] - 2.0f*q0*q3)*(SG[1] - SG[2] + SG[3] - SG[4]) - dvxVar*(SG[7] + 2.0f*q0*q3)*(SG[1] + SG[2] - SG[3] - SG[4]); +SQ[3] = (dayVar*q1*SG[0])*0.5f - (dazVar*q1*SG[0])*0.5f - (daxVar*q2*q3)*0.25f; +SQ[4] = (dazVar*q2*SG[0])*0.5f - (daxVar*q2*SG[0])*0.5f - (dayVar*q1*q3)*0.25f; +SQ[5] = (daxVar*q3*SG[0])*0.5f - (dayVar*q3*SG[0])*0.5f - (dazVar*q1*q2)*0.25f; +SQ[6] = (daxVar*q1*q2)*0.25f - (dazVar*q3*SG[0])*0.5f - (dayVar*q1*q2)*0.25f; +SQ[7] = (dazVar*q1*q3)*0.25f - (daxVar*q1*q3)*0.25f - (dayVar*q2*SG[0])*0.5f; +SQ[8] = (dayVar*q2*q3)*0.25f - (daxVar*q1*SG[0])*0.5f - (dazVar*q2*q3)*0.25f; +SQ[9] = sq(SG[0]); +SQ[10] = sq(q1); -float SPP[23]; -SPP[0] = SF[17]*(2*q0*q1 + 2*q2*q3) + SF[18]*(2*q0*q2 - 2*q1*q3); -SPP[1] = SF[18]*(2*q0*q2 + 2*q1*q3) + SF[16]*(SF[24] - 2*q0*q3); -SPP[2] = 2*q3*SF[8] + 2*q1*SF[11] - 2*q0*SF[14] - 2*q2*SF[13]; -SPP[3] = 2*q1*SF[7] + 2*q2*SF[6] - 2*q0*SF[12] - 2*q3*SF[10]; -SPP[4] = 2*q0*SF[6] - 2*q3*SF[7] - 2*q1*SF[10] + 2*q2*SF[12]; -SPP[5] = 2*q0*SF[8] + 2*q2*SF[11] + 2*q1*SF[13] + 2*q3*SF[14]; -SPP[6] = 2*q0*SF[7] + 2*q3*SF[6] + 2*q2*SF[10] + 2*q1*SF[12]; -SPP[7] = 2*q1*SF[3] - 2*q2*SF[4] - 2*q3*SF[5] + 2*q0*SF[9]; -SPP[8] = 2*q0*SF[5] - 2*q1*SF[4] - 2*q2*SF[3] + 2*q3*SF[9]; -SPP[9] = SF[18]*SF[20] - SF[16]*(2*q0*q1 + 2*q2*q3); -SPP[10] = SF[17]*SF[20] + SF[16]*(2*q0*q2 - 2*q1*q3); -SPP[11] = SF[17]*SF[21] - SF[18]*(SF[24] + 2*q0*q3); -SPP[12] = SF[17]*SF[22] - SF[16]*(SF[24] + 2*q0*q3); -SPP[13] = 2*q0*SF[4] + 2*q1*SF[5] + 2*q3*SF[3] + 2*q2*SF[9]; -SPP[14] = 2*q2*SF[8] - 2*q0*SF[11] - 2*q1*SF[14] + 2*q3*SF[13]; -SPP[15] = SF[18]*SF[23] + SF[17]*(SF[24] - 2*q0*q3); -SPP[16] = daz*SF[19] + daz*sq(q0) + daz*sq(q1) + daz*sq(q3); -SPP[17] = day*SF[19] + day*sq(q0) + day*sq(q1) + day*sq(q3); -SPP[18] = dax*SF[19] + dax*sq(q0) + dax*sq(q1) + dax*sq(q3); -SPP[19] = SF[16]*SF[23] - SF[17]*(2*q0*q2 + 2*q1*q3); -SPP[20] = SF[16]*SF[21] - SF[18]*SF[22]; -SPP[21] = 2*q0*q2 + 2*q1*q3; -SPP[22] = SF[15]; +float SPP[11] = {}; +SPP[0] = SF[12] + SF[13] - 2.0f*q2*SF[2]; +SPP[1] = SF[17] - SF[18] - SF[19] + SF[20]; +SPP[2] = SF[17] - SF[18] + SF[19] - SF[20]; +SPP[3] = SF[17] + SF[18] - SF[19] - SF[20]; +SPP[4] = 2.0f*q0*q2 - 2.0f*q1*q3; +SPP[5] = 2.0f*q0*q1 - 2.0f*q2*q3; +SPP[6] = 2.0f*q0*q3 - 2.0f*q1*q2; +SPP[7] = 2.0f*q0*q1 + 2.0f*q2*q3; +SPP[8] = 2.0f*q0*q3 + 2.0f*q1*q2; +SPP[9] = 2.0f*q0*q2 + 2.0f*q1*q3; +SPP[10] = SF[16]; // Calculate uppder diagonal elements of the predicted covariance matrix // Use symmetry to assign value to lower diagonal // Note: this matrix does not include the process noise for stationary states, it only includes the effect of noise on the inertial measurements. // Process noise for stationary states must be added later. float nextP[24][24]; -nextP[0][0] = SPP[5]*(P[0][0]*SPP[5] - P[1][0]*SPP[4] + P[2][0]*SPP[7] + P[9][0]*SPP[22] + P[12][0]*SPP[18]) - SPP[4]*(P[0][1]*SPP[5] - P[1][1]*SPP[4] + P[2][1]*SPP[7] + P[9][1]*SPP[22] + P[12][1]*SPP[18]) + SPP[7]*(P[0][2]*SPP[5] - P[1][2]*SPP[4] + P[2][2]*SPP[7] + P[9][2]*SPP[22] + P[12][2]*SPP[18]) + SPP[22]*(P[0][9]*SPP[5] - P[1][9]*SPP[4] + P[2][9]*SPP[7] + P[9][9]*SPP[22] + P[12][9]*SPP[18]) + SPP[18]*(P[0][12]*SPP[5] - P[1][12]*SPP[4] + P[2][12]*SPP[7] + P[9][12]*SPP[22] + P[12][12]*SPP[18]) + sq(daxNoise)*SQ[3]; -nextP[0][1] = SPP[6]*(P[0][1]*SPP[5] - P[1][1]*SPP[4] + P[2][1]*SPP[7] + P[9][1]*SPP[22] + P[12][1]*SPP[18]) - SPP[2]*(P[0][0]*SPP[5] - P[1][0]*SPP[4] + P[2][0]*SPP[7] + P[9][0]*SPP[22] + P[12][0]*SPP[18]) - SPP[8]*(P[0][2]*SPP[5] - P[1][2]*SPP[4] + P[2][2]*SPP[7] + P[9][2]*SPP[22] + P[12][2]*SPP[18]) + SPP[22]*(P[0][10]*SPP[5] - P[1][10]*SPP[4] + P[2][10]*SPP[7] + P[9][10]*SPP[22] + P[12][10]*SPP[18]) + SPP[17]*(P[0][13]*SPP[5] - P[1][13]*SPP[4] + P[2][13]*SPP[7] + P[9][13]*SPP[22] + P[12][13]*SPP[18]); -nextP[1][1] = SPP[6]*(P[1][1]*SPP[6] - P[0][1]*SPP[2] - P[2][1]*SPP[8] + P[10][1]*SPP[22] + P[13][1]*SPP[17]) - SPP[2]*(P[1][0]*SPP[6] - P[0][0]*SPP[2] - P[2][0]*SPP[8] + P[10][0]*SPP[22] + P[13][0]*SPP[17]) - SPP[8]*(P[1][2]*SPP[6] - P[0][2]*SPP[2] - P[2][2]*SPP[8] + P[10][2]*SPP[22] + P[13][2]*SPP[17]) + SPP[22]*(P[1][10]*SPP[6] - P[0][10]*SPP[2] - P[2][10]*SPP[8] + P[10][10]*SPP[22] + P[13][10]*SPP[17]) + SPP[17]*(P[1][13]*SPP[6] - P[0][13]*SPP[2] - P[2][13]*SPP[8] + P[10][13]*SPP[22] + P[13][13]*SPP[17]) + sq(dayNoise)*SQ[3]; -nextP[0][2] = SPP[14]*(P[0][0]*SPP[5] - P[1][0]*SPP[4] + P[2][0]*SPP[7] + P[9][0]*SPP[22] + P[12][0]*SPP[18]) - SPP[3]*(P[0][1]*SPP[5] - P[1][1]*SPP[4] + P[2][1]*SPP[7] + P[9][1]*SPP[22] + P[12][1]*SPP[18]) + SPP[13]*(P[0][2]*SPP[5] - P[1][2]*SPP[4] + P[2][2]*SPP[7] + P[9][2]*SPP[22] + P[12][2]*SPP[18]) + SPP[22]*(P[0][11]*SPP[5] - P[1][11]*SPP[4] + P[2][11]*SPP[7] + P[9][11]*SPP[22] + P[12][11]*SPP[18]) + SPP[16]*(P[0][14]*SPP[5] - P[1][14]*SPP[4] + P[2][14]*SPP[7] + P[9][14]*SPP[22] + P[12][14]*SPP[18]); -nextP[1][2] = SPP[14]*(P[1][0]*SPP[6] - P[0][0]*SPP[2] - P[2][0]*SPP[8] + P[10][0]*SPP[22] + P[13][0]*SPP[17]) - SPP[3]*(P[1][1]*SPP[6] - P[0][1]*SPP[2] - P[2][1]*SPP[8] + P[10][1]*SPP[22] + P[13][1]*SPP[17]) + SPP[13]*(P[1][2]*SPP[6] - P[0][2]*SPP[2] - P[2][2]*SPP[8] + P[10][2]*SPP[22] + P[13][2]*SPP[17]) + SPP[22]*(P[1][11]*SPP[6] - P[0][11]*SPP[2] - P[2][11]*SPP[8] + P[10][11]*SPP[22] + P[13][11]*SPP[17]) + SPP[16]*(P[1][14]*SPP[6] - P[0][14]*SPP[2] - P[2][14]*SPP[8] + P[10][14]*SPP[22] + P[13][14]*SPP[17]); -nextP[2][2] = SPP[14]*(P[0][0]*SPP[14] - P[1][0]*SPP[3] + P[2][0]*SPP[13] + P[11][0]*SPP[22] + P[14][0]*SPP[16]) - SPP[3]*(P[0][1]*SPP[14] - P[1][1]*SPP[3] + P[2][1]*SPP[13] + P[11][1]*SPP[22] + P[14][1]*SPP[16]) + SPP[13]*(P[0][2]*SPP[14] - P[1][2]*SPP[3] + P[2][2]*SPP[13] + P[11][2]*SPP[22] + P[14][2]*SPP[16]) + SPP[22]*(P[0][11]*SPP[14] - P[1][11]*SPP[3] + P[2][11]*SPP[13] + P[11][11]*SPP[22] + P[14][11]*SPP[16]) + SPP[16]*(P[0][14]*SPP[14] - P[1][14]*SPP[3] + P[2][14]*SPP[13] + P[11][14]*SPP[22] + P[14][14]*SPP[16]) + sq(dazNoise)*SQ[3]; -nextP[0][3] = P[0][3]*SPP[5] - P[1][3]*SPP[4] + P[2][3]*SPP[7] + P[9][3]*SPP[22] + P[12][3]*SPP[18] + SPP[1]*(P[0][0]*SPP[5] - P[1][0]*SPP[4] + P[2][0]*SPP[7] + P[9][0]*SPP[22] + P[12][0]*SPP[18]) + SPP[19]*(P[0][1]*SPP[5] - P[1][1]*SPP[4] + P[2][1]*SPP[7] + P[9][1]*SPP[22] + P[12][1]*SPP[18]) + SPP[15]*(P[0][2]*SPP[5] - P[1][2]*SPP[4] + P[2][2]*SPP[7] + P[9][2]*SPP[22] + P[12][2]*SPP[18]) - SPP[21]*(P[0][15]*SPP[5] - P[1][15]*SPP[4] + P[2][15]*SPP[7] + P[9][15]*SPP[22] + P[12][15]*SPP[18]); -nextP[1][3] = P[1][3]*SPP[6] - P[0][3]*SPP[2] - P[2][3]*SPP[8] + P[10][3]*SPP[22] + P[13][3]*SPP[17] + SPP[1]*(P[1][0]*SPP[6] - P[0][0]*SPP[2] - P[2][0]*SPP[8] + P[10][0]*SPP[22] + P[13][0]*SPP[17]) + SPP[19]*(P[1][1]*SPP[6] - P[0][1]*SPP[2] - P[2][1]*SPP[8] + P[10][1]*SPP[22] + P[13][1]*SPP[17]) + SPP[15]*(P[1][2]*SPP[6] - P[0][2]*SPP[2] - P[2][2]*SPP[8] + P[10][2]*SPP[22] + P[13][2]*SPP[17]) - SPP[21]*(P[1][15]*SPP[6] - P[0][15]*SPP[2] - P[2][15]*SPP[8] + P[10][15]*SPP[22] + P[13][15]*SPP[17]); -nextP[2][3] = P[0][3]*SPP[14] - P[1][3]*SPP[3] + P[2][3]*SPP[13] + P[11][3]*SPP[22] + P[14][3]*SPP[16] + SPP[1]*(P[0][0]*SPP[14] - P[1][0]*SPP[3] + P[2][0]*SPP[13] + P[11][0]*SPP[22] + P[14][0]*SPP[16]) + SPP[19]*(P[0][1]*SPP[14] - P[1][1]*SPP[3] + P[2][1]*SPP[13] + P[11][1]*SPP[22] + P[14][1]*SPP[16]) + SPP[15]*(P[0][2]*SPP[14] - P[1][2]*SPP[3] + P[2][2]*SPP[13] + P[11][2]*SPP[22] + P[14][2]*SPP[16]) - SPP[21]*(P[0][15]*SPP[14] - P[1][15]*SPP[3] + P[2][15]*SPP[13] + P[11][15]*SPP[22] + P[14][15]*SPP[16]); -nextP[3][3] = P[3][3] + P[0][3]*SPP[1] + P[1][3]*SPP[19] + P[2][3]*SPP[15] - P[15][3]*SPP[21] + SQ[5]*sq(SQ[8] + 2*q0*q2) + SQ[4]*sq(SQ[9] - 2*q0*q3) + SPP[1]*(P[3][0] + P[0][0]*SPP[1] + P[1][0]*SPP[19] + P[2][0]*SPP[15] - P[15][0]*SPP[21]) + SPP[19]*(P[3][1] + P[0][1]*SPP[1] + P[1][1]*SPP[19] + P[2][1]*SPP[15] - P[15][1]*SPP[21]) + SPP[15]*(P[3][2] + P[0][2]*SPP[1] + P[1][2]*SPP[19] + P[2][2]*SPP[15] - P[15][2]*SPP[21]) - SPP[21]*(P[3][15] + P[0][15]*SPP[1] + P[1][15]*SPP[19] + P[2][15]*SPP[15] - P[15][15]*SPP[21]) + SQ[6]*sq(SG[1] + SG[2] - SG[3] - SG[4]); -nextP[0][4] = P[0][4]*SPP[5] - P[1][4]*SPP[4] + P[2][4]*SPP[7] + P[9][4]*SPP[22] + P[12][4]*SPP[18] + SF[22]*(P[0][15]*SPP[5] - P[1][15]*SPP[4] + P[2][15]*SPP[7] + P[9][15]*SPP[22] + P[12][15]*SPP[18]) + SPP[12]*(P[0][1]*SPP[5] - P[1][1]*SPP[4] + P[2][1]*SPP[7] + P[9][1]*SPP[22] + P[12][1]*SPP[18]) + SPP[20]*(P[0][0]*SPP[5] - P[1][0]*SPP[4] + P[2][0]*SPP[7] + P[9][0]*SPP[22] + P[12][0]*SPP[18]) + SPP[11]*(P[0][2]*SPP[5] - P[1][2]*SPP[4] + P[2][2]*SPP[7] + P[9][2]*SPP[22] + P[12][2]*SPP[18]); -nextP[1][4] = P[1][4]*SPP[6] - P[0][4]*SPP[2] - P[2][4]*SPP[8] + P[10][4]*SPP[22] + P[13][4]*SPP[17] + SF[22]*(P[1][15]*SPP[6] - P[0][15]*SPP[2] - P[2][15]*SPP[8] + P[10][15]*SPP[22] + P[13][15]*SPP[17]) + SPP[12]*(P[1][1]*SPP[6] - P[0][1]*SPP[2] - P[2][1]*SPP[8] + P[10][1]*SPP[22] + P[13][1]*SPP[17]) + SPP[20]*(P[1][0]*SPP[6] - P[0][0]*SPP[2] - P[2][0]*SPP[8] + P[10][0]*SPP[22] + P[13][0]*SPP[17]) + SPP[11]*(P[1][2]*SPP[6] - P[0][2]*SPP[2] - P[2][2]*SPP[8] + P[10][2]*SPP[22] + P[13][2]*SPP[17]); -nextP[2][4] = P[0][4]*SPP[14] - P[1][4]*SPP[3] + P[2][4]*SPP[13] + P[11][4]*SPP[22] + P[14][4]*SPP[16] + SF[22]*(P[0][15]*SPP[14] - P[1][15]*SPP[3] + P[2][15]*SPP[13] + P[11][15]*SPP[22] + P[14][15]*SPP[16]) + SPP[12]*(P[0][1]*SPP[14] - P[1][1]*SPP[3] + P[2][1]*SPP[13] + P[11][1]*SPP[22] + P[14][1]*SPP[16]) + SPP[20]*(P[0][0]*SPP[14] - P[1][0]*SPP[3] + P[2][0]*SPP[13] + P[11][0]*SPP[22] + P[14][0]*SPP[16]) + SPP[11]*(P[0][2]*SPP[14] - P[1][2]*SPP[3] + P[2][2]*SPP[13] + P[11][2]*SPP[22] + P[14][2]*SPP[16]); -nextP[3][4] = P[3][4] + SQ[2] + P[0][4]*SPP[1] + P[1][4]*SPP[19] + P[2][4]*SPP[15] - P[15][4]*SPP[21] + SF[22]*(P[3][15] + P[0][15]*SPP[1] + P[1][15]*SPP[19] + P[2][15]*SPP[15] - P[15][15]*SPP[21]) + SPP[12]*(P[3][1] + P[0][1]*SPP[1] + P[1][1]*SPP[19] + P[2][1]*SPP[15] - P[15][1]*SPP[21]) + SPP[20]*(P[3][0] + P[0][0]*SPP[1] + P[1][0]*SPP[19] + P[2][0]*SPP[15] - P[15][0]*SPP[21]) + SPP[11]*(P[3][2] + P[0][2]*SPP[1] + P[1][2]*SPP[19] + P[2][2]*SPP[15] - P[15][2]*SPP[21]); -nextP[4][4] = P[4][4] + P[15][4]*SF[22] + P[0][4]*SPP[20] + P[1][4]*SPP[12] + P[2][4]*SPP[11] + SQ[5]*sq(SQ[7] - 2*q0*q1) + SQ[6]*sq(SQ[9] + 2*q0*q3) + SF[22]*(P[4][15] + P[15][15]*SF[22] + P[0][15]*SPP[20] + P[1][15]*SPP[12] + P[2][15]*SPP[11]) + SPP[12]*(P[4][1] + P[15][1]*SF[22] + P[0][1]*SPP[20] + P[1][1]*SPP[12] + P[2][1]*SPP[11]) + SPP[20]*(P[4][0] + P[15][0]*SF[22] + P[0][0]*SPP[20] + P[1][0]*SPP[12] + P[2][0]*SPP[11]) + SPP[11]*(P[4][2] + P[15][2]*SF[22] + P[0][2]*SPP[20] + P[1][2]*SPP[12] + P[2][2]*SPP[11]) + SQ[4]*sq(SG[1] - SG[2] + SG[3] - SG[4]); -nextP[0][5] = P[0][5]*SPP[5] - P[1][5]*SPP[4] + P[2][5]*SPP[7] + P[9][5]*SPP[22] + P[12][5]*SPP[18] + SF[20]*(P[0][15]*SPP[5] - P[1][15]*SPP[4] + P[2][15]*SPP[7] + P[9][15]*SPP[22] + P[12][15]*SPP[18]) - SPP[9]*(P[0][0]*SPP[5] - P[1][0]*SPP[4] + P[2][0]*SPP[7] + P[9][0]*SPP[22] + P[12][0]*SPP[18]) + SPP[0]*(P[0][2]*SPP[5] - P[1][2]*SPP[4] + P[2][2]*SPP[7] + P[9][2]*SPP[22] + P[12][2]*SPP[18]) + SPP[10]*(P[0][1]*SPP[5] - P[1][1]*SPP[4] + P[2][1]*SPP[7] + P[9][1]*SPP[22] + P[12][1]*SPP[18]); -nextP[1][5] = P[1][5]*SPP[6] - P[0][5]*SPP[2] - P[2][5]*SPP[8] + P[10][5]*SPP[22] + P[13][5]*SPP[17] + SF[20]*(P[1][15]*SPP[6] - P[0][15]*SPP[2] - P[2][15]*SPP[8] + P[10][15]*SPP[22] + P[13][15]*SPP[17]) - SPP[9]*(P[1][0]*SPP[6] - P[0][0]*SPP[2] - P[2][0]*SPP[8] + P[10][0]*SPP[22] + P[13][0]*SPP[17]) + SPP[0]*(P[1][2]*SPP[6] - P[0][2]*SPP[2] - P[2][2]*SPP[8] + P[10][2]*SPP[22] + P[13][2]*SPP[17]) + SPP[10]*(P[1][1]*SPP[6] - P[0][1]*SPP[2] - P[2][1]*SPP[8] + P[10][1]*SPP[22] + P[13][1]*SPP[17]); -nextP[2][5] = P[0][5]*SPP[14] - P[1][5]*SPP[3] + P[2][5]*SPP[13] + P[11][5]*SPP[22] + P[14][5]*SPP[16] + SF[20]*(P[0][15]*SPP[14] - P[1][15]*SPP[3] + P[2][15]*SPP[13] + P[11][15]*SPP[22] + P[14][15]*SPP[16]) - SPP[9]*(P[0][0]*SPP[14] - P[1][0]*SPP[3] + P[2][0]*SPP[13] + P[11][0]*SPP[22] + P[14][0]*SPP[16]) + SPP[0]*(P[0][2]*SPP[14] - P[1][2]*SPP[3] + P[2][2]*SPP[13] + P[11][2]*SPP[22] + P[14][2]*SPP[16]) + SPP[10]*(P[0][1]*SPP[14] - P[1][1]*SPP[3] + P[2][1]*SPP[13] + P[11][1]*SPP[22] + P[14][1]*SPP[16]); -nextP[3][5] = P[3][5] + SQ[1] + P[0][5]*SPP[1] + P[1][5]*SPP[19] + P[2][5]*SPP[15] - P[15][5]*SPP[21] + SF[20]*(P[3][15] + P[0][15]*SPP[1] + P[1][15]*SPP[19] + P[2][15]*SPP[15] - P[15][15]*SPP[21]) - SPP[9]*(P[3][0] + P[0][0]*SPP[1] + P[1][0]*SPP[19] + P[2][0]*SPP[15] - P[15][0]*SPP[21]) + SPP[0]*(P[3][2] + P[0][2]*SPP[1] + P[1][2]*SPP[19] + P[2][2]*SPP[15] - P[15][2]*SPP[21]) + SPP[10]*(P[3][1] + P[0][1]*SPP[1] + P[1][1]*SPP[19] + P[2][1]*SPP[15] - P[15][1]*SPP[21]); -nextP[4][5] = P[4][5] + SQ[0] + P[15][5]*SF[22] + P[0][5]*SPP[20] + P[1][5]*SPP[12] + P[2][5]*SPP[11] + SF[20]*(P[4][15] + P[15][15]*SF[22] + P[0][15]*SPP[20] + P[1][15]*SPP[12] + P[2][15]*SPP[11]) - SPP[9]*(P[4][0] + P[15][0]*SF[22] + P[0][0]*SPP[20] + P[1][0]*SPP[12] + P[2][0]*SPP[11]) + SPP[0]*(P[4][2] + P[15][2]*SF[22] + P[0][2]*SPP[20] + P[1][2]*SPP[12] + P[2][2]*SPP[11]) + SPP[10]*(P[4][1] + P[15][1]*SF[22] + P[0][1]*SPP[20] + P[1][1]*SPP[12] + P[2][1]*SPP[11]); -nextP[5][5] = P[5][5] + P[15][5]*SF[20] - P[0][5]*SPP[9] + P[1][5]*SPP[10] + P[2][5]*SPP[0] + SQ[4]*sq(SQ[7] + 2*q0*q1) + SQ[6]*sq(SQ[8] - 2*q0*q2) + SF[20]*(P[5][15] + P[15][15]*SF[20] - P[0][15]*SPP[9] + P[1][15]*SPP[10] + P[2][15]*SPP[0]) - SPP[9]*(P[5][0] + P[15][0]*SF[20] - P[0][0]*SPP[9] + P[1][0]*SPP[10] + P[2][0]*SPP[0]) + SPP[0]*(P[5][2] + P[15][2]*SF[20] - P[0][2]*SPP[9] + P[1][2]*SPP[10] + P[2][2]*SPP[0]) + SPP[10]*(P[5][1] + P[15][1]*SF[20] - P[0][1]*SPP[9] + P[1][1]*SPP[10] + P[2][1]*SPP[0]) + SQ[5]*sq(SG[1] - SG[2] - SG[3] + SG[4]); -nextP[0][6] = P[0][6]*SPP[5] - P[1][6]*SPP[4] + P[2][6]*SPP[7] + P[9][6]*SPP[22] + P[12][6]*SPP[18] + dt*(P[0][3]*SPP[5] - P[1][3]*SPP[4] + P[2][3]*SPP[7] + P[9][3]*SPP[22] + P[12][3]*SPP[18]); -nextP[1][6] = P[1][6]*SPP[6] - P[0][6]*SPP[2] - P[2][6]*SPP[8] + P[10][6]*SPP[22] + P[13][6]*SPP[17] + dt*(P[1][3]*SPP[6] - P[0][3]*SPP[2] - P[2][3]*SPP[8] + P[10][3]*SPP[22] + P[13][3]*SPP[17]); -nextP[2][6] = P[0][6]*SPP[14] - P[1][6]*SPP[3] + P[2][6]*SPP[13] + P[11][6]*SPP[22] + P[14][6]*SPP[16] + dt*(P[0][3]*SPP[14] - P[1][3]*SPP[3] + P[2][3]*SPP[13] + P[11][3]*SPP[22] + P[14][3]*SPP[16]); -nextP[3][6] = P[3][6] + P[0][6]*SPP[1] + P[1][6]*SPP[19] + P[2][6]*SPP[15] - P[15][6]*SPP[21] + dt*(P[3][3] + P[0][3]*SPP[1] + P[1][3]*SPP[19] + P[2][3]*SPP[15] - P[15][3]*SPP[21]); -nextP[4][6] = P[4][6] + P[15][6]*SF[22] + P[0][6]*SPP[20] + P[1][6]*SPP[12] + P[2][6]*SPP[11] + dt*(P[4][3] + P[15][3]*SF[22] + P[0][3]*SPP[20] + P[1][3]*SPP[12] + P[2][3]*SPP[11]); -nextP[5][6] = P[5][6] + P[15][6]*SF[20] - P[0][6]*SPP[9] + P[1][6]*SPP[10] + P[2][6]*SPP[0] + dt*(P[5][3] + P[15][3]*SF[20] - P[0][3]*SPP[9] + P[1][3]*SPP[10] + P[2][3]*SPP[0]); -nextP[6][6] = P[6][6] + P[3][6]*dt + dt*(P[6][3] + P[3][3]*dt); -nextP[0][7] = P[0][7]*SPP[5] - P[1][7]*SPP[4] + P[2][7]*SPP[7] + P[9][7]*SPP[22] + P[12][7]*SPP[18] + dt*(P[0][4]*SPP[5] - P[1][4]*SPP[4] + P[2][4]*SPP[7] + P[9][4]*SPP[22] + P[12][4]*SPP[18]); -nextP[1][7] = P[1][7]*SPP[6] - P[0][7]*SPP[2] - P[2][7]*SPP[8] + P[10][7]*SPP[22] + P[13][7]*SPP[17] + dt*(P[1][4]*SPP[6] - P[0][4]*SPP[2] - P[2][4]*SPP[8] + P[10][4]*SPP[22] + P[13][4]*SPP[17]); -nextP[2][7] = P[0][7]*SPP[14] - P[1][7]*SPP[3] + P[2][7]*SPP[13] + P[11][7]*SPP[22] + P[14][7]*SPP[16] + dt*(P[0][4]*SPP[14] - P[1][4]*SPP[3] + P[2][4]*SPP[13] + P[11][4]*SPP[22] + P[14][4]*SPP[16]); -nextP[3][7] = P[3][7] + P[0][7]*SPP[1] + P[1][7]*SPP[19] + P[2][7]*SPP[15] - P[15][7]*SPP[21] + dt*(P[3][4] + P[0][4]*SPP[1] + P[1][4]*SPP[19] + P[2][4]*SPP[15] - P[15][4]*SPP[21]); -nextP[4][7] = P[4][7] + P[15][7]*SF[22] + P[0][7]*SPP[20] + P[1][7]*SPP[12] + P[2][7]*SPP[11] + dt*(P[4][4] + P[15][4]*SF[22] + P[0][4]*SPP[20] + P[1][4]*SPP[12] + P[2][4]*SPP[11]); -nextP[5][7] = P[5][7] + P[15][7]*SF[20] - P[0][7]*SPP[9] + P[1][7]*SPP[10] + P[2][7]*SPP[0] + dt*(P[5][4] + P[15][4]*SF[20] - P[0][4]*SPP[9] + P[1][4]*SPP[10] + P[2][4]*SPP[0]); -nextP[6][7] = P[6][7] + P[3][7]*dt + dt*(P[6][4] + P[3][4]*dt); +nextP[0][0] = P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10] + (daxVar*SQ[10])*0.25f + SF[9]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) + SF[11]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SF[10]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) + SF[14]*(P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10]) + SF[15]*(P[0][11] + P[1][11]*SF[9] + P[2][11]*SF[11] + P[3][11]*SF[10] + P[10][11]*SF[14] + P[11][11]*SF[15] + P[12][11]*SPP[10]) + SPP[10]*(P[0][12] + P[1][12]*SF[9] + P[2][12]*SF[11] + P[3][12]*SF[10] + P[10][12]*SF[14] + P[11][12]*SF[15] + P[12][12]*SPP[10]) + (dayVar*sq(q2))*0.25f + (dazVar*sq(q3))*0.25f; +nextP[0][1] = P[0][1] + SQ[8] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10] + SF[8]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SF[7]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SF[11]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) - SF[15]*(P[0][12] + P[1][12]*SF[9] + P[2][12]*SF[11] + P[3][12]*SF[10] + P[10][12]*SF[14] + P[11][12]*SF[15] + P[12][12]*SPP[10]) + SPP[10]*(P[0][11] + P[1][11]*SF[9] + P[2][11]*SF[11] + P[3][11]*SF[10] + P[10][11]*SF[14] + P[11][11]*SF[15] + P[12][11]*SPP[10]) - (q0*(P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10]))*0.5f; +nextP[1][1] = P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] + daxVar*SQ[9] - (P[10][1]*q0)*0.5f + SF[8]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SF[7]*(P[1][2] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2) + SF[11]*(P[1][3] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2) - SF[15]*(P[1][12] + P[0][12]*SF[8] + P[2][12]*SF[7] + P[3][12]*SF[11] - P[12][12]*SF[15] + P[11][12]*SPP[10] - (P[10][12]*q0)/2) + SPP[10]*(P[1][11] + P[0][11]*SF[8] + P[2][11]*SF[7] + P[3][11]*SF[11] - P[12][11]*SF[15] + P[11][11]*SPP[10] - (P[10][11]*q0)/2) + (dayVar*sq(q3))*0.25f + (dazVar*sq(q2))*0.25f - (q0*(P[1][10] + P[0][10]*SF[8] + P[2][10]*SF[7] + P[3][10]*SF[11] - P[12][10]*SF[15] + P[11][10]*SPP[10] - (P[10][10]*q0)/2))*0.5f; +nextP[0][2] = P[0][2] + SQ[7] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10] + SF[6]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SF[10]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) + SF[8]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) + SF[14]*(P[0][12] + P[1][12]*SF[9] + P[2][12]*SF[11] + P[3][12]*SF[10] + P[10][12]*SF[14] + P[11][12]*SF[15] + P[12][12]*SPP[10]) - SPP[10]*(P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10]) - (q0*(P[0][11] + P[1][11]*SF[9] + P[2][11]*SF[11] + P[3][11]*SF[10] + P[10][11]*SF[14] + P[11][11]*SF[15] + P[12][11]*SPP[10]))*0.5f; +nextP[1][2] = P[1][2] + SQ[5] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)*0.5f + SF[6]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SF[10]*(P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] - (P[10][1]*q0)/2) + SF[8]*(P[1][3] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2) + SF[14]*(P[1][12] + P[0][12]*SF[8] + P[2][12]*SF[7] + P[3][12]*SF[11] - P[12][12]*SF[15] + P[11][12]*SPP[10] - (P[10][12]*q0)/2) - SPP[10]*(P[1][10] + P[0][10]*SF[8] + P[2][10]*SF[7] + P[3][10]*SF[11] - P[12][10]*SF[15] + P[11][10]*SPP[10] - (P[10][10]*q0)/2) - (q0*(P[1][11] + P[0][11]*SF[8] + P[2][11]*SF[7] + P[3][11]*SF[11] - P[12][11]*SF[15] + P[11][11]*SPP[10] - (P[10][11]*q0)/2))*0.5f; +nextP[2][2] = P[2][2] + P[0][2]*SF[6] + P[1][2]*SF[10] + P[3][2]*SF[8] + P[12][2]*SF[14] - P[10][2]*SPP[10] + dayVar*SQ[9] + (dazVar*SQ[10])*0.25f - (P[11][2]*q0)*0.5f + SF[6]*(P[2][0] + P[0][0]*SF[6] + P[1][0]*SF[10] + P[3][0]*SF[8] + P[12][0]*SF[14] - P[10][0]*SPP[10] - (P[11][0]*q0)/2) + SF[10]*(P[2][1] + P[0][1]*SF[6] + P[1][1]*SF[10] + P[3][1]*SF[8] + P[12][1]*SF[14] - P[10][1]*SPP[10] - (P[11][1]*q0)/2) + SF[8]*(P[2][3] + P[0][3]*SF[6] + P[1][3]*SF[10] + P[3][3]*SF[8] + P[12][3]*SF[14] - P[10][3]*SPP[10] - (P[11][3]*q0)/2) + SF[14]*(P[2][12] + P[0][12]*SF[6] + P[1][12]*SF[10] + P[3][12]*SF[8] + P[12][12]*SF[14] - P[10][12]*SPP[10] - (P[11][12]*q0)/2) - SPP[10]*(P[2][10] + P[0][10]*SF[6] + P[1][10]*SF[10] + P[3][10]*SF[8] + P[12][10]*SF[14] - P[10][10]*SPP[10] - (P[11][10]*q0)/2) + (daxVar*sq(q3))*0.25f - (q0*(P[2][11] + P[0][11]*SF[6] + P[1][11]*SF[10] + P[3][11]*SF[8] + P[12][11]*SF[14] - P[10][11]*SPP[10] - (P[11][11]*q0)/2))*0.5f; +nextP[0][3] = P[0][3] + SQ[6] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10] + SF[7]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SF[6]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) + SF[9]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SF[15]*(P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10]) - SF[14]*(P[0][11] + P[1][11]*SF[9] + P[2][11]*SF[11] + P[3][11]*SF[10] + P[10][11]*SF[14] + P[11][11]*SF[15] + P[12][11]*SPP[10]) - (q0*(P[0][12] + P[1][12]*SF[9] + P[2][12]*SF[11] + P[3][12]*SF[10] + P[10][12]*SF[14] + P[11][12]*SF[15] + P[12][12]*SPP[10]))*0.5f; +nextP[1][3] = P[1][3] + SQ[4] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)*0.5f + SF[7]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SF[6]*(P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] - (P[10][1]*q0)/2) + SF[9]*(P[1][2] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2) + SF[15]*(P[1][10] + P[0][10]*SF[8] + P[2][10]*SF[7] + P[3][10]*SF[11] - P[12][10]*SF[15] + P[11][10]*SPP[10] - (P[10][10]*q0)/2) - SF[14]*(P[1][11] + P[0][11]*SF[8] + P[2][11]*SF[7] + P[3][11]*SF[11] - P[12][11]*SF[15] + P[11][11]*SPP[10] - (P[10][11]*q0)/2) - (q0*(P[1][12] + P[0][12]*SF[8] + P[2][12]*SF[7] + P[3][12]*SF[11] - P[12][12]*SF[15] + P[11][12]*SPP[10] - (P[10][12]*q0)/2))*0.5f; +nextP[2][3] = P[2][3] + SQ[3] + P[0][3]*SF[6] + P[1][3]*SF[10] + P[3][3]*SF[8] + P[12][3]*SF[14] - P[10][3]*SPP[10] - (P[11][3]*q0)*0.5f + SF[7]*(P[2][0] + P[0][0]*SF[6] + P[1][0]*SF[10] + P[3][0]*SF[8] + P[12][0]*SF[14] - P[10][0]*SPP[10] - (P[11][0]*q0)/2) + SF[6]*(P[2][1] + P[0][1]*SF[6] + P[1][1]*SF[10] + P[3][1]*SF[8] + P[12][1]*SF[14] - P[10][1]*SPP[10] - (P[11][1]*q0)/2) + SF[9]*(P[2][2] + P[0][2]*SF[6] + P[1][2]*SF[10] + P[3][2]*SF[8] + P[12][2]*SF[14] - P[10][2]*SPP[10] - (P[11][2]*q0)/2) + SF[15]*(P[2][10] + P[0][10]*SF[6] + P[1][10]*SF[10] + P[3][10]*SF[8] + P[12][10]*SF[14] - P[10][10]*SPP[10] - (P[11][10]*q0)/2) - SF[14]*(P[2][11] + P[0][11]*SF[6] + P[1][11]*SF[10] + P[3][11]*SF[8] + P[12][11]*SF[14] - P[10][11]*SPP[10] - (P[11][11]*q0)/2) - (q0*(P[2][12] + P[0][12]*SF[6] + P[1][12]*SF[10] + P[3][12]*SF[8] + P[12][12]*SF[14] - P[10][12]*SPP[10] - (P[11][12]*q0)/2))*0.5f; +nextP[3][3] = P[3][3] + P[0][3]*SF[7] + P[1][3]*SF[6] + P[2][3]*SF[9] + P[10][3]*SF[15] - P[11][3]*SF[14] + (dayVar*SQ[10])*0.25f + dazVar*SQ[9] - (P[12][3]*q0)*0.5f + SF[7]*(P[3][0] + P[0][0]*SF[7] + P[1][0]*SF[6] + P[2][0]*SF[9] + P[10][0]*SF[15] - P[11][0]*SF[14] - (P[12][0]*q0)/2) + SF[6]*(P[3][1] + P[0][1]*SF[7] + P[1][1]*SF[6] + P[2][1]*SF[9] + P[10][1]*SF[15] - P[11][1]*SF[14] - (P[12][1]*q0)/2) + SF[9]*(P[3][2] + P[0][2]*SF[7] + P[1][2]*SF[6] + P[2][2]*SF[9] + P[10][2]*SF[15] - P[11][2]*SF[14] - (P[12][2]*q0)/2) + SF[15]*(P[3][10] + P[0][10]*SF[7] + P[1][10]*SF[6] + P[2][10]*SF[9] + P[10][10]*SF[15] - P[11][10]*SF[14] - (P[12][10]*q0)/2) - SF[14]*(P[3][11] + P[0][11]*SF[7] + P[1][11]*SF[6] + P[2][11]*SF[9] + P[10][11]*SF[15] - P[11][11]*SF[14] - (P[12][11]*q0)/2) + (daxVar*sq(q2))*0.25f - (q0*(P[3][12] + P[0][12]*SF[7] + P[1][12]*SF[6] + P[2][12]*SF[9] + P[10][12]*SF[15] - P[11][12]*SF[14] - (P[12][12]*q0)/2))*0.5f; +nextP[0][4] = P[0][4] + P[1][4]*SF[9] + P[2][4]*SF[11] + P[3][4]*SF[10] + P[10][4]*SF[14] + P[11][4]*SF[15] + P[12][4]*SPP[10] + SF[5]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SF[3]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) - SF[4]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) + SPP[0]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SPP[3]*(P[0][13] + P[1][13]*SF[9] + P[2][13]*SF[11] + P[3][13]*SF[10] + P[10][13]*SF[14] + P[11][13]*SF[15] + P[12][13]*SPP[10]) + SPP[6]*(P[0][14] + P[1][14]*SF[9] + P[2][14]*SF[11] + P[3][14]*SF[10] + P[10][14]*SF[14] + P[11][14]*SF[15] + P[12][14]*SPP[10]) - SPP[9]*(P[0][15] + P[1][15]*SF[9] + P[2][15]*SF[11] + P[3][15]*SF[10] + P[10][15]*SF[14] + P[11][15]*SF[15] + P[12][15]*SPP[10]); +nextP[1][4] = P[1][4] + P[0][4]*SF[8] + P[2][4]*SF[7] + P[3][4]*SF[11] - P[12][4]*SF[15] + P[11][4]*SPP[10] - (P[10][4]*q0)*0.5f + SF[5]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SF[3]*(P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] - (P[10][1]*q0)/2) - SF[4]*(P[1][3] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2) + SPP[0]*(P[1][2] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2) + SPP[3]*(P[1][13] + P[0][13]*SF[8] + P[2][13]*SF[7] + P[3][13]*SF[11] - P[12][13]*SF[15] + P[11][13]*SPP[10] - (P[10][13]*q0)/2) + SPP[6]*(P[1][14] + P[0][14]*SF[8] + P[2][14]*SF[7] + P[3][14]*SF[11] - P[12][14]*SF[15] + P[11][14]*SPP[10] - (P[10][14]*q0)/2) - SPP[9]*(P[1][15] + P[0][15]*SF[8] + P[2][15]*SF[7] + P[3][15]*SF[11] - P[12][15]*SF[15] + P[11][15]*SPP[10] - (P[10][15]*q0)/2); +nextP[2][4] = P[2][4] + P[0][4]*SF[6] + P[1][4]*SF[10] + P[3][4]*SF[8] + P[12][4]*SF[14] - P[10][4]*SPP[10] - (P[11][4]*q0)*0.5f + SF[5]*(P[2][0] + P[0][0]*SF[6] + P[1][0]*SF[10] + P[3][0]*SF[8] + P[12][0]*SF[14] - P[10][0]*SPP[10] - (P[11][0]*q0)/2) + SF[3]*(P[2][1] + P[0][1]*SF[6] + P[1][1]*SF[10] + P[3][1]*SF[8] + P[12][1]*SF[14] - P[10][1]*SPP[10] - (P[11][1]*q0)/2) - SF[4]*(P[2][3] + P[0][3]*SF[6] + P[1][3]*SF[10] + P[3][3]*SF[8] + P[12][3]*SF[14] - P[10][3]*SPP[10] - (P[11][3]*q0)/2) + SPP[0]*(P[2][2] + P[0][2]*SF[6] + P[1][2]*SF[10] + P[3][2]*SF[8] + P[12][2]*SF[14] - P[10][2]*SPP[10] - (P[11][2]*q0)/2) + SPP[3]*(P[2][13] + P[0][13]*SF[6] + P[1][13]*SF[10] + P[3][13]*SF[8] + P[12][13]*SF[14] - P[10][13]*SPP[10] - (P[11][13]*q0)/2) + SPP[6]*(P[2][14] + P[0][14]*SF[6] + P[1][14]*SF[10] + P[3][14]*SF[8] + P[12][14]*SF[14] - P[10][14]*SPP[10] - (P[11][14]*q0)/2) - SPP[9]*(P[2][15] + P[0][15]*SF[6] + P[1][15]*SF[10] + P[3][15]*SF[8] + P[12][15]*SF[14] - P[10][15]*SPP[10] - (P[11][15]*q0)/2); +nextP[3][4] = P[3][4] + P[0][4]*SF[7] + P[1][4]*SF[6] + P[2][4]*SF[9] + P[10][4]*SF[15] - P[11][4]*SF[14] - (P[12][4]*q0)*0.5f + SF[5]*(P[3][0] + P[0][0]*SF[7] + P[1][0]*SF[6] + P[2][0]*SF[9] + P[10][0]*SF[15] - P[11][0]*SF[14] - (P[12][0]*q0)/2) + SF[3]*(P[3][1] + P[0][1]*SF[7] + P[1][1]*SF[6] + P[2][1]*SF[9] + P[10][1]*SF[15] - P[11][1]*SF[14] - (P[12][1]*q0)/2) - SF[4]*(P[3][3] + P[0][3]*SF[7] + P[1][3]*SF[6] + P[2][3]*SF[9] + P[10][3]*SF[15] - P[11][3]*SF[14] - (P[12][3]*q0)/2) + SPP[0]*(P[3][2] + P[0][2]*SF[7] + P[1][2]*SF[6] + P[2][2]*SF[9] + P[10][2]*SF[15] - P[11][2]*SF[14] - (P[12][2]*q0)/2) + SPP[3]*(P[3][13] + P[0][13]*SF[7] + P[1][13]*SF[6] + P[2][13]*SF[9] + P[10][13]*SF[15] - P[11][13]*SF[14] - (P[12][13]*q0)/2) + SPP[6]*(P[3][14] + P[0][14]*SF[7] + P[1][14]*SF[6] + P[2][14]*SF[9] + P[10][14]*SF[15] - P[11][14]*SF[14] - (P[12][14]*q0)/2) - SPP[9]*(P[3][15] + P[0][15]*SF[7] + P[1][15]*SF[6] + P[2][15]*SF[9] + P[10][15]*SF[15] - P[11][15]*SF[14] - (P[12][15]*q0)/2); +nextP[4][4] = P[4][4] + P[0][4]*SF[5] + P[1][4]*SF[3] - P[3][4]*SF[4] + P[2][4]*SPP[0] + P[13][4]*SPP[3] + P[14][4]*SPP[6] - P[15][4]*SPP[9] + dvyVar*sq(SG[7] - 2.0f*q0*q3) + dvzVar*sq(SG[6] + 2.0f*q0*q2) + SF[5]*(P[4][0] + P[0][0]*SF[5] + P[1][0]*SF[3] - P[3][0]*SF[4] + P[2][0]*SPP[0] + P[13][0]*SPP[3] + P[14][0]*SPP[6] - P[15][0]*SPP[9]) + SF[3]*(P[4][1] + P[0][1]*SF[5] + P[1][1]*SF[3] - P[3][1]*SF[4] + P[2][1]*SPP[0] + P[13][1]*SPP[3] + P[14][1]*SPP[6] - P[15][1]*SPP[9]) - SF[4]*(P[4][3] + P[0][3]*SF[5] + P[1][3]*SF[3] - P[3][3]*SF[4] + P[2][3]*SPP[0] + P[13][3]*SPP[3] + P[14][3]*SPP[6] - P[15][3]*SPP[9]) + SPP[0]*(P[4][2] + P[0][2]*SF[5] + P[1][2]*SF[3] - P[3][2]*SF[4] + P[2][2]*SPP[0] + P[13][2]*SPP[3] + P[14][2]*SPP[6] - P[15][2]*SPP[9]) + SPP[3]*(P[4][13] + P[0][13]*SF[5] + P[1][13]*SF[3] - P[3][13]*SF[4] + P[2][13]*SPP[0] + P[13][13]*SPP[3] + P[14][13]*SPP[6] - P[15][13]*SPP[9]) + SPP[6]*(P[4][14] + P[0][14]*SF[5] + P[1][14]*SF[3] - P[3][14]*SF[4] + P[2][14]*SPP[0] + P[13][14]*SPP[3] + P[14][14]*SPP[6] - P[15][14]*SPP[9]) - SPP[9]*(P[4][15] + P[0][15]*SF[5] + P[1][15]*SF[3] - P[3][15]*SF[4] + P[2][15]*SPP[0] + P[13][15]*SPP[3] + P[14][15]*SPP[6] - P[15][15]*SPP[9]) + dvxVar*sq(SG[1] + SG[2] - SG[3] - SG[4]); +nextP[0][5] = P[0][5] + P[1][5]*SF[9] + P[2][5]*SF[11] + P[3][5]*SF[10] + P[10][5]*SF[14] + P[11][5]*SF[15] + P[12][5]*SPP[10] + SF[4]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SF[3]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SF[5]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) - SPP[0]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) - SPP[8]*(P[0][13] + P[1][13]*SF[9] + P[2][13]*SF[11] + P[3][13]*SF[10] + P[10][13]*SF[14] + P[11][13]*SF[15] + P[12][13]*SPP[10]) + SPP[2]*(P[0][14] + P[1][14]*SF[9] + P[2][14]*SF[11] + P[3][14]*SF[10] + P[10][14]*SF[14] + P[11][14]*SF[15] + P[12][14]*SPP[10]) + SPP[5]*(P[0][15] + P[1][15]*SF[9] + P[2][15]*SF[11] + P[3][15]*SF[10] + P[10][15]*SF[14] + P[11][15]*SF[15] + P[12][15]*SPP[10]); +nextP[1][5] = P[1][5] + P[0][5]*SF[8] + P[2][5]*SF[7] + P[3][5]*SF[11] - P[12][5]*SF[15] + P[11][5]*SPP[10] - (P[10][5]*q0)*0.5f + SF[4]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SF[3]*(P[1][2] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2) + SF[5]*(P[1][3] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2) - SPP[0]*(P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] - (P[10][1]*q0)/2) - SPP[8]*(P[1][13] + P[0][13]*SF[8] + P[2][13]*SF[7] + P[3][13]*SF[11] - P[12][13]*SF[15] + P[11][13]*SPP[10] - (P[10][13]*q0)/2) + SPP[2]*(P[1][14] + P[0][14]*SF[8] + P[2][14]*SF[7] + P[3][14]*SF[11] - P[12][14]*SF[15] + P[11][14]*SPP[10] - (P[10][14]*q0)/2) + SPP[5]*(P[1][15] + P[0][15]*SF[8] + P[2][15]*SF[7] + P[3][15]*SF[11] - P[12][15]*SF[15] + P[11][15]*SPP[10] - (P[10][15]*q0)/2); +nextP[2][5] = P[2][5] + P[0][5]*SF[6] + P[1][5]*SF[10] + P[3][5]*SF[8] + P[12][5]*SF[14] - P[10][5]*SPP[10] - (P[11][5]*q0)*0.5f + SF[4]*(P[2][0] + P[0][0]*SF[6] + P[1][0]*SF[10] + P[3][0]*SF[8] + P[12][0]*SF[14] - P[10][0]*SPP[10] - (P[11][0]*q0)/2) + SF[3]*(P[2][2] + P[0][2]*SF[6] + P[1][2]*SF[10] + P[3][2]*SF[8] + P[12][2]*SF[14] - P[10][2]*SPP[10] - (P[11][2]*q0)/2) + SF[5]*(P[2][3] + P[0][3]*SF[6] + P[1][3]*SF[10] + P[3][3]*SF[8] + P[12][3]*SF[14] - P[10][3]*SPP[10] - (P[11][3]*q0)/2) - SPP[0]*(P[2][1] + P[0][1]*SF[6] + P[1][1]*SF[10] + P[3][1]*SF[8] + P[12][1]*SF[14] - P[10][1]*SPP[10] - (P[11][1]*q0)/2) - SPP[8]*(P[2][13] + P[0][13]*SF[6] + P[1][13]*SF[10] + P[3][13]*SF[8] + P[12][13]*SF[14] - P[10][13]*SPP[10] - (P[11][13]*q0)/2) + SPP[2]*(P[2][14] + P[0][14]*SF[6] + P[1][14]*SF[10] + P[3][14]*SF[8] + P[12][14]*SF[14] - P[10][14]*SPP[10] - (P[11][14]*q0)/2) + SPP[5]*(P[2][15] + P[0][15]*SF[6] + P[1][15]*SF[10] + P[3][15]*SF[8] + P[12][15]*SF[14] - P[10][15]*SPP[10] - (P[11][15]*q0)/2); +nextP[3][5] = P[3][5] + P[0][5]*SF[7] + P[1][5]*SF[6] + P[2][5]*SF[9] + P[10][5]*SF[15] - P[11][5]*SF[14] - (P[12][5]*q0)*0.5f + SF[4]*(P[3][0] + P[0][0]*SF[7] + P[1][0]*SF[6] + P[2][0]*SF[9] + P[10][0]*SF[15] - P[11][0]*SF[14] - (P[12][0]*q0)/2) + SF[3]*(P[3][2] + P[0][2]*SF[7] + P[1][2]*SF[6] + P[2][2]*SF[9] + P[10][2]*SF[15] - P[11][2]*SF[14] - (P[12][2]*q0)/2) + SF[5]*(P[3][3] + P[0][3]*SF[7] + P[1][3]*SF[6] + P[2][3]*SF[9] + P[10][3]*SF[15] - P[11][3]*SF[14] - (P[12][3]*q0)/2) - SPP[0]*(P[3][1] + P[0][1]*SF[7] + P[1][1]*SF[6] + P[2][1]*SF[9] + P[10][1]*SF[15] - P[11][1]*SF[14] - (P[12][1]*q0)/2) - SPP[8]*(P[3][13] + P[0][13]*SF[7] + P[1][13]*SF[6] + P[2][13]*SF[9] + P[10][13]*SF[15] - P[11][13]*SF[14] - (P[12][13]*q0)/2) + SPP[2]*(P[3][14] + P[0][14]*SF[7] + P[1][14]*SF[6] + P[2][14]*SF[9] + P[10][14]*SF[15] - P[11][14]*SF[14] - (P[12][14]*q0)/2) + SPP[5]*(P[3][15] + P[0][15]*SF[7] + P[1][15]*SF[6] + P[2][15]*SF[9] + P[10][15]*SF[15] - P[11][15]*SF[14] - (P[12][15]*q0)/2); +nextP[4][5] = P[4][5] + SQ[2] + P[0][5]*SF[5] + P[1][5]*SF[3] - P[3][5]*SF[4] + P[2][5]*SPP[0] + P[13][5]*SPP[3] + P[14][5]*SPP[6] - P[15][5]*SPP[9] + SF[4]*(P[4][0] + P[0][0]*SF[5] + P[1][0]*SF[3] - P[3][0]*SF[4] + P[2][0]*SPP[0] + P[13][0]*SPP[3] + P[14][0]*SPP[6] - P[15][0]*SPP[9]) + SF[3]*(P[4][2] + P[0][2]*SF[5] + P[1][2]*SF[3] - P[3][2]*SF[4] + P[2][2]*SPP[0] + P[13][2]*SPP[3] + P[14][2]*SPP[6] - P[15][2]*SPP[9]) + SF[5]*(P[4][3] + P[0][3]*SF[5] + P[1][3]*SF[3] - P[3][3]*SF[4] + P[2][3]*SPP[0] + P[13][3]*SPP[3] + P[14][3]*SPP[6] - P[15][3]*SPP[9]) - SPP[0]*(P[4][1] + P[0][1]*SF[5] + P[1][1]*SF[3] - P[3][1]*SF[4] + P[2][1]*SPP[0] + P[13][1]*SPP[3] + P[14][1]*SPP[6] - P[15][1]*SPP[9]) - SPP[8]*(P[4][13] + P[0][13]*SF[5] + P[1][13]*SF[3] - P[3][13]*SF[4] + P[2][13]*SPP[0] + P[13][13]*SPP[3] + P[14][13]*SPP[6] - P[15][13]*SPP[9]) + SPP[2]*(P[4][14] + P[0][14]*SF[5] + P[1][14]*SF[3] - P[3][14]*SF[4] + P[2][14]*SPP[0] + P[13][14]*SPP[3] + P[14][14]*SPP[6] - P[15][14]*SPP[9]) + SPP[5]*(P[4][15] + P[0][15]*SF[5] + P[1][15]*SF[3] - P[3][15]*SF[4] + P[2][15]*SPP[0] + P[13][15]*SPP[3] + P[14][15]*SPP[6] - P[15][15]*SPP[9]); +nextP[5][5] = P[5][5] + P[0][5]*SF[4] + P[2][5]*SF[3] + P[3][5]*SF[5] - P[1][5]*SPP[0] - P[13][5]*SPP[8] + P[14][5]*SPP[2] + P[15][5]*SPP[5] + dvxVar*sq(SG[7] + 2.0f*q0*q3) + dvzVar*sq(SG[5] - 2.0f*q0*q1) + SF[4]*(P[5][0] + P[0][0]*SF[4] + P[2][0]*SF[3] + P[3][0]*SF[5] - P[1][0]*SPP[0] - P[13][0]*SPP[8] + P[14][0]*SPP[2] + P[15][0]*SPP[5]) + SF[3]*(P[5][2] + P[0][2]*SF[4] + P[2][2]*SF[3] + P[3][2]*SF[5] - P[1][2]*SPP[0] - P[13][2]*SPP[8] + P[14][2]*SPP[2] + P[15][2]*SPP[5]) + SF[5]*(P[5][3] + P[0][3]*SF[4] + P[2][3]*SF[3] + P[3][3]*SF[5] - P[1][3]*SPP[0] - P[13][3]*SPP[8] + P[14][3]*SPP[2] + P[15][3]*SPP[5]) - SPP[0]*(P[5][1] + P[0][1]*SF[4] + P[2][1]*SF[3] + P[3][1]*SF[5] - P[1][1]*SPP[0] - P[13][1]*SPP[8] + P[14][1]*SPP[2] + P[15][1]*SPP[5]) - SPP[8]*(P[5][13] + P[0][13]*SF[4] + P[2][13]*SF[3] + P[3][13]*SF[5] - P[1][13]*SPP[0] - P[13][13]*SPP[8] + P[14][13]*SPP[2] + P[15][13]*SPP[5]) + SPP[2]*(P[5][14] + P[0][14]*SF[4] + P[2][14]*SF[3] + P[3][14]*SF[5] - P[1][14]*SPP[0] - P[13][14]*SPP[8] + P[14][14]*SPP[2] + P[15][14]*SPP[5]) + SPP[5]*(P[5][15] + P[0][15]*SF[4] + P[2][15]*SF[3] + P[3][15]*SF[5] - P[1][15]*SPP[0] - P[13][15]*SPP[8] + P[14][15]*SPP[2] + P[15][15]*SPP[5]) + dvyVar*sq(SG[1] - SG[2] + SG[3] - SG[4]); +nextP[0][6] = P[0][6] + P[1][6]*SF[9] + P[2][6]*SF[11] + P[3][6]*SF[10] + P[10][6]*SF[14] + P[11][6]*SF[15] + P[12][6]*SPP[10] + SF[4]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) - SF[5]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SF[3]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) + SPP[0]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SPP[4]*(P[0][13] + P[1][13]*SF[9] + P[2][13]*SF[11] + P[3][13]*SF[10] + P[10][13]*SF[14] + P[11][13]*SF[15] + P[12][13]*SPP[10]) - SPP[7]*(P[0][14] + P[1][14]*SF[9] + P[2][14]*SF[11] + P[3][14]*SF[10] + P[10][14]*SF[14] + P[11][14]*SF[15] + P[12][14]*SPP[10]) - SPP[1]*(P[0][15] + P[1][15]*SF[9] + P[2][15]*SF[11] + P[3][15]*SF[10] + P[10][15]*SF[14] + P[11][15]*SF[15] + P[12][15]*SPP[10]); +nextP[1][6] = P[1][6] + P[0][6]*SF[8] + P[2][6]*SF[7] + P[3][6]*SF[11] - P[12][6]*SF[15] + P[11][6]*SPP[10] - (P[10][6]*q0)*0.5f + SF[4]*(P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] - (P[10][1]*q0)/2) - SF[5]*(P[1][2] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2) + SF[3]*(P[1][3] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2) + SPP[0]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SPP[4]*(P[1][13] + P[0][13]*SF[8] + P[2][13]*SF[7] + P[3][13]*SF[11] - P[12][13]*SF[15] + P[11][13]*SPP[10] - (P[10][13]*q0)/2) - SPP[7]*(P[1][14] + P[0][14]*SF[8] + P[2][14]*SF[7] + P[3][14]*SF[11] - P[12][14]*SF[15] + P[11][14]*SPP[10] - (P[10][14]*q0)/2) - SPP[1]*(P[1][15] + P[0][15]*SF[8] + P[2][15]*SF[7] + P[3][15]*SF[11] - P[12][15]*SF[15] + P[11][15]*SPP[10] - (P[10][15]*q0)/2); +nextP[2][6] = P[2][6] + P[0][6]*SF[6] + P[1][6]*SF[10] + P[3][6]*SF[8] + P[12][6]*SF[14] - P[10][6]*SPP[10] - (P[11][6]*q0)*0.5f + SF[4]*(P[2][1] + P[0][1]*SF[6] + P[1][1]*SF[10] + P[3][1]*SF[8] + P[12][1]*SF[14] - P[10][1]*SPP[10] - (P[11][1]*q0)/2) - SF[5]*(P[2][2] + P[0][2]*SF[6] + P[1][2]*SF[10] + P[3][2]*SF[8] + P[12][2]*SF[14] - P[10][2]*SPP[10] - (P[11][2]*q0)/2) + SF[3]*(P[2][3] + P[0][3]*SF[6] + P[1][3]*SF[10] + P[3][3]*SF[8] + P[12][3]*SF[14] - P[10][3]*SPP[10] - (P[11][3]*q0)/2) + SPP[0]*(P[2][0] + P[0][0]*SF[6] + P[1][0]*SF[10] + P[3][0]*SF[8] + P[12][0]*SF[14] - P[10][0]*SPP[10] - (P[11][0]*q0)/2) + SPP[4]*(P[2][13] + P[0][13]*SF[6] + P[1][13]*SF[10] + P[3][13]*SF[8] + P[12][13]*SF[14] - P[10][13]*SPP[10] - (P[11][13]*q0)/2) - SPP[7]*(P[2][14] + P[0][14]*SF[6] + P[1][14]*SF[10] + P[3][14]*SF[8] + P[12][14]*SF[14] - P[10][14]*SPP[10] - (P[11][14]*q0)/2) - SPP[1]*(P[2][15] + P[0][15]*SF[6] + P[1][15]*SF[10] + P[3][15]*SF[8] + P[12][15]*SF[14] - P[10][15]*SPP[10] - (P[11][15]*q0)/2); +nextP[3][6] = P[3][6] + P[0][6]*SF[7] + P[1][6]*SF[6] + P[2][6]*SF[9] + P[10][6]*SF[15] - P[11][6]*SF[14] - (P[12][6]*q0)*0.5f + SF[4]*(P[3][1] + P[0][1]*SF[7] + P[1][1]*SF[6] + P[2][1]*SF[9] + P[10][1]*SF[15] - P[11][1]*SF[14] - (P[12][1]*q0)/2) - SF[5]*(P[3][2] + P[0][2]*SF[7] + P[1][2]*SF[6] + P[2][2]*SF[9] + P[10][2]*SF[15] - P[11][2]*SF[14] - (P[12][2]*q0)/2) + SF[3]*(P[3][3] + P[0][3]*SF[7] + P[1][3]*SF[6] + P[2][3]*SF[9] + P[10][3]*SF[15] - P[11][3]*SF[14] - (P[12][3]*q0)/2) + SPP[0]*(P[3][0] + P[0][0]*SF[7] + P[1][0]*SF[6] + P[2][0]*SF[9] + P[10][0]*SF[15] - P[11][0]*SF[14] - (P[12][0]*q0)/2) + SPP[4]*(P[3][13] + P[0][13]*SF[7] + P[1][13]*SF[6] + P[2][13]*SF[9] + P[10][13]*SF[15] - P[11][13]*SF[14] - (P[12][13]*q0)/2) - SPP[7]*(P[3][14] + P[0][14]*SF[7] + P[1][14]*SF[6] + P[2][14]*SF[9] + P[10][14]*SF[15] - P[11][14]*SF[14] - (P[12][14]*q0)/2) - SPP[1]*(P[3][15] + P[0][15]*SF[7] + P[1][15]*SF[6] + P[2][15]*SF[9] + P[10][15]*SF[15] - P[11][15]*SF[14] - (P[12][15]*q0)/2); +nextP[4][6] = P[4][6] + SQ[1] + P[0][6]*SF[5] + P[1][6]*SF[3] - P[3][6]*SF[4] + P[2][6]*SPP[0] + P[13][6]*SPP[3] + P[14][6]*SPP[6] - P[15][6]*SPP[9] + SF[4]*(P[4][1] + P[0][1]*SF[5] + P[1][1]*SF[3] - P[3][1]*SF[4] + P[2][1]*SPP[0] + P[13][1]*SPP[3] + P[14][1]*SPP[6] - P[15][1]*SPP[9]) - SF[5]*(P[4][2] + P[0][2]*SF[5] + P[1][2]*SF[3] - P[3][2]*SF[4] + P[2][2]*SPP[0] + P[13][2]*SPP[3] + P[14][2]*SPP[6] - P[15][2]*SPP[9]) + SF[3]*(P[4][3] + P[0][3]*SF[5] + P[1][3]*SF[3] - P[3][3]*SF[4] + P[2][3]*SPP[0] + P[13][3]*SPP[3] + P[14][3]*SPP[6] - P[15][3]*SPP[9]) + SPP[0]*(P[4][0] + P[0][0]*SF[5] + P[1][0]*SF[3] - P[3][0]*SF[4] + P[2][0]*SPP[0] + P[13][0]*SPP[3] + P[14][0]*SPP[6] - P[15][0]*SPP[9]) + SPP[4]*(P[4][13] + P[0][13]*SF[5] + P[1][13]*SF[3] - P[3][13]*SF[4] + P[2][13]*SPP[0] + P[13][13]*SPP[3] + P[14][13]*SPP[6] - P[15][13]*SPP[9]) - SPP[7]*(P[4][14] + P[0][14]*SF[5] + P[1][14]*SF[3] - P[3][14]*SF[4] + P[2][14]*SPP[0] + P[13][14]*SPP[3] + P[14][14]*SPP[6] - P[15][14]*SPP[9]) - SPP[1]*(P[4][15] + P[0][15]*SF[5] + P[1][15]*SF[3] - P[3][15]*SF[4] + P[2][15]*SPP[0] + P[13][15]*SPP[3] + P[14][15]*SPP[6] - P[15][15]*SPP[9]); +nextP[5][6] = P[5][6] + SQ[0] + P[0][6]*SF[4] + P[2][6]*SF[3] + P[3][6]*SF[5] - P[1][6]*SPP[0] - P[13][6]*SPP[8] + P[14][6]*SPP[2] + P[15][6]*SPP[5] + SF[4]*(P[5][1] + P[0][1]*SF[4] + P[2][1]*SF[3] + P[3][1]*SF[5] - P[1][1]*SPP[0] - P[13][1]*SPP[8] + P[14][1]*SPP[2] + P[15][1]*SPP[5]) - SF[5]*(P[5][2] + P[0][2]*SF[4] + P[2][2]*SF[3] + P[3][2]*SF[5] - P[1][2]*SPP[0] - P[13][2]*SPP[8] + P[14][2]*SPP[2] + P[15][2]*SPP[5]) + SF[3]*(P[5][3] + P[0][3]*SF[4] + P[2][3]*SF[3] + P[3][3]*SF[5] - P[1][3]*SPP[0] - P[13][3]*SPP[8] + P[14][3]*SPP[2] + P[15][3]*SPP[5]) + SPP[0]*(P[5][0] + P[0][0]*SF[4] + P[2][0]*SF[3] + P[3][0]*SF[5] - P[1][0]*SPP[0] - P[13][0]*SPP[8] + P[14][0]*SPP[2] + P[15][0]*SPP[5]) + SPP[4]*(P[5][13] + P[0][13]*SF[4] + P[2][13]*SF[3] + P[3][13]*SF[5] - P[1][13]*SPP[0] - P[13][13]*SPP[8] + P[14][13]*SPP[2] + P[15][13]*SPP[5]) - SPP[7]*(P[5][14] + P[0][14]*SF[4] + P[2][14]*SF[3] + P[3][14]*SF[5] - P[1][14]*SPP[0] - P[13][14]*SPP[8] + P[14][14]*SPP[2] + P[15][14]*SPP[5]) - SPP[1]*(P[5][15] + P[0][15]*SF[4] + P[2][15]*SF[3] + P[3][15]*SF[5] - P[1][15]*SPP[0] - P[13][15]*SPP[8] + P[14][15]*SPP[2] + P[15][15]*SPP[5]); +nextP[6][6] = P[6][6] + P[1][6]*SF[4] - P[2][6]*SF[5] + P[3][6]*SF[3] + P[0][6]*SPP[0] + P[13][6]*SPP[4] - P[14][6]*SPP[7] - P[15][6]*SPP[1] + dvxVar*sq(SG[6] - 2.0f*q0*q2) + dvyVar*sq(SG[5] + 2.0f*q0*q1) + SF[4]*(P[6][1] + P[1][1]*SF[4] - P[2][1]*SF[5] + P[3][1]*SF[3] + P[0][1]*SPP[0] + P[13][1]*SPP[4] - P[14][1]*SPP[7] - P[15][1]*SPP[1]) - SF[5]*(P[6][2] + P[1][2]*SF[4] - P[2][2]*SF[5] + P[3][2]*SF[3] + P[0][2]*SPP[0] + P[13][2]*SPP[4] - P[14][2]*SPP[7] - P[15][2]*SPP[1]) + SF[3]*(P[6][3] + P[1][3]*SF[4] - P[2][3]*SF[5] + P[3][3]*SF[3] + P[0][3]*SPP[0] + P[13][3]*SPP[4] - P[14][3]*SPP[7] - P[15][3]*SPP[1]) + SPP[0]*(P[6][0] + P[1][0]*SF[4] - P[2][0]*SF[5] + P[3][0]*SF[3] + P[0][0]*SPP[0] + P[13][0]*SPP[4] - P[14][0]*SPP[7] - P[15][0]*SPP[1]) + SPP[4]*(P[6][13] + P[1][13]*SF[4] - P[2][13]*SF[5] + P[3][13]*SF[3] + P[0][13]*SPP[0] + P[13][13]*SPP[4] - P[14][13]*SPP[7] - P[15][13]*SPP[1]) - SPP[7]*(P[6][14] + P[1][14]*SF[4] - P[2][14]*SF[5] + P[3][14]*SF[3] + P[0][14]*SPP[0] + P[13][14]*SPP[4] - P[14][14]*SPP[7] - P[15][14]*SPP[1]) - SPP[1]*(P[6][15] + P[1][15]*SF[4] - P[2][15]*SF[5] + P[3][15]*SF[3] + P[0][15]*SPP[0] + P[13][15]*SPP[4] - P[14][15]*SPP[7] - P[15][15]*SPP[1]) + dvzVar*sq(SG[1] - SG[2] - SG[3] + SG[4]); +nextP[0][7] = P[0][7] + P[1][7]*SF[9] + P[2][7]*SF[11] + P[3][7]*SF[10] + P[10][7]*SF[14] + P[11][7]*SF[15] + P[12][7]*SPP[10] + dt*(P[0][4] + P[1][4]*SF[9] + P[2][4]*SF[11] + P[3][4]*SF[10] + P[10][4]*SF[14] + P[11][4]*SF[15] + P[12][4]*SPP[10]); +nextP[1][7] = P[1][7] + P[0][7]*SF[8] + P[2][7]*SF[7] + P[3][7]*SF[11] - P[12][7]*SF[15] + P[11][7]*SPP[10] - (P[10][7]*q0)*0.5f + dt*(P[1][4] + P[0][4]*SF[8] + P[2][4]*SF[7] + P[3][4]*SF[11] - P[12][4]*SF[15] + P[11][4]*SPP[10] - (P[10][4]*q0)/2); +nextP[2][7] = P[2][7] + P[0][7]*SF[6] + P[1][7]*SF[10] + P[3][7]*SF[8] + P[12][7]*SF[14] - P[10][7]*SPP[10] - (P[11][7]*q0)*0.5f + dt*(P[2][4] + P[0][4]*SF[6] + P[1][4]*SF[10] + P[3][4]*SF[8] + P[12][4]*SF[14] - P[10][4]*SPP[10] - (P[11][4]*q0)/2); +nextP[3][7] = P[3][7] + P[0][7]*SF[7] + P[1][7]*SF[6] + P[2][7]*SF[9] + P[10][7]*SF[15] - P[11][7]*SF[14] - (P[12][7]*q0)*0.5f + dt*(P[3][4] + P[0][4]*SF[7] + P[1][4]*SF[6] + P[2][4]*SF[9] + P[10][4]*SF[15] - P[11][4]*SF[14] - (P[12][4]*q0)/2); +nextP[4][7] = P[4][7] + P[0][7]*SF[5] + P[1][7]*SF[3] - P[3][7]*SF[4] + P[2][7]*SPP[0] + P[13][7]*SPP[3] + P[14][7]*SPP[6] - P[15][7]*SPP[9] + dt*(P[4][4] + P[0][4]*SF[5] + P[1][4]*SF[3] - P[3][4]*SF[4] + P[2][4]*SPP[0] + P[13][4]*SPP[3] + P[14][4]*SPP[6] - P[15][4]*SPP[9]); +nextP[5][7] = P[5][7] + P[0][7]*SF[4] + P[2][7]*SF[3] + P[3][7]*SF[5] - P[1][7]*SPP[0] - P[13][7]*SPP[8] + P[14][7]*SPP[2] + P[15][7]*SPP[5] + dt*(P[5][4] + P[0][4]*SF[4] + P[2][4]*SF[3] + P[3][4]*SF[5] - P[1][4]*SPP[0] - P[13][4]*SPP[8] + P[14][4]*SPP[2] + P[15][4]*SPP[5]); +nextP[6][7] = P[6][7] + P[1][7]*SF[4] - P[2][7]*SF[5] + P[3][7]*SF[3] + P[0][7]*SPP[0] + P[13][7]*SPP[4] - P[14][7]*SPP[7] - P[15][7]*SPP[1] + dt*(P[6][4] + P[1][4]*SF[4] - P[2][4]*SF[5] + P[3][4]*SF[3] + P[0][4]*SPP[0] + P[13][4]*SPP[4] - P[14][4]*SPP[7] - P[15][4]*SPP[1]); nextP[7][7] = P[7][7] + P[4][7]*dt + dt*(P[7][4] + P[4][4]*dt); -nextP[0][8] = P[0][8]*SPP[5] - P[1][8]*SPP[4] + P[2][8]*SPP[7] + P[9][8]*SPP[22] + P[12][8]*SPP[18] + dt*(P[0][5]*SPP[5] - P[1][5]*SPP[4] + P[2][5]*SPP[7] + P[9][5]*SPP[22] + P[12][5]*SPP[18]); -nextP[1][8] = P[1][8]*SPP[6] - P[0][8]*SPP[2] - P[2][8]*SPP[8] + P[10][8]*SPP[22] + P[13][8]*SPP[17] + dt*(P[1][5]*SPP[6] - P[0][5]*SPP[2] - P[2][5]*SPP[8] + P[10][5]*SPP[22] + P[13][5]*SPP[17]); -nextP[2][8] = P[0][8]*SPP[14] - P[1][8]*SPP[3] + P[2][8]*SPP[13] + P[11][8]*SPP[22] + P[14][8]*SPP[16] + dt*(P[0][5]*SPP[14] - P[1][5]*SPP[3] + P[2][5]*SPP[13] + P[11][5]*SPP[22] + P[14][5]*SPP[16]); -nextP[3][8] = P[3][8] + P[0][8]*SPP[1] + P[1][8]*SPP[19] + P[2][8]*SPP[15] - P[15][8]*SPP[21] + dt*(P[3][5] + P[0][5]*SPP[1] + P[1][5]*SPP[19] + P[2][5]*SPP[15] - P[15][5]*SPP[21]); -nextP[4][8] = P[4][8] + P[15][8]*SF[22] + P[0][8]*SPP[20] + P[1][8]*SPP[12] + P[2][8]*SPP[11] + dt*(P[4][5] + P[15][5]*SF[22] + P[0][5]*SPP[20] + P[1][5]*SPP[12] + P[2][5]*SPP[11]); -nextP[5][8] = P[5][8] + P[15][8]*SF[20] - P[0][8]*SPP[9] + P[1][8]*SPP[10] + P[2][8]*SPP[0] + dt*(P[5][5] + P[15][5]*SF[20] - P[0][5]*SPP[9] + P[1][5]*SPP[10] + P[2][5]*SPP[0]); -nextP[6][8] = P[6][8] + P[3][8]*dt + dt*(P[6][5] + P[3][5]*dt); +nextP[0][8] = P[0][8] + P[1][8]*SF[9] + P[2][8]*SF[11] + P[3][8]*SF[10] + P[10][8]*SF[14] + P[11][8]*SF[15] + P[12][8]*SPP[10] + dt*(P[0][5] + P[1][5]*SF[9] + P[2][5]*SF[11] + P[3][5]*SF[10] + P[10][5]*SF[14] + P[11][5]*SF[15] + P[12][5]*SPP[10]); +nextP[1][8] = P[1][8] + P[0][8]*SF[8] + P[2][8]*SF[7] + P[3][8]*SF[11] - P[12][8]*SF[15] + P[11][8]*SPP[10] - (P[10][8]*q0)*0.5f + dt*(P[1][5] + P[0][5]*SF[8] + P[2][5]*SF[7] + P[3][5]*SF[11] - P[12][5]*SF[15] + P[11][5]*SPP[10] - (P[10][5]*q0)/2); +nextP[2][8] = P[2][8] + P[0][8]*SF[6] + P[1][8]*SF[10] + P[3][8]*SF[8] + P[12][8]*SF[14] - P[10][8]*SPP[10] - (P[11][8]*q0)*0.5f + dt*(P[2][5] + P[0][5]*SF[6] + P[1][5]*SF[10] + P[3][5]*SF[8] + P[12][5]*SF[14] - P[10][5]*SPP[10] - (P[11][5]*q0)/2); +nextP[3][8] = P[3][8] + P[0][8]*SF[7] + P[1][8]*SF[6] + P[2][8]*SF[9] + P[10][8]*SF[15] - P[11][8]*SF[14] - (P[12][8]*q0)*0.5f + dt*(P[3][5] + P[0][5]*SF[7] + P[1][5]*SF[6] + P[2][5]*SF[9] + P[10][5]*SF[15] - P[11][5]*SF[14] - (P[12][5]*q0)/2); +nextP[4][8] = P[4][8] + P[0][8]*SF[5] + P[1][8]*SF[3] - P[3][8]*SF[4] + P[2][8]*SPP[0] + P[13][8]*SPP[3] + P[14][8]*SPP[6] - P[15][8]*SPP[9] + dt*(P[4][5] + P[0][5]*SF[5] + P[1][5]*SF[3] - P[3][5]*SF[4] + P[2][5]*SPP[0] + P[13][5]*SPP[3] + P[14][5]*SPP[6] - P[15][5]*SPP[9]); +nextP[5][8] = P[5][8] + P[0][8]*SF[4] + P[2][8]*SF[3] + P[3][8]*SF[5] - P[1][8]*SPP[0] - P[13][8]*SPP[8] + P[14][8]*SPP[2] + P[15][8]*SPP[5] + dt*(P[5][5] + P[0][5]*SF[4] + P[2][5]*SF[3] + P[3][5]*SF[5] - P[1][5]*SPP[0] - P[13][5]*SPP[8] + P[14][5]*SPP[2] + P[15][5]*SPP[5]); +nextP[6][8] = P[6][8] + P[1][8]*SF[4] - P[2][8]*SF[5] + P[3][8]*SF[3] + P[0][8]*SPP[0] + P[13][8]*SPP[4] - P[14][8]*SPP[7] - P[15][8]*SPP[1] + dt*(P[6][5] + P[1][5]*SF[4] - P[2][5]*SF[5] + P[3][5]*SF[3] + P[0][5]*SPP[0] + P[13][5]*SPP[4] - P[14][5]*SPP[7] - P[15][5]*SPP[1]); nextP[7][8] = P[7][8] + P[4][8]*dt + dt*(P[7][5] + P[4][5]*dt); nextP[8][8] = P[8][8] + P[5][8]*dt + dt*(P[8][5] + P[5][5]*dt); -nextP[0][9] = P[0][9]*SPP[5] - P[1][9]*SPP[4] + P[2][9]*SPP[7] + P[9][9]*SPP[22] + P[12][9]*SPP[18]; -nextP[1][9] = P[1][9]*SPP[6] - P[0][9]*SPP[2] - P[2][9]*SPP[8] + P[10][9]*SPP[22] + P[13][9]*SPP[17]; -nextP[2][9] = P[0][9]*SPP[14] - P[1][9]*SPP[3] + P[2][9]*SPP[13] + P[11][9]*SPP[22] + P[14][9]*SPP[16]; -nextP[3][9] = P[3][9] + P[0][9]*SPP[1] + P[1][9]*SPP[19] + P[2][9]*SPP[15] - P[15][9]*SPP[21]; -nextP[4][9] = P[4][9] + P[15][9]*SF[22] + P[0][9]*SPP[20] + P[1][9]*SPP[12] + P[2][9]*SPP[11]; -nextP[5][9] = P[5][9] + P[15][9]*SF[20] - P[0][9]*SPP[9] + P[1][9]*SPP[10] + P[2][9]*SPP[0]; -nextP[6][9] = P[6][9] + P[3][9]*dt; -nextP[7][9] = P[7][9] + P[4][9]*dt; -nextP[8][9] = P[8][9] + P[5][9]*dt; -nextP[9][9] = P[9][9]; -nextP[0][10] = P[0][10]*SPP[5] - P[1][10]*SPP[4] + P[2][10]*SPP[7] + P[9][10]*SPP[22] + P[12][10]*SPP[18]; -nextP[1][10] = P[1][10]*SPP[6] - P[0][10]*SPP[2] - P[2][10]*SPP[8] + P[10][10]*SPP[22] + P[13][10]*SPP[17]; -nextP[2][10] = P[0][10]*SPP[14] - P[1][10]*SPP[3] + P[2][10]*SPP[13] + P[11][10]*SPP[22] + P[14][10]*SPP[16]; -nextP[3][10] = P[3][10] + P[0][10]*SPP[1] + P[1][10]*SPP[19] + P[2][10]*SPP[15] - P[15][10]*SPP[21]; -nextP[4][10] = P[4][10] + P[15][10]*SF[22] + P[0][10]*SPP[20] + P[1][10]*SPP[12] + P[2][10]*SPP[11]; -nextP[5][10] = P[5][10] + P[15][10]*SF[20] - P[0][10]*SPP[9] + P[1][10]*SPP[10] + P[2][10]*SPP[0]; -nextP[6][10] = P[6][10] + P[3][10]*dt; +nextP[0][9] = P[0][9] + P[1][9]*SF[9] + P[2][9]*SF[11] + P[3][9]*SF[10] + P[10][9]*SF[14] + P[11][9]*SF[15] + P[12][9]*SPP[10] + dt*(P[0][6] + P[1][6]*SF[9] + P[2][6]*SF[11] + P[3][6]*SF[10] + P[10][6]*SF[14] + P[11][6]*SF[15] + P[12][6]*SPP[10]); +nextP[1][9] = P[1][9] + P[0][9]*SF[8] + P[2][9]*SF[7] + P[3][9]*SF[11] - P[12][9]*SF[15] + P[11][9]*SPP[10] - (P[10][9]*q0)*0.5f + dt*(P[1][6] + P[0][6]*SF[8] + P[2][6]*SF[7] + P[3][6]*SF[11] - P[12][6]*SF[15] + P[11][6]*SPP[10] - (P[10][6]*q0)/2); +nextP[2][9] = P[2][9] + P[0][9]*SF[6] + P[1][9]*SF[10] + P[3][9]*SF[8] + P[12][9]*SF[14] - P[10][9]*SPP[10] - (P[11][9]*q0)*0.5f + dt*(P[2][6] + P[0][6]*SF[6] + P[1][6]*SF[10] + P[3][6]*SF[8] + P[12][6]*SF[14] - P[10][6]*SPP[10] - (P[11][6]*q0)/2); +nextP[3][9] = P[3][9] + P[0][9]*SF[7] + P[1][9]*SF[6] + P[2][9]*SF[9] + P[10][9]*SF[15] - P[11][9]*SF[14] - (P[12][9]*q0)*0.5f + dt*(P[3][6] + P[0][6]*SF[7] + P[1][6]*SF[6] + P[2][6]*SF[9] + P[10][6]*SF[15] - P[11][6]*SF[14] - (P[12][6]*q0)/2); +nextP[4][9] = P[4][9] + P[0][9]*SF[5] + P[1][9]*SF[3] - P[3][9]*SF[4] + P[2][9]*SPP[0] + P[13][9]*SPP[3] + P[14][9]*SPP[6] - P[15][9]*SPP[9] + dt*(P[4][6] + P[0][6]*SF[5] + P[1][6]*SF[3] - P[3][6]*SF[4] + P[2][6]*SPP[0] + P[13][6]*SPP[3] + P[14][6]*SPP[6] - P[15][6]*SPP[9]); +nextP[5][9] = P[5][9] + P[0][9]*SF[4] + P[2][9]*SF[3] + P[3][9]*SF[5] - P[1][9]*SPP[0] - P[13][9]*SPP[8] + P[14][9]*SPP[2] + P[15][9]*SPP[5] + dt*(P[5][6] + P[0][6]*SF[4] + P[2][6]*SF[3] + P[3][6]*SF[5] - P[1][6]*SPP[0] - P[13][6]*SPP[8] + P[14][6]*SPP[2] + P[15][6]*SPP[5]); +nextP[6][9] = P[6][9] + P[1][9]*SF[4] - P[2][9]*SF[5] + P[3][9]*SF[3] + P[0][9]*SPP[0] + P[13][9]*SPP[4] - P[14][9]*SPP[7] - P[15][9]*SPP[1] + dt*(P[6][6] + P[1][6]*SF[4] - P[2][6]*SF[5] + P[3][6]*SF[3] + P[0][6]*SPP[0] + P[13][6]*SPP[4] - P[14][6]*SPP[7] - P[15][6]*SPP[1]); +nextP[7][9] = P[7][9] + P[4][9]*dt + dt*(P[7][6] + P[4][6]*dt); +nextP[8][9] = P[8][9] + P[5][9]*dt + dt*(P[8][6] + P[5][6]*dt); +nextP[9][9] = P[9][9] + P[6][9]*dt + dt*(P[9][6] + P[6][6]*dt); +nextP[0][10] = P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10]; +nextP[1][10] = P[1][10] + P[0][10]*SF[8] + P[2][10]*SF[7] + P[3][10]*SF[11] - P[12][10]*SF[15] + P[11][10]*SPP[10] - (P[10][10]*q0)*0.5f; +nextP[2][10] = P[2][10] + P[0][10]*SF[6] + P[1][10]*SF[10] + P[3][10]*SF[8] + P[12][10]*SF[14] - P[10][10]*SPP[10] - (P[11][10]*q0)*0.5f; +nextP[3][10] = P[3][10] + P[0][10]*SF[7] + P[1][10]*SF[6] + P[2][10]*SF[9] + P[10][10]*SF[15] - P[11][10]*SF[14] - (P[12][10]*q0)*0.5f; +nextP[4][10] = P[4][10] + P[0][10]*SF[5] + P[1][10]*SF[3] - P[3][10]*SF[4] + P[2][10]*SPP[0] + P[13][10]*SPP[3] + P[14][10]*SPP[6] - P[15][10]*SPP[9]; +nextP[5][10] = P[5][10] + P[0][10]*SF[4] + P[2][10]*SF[3] + P[3][10]*SF[5] - P[1][10]*SPP[0] - P[13][10]*SPP[8] + P[14][10]*SPP[2] + P[15][10]*SPP[5]; +nextP[6][10] = P[6][10] + P[1][10]*SF[4] - P[2][10]*SF[5] + P[3][10]*SF[3] + P[0][10]*SPP[0] + P[13][10]*SPP[4] - P[14][10]*SPP[7] - P[15][10]*SPP[1]; nextP[7][10] = P[7][10] + P[4][10]*dt; nextP[8][10] = P[8][10] + P[5][10]*dt; -nextP[9][10] = P[9][10]; +nextP[9][10] = P[9][10] + P[6][10]*dt; nextP[10][10] = P[10][10]; -nextP[0][11] = P[0][11]*SPP[5] - P[1][11]*SPP[4] + P[2][11]*SPP[7] + P[9][11]*SPP[22] + P[12][11]*SPP[18]; -nextP[1][11] = P[1][11]*SPP[6] - P[0][11]*SPP[2] - P[2][11]*SPP[8] + P[10][11]*SPP[22] + P[13][11]*SPP[17]; -nextP[2][11] = P[0][11]*SPP[14] - P[1][11]*SPP[3] + P[2][11]*SPP[13] + P[11][11]*SPP[22] + P[14][11]*SPP[16]; -nextP[3][11] = P[3][11] + P[0][11]*SPP[1] + P[1][11]*SPP[19] + P[2][11]*SPP[15] - P[15][11]*SPP[21]; -nextP[4][11] = P[4][11] + P[15][11]*SF[22] + P[0][11]*SPP[20] + P[1][11]*SPP[12] + P[2][11]*SPP[11]; -nextP[5][11] = P[5][11] + P[15][11]*SF[20] - P[0][11]*SPP[9] + P[1][11]*SPP[10] + P[2][11]*SPP[0]; -nextP[6][11] = P[6][11] + P[3][11]*dt; +nextP[0][11] = P[0][11] + P[1][11]*SF[9] + P[2][11]*SF[11] + P[3][11]*SF[10] + P[10][11]*SF[14] + P[11][11]*SF[15] + P[12][11]*SPP[10]; +nextP[1][11] = P[1][11] + P[0][11]*SF[8] + P[2][11]*SF[7] + P[3][11]*SF[11] - P[12][11]*SF[15] + P[11][11]*SPP[10] - (P[10][11]*q0)*0.5f; +nextP[2][11] = P[2][11] + P[0][11]*SF[6] + P[1][11]*SF[10] + P[3][11]*SF[8] + P[12][11]*SF[14] - P[10][11]*SPP[10] - (P[11][11]*q0)*0.5f; +nextP[3][11] = P[3][11] + P[0][11]*SF[7] + P[1][11]*SF[6] + P[2][11]*SF[9] + P[10][11]*SF[15] - P[11][11]*SF[14] - (P[12][11]*q0)*0.5f; +nextP[4][11] = P[4][11] + P[0][11]*SF[5] + P[1][11]*SF[3] - P[3][11]*SF[4] + P[2][11]*SPP[0] + P[13][11]*SPP[3] + P[14][11]*SPP[6] - P[15][11]*SPP[9]; +nextP[5][11] = P[5][11] + P[0][11]*SF[4] + P[2][11]*SF[3] + P[3][11]*SF[5] - P[1][11]*SPP[0] - P[13][11]*SPP[8] + P[14][11]*SPP[2] + P[15][11]*SPP[5]; +nextP[6][11] = P[6][11] + P[1][11]*SF[4] - P[2][11]*SF[5] + P[3][11]*SF[3] + P[0][11]*SPP[0] + P[13][11]*SPP[4] - P[14][11]*SPP[7] - P[15][11]*SPP[1]; nextP[7][11] = P[7][11] + P[4][11]*dt; nextP[8][11] = P[8][11] + P[5][11]*dt; -nextP[9][11] = P[9][11]; +nextP[9][11] = P[9][11] + P[6][11]*dt; nextP[10][11] = P[10][11]; nextP[11][11] = P[11][11]; -nextP[0][12] = P[0][12]*SPP[5] - P[1][12]*SPP[4] + P[2][12]*SPP[7] + P[9][12]*SPP[22] + P[12][12]*SPP[18]; -nextP[1][12] = P[1][12]*SPP[6] - P[0][12]*SPP[2] - P[2][12]*SPP[8] + P[10][12]*SPP[22] + P[13][12]*SPP[17]; -nextP[2][12] = P[0][12]*SPP[14] - P[1][12]*SPP[3] + P[2][12]*SPP[13] + P[11][12]*SPP[22] + P[14][12]*SPP[16]; -nextP[3][12] = P[3][12] + P[0][12]*SPP[1] + P[1][12]*SPP[19] + P[2][12]*SPP[15] - P[15][12]*SPP[21]; -nextP[4][12] = P[4][12] + P[15][12]*SF[22] + P[0][12]*SPP[20] + P[1][12]*SPP[12] + P[2][12]*SPP[11]; -nextP[5][12] = P[5][12] + P[15][12]*SF[20] - P[0][12]*SPP[9] + P[1][12]*SPP[10] + P[2][12]*SPP[0]; -nextP[6][12] = P[6][12] + P[3][12]*dt; +nextP[0][12] = P[0][12] + P[1][12]*SF[9] + P[2][12]*SF[11] + P[3][12]*SF[10] + P[10][12]*SF[14] + P[11][12]*SF[15] + P[12][12]*SPP[10]; +nextP[1][12] = P[1][12] + P[0][12]*SF[8] + P[2][12]*SF[7] + P[3][12]*SF[11] - P[12][12]*SF[15] + P[11][12]*SPP[10] - (P[10][12]*q0)*0.5f; +nextP[2][12] = P[2][12] + P[0][12]*SF[6] + P[1][12]*SF[10] + P[3][12]*SF[8] + P[12][12]*SF[14] - P[10][12]*SPP[10] - (P[11][12]*q0)*0.5f; +nextP[3][12] = P[3][12] + P[0][12]*SF[7] + P[1][12]*SF[6] + P[2][12]*SF[9] + P[10][12]*SF[15] - P[11][12]*SF[14] - (P[12][12]*q0)*0.5f; +nextP[4][12] = P[4][12] + P[0][12]*SF[5] + P[1][12]*SF[3] - P[3][12]*SF[4] + P[2][12]*SPP[0] + P[13][12]*SPP[3] + P[14][12]*SPP[6] - P[15][12]*SPP[9]; +nextP[5][12] = P[5][12] + P[0][12]*SF[4] + P[2][12]*SF[3] + P[3][12]*SF[5] - P[1][12]*SPP[0] - P[13][12]*SPP[8] + P[14][12]*SPP[2] + P[15][12]*SPP[5]; +nextP[6][12] = P[6][12] + P[1][12]*SF[4] - P[2][12]*SF[5] + P[3][12]*SF[3] + P[0][12]*SPP[0] + P[13][12]*SPP[4] - P[14][12]*SPP[7] - P[15][12]*SPP[1]; nextP[7][12] = P[7][12] + P[4][12]*dt; nextP[8][12] = P[8][12] + P[5][12]*dt; -nextP[9][12] = P[9][12]; +nextP[9][12] = P[9][12] + P[6][12]*dt; nextP[10][12] = P[10][12]; nextP[11][12] = P[11][12]; nextP[12][12] = P[12][12]; -nextP[0][13] = P[0][13]*SPP[5] - P[1][13]*SPP[4] + P[2][13]*SPP[7] + P[9][13]*SPP[22] + P[12][13]*SPP[18]; -nextP[1][13] = P[1][13]*SPP[6] - P[0][13]*SPP[2] - P[2][13]*SPP[8] + P[10][13]*SPP[22] + P[13][13]*SPP[17]; -nextP[2][13] = P[0][13]*SPP[14] - P[1][13]*SPP[3] + P[2][13]*SPP[13] + P[11][13]*SPP[22] + P[14][13]*SPP[16]; -nextP[3][13] = P[3][13] + P[0][13]*SPP[1] + P[1][13]*SPP[19] + P[2][13]*SPP[15] - P[15][13]*SPP[21]; -nextP[4][13] = P[4][13] + P[15][13]*SF[22] + P[0][13]*SPP[20] + P[1][13]*SPP[12] + P[2][13]*SPP[11]; -nextP[5][13] = P[5][13] + P[15][13]*SF[20] - P[0][13]*SPP[9] + P[1][13]*SPP[10] + P[2][13]*SPP[0]; -nextP[6][13] = P[6][13] + P[3][13]*dt; +nextP[0][13] = P[0][13] + P[1][13]*SF[9] + P[2][13]*SF[11] + P[3][13]*SF[10] + P[10][13]*SF[14] + P[11][13]*SF[15] + P[12][13]*SPP[10]; +nextP[1][13] = P[1][13] + P[0][13]*SF[8] + P[2][13]*SF[7] + P[3][13]*SF[11] - P[12][13]*SF[15] + P[11][13]*SPP[10] - (P[10][13]*q0)*0.5f; +nextP[2][13] = P[2][13] + P[0][13]*SF[6] + P[1][13]*SF[10] + P[3][13]*SF[8] + P[12][13]*SF[14] - P[10][13]*SPP[10] - (P[11][13]*q0)*0.5f; +nextP[3][13] = P[3][13] + P[0][13]*SF[7] + P[1][13]*SF[6] + P[2][13]*SF[9] + P[10][13]*SF[15] - P[11][13]*SF[14] - (P[12][13]*q0)*0.5f; +nextP[4][13] = P[4][13] + P[0][13]*SF[5] + P[1][13]*SF[3] - P[3][13]*SF[4] + P[2][13]*SPP[0] + P[13][13]*SPP[3] + P[14][13]*SPP[6] - P[15][13]*SPP[9]; +nextP[5][13] = P[5][13] + P[0][13]*SF[4] + P[2][13]*SF[3] + P[3][13]*SF[5] - P[1][13]*SPP[0] - P[13][13]*SPP[8] + P[14][13]*SPP[2] + P[15][13]*SPP[5]; +nextP[6][13] = P[6][13] + P[1][13]*SF[4] - P[2][13]*SF[5] + P[3][13]*SF[3] + P[0][13]*SPP[0] + P[13][13]*SPP[4] - P[14][13]*SPP[7] - P[15][13]*SPP[1]; nextP[7][13] = P[7][13] + P[4][13]*dt; nextP[8][13] = P[8][13] + P[5][13]*dt; -nextP[9][13] = P[9][13]; +nextP[9][13] = P[9][13] + P[6][13]*dt; nextP[10][13] = P[10][13]; nextP[11][13] = P[11][13]; nextP[12][13] = P[12][13]; nextP[13][13] = P[13][13]; -nextP[0][14] = P[0][14]*SPP[5] - P[1][14]*SPP[4] + P[2][14]*SPP[7] + P[9][14]*SPP[22] + P[12][14]*SPP[18]; -nextP[1][14] = P[1][14]*SPP[6] - P[0][14]*SPP[2] - P[2][14]*SPP[8] + P[10][14]*SPP[22] + P[13][14]*SPP[17]; -nextP[2][14] = P[0][14]*SPP[14] - P[1][14]*SPP[3] + P[2][14]*SPP[13] + P[11][14]*SPP[22] + P[14][14]*SPP[16]; -nextP[3][14] = P[3][14] + P[0][14]*SPP[1] + P[1][14]*SPP[19] + P[2][14]*SPP[15] - P[15][14]*SPP[21]; -nextP[4][14] = P[4][14] + P[15][14]*SF[22] + P[0][14]*SPP[20] + P[1][14]*SPP[12] + P[2][14]*SPP[11]; -nextP[5][14] = P[5][14] + P[15][14]*SF[20] - P[0][14]*SPP[9] + P[1][14]*SPP[10] + P[2][14]*SPP[0]; -nextP[6][14] = P[6][14] + P[3][14]*dt; +nextP[0][14] = P[0][14] + P[1][14]*SF[9] + P[2][14]*SF[11] + P[3][14]*SF[10] + P[10][14]*SF[14] + P[11][14]*SF[15] + P[12][14]*SPP[10]; +nextP[1][14] = P[1][14] + P[0][14]*SF[8] + P[2][14]*SF[7] + P[3][14]*SF[11] - P[12][14]*SF[15] + P[11][14]*SPP[10] - (P[10][14]*q0)*0.5f; +nextP[2][14] = P[2][14] + P[0][14]*SF[6] + P[1][14]*SF[10] + P[3][14]*SF[8] + P[12][14]*SF[14] - P[10][14]*SPP[10] - (P[11][14]*q0)*0.5f; +nextP[3][14] = P[3][14] + P[0][14]*SF[7] + P[1][14]*SF[6] + P[2][14]*SF[9] + P[10][14]*SF[15] - P[11][14]*SF[14] - (P[12][14]*q0)*0.5f; +nextP[4][14] = P[4][14] + P[0][14]*SF[5] + P[1][14]*SF[3] - P[3][14]*SF[4] + P[2][14]*SPP[0] + P[13][14]*SPP[3] + P[14][14]*SPP[6] - P[15][14]*SPP[9]; +nextP[5][14] = P[5][14] + P[0][14]*SF[4] + P[2][14]*SF[3] + P[3][14]*SF[5] - P[1][14]*SPP[0] - P[13][14]*SPP[8] + P[14][14]*SPP[2] + P[15][14]*SPP[5]; +nextP[6][14] = P[6][14] + P[1][14]*SF[4] - P[2][14]*SF[5] + P[3][14]*SF[3] + P[0][14]*SPP[0] + P[13][14]*SPP[4] - P[14][14]*SPP[7] - P[15][14]*SPP[1]; nextP[7][14] = P[7][14] + P[4][14]*dt; nextP[8][14] = P[8][14] + P[5][14]*dt; -nextP[9][14] = P[9][14]; +nextP[9][14] = P[9][14] + P[6][14]*dt; nextP[10][14] = P[10][14]; nextP[11][14] = P[11][14]; nextP[12][14] = P[12][14]; nextP[13][14] = P[13][14]; nextP[14][14] = P[14][14]; -nextP[0][15] = P[0][15]*SPP[5] - P[1][15]*SPP[4] + P[2][15]*SPP[7] + P[9][15]*SPP[22] + P[12][15]*SPP[18]; -nextP[1][15] = P[1][15]*SPP[6] - P[0][15]*SPP[2] - P[2][15]*SPP[8] + P[10][15]*SPP[22] + P[13][15]*SPP[17]; -nextP[2][15] = P[0][15]*SPP[14] - P[1][15]*SPP[3] + P[2][15]*SPP[13] + P[11][15]*SPP[22] + P[14][15]*SPP[16]; -nextP[3][15] = P[3][15] + P[0][15]*SPP[1] + P[1][15]*SPP[19] + P[2][15]*SPP[15] - P[15][15]*SPP[21]; -nextP[4][15] = P[4][15] + P[15][15]*SF[22] + P[0][15]*SPP[20] + P[1][15]*SPP[12] + P[2][15]*SPP[11]; -nextP[5][15] = P[5][15] + P[15][15]*SF[20] - P[0][15]*SPP[9] + P[1][15]*SPP[10] + P[2][15]*SPP[0]; -nextP[6][15] = P[6][15] + P[3][15]*dt; +nextP[0][15] = P[0][15] + P[1][15]*SF[9] + P[2][15]*SF[11] + P[3][15]*SF[10] + P[10][15]*SF[14] + P[11][15]*SF[15] + P[12][15]*SPP[10]; +nextP[1][15] = P[1][15] + P[0][15]*SF[8] + P[2][15]*SF[7] + P[3][15]*SF[11] - P[12][15]*SF[15] + P[11][15]*SPP[10] - (P[10][15]*q0)*0.5f; +nextP[2][15] = P[2][15] + P[0][15]*SF[6] + P[1][15]*SF[10] + P[3][15]*SF[8] + P[12][15]*SF[14] - P[10][15]*SPP[10] - (P[11][15]*q0)*0.5f; +nextP[3][15] = P[3][15] + P[0][15]*SF[7] + P[1][15]*SF[6] + P[2][15]*SF[9] + P[10][15]*SF[15] - P[11][15]*SF[14] - (P[12][15]*q0)*0.5f; +nextP[4][15] = P[4][15] + P[0][15]*SF[5] + P[1][15]*SF[3] - P[3][15]*SF[4] + P[2][15]*SPP[0] + P[13][15]*SPP[3] + P[14][15]*SPP[6] - P[15][15]*SPP[9]; +nextP[5][15] = P[5][15] + P[0][15]*SF[4] + P[2][15]*SF[3] + P[3][15]*SF[5] - P[1][15]*SPP[0] - P[13][15]*SPP[8] + P[14][15]*SPP[2] + P[15][15]*SPP[5]; +nextP[6][15] = P[6][15] + P[1][15]*SF[4] - P[2][15]*SF[5] + P[3][15]*SF[3] + P[0][15]*SPP[0] + P[13][15]*SPP[4] - P[14][15]*SPP[7] - P[15][15]*SPP[1]; nextP[7][15] = P[7][15] + P[4][15]*dt; nextP[8][15] = P[8][15] + P[5][15]*dt; -nextP[9][15] = P[9][15]; +nextP[9][15] = P[9][15] + P[6][15]*dt; nextP[10][15] = P[10][15]; nextP[11][15] = P[11][15]; nextP[12][15] = P[12][15]; nextP[13][15] = P[13][15]; nextP[14][15] = P[14][15]; nextP[15][15] = P[15][15]; -nextP[0][16] = P[0][16]*SPP[5] - P[1][16]*SPP[4] + P[2][16]*SPP[7] + P[9][16]*SPP[22] + P[12][16]*SPP[18]; -nextP[1][16] = P[1][16]*SPP[6] - P[0][16]*SPP[2] - P[2][16]*SPP[8] + P[10][16]*SPP[22] + P[13][16]*SPP[17]; -nextP[2][16] = P[0][16]*SPP[14] - P[1][16]*SPP[3] + P[2][16]*SPP[13] + P[11][16]*SPP[22] + P[14][16]*SPP[16]; -nextP[3][16] = P[3][16] + P[0][16]*SPP[1] + P[1][16]*SPP[19] + P[2][16]*SPP[15] - P[15][16]*SPP[21]; -nextP[4][16] = P[4][16] + P[15][16]*SF[22] + P[0][16]*SPP[20] + P[1][16]*SPP[12] + P[2][16]*SPP[11]; -nextP[5][16] = P[5][16] + P[15][16]*SF[20] - P[0][16]*SPP[9] + P[1][16]*SPP[10] + P[2][16]*SPP[0]; -nextP[6][16] = P[6][16] + P[3][16]*dt; +nextP[0][16] = P[0][16] + P[1][16]*SF[9] + P[2][16]*SF[11] + P[3][16]*SF[10] + P[10][16]*SF[14] + P[11][16]*SF[15] + P[12][16]*SPP[10]; +nextP[1][16] = P[1][16] + P[0][16]*SF[8] + P[2][16]*SF[7] + P[3][16]*SF[11] - P[12][16]*SF[15] + P[11][16]*SPP[10] - (P[10][16]*q0)*0.5f; +nextP[2][16] = P[2][16] + P[0][16]*SF[6] + P[1][16]*SF[10] + P[3][16]*SF[8] + P[12][16]*SF[14] - P[10][16]*SPP[10] - (P[11][16]*q0)*0.5f; +nextP[3][16] = P[3][16] + P[0][16]*SF[7] + P[1][16]*SF[6] + P[2][16]*SF[9] + P[10][16]*SF[15] - P[11][16]*SF[14] - (P[12][16]*q0)*0.5f; +nextP[4][16] = P[4][16] + P[0][16]*SF[5] + P[1][16]*SF[3] - P[3][16]*SF[4] + P[2][16]*SPP[0] + P[13][16]*SPP[3] + P[14][16]*SPP[6] - P[15][16]*SPP[9]; +nextP[5][16] = P[5][16] + P[0][16]*SF[4] + P[2][16]*SF[3] + P[3][16]*SF[5] - P[1][16]*SPP[0] - P[13][16]*SPP[8] + P[14][16]*SPP[2] + P[15][16]*SPP[5]; +nextP[6][16] = P[6][16] + P[1][16]*SF[4] - P[2][16]*SF[5] + P[3][16]*SF[3] + P[0][16]*SPP[0] + P[13][16]*SPP[4] - P[14][16]*SPP[7] - P[15][16]*SPP[1]; nextP[7][16] = P[7][16] + P[4][16]*dt; nextP[8][16] = P[8][16] + P[5][16]*dt; -nextP[9][16] = P[9][16]; +nextP[9][16] = P[9][16] + P[6][16]*dt; nextP[10][16] = P[10][16]; nextP[11][16] = P[11][16]; nextP[12][16] = P[12][16]; @@ -230,16 +218,16 @@ nextP[13][16] = P[13][16]; nextP[14][16] = P[14][16]; nextP[15][16] = P[15][16]; nextP[16][16] = P[16][16]; -nextP[0][17] = P[0][17]*SPP[5] - P[1][17]*SPP[4] + P[2][17]*SPP[7] + P[9][17]*SPP[22] + P[12][17]*SPP[18]; -nextP[1][17] = P[1][17]*SPP[6] - P[0][17]*SPP[2] - P[2][17]*SPP[8] + P[10][17]*SPP[22] + P[13][17]*SPP[17]; -nextP[2][17] = P[0][17]*SPP[14] - P[1][17]*SPP[3] + P[2][17]*SPP[13] + P[11][17]*SPP[22] + P[14][17]*SPP[16]; -nextP[3][17] = P[3][17] + P[0][17]*SPP[1] + P[1][17]*SPP[19] + P[2][17]*SPP[15] - P[15][17]*SPP[21]; -nextP[4][17] = P[4][17] + P[15][17]*SF[22] + P[0][17]*SPP[20] + P[1][17]*SPP[12] + P[2][17]*SPP[11]; -nextP[5][17] = P[5][17] + P[15][17]*SF[20] - P[0][17]*SPP[9] + P[1][17]*SPP[10] + P[2][17]*SPP[0]; -nextP[6][17] = P[6][17] + P[3][17]*dt; +nextP[0][17] = P[0][17] + P[1][17]*SF[9] + P[2][17]*SF[11] + P[3][17]*SF[10] + P[10][17]*SF[14] + P[11][17]*SF[15] + P[12][17]*SPP[10]; +nextP[1][17] = P[1][17] + P[0][17]*SF[8] + P[2][17]*SF[7] + P[3][17]*SF[11] - P[12][17]*SF[15] + P[11][17]*SPP[10] - (P[10][17]*q0)*0.5f; +nextP[2][17] = P[2][17] + P[0][17]*SF[6] + P[1][17]*SF[10] + P[3][17]*SF[8] + P[12][17]*SF[14] - P[10][17]*SPP[10] - (P[11][17]*q0)*0.5f; +nextP[3][17] = P[3][17] + P[0][17]*SF[7] + P[1][17]*SF[6] + P[2][17]*SF[9] + P[10][17]*SF[15] - P[11][17]*SF[14] - (P[12][17]*q0)*0.5f; +nextP[4][17] = P[4][17] + P[0][17]*SF[5] + P[1][17]*SF[3] - P[3][17]*SF[4] + P[2][17]*SPP[0] + P[13][17]*SPP[3] + P[14][17]*SPP[6] - P[15][17]*SPP[9]; +nextP[5][17] = P[5][17] + P[0][17]*SF[4] + P[2][17]*SF[3] + P[3][17]*SF[5] - P[1][17]*SPP[0] - P[13][17]*SPP[8] + P[14][17]*SPP[2] + P[15][17]*SPP[5]; +nextP[6][17] = P[6][17] + P[1][17]*SF[4] - P[2][17]*SF[5] + P[3][17]*SF[3] + P[0][17]*SPP[0] + P[13][17]*SPP[4] - P[14][17]*SPP[7] - P[15][17]*SPP[1]; nextP[7][17] = P[7][17] + P[4][17]*dt; nextP[8][17] = P[8][17] + P[5][17]*dt; -nextP[9][17] = P[9][17]; +nextP[9][17] = P[9][17] + P[6][17]*dt; nextP[10][17] = P[10][17]; nextP[11][17] = P[11][17]; nextP[12][17] = P[12][17]; @@ -248,16 +236,16 @@ nextP[14][17] = P[14][17]; nextP[15][17] = P[15][17]; nextP[16][17] = P[16][17]; nextP[17][17] = P[17][17]; -nextP[0][18] = P[0][18]*SPP[5] - P[1][18]*SPP[4] + P[2][18]*SPP[7] + P[9][18]*SPP[22] + P[12][18]*SPP[18]; -nextP[1][18] = P[1][18]*SPP[6] - P[0][18]*SPP[2] - P[2][18]*SPP[8] + P[10][18]*SPP[22] + P[13][18]*SPP[17]; -nextP[2][18] = P[0][18]*SPP[14] - P[1][18]*SPP[3] + P[2][18]*SPP[13] + P[11][18]*SPP[22] + P[14][18]*SPP[16]; -nextP[3][18] = P[3][18] + P[0][18]*SPP[1] + P[1][18]*SPP[19] + P[2][18]*SPP[15] - P[15][18]*SPP[21]; -nextP[4][18] = P[4][18] + P[15][18]*SF[22] + P[0][18]*SPP[20] + P[1][18]*SPP[12] + P[2][18]*SPP[11]; -nextP[5][18] = P[5][18] + P[15][18]*SF[20] - P[0][18]*SPP[9] + P[1][18]*SPP[10] + P[2][18]*SPP[0]; -nextP[6][18] = P[6][18] + P[3][18]*dt; +nextP[0][18] = P[0][18] + P[1][18]*SF[9] + P[2][18]*SF[11] + P[3][18]*SF[10] + P[10][18]*SF[14] + P[11][18]*SF[15] + P[12][18]*SPP[10]; +nextP[1][18] = P[1][18] + P[0][18]*SF[8] + P[2][18]*SF[7] + P[3][18]*SF[11] - P[12][18]*SF[15] + P[11][18]*SPP[10] - (P[10][18]*q0)*0.5f; +nextP[2][18] = P[2][18] + P[0][18]*SF[6] + P[1][18]*SF[10] + P[3][18]*SF[8] + P[12][18]*SF[14] - P[10][18]*SPP[10] - (P[11][18]*q0)*0.5f; +nextP[3][18] = P[3][18] + P[0][18]*SF[7] + P[1][18]*SF[6] + P[2][18]*SF[9] + P[10][18]*SF[15] - P[11][18]*SF[14] - (P[12][18]*q0)*0.5f; +nextP[4][18] = P[4][18] + P[0][18]*SF[5] + P[1][18]*SF[3] - P[3][18]*SF[4] + P[2][18]*SPP[0] + P[13][18]*SPP[3] + P[14][18]*SPP[6] - P[15][18]*SPP[9]; +nextP[5][18] = P[5][18] + P[0][18]*SF[4] + P[2][18]*SF[3] + P[3][18]*SF[5] - P[1][18]*SPP[0] - P[13][18]*SPP[8] + P[14][18]*SPP[2] + P[15][18]*SPP[5]; +nextP[6][18] = P[6][18] + P[1][18]*SF[4] - P[2][18]*SF[5] + P[3][18]*SF[3] + P[0][18]*SPP[0] + P[13][18]*SPP[4] - P[14][18]*SPP[7] - P[15][18]*SPP[1]; nextP[7][18] = P[7][18] + P[4][18]*dt; nextP[8][18] = P[8][18] + P[5][18]*dt; -nextP[9][18] = P[9][18]; +nextP[9][18] = P[9][18] + P[6][18]*dt; nextP[10][18] = P[10][18]; nextP[11][18] = P[11][18]; nextP[12][18] = P[12][18]; @@ -267,16 +255,16 @@ nextP[15][18] = P[15][18]; nextP[16][18] = P[16][18]; nextP[17][18] = P[17][18]; nextP[18][18] = P[18][18]; -nextP[0][19] = P[0][19]*SPP[5] - P[1][19]*SPP[4] + P[2][19]*SPP[7] + P[9][19]*SPP[22] + P[12][19]*SPP[18]; -nextP[1][19] = P[1][19]*SPP[6] - P[0][19]*SPP[2] - P[2][19]*SPP[8] + P[10][19]*SPP[22] + P[13][19]*SPP[17]; -nextP[2][19] = P[0][19]*SPP[14] - P[1][19]*SPP[3] + P[2][19]*SPP[13] + P[11][19]*SPP[22] + P[14][19]*SPP[16]; -nextP[3][19] = P[3][19] + P[0][19]*SPP[1] + P[1][19]*SPP[19] + P[2][19]*SPP[15] - P[15][19]*SPP[21]; -nextP[4][19] = P[4][19] + P[15][19]*SF[22] + P[0][19]*SPP[20] + P[1][19]*SPP[12] + P[2][19]*SPP[11]; -nextP[5][19] = P[5][19] + P[15][19]*SF[20] - P[0][19]*SPP[9] + P[1][19]*SPP[10] + P[2][19]*SPP[0]; -nextP[6][19] = P[6][19] + P[3][19]*dt; +nextP[0][19] = P[0][19] + P[1][19]*SF[9] + P[2][19]*SF[11] + P[3][19]*SF[10] + P[10][19]*SF[14] + P[11][19]*SF[15] + P[12][19]*SPP[10]; +nextP[1][19] = P[1][19] + P[0][19]*SF[8] + P[2][19]*SF[7] + P[3][19]*SF[11] - P[12][19]*SF[15] + P[11][19]*SPP[10] - (P[10][19]*q0)*0.5f; +nextP[2][19] = P[2][19] + P[0][19]*SF[6] + P[1][19]*SF[10] + P[3][19]*SF[8] + P[12][19]*SF[14] - P[10][19]*SPP[10] - (P[11][19]*q0)*0.5f; +nextP[3][19] = P[3][19] + P[0][19]*SF[7] + P[1][19]*SF[6] + P[2][19]*SF[9] + P[10][19]*SF[15] - P[11][19]*SF[14] - (P[12][19]*q0)*0.5f; +nextP[4][19] = P[4][19] + P[0][19]*SF[5] + P[1][19]*SF[3] - P[3][19]*SF[4] + P[2][19]*SPP[0] + P[13][19]*SPP[3] + P[14][19]*SPP[6] - P[15][19]*SPP[9]; +nextP[5][19] = P[5][19] + P[0][19]*SF[4] + P[2][19]*SF[3] + P[3][19]*SF[5] - P[1][19]*SPP[0] - P[13][19]*SPP[8] + P[14][19]*SPP[2] + P[15][19]*SPP[5]; +nextP[6][19] = P[6][19] + P[1][19]*SF[4] - P[2][19]*SF[5] + P[3][19]*SF[3] + P[0][19]*SPP[0] + P[13][19]*SPP[4] - P[14][19]*SPP[7] - P[15][19]*SPP[1]; nextP[7][19] = P[7][19] + P[4][19]*dt; nextP[8][19] = P[8][19] + P[5][19]*dt; -nextP[9][19] = P[9][19]; +nextP[9][19] = P[9][19] + P[6][19]*dt; nextP[10][19] = P[10][19]; nextP[11][19] = P[11][19]; nextP[12][19] = P[12][19]; @@ -287,16 +275,16 @@ nextP[16][19] = P[16][19]; nextP[17][19] = P[17][19]; nextP[18][19] = P[18][19]; nextP[19][19] = P[19][19]; -nextP[0][20] = P[0][20]*SPP[5] - P[1][20]*SPP[4] + P[2][20]*SPP[7] + P[9][20]*SPP[22] + P[12][20]*SPP[18]; -nextP[1][20] = P[1][20]*SPP[6] - P[0][20]*SPP[2] - P[2][20]*SPP[8] + P[10][20]*SPP[22] + P[13][20]*SPP[17]; -nextP[2][20] = P[0][20]*SPP[14] - P[1][20]*SPP[3] + P[2][20]*SPP[13] + P[11][20]*SPP[22] + P[14][20]*SPP[16]; -nextP[3][20] = P[3][20] + P[0][20]*SPP[1] + P[1][20]*SPP[19] + P[2][20]*SPP[15] - P[15][20]*SPP[21]; -nextP[4][20] = P[4][20] + P[15][20]*SF[22] + P[0][20]*SPP[20] + P[1][20]*SPP[12] + P[2][20]*SPP[11]; -nextP[5][20] = P[5][20] + P[15][20]*SF[20] - P[0][20]*SPP[9] + P[1][20]*SPP[10] + P[2][20]*SPP[0]; -nextP[6][20] = P[6][20] + P[3][20]*dt; +nextP[0][20] = P[0][20] + P[1][20]*SF[9] + P[2][20]*SF[11] + P[3][20]*SF[10] + P[10][20]*SF[14] + P[11][20]*SF[15] + P[12][20]*SPP[10]; +nextP[1][20] = P[1][20] + P[0][20]*SF[8] + P[2][20]*SF[7] + P[3][20]*SF[11] - P[12][20]*SF[15] + P[11][20]*SPP[10] - (P[10][20]*q0)*0.5f; +nextP[2][20] = P[2][20] + P[0][20]*SF[6] + P[1][20]*SF[10] + P[3][20]*SF[8] + P[12][20]*SF[14] - P[10][20]*SPP[10] - (P[11][20]*q0)*0.5f; +nextP[3][20] = P[3][20] + P[0][20]*SF[7] + P[1][20]*SF[6] + P[2][20]*SF[9] + P[10][20]*SF[15] - P[11][20]*SF[14] - (P[12][20]*q0)*0.5f; +nextP[4][20] = P[4][20] + P[0][20]*SF[5] + P[1][20]*SF[3] - P[3][20]*SF[4] + P[2][20]*SPP[0] + P[13][20]*SPP[3] + P[14][20]*SPP[6] - P[15][20]*SPP[9]; +nextP[5][20] = P[5][20] + P[0][20]*SF[4] + P[2][20]*SF[3] + P[3][20]*SF[5] - P[1][20]*SPP[0] - P[13][20]*SPP[8] + P[14][20]*SPP[2] + P[15][20]*SPP[5]; +nextP[6][20] = P[6][20] + P[1][20]*SF[4] - P[2][20]*SF[5] + P[3][20]*SF[3] + P[0][20]*SPP[0] + P[13][20]*SPP[4] - P[14][20]*SPP[7] - P[15][20]*SPP[1]; nextP[7][20] = P[7][20] + P[4][20]*dt; nextP[8][20] = P[8][20] + P[5][20]*dt; -nextP[9][20] = P[9][20]; +nextP[9][20] = P[9][20] + P[6][20]*dt; nextP[10][20] = P[10][20]; nextP[11][20] = P[11][20]; nextP[12][20] = P[12][20]; @@ -308,16 +296,16 @@ nextP[17][20] = P[17][20]; nextP[18][20] = P[18][20]; nextP[19][20] = P[19][20]; nextP[20][20] = P[20][20]; -nextP[0][21] = P[0][21]*SPP[5] - P[1][21]*SPP[4] + P[2][21]*SPP[7] + P[9][21]*SPP[22] + P[12][21]*SPP[18]; -nextP[1][21] = P[1][21]*SPP[6] - P[0][21]*SPP[2] - P[2][21]*SPP[8] + P[10][21]*SPP[22] + P[13][21]*SPP[17]; -nextP[2][21] = P[0][21]*SPP[14] - P[1][21]*SPP[3] + P[2][21]*SPP[13] + P[11][21]*SPP[22] + P[14][21]*SPP[16]; -nextP[3][21] = P[3][21] + P[0][21]*SPP[1] + P[1][21]*SPP[19] + P[2][21]*SPP[15] - P[15][21]*SPP[21]; -nextP[4][21] = P[4][21] + P[15][21]*SF[22] + P[0][21]*SPP[20] + P[1][21]*SPP[12] + P[2][21]*SPP[11]; -nextP[5][21] = P[5][21] + P[15][21]*SF[20] - P[0][21]*SPP[9] + P[1][21]*SPP[10] + P[2][21]*SPP[0]; -nextP[6][21] = P[6][21] + P[3][21]*dt; +nextP[0][21] = P[0][21] + P[1][21]*SF[9] + P[2][21]*SF[11] + P[3][21]*SF[10] + P[10][21]*SF[14] + P[11][21]*SF[15] + P[12][21]*SPP[10]; +nextP[1][21] = P[1][21] + P[0][21]*SF[8] + P[2][21]*SF[7] + P[3][21]*SF[11] - P[12][21]*SF[15] + P[11][21]*SPP[10] - (P[10][21]*q0)*0.5f; +nextP[2][21] = P[2][21] + P[0][21]*SF[6] + P[1][21]*SF[10] + P[3][21]*SF[8] + P[12][21]*SF[14] - P[10][21]*SPP[10] - (P[11][21]*q0)*0.5f; +nextP[3][21] = P[3][21] + P[0][21]*SF[7] + P[1][21]*SF[6] + P[2][21]*SF[9] + P[10][21]*SF[15] - P[11][21]*SF[14] - (P[12][21]*q0)*0.5f; +nextP[4][21] = P[4][21] + P[0][21]*SF[5] + P[1][21]*SF[3] - P[3][21]*SF[4] + P[2][21]*SPP[0] + P[13][21]*SPP[3] + P[14][21]*SPP[6] - P[15][21]*SPP[9]; +nextP[5][21] = P[5][21] + P[0][21]*SF[4] + P[2][21]*SF[3] + P[3][21]*SF[5] - P[1][21]*SPP[0] - P[13][21]*SPP[8] + P[14][21]*SPP[2] + P[15][21]*SPP[5]; +nextP[6][21] = P[6][21] + P[1][21]*SF[4] - P[2][21]*SF[5] + P[3][21]*SF[3] + P[0][21]*SPP[0] + P[13][21]*SPP[4] - P[14][21]*SPP[7] - P[15][21]*SPP[1]; nextP[7][21] = P[7][21] + P[4][21]*dt; nextP[8][21] = P[8][21] + P[5][21]*dt; -nextP[9][21] = P[9][21]; +nextP[9][21] = P[9][21] + P[6][21]*dt; nextP[10][21] = P[10][21]; nextP[11][21] = P[11][21]; nextP[12][21] = P[12][21]; @@ -330,16 +318,16 @@ nextP[18][21] = P[18][21]; nextP[19][21] = P[19][21]; nextP[20][21] = P[20][21]; nextP[21][21] = P[21][21]; -nextP[0][22] = P[0][22]*SPP[5] - P[1][22]*SPP[4] + P[2][22]*SPP[7] + P[9][22]*SPP[22] + P[12][22]*SPP[18]; -nextP[1][22] = P[1][22]*SPP[6] - P[0][22]*SPP[2] - P[2][22]*SPP[8] + P[10][22]*SPP[22] + P[13][22]*SPP[17]; -nextP[2][22] = P[0][22]*SPP[14] - P[1][22]*SPP[3] + P[2][22]*SPP[13] + P[11][22]*SPP[22] + P[14][22]*SPP[16]; -nextP[3][22] = P[3][22] + P[0][22]*SPP[1] + P[1][22]*SPP[19] + P[2][22]*SPP[15] - P[15][22]*SPP[21]; -nextP[4][22] = P[4][22] + P[15][22]*SF[22] + P[0][22]*SPP[20] + P[1][22]*SPP[12] + P[2][22]*SPP[11]; -nextP[5][22] = P[5][22] + P[15][22]*SF[20] - P[0][22]*SPP[9] + P[1][22]*SPP[10] + P[2][22]*SPP[0]; -nextP[6][22] = P[6][22] + P[3][22]*dt; +nextP[0][22] = P[0][22] + P[1][22]*SF[9] + P[2][22]*SF[11] + P[3][22]*SF[10] + P[10][22]*SF[14] + P[11][22]*SF[15] + P[12][22]*SPP[10]; +nextP[1][22] = P[1][22] + P[0][22]*SF[8] + P[2][22]*SF[7] + P[3][22]*SF[11] - P[12][22]*SF[15] + P[11][22]*SPP[10] - (P[10][22]*q0)*0.5f; +nextP[2][22] = P[2][22] + P[0][22]*SF[6] + P[1][22]*SF[10] + P[3][22]*SF[8] + P[12][22]*SF[14] - P[10][22]*SPP[10] - (P[11][22]*q0)*0.5f; +nextP[3][22] = P[3][22] + P[0][22]*SF[7] + P[1][22]*SF[6] + P[2][22]*SF[9] + P[10][22]*SF[15] - P[11][22]*SF[14] - (P[12][22]*q0)*0.5f; +nextP[4][22] = P[4][22] + P[0][22]*SF[5] + P[1][22]*SF[3] - P[3][22]*SF[4] + P[2][22]*SPP[0] + P[13][22]*SPP[3] + P[14][22]*SPP[6] - P[15][22]*SPP[9]; +nextP[5][22] = P[5][22] + P[0][22]*SF[4] + P[2][22]*SF[3] + P[3][22]*SF[5] - P[1][22]*SPP[0] - P[13][22]*SPP[8] + P[14][22]*SPP[2] + P[15][22]*SPP[5]; +nextP[6][22] = P[6][22] + P[1][22]*SF[4] - P[2][22]*SF[5] + P[3][22]*SF[3] + P[0][22]*SPP[0] + P[13][22]*SPP[4] - P[14][22]*SPP[7] - P[15][22]*SPP[1]; nextP[7][22] = P[7][22] + P[4][22]*dt; nextP[8][22] = P[8][22] + P[5][22]*dt; -nextP[9][22] = P[9][22]; +nextP[9][22] = P[9][22] + P[6][22]*dt; nextP[10][22] = P[10][22]; nextP[11][22] = P[11][22]; nextP[12][22] = P[12][22]; @@ -353,16 +341,16 @@ nextP[19][22] = P[19][22]; nextP[20][22] = P[20][22]; nextP[21][22] = P[21][22]; nextP[22][22] = P[22][22]; -nextP[0][23] = P[0][23]*SPP[5] - P[1][23]*SPP[4] + P[2][23]*SPP[7] + P[9][23]*SPP[22] + P[12][23]*SPP[18]; -nextP[1][23] = P[1][23]*SPP[6] - P[0][23]*SPP[2] - P[2][23]*SPP[8] + P[10][23]*SPP[22] + P[13][23]*SPP[17]; -nextP[2][23] = P[0][23]*SPP[14] - P[1][23]*SPP[3] + P[2][23]*SPP[13] + P[11][23]*SPP[22] + P[14][23]*SPP[16]; -nextP[3][23] = P[3][23] + P[0][23]*SPP[1] + P[1][23]*SPP[19] + P[2][23]*SPP[15] - P[15][23]*SPP[21]; -nextP[4][23] = P[4][23] + P[15][23]*SF[22] + P[0][23]*SPP[20] + P[1][23]*SPP[12] + P[2][23]*SPP[11]; -nextP[5][23] = P[5][23] + P[15][23]*SF[20] - P[0][23]*SPP[9] + P[1][23]*SPP[10] + P[2][23]*SPP[0]; -nextP[6][23] = P[6][23] + P[3][23]*dt; +nextP[0][23] = P[0][23] + P[1][23]*SF[9] + P[2][23]*SF[11] + P[3][23]*SF[10] + P[10][23]*SF[14] + P[11][23]*SF[15] + P[12][23]*SPP[10]; +nextP[1][23] = P[1][23] + P[0][23]*SF[8] + P[2][23]*SF[7] + P[3][23]*SF[11] - P[12][23]*SF[15] + P[11][23]*SPP[10] - (P[10][23]*q0)*0.5f; +nextP[2][23] = P[2][23] + P[0][23]*SF[6] + P[1][23]*SF[10] + P[3][23]*SF[8] + P[12][23]*SF[14] - P[10][23]*SPP[10] - (P[11][23]*q0)*0.5f; +nextP[3][23] = P[3][23] + P[0][23]*SF[7] + P[1][23]*SF[6] + P[2][23]*SF[9] + P[10][23]*SF[15] - P[11][23]*SF[14] - (P[12][23]*q0)*0.5f; +nextP[4][23] = P[4][23] + P[0][23]*SF[5] + P[1][23]*SF[3] - P[3][23]*SF[4] + P[2][23]*SPP[0] + P[13][23]*SPP[3] + P[14][23]*SPP[6] - P[15][23]*SPP[9]; +nextP[5][23] = P[5][23] + P[0][23]*SF[4] + P[2][23]*SF[3] + P[3][23]*SF[5] - P[1][23]*SPP[0] - P[13][23]*SPP[8] + P[14][23]*SPP[2] + P[15][23]*SPP[5]; +nextP[6][23] = P[6][23] + P[1][23]*SF[4] - P[2][23]*SF[5] + P[3][23]*SF[3] + P[0][23]*SPP[0] + P[13][23]*SPP[4] - P[14][23]*SPP[7] - P[15][23]*SPP[1]; nextP[7][23] = P[7][23] + P[4][23]*dt; nextP[8][23] = P[8][23] + P[5][23]*dt; -nextP[9][23] = P[9][23]; +nextP[9][23] = P[9][23] + P[6][23]*dt; nextP[10][23] = P[10][23]; nextP[11][23] = P[11][23]; nextP[12][23] = P[12][23]; diff --git a/matlab/generated/Inertial Nav EKF/Magnetic Declination Fusion.txt b/matlab/generated/Inertial Nav EKF/Magnetic Declination Fusion.txt index c1455008a5..f3bd513e24 100644 --- a/matlab/generated/Inertial Nav EKF/Magnetic Declination Fusion.txt +++ b/matlab/generated/Inertial Nav EKF/Magnetic Declination Fusion.txt @@ -17,48 +17,51 @@ Divide by zero protection and protection against a badly conditioned covariance float t2 = magE*magE; float t3 = magN*magN; float t4 = t2+t3; -float t5 = 1.0f/t4; -float t22 = magE*t5; -float t23 = magN*t5; -float t6 = P[16][16]*t22; -float t13 = P[17][16]*t23; -float t7 = t6-t13; -float t8 = t22*t7; -float t9 = P[16][17]*t22; -float t14 = P[17][17]*t23; -float t10 = t9-t14; -float t15 = t23*t10; -float t11 = R_DECL+t8-t15; // innovation variance -float t12 = 1.0f/t11; +float t5 = P[16][16]*t2; +float t6 = P[17][17]*t3; +float t7 = t2*t2; +float t8 = R_DECL*t7; +float t9 = t3*t3; +float t10 = R_DECL*t9; +float t11 = R_DECL*t2*t3*2.0f; +float t14 = P[16][17]*magE*magN; +float t15 = P[17][16]*magE*magN; +float t12 = t5+t6+t8+t10+t11-t14-t15; +float t13 = 1.0f / t12; +float t16 = magE; +float t17 = magN; +float t18 = t16*t16; +float t19 = t17*t17; +float t20 = t18+t19; +float t21 = 1.0f/t20; // Calculate the observation Jacobian // Note only 2 terms are non-zero which can be used in matrix operations for calculation of Kalman gains and covariance update to significantly reduce cost -H_DECL[16] = -magE*t5; -H_DECL[17] = magN*t5; +H_DECL[16] = -t16*t21; +H_DECL[17] = t17*t21; // Calculate the Kalman gains -Kfusion[0] = -t12*(P[0][16]*t22-P[0][17]*t23); -Kfusion[1] = -t12*(P[1][16]*t22-P[1][17]*t23); -Kfusion[2] = -t12*(P[2][16]*t22-P[2][17]*t23); -Kfusion[3] = -t12*(P[3][16]*t22-P[3][17]*t23); -Kfusion[4] = -t12*(P[4][16]*t22-P[4][17]*t23); -Kfusion[5] = -t12*(P[5][16]*t22-P[5][17]*t23); -Kfusion[6] = -t12*(P[6][16]*t22-P[6][17]*t23); -Kfusion[7] = -t12*(P[7][16]*t22-P[7][17]*t23); -Kfusion[8] = -t12*(P[8][16]*t22-P[8][17]*t23); -Kfusion[9] = -t12*(P[9][16]*t22-P[9][17]*t23); -Kfusion[10] = -t12*(P[10][16]*t22-P[10][17]*t23); -Kfusion[11] = -t12*(P[11][16]*t22-P[11][17]*t23); -Kfusion[12] = -t12*(P[12][16]*t22-P[12][17]*t23); -Kfusion[13] = -t12*(P[13][16]*t22-P[13][17]*t23); -Kfusion[14] = -t12*(P[14][16]*t22-P[14][17]*t23); -Kfusion[15] = -t12*(P[15][16]*t22-P[15][17]*t23); -Kfusion[16] = -t12*(t6-P[16][17]*t23); -Kfusion[17] = t12*(t14-P[17][16]*t22); -Kfusion[18] = -t12*(P[18][16]*t22-P[18][17]*t23); -Kfusion[19] = -t12*(P[19][16]*t22-P[19][17]*t23); -Kfusion[20] = -t12*(P[20][16]*t22-P[20][17]*t23); -Kfusion[21] = -t12*(P[21][16]*t22-P[21][17]*t23); -Kfusion[22] = -t12*(P[22][16]*t22-P[22][17]*t23); -Kfusion[23] = -t12*(P[23][16]*t22-P[23][17]*t23); - +Kfusion[0] = -t4*t13*(P[0][16]*magE-P[0][17]*magN); +Kfusion[1] = -t4*t13*(P[1][16]*magE-P[1][17]*magN); +Kfusion[2] = -t4*t13*(P[2][16]*magE-P[2][17]*magN); +Kfusion[3] = -t4*t13*(P[3][16]*magE-P[3][17]*magN); +Kfusion[4] = -t4*t13*(P[4][16]*magE-P[4][17]*magN); +Kfusion[5] = -t4*t13*(P[5][16]*magE-P[5][17]*magN); +Kfusion[6] = -t4*t13*(P[6][16]*magE-P[6][17]*magN); +Kfusion[7] = -t4*t13*(P[7][16]*magE-P[7][17]*magN); +Kfusion[8] = -t4*t13*(P[8][16]*magE-P[8][17]*magN); +Kfusion[9] = -t4*t13*(P[9][16]*magE-P[9][17]*magN); +Kfusion[10] = -t4*t13*(P[10][16]*magE-P[10][17]*magN); +Kfusion[11] = -t4*t13*(P[11][16]*magE-P[11][17]*magN); +Kfusion[12] = -t4*t13*(P[12][16]*magE-P[12][17]*magN); +Kfusion[13] = -t4*t13*(P[13][16]*magE-P[13][17]*magN); +Kfusion[14] = -t4*t13*(P[14][16]*magE-P[14][17]*magN); +Kfusion[15] = -t4*t13*(P[15][16]*magE-P[15][17]*magN); +Kfusion[16] = -t4*t13*(P[16][16]*magE-P[16][17]*magN); +Kfusion[17] = -t4*t13*(P[17][16]*magE-P[17][17]*magN); +Kfusion[18] = -t4*t13*(P[18][16]*magE-P[18][17]*magN); +Kfusion[19] = -t4*t13*(P[19][16]*magE-P[19][17]*magN); +Kfusion[20] = -t4*t13*(P[20][16]*magE-P[20][17]*magN); +Kfusion[21] = -t4*t13*(P[21][16]*magE-P[21][17]*magN); +Kfusion[22] = -t4*t13*(P[22][16]*magE-P[22][17]*magN); +Kfusion[23] = -t4*t13*(P[23][16]*magE-P[23][17]*magN); diff --git a/matlab/generated/Inertial Nav EKF/Magnetometer Fusion.txt b/matlab/generated/Inertial Nav EKF/Magnetometer Fusion.txt index 3ddb85563b..9df90c509f 100644 --- a/matlab/generated/Inertial Nav EKF/Magnetometer Fusion.txt +++ b/matlab/generated/Inertial Nav EKF/Magnetometer Fusion.txt @@ -1,146 +1,142 @@ // Auto code for fusion of XYZ magnetometer vector measurement // Sequential fusion of each axis is used (assumes uncorrrelated observation errors) -// intermediate variables from algebraic optimisation -float SH_MAG[9]; -SH_MAG[0] = sq(q0) - sq(q1) + sq(q2) - sq(q3); -SH_MAG[1] = sq(q0) + sq(q1) - sq(q2) - sq(q3); -SH_MAG[2] = sq(q0) - sq(q1) - sq(q2) + sq(q3); -SH_MAG[3] = 2*q0*q1 + 2*q2*q3; -SH_MAG[4] = 2*q0*q3 + 2*q1*q2; -SH_MAG[5] = 2*q0*q2 + 2*q1*q3; -SH_MAG[6] = magE*(2*q0*q1 - 2*q2*q3); -SH_MAG[7] = 2*q1*q3 - 2*q0*q2; -SH_MAG[8] = 2*q0*q3; +// common expressions for XYZ axis observation jacobians +SH_MAG[0] = 2.0f*magD*q3 + 2.0f*magE*q2 + 2.0f*magN*q1; +SH_MAG[1] = 2.0f*magD*q0 - 2.0f*magE*q1 + 2.0f*magN*q2; +SH_MAG[2] = 2.0f*magD*q1 + 2.0f*magE*q0 - 2.0f*magN*q3; +SH_MAG[3] = sq(q3); +SH_MAG[4] = sq(q2); +SH_MAG[5] = sq(q1); +SH_MAG[6] = sq(q0); +SH_MAG[7] = 2.0f*magN*q0; +SH_MAG[8] = 2.0f*magE*q3; -// Calculate the observation jacobian for the X component -float H_MAG[24]; -H_MAG[1] = SH_MAG[6] - magD*SH_MAG[2] - magN*SH_MAG[5]; -H_MAG[2] = magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2); -H_MAG[16] = SH_MAG[1]; -H_MAG[17] = SH_MAG[4]; -H_MAG[18] = SH_MAG[7]; -H_MAG[19] = 1; +// X axis observation jacobians +H_MAG[0] = SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2; +H_MAG[1] = SH_MAG[0]; +H_MAG[2] = -SH_MAG[1]; +H_MAG[3] = SH_MAG[2]; +H_MAG[16] = SH_MAG[5] - SH_MAG[4] - SH_MAG[3] + SH_MAG[6]; +H_MAG[17] = 2.0f*q0*q3 + 2.0f*q1*q2; +H_MAG[18] = 2.0f*q1*q3 - 2.0f*q0*q2; +H_MAG[19] = 1.0f; -// calculate the Kalman gain matrix for the X component +// common expressions for X axis Kalman gains +SK_MX[0] = 1.0f/(P[19][19] + R_MAG + P[1][19]*SH_MAG[0] - P[2][19]*SH_MAG[1] + P[3][19]*SH_MAG[2] - P[16][19]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + (2*q0*q3 + 2.0f*q1*q2)*(P[19][17] + P[1][17]*SH_MAG[0] - P[2][17]*SH_MAG[1] + P[3][17]*SH_MAG[2] - P[16][17]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][17]*(2*q0*q3 + 2.0f*q1*q2) - P[18][17]*(2*q0*q2 - 2.0f*q1*q3) + P[0][17]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (2*q0*q2 - 2.0f*q1*q3)*(P[19][18] + P[1][18]*SH_MAG[0] - P[2][18]*SH_MAG[1] + P[3][18]*SH_MAG[2] - P[16][18]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][18]*(2*q0*q3 + 2.0f*q1*q2) - P[18][18]*(2*q0*q2 - 2.0f*q1*q3) + P[0][18]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)*(P[19][0] + P[1][0]*SH_MAG[0] - P[2][0]*SH_MAG[1] + P[3][0]*SH_MAG[2] - P[16][0]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][0]*(2*q0*q3 + 2.0f*q1*q2) - P[18][0]*(2*q0*q2 - 2.0f*q1*q3) + P[0][0]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P[17][19]*(2*q0*q3 + 2.0f*q1*q2) - P[18][19]*(2*q0*q2 - 2.0f*q1*q3) + SH_MAG[0]*(P[19][1] + P[1][1]*SH_MAG[0] - P[2][1]*SH_MAG[1] + P[3][1]*SH_MAG[2] - P[16][1]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][1]*(2*q0*q3 + 2.0f*q1*q2) - P[18][1]*(2*q0*q2 - 2.0f*q1*q3) + P[0][1]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - SH_MAG[1]*(P[19][2] + P[1][2]*SH_MAG[0] - P[2][2]*SH_MAG[1] + P[3][2]*SH_MAG[2] - P[16][2]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][2]*(2*q0*q3 + 2.0f*q1*q2) - P[18][2]*(2*q0*q2 - 2.0f*q1*q3) + P[0][2]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + SH_MAG[2]*(P[19][3] + P[1][3]*SH_MAG[0] - P[2][3]*SH_MAG[1] + P[3][3]*SH_MAG[2] - P[16][3]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][3]*(2*q0*q3 + 2.0f*q1*q2) - P[18][3]*(2*q0*q2 - 2.0f*q1*q3) + P[0][3]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P[19][16] + P[1][16]*SH_MAG[0] - P[2][16]*SH_MAG[1] + P[3][16]*SH_MAG[2] - P[16][16]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][16]*(2*q0*q3 + 2.0f*q1*q2) - P[18][16]*(2*q0*q2 - 2.0f*q1*q3) + P[0][16]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P[0][19]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)); +SK_MX[1] = SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]; +SK_MX[2] = SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2; +SK_MX[3] = 2.0f*q0*q2 - 2.0f*q1*q3; +SK_MX[4] = 2.0f*q0*q3 + 2.0f*q1*q2; -// intermediate variables - note SK_MX[0] is 1/(innovation variance) -float SK_MX[4]; -SK_MX[0] = 1/(P[19][19] + R_MAG - P[1][19]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][19]*SH_MAG[1] + P[17][19]*SH_MAG[4] + P[18][19]*SH_MAG[7] + P[2][19]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2)) - (magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5])*(P[19][1] - P[1][1]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][1]*SH_MAG[1] + P[17][1]*SH_MAG[4] + P[18][1]*SH_MAG[7] + P[2][1]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2))) + SH_MAG[1]*(P[19][16] - P[1][16]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][16]*SH_MAG[1] + P[17][16]*SH_MAG[4] + P[18][16]*SH_MAG[7] + P[2][16]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2))) + SH_MAG[4]*(P[19][17] - P[1][17]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][17]*SH_MAG[1] + P[17][17]*SH_MAG[4] + P[18][17]*SH_MAG[7] + P[2][17]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2))) + SH_MAG[7]*(P[19][18] - P[1][18]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][18]*SH_MAG[1] + P[17][18]*SH_MAG[4] + P[18][18]*SH_MAG[7] + P[2][18]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2))) + (magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2))*(P[19][2] - P[1][2]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][2]*SH_MAG[1] + P[17][2]*SH_MAG[4] + P[18][2]*SH_MAG[7] + P[2][2]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2)))); -SK_MX[1] = magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2); -SK_MX[2] = magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]; -SK_MX[3] = SH_MAG[7]; +// X axis Kalman gains +Kfusion[0] = SK_MX[0]*(P[0][19] + P[0][1]*SH_MAG[0] - P[0][2]*SH_MAG[1] + P[0][3]*SH_MAG[2] + P[0][0]*SK_MX[2] - P[0][16]*SK_MX[1] + P[0][17]*SK_MX[4] - P[0][18]*SK_MX[3]); +Kfusion[1] = SK_MX[0]*(P[1][19] + P[1][1]*SH_MAG[0] - P[1][2]*SH_MAG[1] + P[1][3]*SH_MAG[2] + P[1][0]*SK_MX[2] - P[1][16]*SK_MX[1] + P[1][17]*SK_MX[4] - P[1][18]*SK_MX[3]); +Kfusion[2] = SK_MX[0]*(P[2][19] + P[2][1]*SH_MAG[0] - P[2][2]*SH_MAG[1] + P[2][3]*SH_MAG[2] + P[2][0]*SK_MX[2] - P[2][16]*SK_MX[1] + P[2][17]*SK_MX[4] - P[2][18]*SK_MX[3]); +Kfusion[3] = SK_MX[0]*(P[3][19] + P[3][1]*SH_MAG[0] - P[3][2]*SH_MAG[1] + P[3][3]*SH_MAG[2] + P[3][0]*SK_MX[2] - P[3][16]*SK_MX[1] + P[3][17]*SK_MX[4] - P[3][18]*SK_MX[3]); +Kfusion[4] = SK_MX[0]*(P[4][19] + P[4][1]*SH_MAG[0] - P[4][2]*SH_MAG[1] + P[4][3]*SH_MAG[2] + P[4][0]*SK_MX[2] - P[4][16]*SK_MX[1] + P[4][17]*SK_MX[4] - P[4][18]*SK_MX[3]); +Kfusion[5] = SK_MX[0]*(P[5][19] + P[5][1]*SH_MAG[0] - P[5][2]*SH_MAG[1] + P[5][3]*SH_MAG[2] + P[5][0]*SK_MX[2] - P[5][16]*SK_MX[1] + P[5][17]*SK_MX[4] - P[5][18]*SK_MX[3]); +Kfusion[6] = SK_MX[0]*(P[6][19] + P[6][1]*SH_MAG[0] - P[6][2]*SH_MAG[1] + P[6][3]*SH_MAG[2] + P[6][0]*SK_MX[2] - P[6][16]*SK_MX[1] + P[6][17]*SK_MX[4] - P[6][18]*SK_MX[3]); +Kfusion[7] = SK_MX[0]*(P[7][19] + P[7][1]*SH_MAG[0] - P[7][2]*SH_MAG[1] + P[7][3]*SH_MAG[2] + P[7][0]*SK_MX[2] - P[7][16]*SK_MX[1] + P[7][17]*SK_MX[4] - P[7][18]*SK_MX[3]); +Kfusion[8] = SK_MX[0]*(P[8][19] + P[8][1]*SH_MAG[0] - P[8][2]*SH_MAG[1] + P[8][3]*SH_MAG[2] + P[8][0]*SK_MX[2] - P[8][16]*SK_MX[1] + P[8][17]*SK_MX[4] - P[8][18]*SK_MX[3]); +Kfusion[9] = SK_MX[0]*(P[9][19] + P[9][1]*SH_MAG[0] - P[9][2]*SH_MAG[1] + P[9][3]*SH_MAG[2] + P[9][0]*SK_MX[2] - P[9][16]*SK_MX[1] + P[9][17]*SK_MX[4] - P[9][18]*SK_MX[3]); +Kfusion[10] = SK_MX[0]*(P[10][19] + P[10][1]*SH_MAG[0] - P[10][2]*SH_MAG[1] + P[10][3]*SH_MAG[2] + P[10][0]*SK_MX[2] - P[10][16]*SK_MX[1] + P[10][17]*SK_MX[4] - P[10][18]*SK_MX[3]); +Kfusion[11] = SK_MX[0]*(P[11][19] + P[11][1]*SH_MAG[0] - P[11][2]*SH_MAG[1] + P[11][3]*SH_MAG[2] + P[11][0]*SK_MX[2] - P[11][16]*SK_MX[1] + P[11][17]*SK_MX[4] - P[11][18]*SK_MX[3]); +Kfusion[12] = SK_MX[0]*(P[12][19] + P[12][1]*SH_MAG[0] - P[12][2]*SH_MAG[1] + P[12][3]*SH_MAG[2] + P[12][0]*SK_MX[2] - P[12][16]*SK_MX[1] + P[12][17]*SK_MX[4] - P[12][18]*SK_MX[3]); +Kfusion[13] = SK_MX[0]*(P[13][19] + P[13][1]*SH_MAG[0] - P[13][2]*SH_MAG[1] + P[13][3]*SH_MAG[2] + P[13][0]*SK_MX[2] - P[13][16]*SK_MX[1] + P[13][17]*SK_MX[4] - P[13][18]*SK_MX[3]); +Kfusion[14] = SK_MX[0]*(P[14][19] + P[14][1]*SH_MAG[0] - P[14][2]*SH_MAG[1] + P[14][3]*SH_MAG[2] + P[14][0]*SK_MX[2] - P[14][16]*SK_MX[1] + P[14][17]*SK_MX[4] - P[14][18]*SK_MX[3]); +Kfusion[15] = SK_MX[0]*(P[15][19] + P[15][1]*SH_MAG[0] - P[15][2]*SH_MAG[1] + P[15][3]*SH_MAG[2] + P[15][0]*SK_MX[2] - P[15][16]*SK_MX[1] + P[15][17]*SK_MX[4] - P[15][18]*SK_MX[3]); +Kfusion[16] = SK_MX[0]*(P[16][19] + P[16][1]*SH_MAG[0] - P[16][2]*SH_MAG[1] + P[16][3]*SH_MAG[2] + P[16][0]*SK_MX[2] - P[16][16]*SK_MX[1] + P[16][17]*SK_MX[4] - P[16][18]*SK_MX[3]); +Kfusion[17] = SK_MX[0]*(P[17][19] + P[17][1]*SH_MAG[0] - P[17][2]*SH_MAG[1] + P[17][3]*SH_MAG[2] + P[17][0]*SK_MX[2] - P[17][16]*SK_MX[1] + P[17][17]*SK_MX[4] - P[17][18]*SK_MX[3]); +Kfusion[18] = SK_MX[0]*(P[18][19] + P[18][1]*SH_MAG[0] - P[18][2]*SH_MAG[1] + P[18][3]*SH_MAG[2] + P[18][0]*SK_MX[2] - P[18][16]*SK_MX[1] + P[18][17]*SK_MX[4] - P[18][18]*SK_MX[3]); +Kfusion[19] = SK_MX[0]*(P[19][19] + P[19][1]*SH_MAG[0] - P[19][2]*SH_MAG[1] + P[19][3]*SH_MAG[2] + P[19][0]*SK_MX[2] - P[19][16]*SK_MX[1] + P[19][17]*SK_MX[4] - P[19][18]*SK_MX[3]); +Kfusion[20] = SK_MX[0]*(P[20][19] + P[20][1]*SH_MAG[0] - P[20][2]*SH_MAG[1] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[2] - P[20][16]*SK_MX[1] + P[20][17]*SK_MX[4] - P[20][18]*SK_MX[3]); +Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][1]*SH_MAG[0] - P[21][2]*SH_MAG[1] + P[21][3]*SH_MAG[2] + P[21][0]*SK_MX[2] - P[21][16]*SK_MX[1] + P[21][17]*SK_MX[4] - P[21][18]*SK_MX[3]); +Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] - P[22][2]*SH_MAG[1] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[4] - P[22][18]*SK_MX[3]); +Kfusion[23] = SK_MX[0]*(P[23][19] + P[23][1]*SH_MAG[0] - P[23][2]*SH_MAG[1] + P[23][3]*SH_MAG[2] + P[23][0]*SK_MX[2] - P[23][16]*SK_MX[1] + P[23][17]*SK_MX[4] - P[23][18]*SK_MX[3]); -float Kfusion[24]; -Kfusion[0] = SK_MX[0]*(P[0][19] + P[0][16]*SH_MAG[1] + P[0][17]*SH_MAG[4] - P[0][1]*SK_MX[2] + P[0][2]*SK_MX[1] + P[0][18]*SK_MX[3]); -Kfusion[1] = SK_MX[0]*(P[1][19] + P[1][16]*SH_MAG[1] + P[1][17]*SH_MAG[4] - P[1][1]*SK_MX[2] + P[1][2]*SK_MX[1] + P[1][18]*SK_MX[3]); -Kfusion[2] = SK_MX[0]*(P[2][19] + P[2][16]*SH_MAG[1] + P[2][17]*SH_MAG[4] - P[2][1]*SK_MX[2] + P[2][2]*SK_MX[1] + P[2][18]*SK_MX[3]); -Kfusion[3] = SK_MX[0]*(P[3][19] + P[3][16]*SH_MAG[1] + P[3][17]*SH_MAG[4] - P[3][1]*SK_MX[2] + P[3][2]*SK_MX[1] + P[3][18]*SK_MX[3]); -Kfusion[4] = SK_MX[0]*(P[4][19] + P[4][16]*SH_MAG[1] + P[4][17]*SH_MAG[4] - P[4][1]*SK_MX[2] + P[4][2]*SK_MX[1] + P[4][18]*SK_MX[3]); -Kfusion[5] = SK_MX[0]*(P[5][19] + P[5][16]*SH_MAG[1] + P[5][17]*SH_MAG[4] - P[5][1]*SK_MX[2] + P[5][2]*SK_MX[1] + P[5][18]*SK_MX[3]); -Kfusion[6] = SK_MX[0]*(P[6][19] + P[6][16]*SH_MAG[1] + P[6][17]*SH_MAG[4] - P[6][1]*SK_MX[2] + P[6][2]*SK_MX[1] + P[6][18]*SK_MX[3]); -Kfusion[7] = SK_MX[0]*(P[7][19] + P[7][16]*SH_MAG[1] + P[7][17]*SH_MAG[4] - P[7][1]*SK_MX[2] + P[7][2]*SK_MX[1] + P[7][18]*SK_MX[3]); -Kfusion[8] = SK_MX[0]*(P[8][19] + P[8][16]*SH_MAG[1] + P[8][17]*SH_MAG[4] - P[8][1]*SK_MX[2] + P[8][2]*SK_MX[1] + P[8][18]*SK_MX[3]); -Kfusion[9] = SK_MX[0]*(P[9][19] + P[9][16]*SH_MAG[1] + P[9][17]*SH_MAG[4] - P[9][1]*SK_MX[2] + P[9][2]*SK_MX[1] + P[9][18]*SK_MX[3]); -Kfusion[10] = SK_MX[0]*(P[10][19] + P[10][16]*SH_MAG[1] + P[10][17]*SH_MAG[4] - P[10][1]*SK_MX[2] + P[10][2]*SK_MX[1] + P[10][18]*SK_MX[3]); -Kfusion[11] = SK_MX[0]*(P[11][19] + P[11][16]*SH_MAG[1] + P[11][17]*SH_MAG[4] - P[11][1]*SK_MX[2] + P[11][2]*SK_MX[1] + P[11][18]*SK_MX[3]); -Kfusion[12] = SK_MX[0]*(P[12][19] + P[12][16]*SH_MAG[1] + P[12][17]*SH_MAG[4] - P[12][1]*SK_MX[2] + P[12][2]*SK_MX[1] + P[12][18]*SK_MX[3]); -Kfusion[13] = SK_MX[0]*(P[13][19] + P[13][16]*SH_MAG[1] + P[13][17]*SH_MAG[4] - P[13][1]*SK_MX[2] + P[13][2]*SK_MX[1] + P[13][18]*SK_MX[3]); -Kfusion[14] = SK_MX[0]*(P[14][19] + P[14][16]*SH_MAG[1] + P[14][17]*SH_MAG[4] - P[14][1]*SK_MX[2] + P[14][2]*SK_MX[1] + P[14][18]*SK_MX[3]); -Kfusion[15] = SK_MX[0]*(P[15][19] + P[15][16]*SH_MAG[1] + P[15][17]*SH_MAG[4] - P[15][1]*SK_MX[2] + P[15][2]*SK_MX[1] + P[15][18]*SK_MX[3]); -Kfusion[16] = SK_MX[0]*(P[16][19] + P[16][16]*SH_MAG[1] + P[16][17]*SH_MAG[4] - P[16][1]*SK_MX[2] + P[16][2]*SK_MX[1] + P[16][18]*SK_MX[3]); -Kfusion[17] = SK_MX[0]*(P[17][19] + P[17][16]*SH_MAG[1] + P[17][17]*SH_MAG[4] - P[17][1]*SK_MX[2] + P[17][2]*SK_MX[1] + P[17][18]*SK_MX[3]); -Kfusion[18] = SK_MX[0]*(P[18][19] + P[18][16]*SH_MAG[1] + P[18][17]*SH_MAG[4] - P[18][1]*SK_MX[2] + P[18][2]*SK_MX[1] + P[18][18]*SK_MX[3]); -Kfusion[19] = SK_MX[0]*(P[19][19] + P[19][16]*SH_MAG[1] + P[19][17]*SH_MAG[4] - P[19][1]*SK_MX[2] + P[19][2]*SK_MX[1] + P[19][18]*SK_MX[3]); -Kfusion[20] = SK_MX[0]*(P[20][19] + P[20][16]*SH_MAG[1] + P[20][17]*SH_MAG[4] - P[20][1]*SK_MX[2] + P[20][2]*SK_MX[1] + P[20][18]*SK_MX[3]); -Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][16]*SH_MAG[1] + P[21][17]*SH_MAG[4] - P[21][1]*SK_MX[2] + P[21][2]*SK_MX[1] + P[21][18]*SK_MX[3]); -Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][16]*SH_MAG[1] + P[22][17]*SH_MAG[4] - P[22][1]*SK_MX[2] + P[22][2]*SK_MX[1] + P[22][18]*SK_MX[3]); -Kfusion[23] = SK_MX[0]*(P[23][19] + P[23][16]*SH_MAG[1] + P[23][17]*SH_MAG[4] - P[23][1]*SK_MX[2] + P[23][2]*SK_MX[1] + P[23][18]*SK_MX[3]); - -// Calculate the observation jacobian for the Y component -float H_MAG[24]; -H_MAG[0] = magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]; -H_MAG[2] = - magE*SH_MAG[4] - magD*SH_MAG[7] - magN*SH_MAG[1]; -H_MAG[16] = 2*q1*q2 - SH_MAG[8]; -H_MAG[17] = SH_MAG[0]; -H_MAG[18] = SH_MAG[3]; +// Y axis observation Jacobians +H_MAG[0] = SH_MAG[2]; +H_MAG[1] = SH_MAG[1]; +H_MAG[2] = SH_MAG[0]; +H_MAG[3] = 2.0f*magD*q2 - SH_MAG[8] - SH_MAG[7]; +H_MAG[16] = 2.0f*q1*q2 - 2.0f*q0*q3; +H_MAG[17] = SH_MAG[4] - SH_MAG[3] - SH_MAG[5] + SH_MAG[6]; +H_MAG[18] = 2.0f*q0*q1 + 2.0f*q2*q3; H_MAG[20] = 1; -// calculate the Kalman gain matrix for the Y component +// Common expressions for Y axis Kalamn gains +SK_MY[0] = 1.0f/(P[20][20] + R_MAG + P[0][20]*SH_MAG[2] + P[1][20]*SH_MAG[1] + P[2][20]*SH_MAG[0] - P[17][20]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - (2*q0*q3 - 2.0f*q1*q2)*(P[20][16] + P[0][16]*SH_MAG[2] + P[1][16]*SH_MAG[1] + P[2][16]*SH_MAG[0] - P[17][16]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][16]*(2*q0*q3 - 2.0f*q1*q2) + P[18][16]*(2*q0*q1 + 2.0f*q2*q3) - P[3][16]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + (2*q0*q1 + 2.0f*q2*q3)*(P[20][18] + P[0][18]*SH_MAG[2] + P[1][18]*SH_MAG[1] + P[2][18]*SH_MAG[0] - P[17][18]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][18]*(2*q0*q3 - 2.0f*q1*q2) + P[18][18]*(2*q0*q1 + 2.0f*q2*q3) - P[3][18]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)*(P[20][3] + P[0][3]*SH_MAG[2] + P[1][3]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[17][3]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][3]*(2*q0*q3 - 2.0f*q1*q2) + P[18][3]*(2*q0*q1 + 2.0f*q2*q3) - P[3][3]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - P[16][20]*(2*q0*q3 - 2.0f*q1*q2) + P[18][20]*(2*q0*q1 + 2.0f*q2*q3) + SH_MAG[2]*(P[20][0] + P[0][0]*SH_MAG[2] + P[1][0]*SH_MAG[1] + P[2][0]*SH_MAG[0] - P[17][0]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][0]*(2*q0*q3 - 2.0f*q1*q2) + P[18][0]*(2*q0*q1 + 2.0f*q2*q3) - P[3][0]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + SH_MAG[1]*(P[20][1] + P[0][1]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[2][1]*SH_MAG[0] - P[17][1]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][1]*(2*q0*q3 - 2.0f*q1*q2) + P[18][1]*(2*q0*q1 + 2.0f*q2*q3) - P[3][1]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + SH_MAG[0]*(P[20][2] + P[0][2]*SH_MAG[2] + P[1][2]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[17][2]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][2]*(2*q0*q3 - 2.0f*q1*q2) + P[18][2]*(2*q0*q1 + 2.0f*q2*q3) - P[3][2]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6])*(P[20][17] + P[0][17]*SH_MAG[2] + P[1][17]*SH_MAG[1] + P[2][17]*SH_MAG[0] - P[17][17]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][17]*(2*q0*q3 - 2.0f*q1*q2) + P[18][17]*(2*q0*q1 + 2.0f*q2*q3) - P[3][17]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - P[3][20]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)); +SK_MY[1] = SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]; +SK_MY[2] = SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2; +SK_MY[3] = 2.0f*q0*q3 - 2.0f*q1*q2; +SK_MY[4] = 2.0f*q0*q1 + 2.0f*q2*q3; -// intermediate variables - note SK_MY[0] is 1/(innovation variance) -float SK_MY[4]; -SK_MY[0] = 1/(P[20][20] + R_MAG + P[0][20]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][20]*SH_MAG[0] + P[18][20]*SH_MAG[3] - (SH_MAG[8] - 2*q1*q2)*(P[20][16] + P[0][16]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][16]*SH_MAG[0] + P[18][16]*SH_MAG[3] - P[2][16]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][16]*(SH_MAG[8] - 2*q1*q2)) - P[2][20]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) + (magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5])*(P[20][0] + P[0][0]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][0]*SH_MAG[0] + P[18][0]*SH_MAG[3] - P[2][0]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][0]*(SH_MAG[8] - 2*q1*q2)) + SH_MAG[0]*(P[20][17] + P[0][17]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][17]*SH_MAG[0] + P[18][17]*SH_MAG[3] - P[2][17]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][17]*(SH_MAG[8] - 2*q1*q2)) + SH_MAG[3]*(P[20][18] + P[0][18]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][18]*SH_MAG[0] + P[18][18]*SH_MAG[3] - P[2][18]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][18]*(SH_MAG[8] - 2*q1*q2)) - P[16][20]*(SH_MAG[8] - 2*q1*q2) - (magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1])*(P[20][2] + P[0][2]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][2]*SH_MAG[0] + P[18][2]*SH_MAG[3] - P[2][2]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][2]*(SH_MAG[8] - 2*q1*q2))); -SK_MY[1] = magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]; -SK_MY[2] = magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]; -SK_MY[3] = SH_MAG[8] - 2*q1*q2; +// Y axis Kalmna gains +Kfusion[0] = SK_MY[0]*(P[0][20] + P[0][0]*SH_MAG[2] + P[0][1]*SH_MAG[1] + P[0][2]*SH_MAG[0] - P[0][3]*SK_MY[2] - P[0][17]*SK_MY[1] - P[0][16]*SK_MY[3] + P[0][18]*SK_MY[4]); +Kfusion[1] = SK_MY[0]*(P[1][20] + P[1][0]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[1][2]*SH_MAG[0] - P[1][3]*SK_MY[2] - P[1][17]*SK_MY[1] - P[1][16]*SK_MY[3] + P[1][18]*SK_MY[4]); +Kfusion[2] = SK_MY[0]*(P[2][20] + P[2][0]*SH_MAG[2] + P[2][1]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[2][3]*SK_MY[2] - P[2][17]*SK_MY[1] - P[2][16]*SK_MY[3] + P[2][18]*SK_MY[4]); +Kfusion[3] = SK_MY[0]*(P[3][20] + P[3][0]*SH_MAG[2] + P[3][1]*SH_MAG[1] + P[3][2]*SH_MAG[0] - P[3][3]*SK_MY[2] - P[3][17]*SK_MY[1] - P[3][16]*SK_MY[3] + P[3][18]*SK_MY[4]); +Kfusion[4] = SK_MY[0]*(P[4][20] + P[4][0]*SH_MAG[2] + P[4][1]*SH_MAG[1] + P[4][2]*SH_MAG[0] - P[4][3]*SK_MY[2] - P[4][17]*SK_MY[1] - P[4][16]*SK_MY[3] + P[4][18]*SK_MY[4]); +Kfusion[5] = SK_MY[0]*(P[5][20] + P[5][0]*SH_MAG[2] + P[5][1]*SH_MAG[1] + P[5][2]*SH_MAG[0] - P[5][3]*SK_MY[2] - P[5][17]*SK_MY[1] - P[5][16]*SK_MY[3] + P[5][18]*SK_MY[4]); +Kfusion[6] = SK_MY[0]*(P[6][20] + P[6][0]*SH_MAG[2] + P[6][1]*SH_MAG[1] + P[6][2]*SH_MAG[0] - P[6][3]*SK_MY[2] - P[6][17]*SK_MY[1] - P[6][16]*SK_MY[3] + P[6][18]*SK_MY[4]); +Kfusion[7] = SK_MY[0]*(P[7][20] + P[7][0]*SH_MAG[2] + P[7][1]*SH_MAG[1] + P[7][2]*SH_MAG[0] - P[7][3]*SK_MY[2] - P[7][17]*SK_MY[1] - P[7][16]*SK_MY[3] + P[7][18]*SK_MY[4]); +Kfusion[8] = SK_MY[0]*(P[8][20] + P[8][0]*SH_MAG[2] + P[8][1]*SH_MAG[1] + P[8][2]*SH_MAG[0] - P[8][3]*SK_MY[2] - P[8][17]*SK_MY[1] - P[8][16]*SK_MY[3] + P[8][18]*SK_MY[4]); +Kfusion[9] = SK_MY[0]*(P[9][20] + P[9][0]*SH_MAG[2] + P[9][1]*SH_MAG[1] + P[9][2]*SH_MAG[0] - P[9][3]*SK_MY[2] - P[9][17]*SK_MY[1] - P[9][16]*SK_MY[3] + P[9][18]*SK_MY[4]); +Kfusion[10] = SK_MY[0]*(P[10][20] + P[10][0]*SH_MAG[2] + P[10][1]*SH_MAG[1] + P[10][2]*SH_MAG[0] - P[10][3]*SK_MY[2] - P[10][17]*SK_MY[1] - P[10][16]*SK_MY[3] + P[10][18]*SK_MY[4]); +Kfusion[11] = SK_MY[0]*(P[11][20] + P[11][0]*SH_MAG[2] + P[11][1]*SH_MAG[1] + P[11][2]*SH_MAG[0] - P[11][3]*SK_MY[2] - P[11][17]*SK_MY[1] - P[11][16]*SK_MY[3] + P[11][18]*SK_MY[4]); +Kfusion[12] = SK_MY[0]*(P[12][20] + P[12][0]*SH_MAG[2] + P[12][1]*SH_MAG[1] + P[12][2]*SH_MAG[0] - P[12][3]*SK_MY[2] - P[12][17]*SK_MY[1] - P[12][16]*SK_MY[3] + P[12][18]*SK_MY[4]); +Kfusion[13] = SK_MY[0]*(P[13][20] + P[13][0]*SH_MAG[2] + P[13][1]*SH_MAG[1] + P[13][2]*SH_MAG[0] - P[13][3]*SK_MY[2] - P[13][17]*SK_MY[1] - P[13][16]*SK_MY[3] + P[13][18]*SK_MY[4]); +Kfusion[14] = SK_MY[0]*(P[14][20] + P[14][0]*SH_MAG[2] + P[14][1]*SH_MAG[1] + P[14][2]*SH_MAG[0] - P[14][3]*SK_MY[2] - P[14][17]*SK_MY[1] - P[14][16]*SK_MY[3] + P[14][18]*SK_MY[4]); +Kfusion[15] = SK_MY[0]*(P[15][20] + P[15][0]*SH_MAG[2] + P[15][1]*SH_MAG[1] + P[15][2]*SH_MAG[0] - P[15][3]*SK_MY[2] - P[15][17]*SK_MY[1] - P[15][16]*SK_MY[3] + P[15][18]*SK_MY[4]); +Kfusion[16] = SK_MY[0]*(P[16][20] + P[16][0]*SH_MAG[2] + P[16][1]*SH_MAG[1] + P[16][2]*SH_MAG[0] - P[16][3]*SK_MY[2] - P[16][17]*SK_MY[1] - P[16][16]*SK_MY[3] + P[16][18]*SK_MY[4]); +Kfusion[17] = SK_MY[0]*(P[17][20] + P[17][0]*SH_MAG[2] + P[17][1]*SH_MAG[1] + P[17][2]*SH_MAG[0] - P[17][3]*SK_MY[2] - P[17][17]*SK_MY[1] - P[17][16]*SK_MY[3] + P[17][18]*SK_MY[4]); +Kfusion[18] = SK_MY[0]*(P[18][20] + P[18][0]*SH_MAG[2] + P[18][1]*SH_MAG[1] + P[18][2]*SH_MAG[0] - P[18][3]*SK_MY[2] - P[18][17]*SK_MY[1] - P[18][16]*SK_MY[3] + P[18][18]*SK_MY[4]); +Kfusion[19] = SK_MY[0]*(P[19][20] + P[19][0]*SH_MAG[2] + P[19][1]*SH_MAG[1] + P[19][2]*SH_MAG[0] - P[19][3]*SK_MY[2] - P[19][17]*SK_MY[1] - P[19][16]*SK_MY[3] + P[19][18]*SK_MY[4]); +Kfusion[20] = SK_MY[0]*(P[20][20] + P[20][0]*SH_MAG[2] + P[20][1]*SH_MAG[1] + P[20][2]*SH_MAG[0] - P[20][3]*SK_MY[2] - P[20][17]*SK_MY[1] - P[20][16]*SK_MY[3] + P[20][18]*SK_MY[4]); +Kfusion[21] = SK_MY[0]*(P[21][20] + P[21][0]*SH_MAG[2] + P[21][1]*SH_MAG[1] + P[21][2]*SH_MAG[0] - P[21][3]*SK_MY[2] - P[21][17]*SK_MY[1] - P[21][16]*SK_MY[3] + P[21][18]*SK_MY[4]); +Kfusion[22] = SK_MY[0]*(P[22][20] + P[22][0]*SH_MAG[2] + P[22][1]*SH_MAG[1] + P[22][2]*SH_MAG[0] - P[22][3]*SK_MY[2] - P[22][17]*SK_MY[1] - P[22][16]*SK_MY[3] + P[22][18]*SK_MY[4]); +Kfusion[23] = SK_MY[0]*(P[23][20] + P[23][0]*SH_MAG[2] + P[23][1]*SH_MAG[1] + P[23][2]*SH_MAG[0] - P[23][3]*SK_MY[2] - P[23][17]*SK_MY[1] - P[23][16]*SK_MY[3] + P[23][18]*SK_MY[4]); -float Kfusion[24]; -Kfusion[0] = SK_MY[0]*(P[0][20] + P[0][17]*SH_MAG[0] + P[0][18]*SH_MAG[3] + P[0][0]*SK_MY[2] - P[0][2]*SK_MY[1] - P[0][16]*SK_MY[3]); -Kfusion[1] = SK_MY[0]*(P[1][20] + P[1][17]*SH_MAG[0] + P[1][18]*SH_MAG[3] + P[1][0]*SK_MY[2] - P[1][2]*SK_MY[1] - P[1][16]*SK_MY[3]); -Kfusion[2] = SK_MY[0]*(P[2][20] + P[2][17]*SH_MAG[0] + P[2][18]*SH_MAG[3] + P[2][0]*SK_MY[2] - P[2][2]*SK_MY[1] - P[2][16]*SK_MY[3]); -Kfusion[3] = SK_MY[0]*(P[3][20] + P[3][17]*SH_MAG[0] + P[3][18]*SH_MAG[3] + P[3][0]*SK_MY[2] - P[3][2]*SK_MY[1] - P[3][16]*SK_MY[3]); -Kfusion[4] = SK_MY[0]*(P[4][20] + P[4][17]*SH_MAG[0] + P[4][18]*SH_MAG[3] + P[4][0]*SK_MY[2] - P[4][2]*SK_MY[1] - P[4][16]*SK_MY[3]); -Kfusion[5] = SK_MY[0]*(P[5][20] + P[5][17]*SH_MAG[0] + P[5][18]*SH_MAG[3] + P[5][0]*SK_MY[2] - P[5][2]*SK_MY[1] - P[5][16]*SK_MY[3]); -Kfusion[6] = SK_MY[0]*(P[6][20] + P[6][17]*SH_MAG[0] + P[6][18]*SH_MAG[3] + P[6][0]*SK_MY[2] - P[6][2]*SK_MY[1] - P[6][16]*SK_MY[3]); -Kfusion[7] = SK_MY[0]*(P[7][20] + P[7][17]*SH_MAG[0] + P[7][18]*SH_MAG[3] + P[7][0]*SK_MY[2] - P[7][2]*SK_MY[1] - P[7][16]*SK_MY[3]); -Kfusion[8] = SK_MY[0]*(P[8][20] + P[8][17]*SH_MAG[0] + P[8][18]*SH_MAG[3] + P[8][0]*SK_MY[2] - P[8][2]*SK_MY[1] - P[8][16]*SK_MY[3]); -Kfusion[9] = SK_MY[0]*(P[9][20] + P[9][17]*SH_MAG[0] + P[9][18]*SH_MAG[3] + P[9][0]*SK_MY[2] - P[9][2]*SK_MY[1] - P[9][16]*SK_MY[3]); -Kfusion[10] = SK_MY[0]*(P[10][20] + P[10][17]*SH_MAG[0] + P[10][18]*SH_MAG[3] + P[10][0]*SK_MY[2] - P[10][2]*SK_MY[1] - P[10][16]*SK_MY[3]); -Kfusion[11] = SK_MY[0]*(P[11][20] + P[11][17]*SH_MAG[0] + P[11][18]*SH_MAG[3] + P[11][0]*SK_MY[2] - P[11][2]*SK_MY[1] - P[11][16]*SK_MY[3]); -Kfusion[12] = SK_MY[0]*(P[12][20] + P[12][17]*SH_MAG[0] + P[12][18]*SH_MAG[3] + P[12][0]*SK_MY[2] - P[12][2]*SK_MY[1] - P[12][16]*SK_MY[3]); -Kfusion[13] = SK_MY[0]*(P[13][20] + P[13][17]*SH_MAG[0] + P[13][18]*SH_MAG[3] + P[13][0]*SK_MY[2] - P[13][2]*SK_MY[1] - P[13][16]*SK_MY[3]); -Kfusion[14] = SK_MY[0]*(P[14][20] + P[14][17]*SH_MAG[0] + P[14][18]*SH_MAG[3] + P[14][0]*SK_MY[2] - P[14][2]*SK_MY[1] - P[14][16]*SK_MY[3]); -Kfusion[15] = SK_MY[0]*(P[15][20] + P[15][17]*SH_MAG[0] + P[15][18]*SH_MAG[3] + P[15][0]*SK_MY[2] - P[15][2]*SK_MY[1] - P[15][16]*SK_MY[3]); -Kfusion[16] = SK_MY[0]*(P[16][20] + P[16][17]*SH_MAG[0] + P[16][18]*SH_MAG[3] + P[16][0]*SK_MY[2] - P[16][2]*SK_MY[1] - P[16][16]*SK_MY[3]); -Kfusion[17] = SK_MY[0]*(P[17][20] + P[17][17]*SH_MAG[0] + P[17][18]*SH_MAG[3] + P[17][0]*SK_MY[2] - P[17][2]*SK_MY[1] - P[17][16]*SK_MY[3]); -Kfusion[18] = SK_MY[0]*(P[18][20] + P[18][17]*SH_MAG[0] + P[18][18]*SH_MAG[3] + P[18][0]*SK_MY[2] - P[18][2]*SK_MY[1] - P[18][16]*SK_MY[3]); -Kfusion[19] = SK_MY[0]*(P[19][20] + P[19][17]*SH_MAG[0] + P[19][18]*SH_MAG[3] + P[19][0]*SK_MY[2] - P[19][2]*SK_MY[1] - P[19][16]*SK_MY[3]); -Kfusion[20] = SK_MY[0]*(P[20][20] + P[20][17]*SH_MAG[0] + P[20][18]*SH_MAG[3] + P[20][0]*SK_MY[2] - P[20][2]*SK_MY[1] - P[20][16]*SK_MY[3]); -Kfusion[21] = SK_MY[0]*(P[21][20] + P[21][17]*SH_MAG[0] + P[21][18]*SH_MAG[3] + P[21][0]*SK_MY[2] - P[21][2]*SK_MY[1] - P[21][16]*SK_MY[3]); -Kfusion[22] = SK_MY[0]*(P[22][20] + P[22][17]*SH_MAG[0] + P[22][18]*SH_MAG[3] + P[22][0]*SK_MY[2] - P[22][2]*SK_MY[1] - P[22][16]*SK_MY[3]); -Kfusion[23] = SK_MY[0]*(P[23][20] + P[23][17]*SH_MAG[0] + P[23][18]*SH_MAG[3] + P[23][0]*SK_MY[2] - P[23][2]*SK_MY[1] - P[23][16]*SK_MY[3]); - -// Calculate the observation jacobian for the Z component -float H_MAG[24]; -H_MAG[0] = magN*(SH_MAG[8] - 2*q1*q2) - magD*SH_MAG[3] - magE*SH_MAG[0]; -H_MAG[1] = magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]; -H_MAG[16] = SH_MAG[5]; -H_MAG[17] = 2*q2*q3 - 2*q0*q1; -H_MAG[18] = SH_MAG[2]; +// Z axis observation Jacobians +H_MAG[0] = SH_MAG[1]; +H_MAG[1] = -SH_MAG[2]; +H_MAG[2] = SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2; +H_MAG[3] = SH_MAG[0]; +H_MAG[16] = 2.0f*q0*q2 + 2.0f*q1*q3; +H_MAG[17] = 2.0f*q2*q3 - 2.0f*q0*q1; +H_MAG[18] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]; H_MAG[21] = 1; -// calculate the Kalman gain matrix for the Z component +// Common expressions for Z axis Kalman gains +SK_MZ[0] = 1.0f/(P[21][21] + R_MAG + P[0][21]*SH_MAG[1] - P[1][21]*SH_MAG[2] + P[3][21]*SH_MAG[0] + P[18][21]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + (2*q0*q2 + 2.0f*q1*q3)*(P[21][16] + P[0][16]*SH_MAG[1] - P[1][16]*SH_MAG[2] + P[3][16]*SH_MAG[0] + P[18][16]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][16]*(2*q0*q2 + 2.0f*q1*q3) - P[17][16]*(2*q0*q1 - 2.0f*q2*q3) + P[2][16]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (2*q0*q1 - 2.0f*q2*q3)*(P[21][17] + P[0][17]*SH_MAG[1] - P[1][17]*SH_MAG[2] + P[3][17]*SH_MAG[0] + P[18][17]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][17]*(2*q0*q2 + 2.0f*q1*q3) - P[17][17]*(2*q0*q1 - 2.0f*q2*q3) + P[2][17]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)*(P[21][2] + P[0][2]*SH_MAG[1] - P[1][2]*SH_MAG[2] + P[3][2]*SH_MAG[0] + P[18][2]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][2]*(2*q0*q2 + 2.0f*q1*q3) - P[17][2]*(2*q0*q1 - 2.0f*q2*q3) + P[2][2]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P[16][21]*(2*q0*q2 + 2.0f*q1*q3) - P[17][21]*(2*q0*q1 - 2.0f*q2*q3) + SH_MAG[1]*(P[21][0] + P[0][0]*SH_MAG[1] - P[1][0]*SH_MAG[2] + P[3][0]*SH_MAG[0] + P[18][0]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][0]*(2*q0*q2 + 2.0f*q1*q3) - P[17][0]*(2*q0*q1 - 2.0f*q2*q3) + P[2][0]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - SH_MAG[2]*(P[21][1] + P[0][1]*SH_MAG[1] - P[1][1]*SH_MAG[2] + P[3][1]*SH_MAG[0] + P[18][1]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][1]*(2*q0*q2 + 2.0f*q1*q3) - P[17][1]*(2*q0*q1 - 2.0f*q2*q3) + P[2][1]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + SH_MAG[0]*(P[21][3] + P[0][3]*SH_MAG[1] - P[1][3]*SH_MAG[2] + P[3][3]*SH_MAG[0] + P[18][3]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][3]*(2*q0*q2 + 2.0f*q1*q3) - P[17][3]*(2*q0*q1 - 2.0f*q2*q3) + P[2][3]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6])*(P[21][18] + P[0][18]*SH_MAG[1] - P[1][18]*SH_MAG[2] + P[3][18]*SH_MAG[0] + P[18][18]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][18]*(2*q0*q2 + 2.0f*q1*q3) - P[17][18]*(2*q0*q1 - 2.0f*q2*q3) + P[2][18]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P[2][21]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)); +SK_MZ[1] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]; +SK_MZ[2] = SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2; +SK_MZ[3] = 2.0f*q0*q1 - 2.0f*q2*q3; +SK_MZ[4] = 2.0f*q0*q2 + 2.0f*q1*q3; -// intermediate variables - note SK_MZ[0] is 1/(innovation variance) -float SK_MZ[4][1]; -SK_MZ[0] = 1/(P[21][21] + R_MAG + P[16][21]*SH_MAG[5] + P[18][21]*SH_MAG[2] - (2*q0*q1 - 2*q2*q3)*(P[21][17] + P[16][17]*SH_MAG[5] + P[18][17]*SH_MAG[2] - P[0][17]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2)) + P[1][17]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][17]*(2*q0*q1 - 2*q2*q3)) - P[0][21]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2)) + P[1][21]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) + SH_MAG[5]*(P[21][16] + P[16][16]*SH_MAG[5] + P[18][16]*SH_MAG[2] - P[0][16]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2)) + P[1][16]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][16]*(2*q0*q1 - 2*q2*q3)) + SH_MAG[2]*(P[21][18] + P[16][18]*SH_MAG[5] + P[18][18]*SH_MAG[2] - P[0][18]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2)) + P[1][18]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][18]*(2*q0*q1 - 2*q2*q3)) - (magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2))*(P[21][0] + P[16][0]*SH_MAG[5] + P[18][0]*SH_MAG[2] - P[0][0]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2)) + P[1][0]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][0]*(2*q0*q1 - 2*q2*q3)) - P[17][21]*(2*q0*q1 - 2*q2*q3) + (magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1])*(P[21][1] + P[16][1]*SH_MAG[5] + P[18][1]*SH_MAG[2] - P[0][1]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2)) + P[1][1]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][1]*(2*q0*q1 - 2*q2*q3))); -SK_MZ[1] = magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2); -SK_MZ[2] = magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]; -SK_MZ[3] = 2*q0*q1 - 2*q2*q3; - -float Kfusion[24]; -Kfusion[0] = SK_MZ[0]*(P[0][21] + P[0][18]*SH_MAG[2] + P[0][16]*SH_MAG[5] - P[0][0]*SK_MZ[1] + P[0][1]*SK_MZ[2] - P[0][17]*SK_MZ[3]); -Kfusion[1] = SK_MZ[0]*(P[1][21] + P[1][18]*SH_MAG[2] + P[1][16]*SH_MAG[5] - P[1][0]*SK_MZ[1] + P[1][1]*SK_MZ[2] - P[1][17]*SK_MZ[3]); -Kfusion[2] = SK_MZ[0]*(P[2][21] + P[2][18]*SH_MAG[2] + P[2][16]*SH_MAG[5] - P[2][0]*SK_MZ[1] + P[2][1]*SK_MZ[2] - P[2][17]*SK_MZ[3]); -Kfusion[3] = SK_MZ[0]*(P[3][21] + P[3][18]*SH_MAG[2] + P[3][16]*SH_MAG[5] - P[3][0]*SK_MZ[1] + P[3][1]*SK_MZ[2] - P[3][17]*SK_MZ[3]); -Kfusion[4] = SK_MZ[0]*(P[4][21] + P[4][18]*SH_MAG[2] + P[4][16]*SH_MAG[5] - P[4][0]*SK_MZ[1] + P[4][1]*SK_MZ[2] - P[4][17]*SK_MZ[3]); -Kfusion[5] = SK_MZ[0]*(P[5][21] + P[5][18]*SH_MAG[2] + P[5][16]*SH_MAG[5] - P[5][0]*SK_MZ[1] + P[5][1]*SK_MZ[2] - P[5][17]*SK_MZ[3]); -Kfusion[6] = SK_MZ[0]*(P[6][21] + P[6][18]*SH_MAG[2] + P[6][16]*SH_MAG[5] - P[6][0]*SK_MZ[1] + P[6][1]*SK_MZ[2] - P[6][17]*SK_MZ[3]); -Kfusion[7] = SK_MZ[0]*(P[7][21] + P[7][18]*SH_MAG[2] + P[7][16]*SH_MAG[5] - P[7][0]*SK_MZ[1] + P[7][1]*SK_MZ[2] - P[7][17]*SK_MZ[3]); -Kfusion[8] = SK_MZ[0]*(P[8][21] + P[8][18]*SH_MAG[2] + P[8][16]*SH_MAG[5] - P[8][0]*SK_MZ[1] + P[8][1]*SK_MZ[2] - P[8][17]*SK_MZ[3]); -Kfusion[9] = SK_MZ[0]*(P[9][21] + P[9][18]*SH_MAG[2] + P[9][16]*SH_MAG[5] - P[9][0]*SK_MZ[1] + P[9][1]*SK_MZ[2] - P[9][17]*SK_MZ[3]); -Kfusion[10] = SK_MZ[0]*(P[10][21] + P[10][18]*SH_MAG[2] + P[10][16]*SH_MAG[5] - P[10][0]*SK_MZ[1] + P[10][1]*SK_MZ[2] - P[10][17]*SK_MZ[3]); -Kfusion[11] = SK_MZ[0]*(P[11][21] + P[11][18]*SH_MAG[2] + P[11][16]*SH_MAG[5] - P[11][0]*SK_MZ[1] + P[11][1]*SK_MZ[2] - P[11][17]*SK_MZ[3]); -Kfusion[12] = SK_MZ[0]*(P[12][21] + P[12][18]*SH_MAG[2] + P[12][16]*SH_MAG[5] - P[12][0]*SK_MZ[1] + P[12][1]*SK_MZ[2] - P[12][17]*SK_MZ[3]); -Kfusion[13] = SK_MZ[0]*(P[13][21] + P[13][18]*SH_MAG[2] + P[13][16]*SH_MAG[5] - P[13][0]*SK_MZ[1] + P[13][1]*SK_MZ[2] - P[13][17]*SK_MZ[3]); -Kfusion[14] = SK_MZ[0]*(P[14][21] + P[14][18]*SH_MAG[2] + P[14][16]*SH_MAG[5] - P[14][0]*SK_MZ[1] + P[14][1]*SK_MZ[2] - P[14][17]*SK_MZ[3]); -Kfusion[15] = SK_MZ[0]*(P[15][21] + P[15][18]*SH_MAG[2] + P[15][16]*SH_MAG[5] - P[15][0]*SK_MZ[1] + P[15][1]*SK_MZ[2] - P[15][17]*SK_MZ[3]); -Kfusion[16] = SK_MZ[0]*(P[16][21] + P[16][18]*SH_MAG[2] + P[16][16]*SH_MAG[5] - P[16][0]*SK_MZ[1] + P[16][1]*SK_MZ[2] - P[16][17]*SK_MZ[3]); -Kfusion[17] = SK_MZ[0]*(P[17][21] + P[17][18]*SH_MAG[2] + P[17][16]*SH_MAG[5] - P[17][0]*SK_MZ[1] + P[17][1]*SK_MZ[2] - P[17][17]*SK_MZ[3]); -Kfusion[18] = SK_MZ[0]*(P[18][21] + P[18][18]*SH_MAG[2] + P[18][16]*SH_MAG[5] - P[18][0]*SK_MZ[1] + P[18][1]*SK_MZ[2] - P[18][17]*SK_MZ[3]); -Kfusion[19] = SK_MZ[0]*(P[19][21] + P[19][18]*SH_MAG[2] + P[19][16]*SH_MAG[5] - P[19][0]*SK_MZ[1] + P[19][1]*SK_MZ[2] - P[19][17]*SK_MZ[3]); -Kfusion[20] = SK_MZ[0]*(P[20][21] + P[20][18]*SH_MAG[2] + P[20][16]*SH_MAG[5] - P[20][0]*SK_MZ[1] + P[20][1]*SK_MZ[2] - P[20][17]*SK_MZ[3]); -Kfusion[21] = SK_MZ[0]*(P[21][21] + P[21][18]*SH_MAG[2] + P[21][16]*SH_MAG[5] - P[21][0]*SK_MZ[1] + P[21][1]*SK_MZ[2] - P[21][17]*SK_MZ[3]); -Kfusion[22] = SK_MZ[0]*(P[22][21] + P[22][18]*SH_MAG[2] + P[22][16]*SH_MAG[5] - P[22][0]*SK_MZ[1] + P[22][1]*SK_MZ[2] - P[22][17]*SK_MZ[3]); -Kfusion[23] = SK_MZ[0]*(P[23][21] + P[23][18]*SH_MAG[2] + P[23][16]*SH_MAG[5] - P[23][0]*SK_MZ[1] + P[23][1]*SK_MZ[2] - P[23][17]*SK_MZ[3]); \ No newline at end of file +// Z axis Kalman gains +Kfusion[0] = SK_MZ[0]*(P[0][21] + P[0][0]*SH_MAG[1] - P[0][1]*SH_MAG[2] + P[0][3]*SH_MAG[0] + P[0][2]*SK_MZ[2] + P[0][18]*SK_MZ[1] + P[0][16]*SK_MZ[4] - P[0][17]*SK_MZ[3]); +Kfusion[1] = SK_MZ[0]*(P[1][21] + P[1][0]*SH_MAG[1] - P[1][1]*SH_MAG[2] + P[1][3]*SH_MAG[0] + P[1][2]*SK_MZ[2] + P[1][18]*SK_MZ[1] + P[1][16]*SK_MZ[4] - P[1][17]*SK_MZ[3]); +Kfusion[2] = SK_MZ[0]*(P[2][21] + P[2][0]*SH_MAG[1] - P[2][1]*SH_MAG[2] + P[2][3]*SH_MAG[0] + P[2][2]*SK_MZ[2] + P[2][18]*SK_MZ[1] + P[2][16]*SK_MZ[4] - P[2][17]*SK_MZ[3]); +Kfusion[3] = SK_MZ[0]*(P[3][21] + P[3][0]*SH_MAG[1] - P[3][1]*SH_MAG[2] + P[3][3]*SH_MAG[0] + P[3][2]*SK_MZ[2] + P[3][18]*SK_MZ[1] + P[3][16]*SK_MZ[4] - P[3][17]*SK_MZ[3]); +Kfusion[4] = SK_MZ[0]*(P[4][21] + P[4][0]*SH_MAG[1] - P[4][1]*SH_MAG[2] + P[4][3]*SH_MAG[0] + P[4][2]*SK_MZ[2] + P[4][18]*SK_MZ[1] + P[4][16]*SK_MZ[4] - P[4][17]*SK_MZ[3]); +Kfusion[5] = SK_MZ[0]*(P[5][21] + P[5][0]*SH_MAG[1] - P[5][1]*SH_MAG[2] + P[5][3]*SH_MAG[0] + P[5][2]*SK_MZ[2] + P[5][18]*SK_MZ[1] + P[5][16]*SK_MZ[4] - P[5][17]*SK_MZ[3]); +Kfusion[6] = SK_MZ[0]*(P[6][21] + P[6][0]*SH_MAG[1] - P[6][1]*SH_MAG[2] + P[6][3]*SH_MAG[0] + P[6][2]*SK_MZ[2] + P[6][18]*SK_MZ[1] + P[6][16]*SK_MZ[4] - P[6][17]*SK_MZ[3]); +Kfusion[7] = SK_MZ[0]*(P[7][21] + P[7][0]*SH_MAG[1] - P[7][1]*SH_MAG[2] + P[7][3]*SH_MAG[0] + P[7][2]*SK_MZ[2] + P[7][18]*SK_MZ[1] + P[7][16]*SK_MZ[4] - P[7][17]*SK_MZ[3]); +Kfusion[8] = SK_MZ[0]*(P[8][21] + P[8][0]*SH_MAG[1] - P[8][1]*SH_MAG[2] + P[8][3]*SH_MAG[0] + P[8][2]*SK_MZ[2] + P[8][18]*SK_MZ[1] + P[8][16]*SK_MZ[4] - P[8][17]*SK_MZ[3]); +Kfusion[9] = SK_MZ[0]*(P[9][21] + P[9][0]*SH_MAG[1] - P[9][1]*SH_MAG[2] + P[9][3]*SH_MAG[0] + P[9][2]*SK_MZ[2] + P[9][18]*SK_MZ[1] + P[9][16]*SK_MZ[4] - P[9][17]*SK_MZ[3]); +Kfusion[10] = SK_MZ[0]*(P[10][21] + P[10][0]*SH_MAG[1] - P[10][1]*SH_MAG[2] + P[10][3]*SH_MAG[0] + P[10][2]*SK_MZ[2] + P[10][18]*SK_MZ[1] + P[10][16]*SK_MZ[4] - P[10][17]*SK_MZ[3]); +Kfusion[11] = SK_MZ[0]*(P[11][21] + P[11][0]*SH_MAG[1] - P[11][1]*SH_MAG[2] + P[11][3]*SH_MAG[0] + P[11][2]*SK_MZ[2] + P[11][18]*SK_MZ[1] + P[11][16]*SK_MZ[4] - P[11][17]*SK_MZ[3]); +Kfusion[12] = SK_MZ[0]*(P[12][21] + P[12][0]*SH_MAG[1] - P[12][1]*SH_MAG[2] + P[12][3]*SH_MAG[0] + P[12][2]*SK_MZ[2] + P[12][18]*SK_MZ[1] + P[12][16]*SK_MZ[4] - P[12][17]*SK_MZ[3]); +Kfusion[13] = SK_MZ[0]*(P[13][21] + P[13][0]*SH_MAG[1] - P[13][1]*SH_MAG[2] + P[13][3]*SH_MAG[0] + P[13][2]*SK_MZ[2] + P[13][18]*SK_MZ[1] + P[13][16]*SK_MZ[4] - P[13][17]*SK_MZ[3]); +Kfusion[14] = SK_MZ[0]*(P[14][21] + P[14][0]*SH_MAG[1] - P[14][1]*SH_MAG[2] + P[14][3]*SH_MAG[0] + P[14][2]*SK_MZ[2] + P[14][18]*SK_MZ[1] + P[14][16]*SK_MZ[4] - P[14][17]*SK_MZ[3]); +Kfusion[15] = SK_MZ[0]*(P[15][21] + P[15][0]*SH_MAG[1] - P[15][1]*SH_MAG[2] + P[15][3]*SH_MAG[0] + P[15][2]*SK_MZ[2] + P[15][18]*SK_MZ[1] + P[15][16]*SK_MZ[4] - P[15][17]*SK_MZ[3]); +Kfusion[16] = SK_MZ[0]*(P[16][21] + P[16][0]*SH_MAG[1] - P[16][1]*SH_MAG[2] + P[16][3]*SH_MAG[0] + P[16][2]*SK_MZ[2] + P[16][18]*SK_MZ[1] + P[16][16]*SK_MZ[4] - P[16][17]*SK_MZ[3]); +Kfusion[17] = SK_MZ[0]*(P[17][21] + P[17][0]*SH_MAG[1] - P[17][1]*SH_MAG[2] + P[17][3]*SH_MAG[0] + P[17][2]*SK_MZ[2] + P[17][18]*SK_MZ[1] + P[17][16]*SK_MZ[4] - P[17][17]*SK_MZ[3]); +Kfusion[18] = SK_MZ[0]*(P[18][21] + P[18][0]*SH_MAG[1] - P[18][1]*SH_MAG[2] + P[18][3]*SH_MAG[0] + P[18][2]*SK_MZ[2] + P[18][18]*SK_MZ[1] + P[18][16]*SK_MZ[4] - P[18][17]*SK_MZ[3]); +Kfusion[19] = SK_MZ[0]*(P[19][21] + P[19][0]*SH_MAG[1] - P[19][1]*SH_MAG[2] + P[19][3]*SH_MAG[0] + P[19][2]*SK_MZ[2] + P[19][18]*SK_MZ[1] + P[19][16]*SK_MZ[4] - P[19][17]*SK_MZ[3]); +Kfusion[20] = SK_MZ[0]*(P[20][21] + P[20][0]*SH_MAG[1] - P[20][1]*SH_MAG[2] + P[20][3]*SH_MAG[0] + P[20][2]*SK_MZ[2] + P[20][18]*SK_MZ[1] + P[20][16]*SK_MZ[4] - P[20][17]*SK_MZ[3]); +Kfusion[21] = SK_MZ[0]*(P[21][21] + P[21][0]*SH_MAG[1] - P[21][1]*SH_MAG[2] + P[21][3]*SH_MAG[0] + P[21][2]*SK_MZ[2] + P[21][18]*SK_MZ[1] + P[21][16]*SK_MZ[4] - P[21][17]*SK_MZ[3]); +Kfusion[22] = SK_MZ[0]*(P[22][21] + P[22][0]*SH_MAG[1] - P[22][1]*SH_MAG[2] + P[22][3]*SH_MAG[0] + P[22][2]*SK_MZ[2] + P[22][18]*SK_MZ[1] + P[22][16]*SK_MZ[4] - P[22][17]*SK_MZ[3]); +Kfusion[23] = SK_MZ[0]*(P[23][21] + P[23][0]*SH_MAG[1] - P[23][1]*SH_MAG[2] + P[23][3]*SH_MAG[0] + P[23][2]*SK_MZ[2] + P[23][18]*SK_MZ[1] + P[23][16]*SK_MZ[4] - P[23][17]*SK_MZ[3]); diff --git a/matlab/generated/Inertial Nav EKF/Notes.txt b/matlab/generated/Inertial Nav EKF/Notes.txt index 3518d28bd4..0e0e06616f 100644 --- a/matlab/generated/Inertial Nav EKF/Notes.txt +++ b/matlab/generated/Inertial Nav EKF/Notes.txt @@ -15,36 +15,8 @@ It is important that maximum useage of the sparsity in the H matrix be taken adv otherwise the matrix operations should need unrolled with inclusion of conditional statements to improve efficiency. Examples of this technique can be found in the existing att_pos_ekf_estimator library -It is important that the attitude error state vector (first three states) be zeroed prior to fusion of a new observation. This is becasue it is assumed -that the predicted quaternion attitude is corrected using the attitude error estimate each time an observation is fused. - -Measurement fusion steps: - -1) Calculate the innovation - -2) Calculate the observation partial derivative (Jacobian) vector. For direct state observations this has not been provided as it is trivial, -eg [0 0 0 1 0 .....] for a north velocity measurement - -3) Calculate the innovation variance and apply an innovatino consistency check to determine if he observation should be fused. BTW, if -(H*P*transpose(H) + R) < R, then the covariance matrix has become ill conditioned and should probably be reset. Certainly fusion of data -should not be performed if H*P*transpose(H) < 0 - -4) Calculate the Kalman gain vector - -5) Zero the first three states in the state vector - -6) Calculate and apply a correction to the states which is the product of the Kalman gain matrix and innovation - -7) The first three states represent the estimated angular misalignment vector. Rotate the predicted quaternion from the last INS calculation through -the reciprocal rotation to remove the error - -8) Update the covariance using the standard P = (I - K*H)*P operator, taking advatage of sparseness in the H and I matrices. This has been implemented -in the code as a P - KHP operation. Note, the use of the short rather thn Joseph or other mumerically more accurate form of the equation and use of single -precision operations means that numerical stability can be an issue, so symmetry and non-negative diagonal elements must be enforced after every -covariance update. - NOTES FOR COVARIANCE PREDICTION Only expression for the upper diagonal is provided. The values will need to be copied across to the lower diagnal elements assuming symmetry Process noise for time invariant states, eg Gyro bias, has not been included in the auto-code. The process noise variance for these states will -need to be added in a separate operation. \ No newline at end of file +need to be added in a separate operation. diff --git a/matlab/generated/Inertial Nav EKF/Optical Flow Fusion.txt b/matlab/generated/Inertial Nav EKF/Optical Flow Fusion.txt index c1f76bf491..6c9aed1462 100644 --- a/matlab/generated/Inertial Nav EKF/Optical Flow Fusion.txt +++ b/matlab/generated/Inertial Nav EKF/Optical Flow Fusion.txt @@ -3,219 +3,265 @@ // Observations are body modtion compensated optica flow rates about the X and Y body axis // Sequential fusion is used (observation errors about each axis are assumed to be uncorrelated) -float H_LOS[2][24]; +float H_LOS[2][24]; // Optical flow observation Jacobians +float Kfusion[24][2]; // Optical flow Kalman gains // calculate X axis observation Jacobian float t2 = 1.0f / range; -float t3 = q0 * q0; -float t4 = q1 * q1; -float t5 = q2 * q2; -float t6 = q3 * q3; -float t7 = q0 * q2 * 2.0f; -float t8 = q1 * q3 * 2.0f; -float t9 = q0 * q3 * 2.0f; -float t10 = q1 * q2 * 2.0f; -float t11 = q0 * q1 * 2.0f; -H_LOS[0][0] = t2 * (vn * (t7 + t8) + vd * (t3 - t4 - t5 + t6) - ve * (t11 - q2 * q3 * 2.0f)); -H_LOS[0][2] = -t2 * (ve * (t9 + t10) - vd * (t7 - t8) + vn * (t3 + t4 - t5 - t6)); -H_LOS[0][3] = -t2 * (t9 - t10); -H_LOS[0][4] = t2 * (t3 - t4 + t5 - t6); -H_LOS[0][5] = t2 * (t11 + q2 * q3 * 2.0f); +H_LOS[0][0] = t2*(q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f); +H_LOS[0][1] = t2*(q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f); +H_LOS[0][2] = t2*(q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f); +H_LOS[0][3] = -t2*(q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f); +H_LOS[0][4] = -t2*(q0*q3*2.0f-q1*q2*2.0f); +H_LOS[0][5] = t2*(q0*q0-q1*q1+q2*q2-q3*q3); +H_LOS[0][6] = t2*(q0*q1*2.0f+q2*q3*2.0f); -// calculate X axis Kalman gains -t2 = 1.0/range; -t3 = q0*q1*2.0; -t4 = q2*q3*2.0; -t5 = q0*q0; -t6 = q1*q1; -t7 = q2*q2; -t8 = q3*q3; -t9 = q0*q2*2.0; -t10 = q1*q3*2.0; -t11 = q0*q3*2.0; -float t12 = q1*q2*2.0; -float t13 = t11-t12; -float t14 = t3+t4; -float t15 = t5-t6-t7+t8; -float t16 = t15*vd; -float t17 = t3-t4; -float t18 = t9+t10; -float t19 = t18*vn; -float t28 = t17*ve; -float t20 = t16+t19-t28; -float t21 = t5+t6-t7-t8; -float t22 = t21*vn; -float t23 = t9-t10; -float t24 = t11+t12; -float t25 = t24*ve; -float t29 = t23*vd; -float t26 = t22+t25-t29; -float t27 = t5-t6+t7-t8; -float t30 = P[0][0]*t2*t20; -float t31 = P[5][3]*t2*t14; -float t32 = P[0][3]*t2*t20; -float t33 = P[4][3]*t2*t27; -float t56 = P[3][3]*t2*t13; -float t57 = P[2][3]*t2*t26; -float t34 = t31+t32+t33-t56-t57; -float t35 = P[5][5]*t2*t14; -float t36 = P[0][5]*t2*t20; -float t37 = P[4][5]*t2*t27; -float t59 = P[3][5]*t2*t13; -float t60 = P[2][5]*t2*t26; -float t38 = t35+t36+t37-t59-t60; -float t39 = t2*t14*t38; -float t40 = P[5][0]*t2*t14; -float t41 = P[4][0]*t2*t27; -float t61 = P[3][0]*t2*t13; -float t62 = P[2][0]*t2*t26; -float t42 = t30+t40+t41-t61-t62; -float t43 = t2*t20*t42; -float t44 = P[5][2]*t2*t14; -float t45 = P[0][2]*t2*t20; -float t46 = P[4][2]*t2*t27; -float t55 = P[2][2]*t2*t26; -float t63 = P[3][2]*t2*t13; -float t47 = t44+t45+t46-t55-t63; -float t48 = P[5][4]*t2*t14; -float t49 = P[0][4]*t2*t20; -float t50 = P[4][4]*t2*t27; -float t65 = P[3][4]*t2*t13; -float t66 = P[2][4]*t2*t26; -float t51 = t48+t49+t50-t65-t66; -float t52 = t2*t27*t51; -float t58 = t2*t13*t34; -float t64 = t2*t26*t47; -float t53 = R_LOS+t39+t43+t52-t58-t64; -float t54 = 1.0/t53; -Kfusion[0][0] = t54*(t30-P[0][3]*t2*(t11-q1*q2*2.0)+P[0][5]*t2*t14-P[0][2]*t2*t26+P[0][4]*t2*t27); -Kfusion[1][0] = t54*(-P[1][3]*t2*t13+P[1][5]*t2*t14+P[1][0]*t2*t20-P[1][2]*t2*t26+P[1][4]*t2*t27); -Kfusion[2][0] = t54*(-t55-P[2][3]*t2*t13+P[2][5]*t2*t14+P[2][0]*t2*t20+P[2][4]*t2*t27); -Kfusion[3][0] = t54*(-t56+P[3][5]*t2*t14+P[3][0]*t2*t20-P[3][2]*t2*t26+P[3][4]*t2*t27); -Kfusion[4][0] = t54*(t50-P[4][3]*t2*t13+P[4][5]*t2*t14+P[4][0]*t2*t20-P[4][2]*t2*t26); -Kfusion[5][0] = t54*(t35-P[5][3]*t2*t13+P[5][0]*t2*t20-P[5][2]*t2*t26+P[5][4]*t2*t27); -Kfusion[6][0] = t54*(-P[6][3]*t2*t13+P[6][5]*t2*t14+P[6][0]*t2*t20-P[6][2]*t2*t26+P[6][4]*t2*t27); -Kfusion[7][0] = t54*(-P[7][3]*t2*t13+P[7][5]*t2*t14+P[7][0]*t2*t20-P[7][2]*t2*t26+P[7][4]*t2*t27); -Kfusion[8][0] = t54*(-P[8][3]*t2*t13+P[8][5]*t2*t14+P[8][0]*t2*t20-P[8][2]*t2*t26+P[8][4]*t2*t27); -Kfusion[9][0] = t54*(-P[9][3]*t2*t13+P[9][5]*t2*t14+P[9][0]*t2*t20-P[9][2]*t2*t26+P[9][4]*t2*t27); -Kfusion[10][0] = t54*(-P[10][3]*t2*t13+P[10][5]*t2*t14+P[10][0]*t2*t20-P[10][2]*t2*t26+P[10][4]*t2*t27); -Kfusion[11][0] = t54*(-P[11][3]*t2*t13+P[11][5]*t2*t14+P[11][0]*t2*t20-P[11][2]*t2*t26+P[11][4]*t2*t27); -Kfusion[12][0] = t54*(-P[12][3]*t2*t13+P[12][5]*t2*t14+P[12][0]*t2*t20-P[12][2]*t2*t26+P[12][4]*t2*t27); -Kfusion[13][0] = t54*(-P[13][3]*t2*t13+P[13][5]*t2*t14+P[13][0]*t2*t20-P[13][2]*t2*t26+P[13][4]*t2*t27); -Kfusion[14][0] = t54*(-P[14][3]*t2*t13+P[14][5]*t2*t14+P[14][0]*t2*t20-P[14][2]*t2*t26+P[14][4]*t2*t27); -Kfusion[15][0] = t54*(-P[15][3]*t2*t13+P[15][5]*t2*t14+P[15][0]*t2*t20-P[15][2]*t2*t26+P[15][4]*t2*t27); -Kfusion[16][0] = t54*(-P[16][3]*t2*t13+P[16][5]*t2*t14+P[16][0]*t2*t20-P[16][2]*t2*t26+P[16][4]*t2*t27); -Kfusion[17][0] = t54*(-P[17][3]*t2*t13+P[17][5]*t2*t14+P[17][0]*t2*t20-P[17][2]*t2*t26+P[17][4]*t2*t27); -Kfusion[18][0] = t54*(-P[18][3]*t2*t13+P[18][5]*t2*t14+P[18][0]*t2*t20-P[18][2]*t2*t26+P[18][4]*t2*t27); -Kfusion[19][0] = t54*(-P[19][3]*t2*t13+P[19][5]*t2*t14+P[19][0]*t2*t20-P[19][2]*t2*t26+P[19][4]*t2*t27); -Kfusion[20][0] = t54*(-P[20][3]*t2*t13+P[20][5]*t2*t14+P[20][0]*t2*t20-P[20][2]*t2*t26+P[20][4]*t2*t27); -Kfusion[21][0] = t54*(-P[21][3]*t2*t13+P[21][5]*t2*t14+P[21][0]*t2*t20-P[21][2]*t2*t26+P[21][4]*t2*t27); -Kfusion[22][0] = t54*(-P[22][3]*t2*t13+P[22][5]*t2*t14+P[22][0]*t2*t20-P[22][2]*t2*t26+P[22][4]*t2*t27); -Kfusion[23][0] = t54*(-P[23][3]*t2*t13+P[23][5]*t2*t14+P[23][0]*t2*t20-P[23][2]*t2*t26+P[23][4]*t2*t27); +// calculate intermediate variables for the X observaton innovatoin variance and Kalman gains +float t3 = q1*vd*2.0f; +float t4 = q0*ve*2.0f; +float t11 = q3*vn*2.0f; +float t5 = t3+t4-t11; +float t6 = q0*q3*2.0f; +float t29 = q1*q2*2.0f; +float t7 = t6-t29; +float t8 = q0*q1*2.0f; +float t9 = q2*q3*2.0f; +float t10 = t8+t9; +float t12 = P[0][0]*t2*t5; +float t13 = q0*vd*2.0f; +float t14 = q2*vn*2.0f; +float t28 = q1*ve*2.0f; +float t15 = t13+t14-t28; +float t16 = q3*vd*2.0f; +float t17 = q2*ve*2.0f; +float t18 = q1*vn*2.0f; +float t19 = t16+t17+t18; +float t20 = q3*ve*2.0f; +float t21 = q0*vn*2.0f; +float t30 = q2*vd*2.0f; +float t22 = t20+t21-t30; +float t23 = q0*q0; +float t24 = q1*q1; +float t25 = q2*q2; +float t26 = q3*q3; +float t27 = t23-t24+t25-t26; +float t31 = P[1][1]*t2*t15; +float t32 = P[6][0]*t2*t10; +float t33 = P[1][0]*t2*t15; +float t34 = P[2][0]*t2*t19; +float t35 = P[5][0]*t2*t27; +float t79 = P[4][0]*t2*t7; +float t80 = P[3][0]*t2*t22; +float t36 = t12+t32+t33+t34+t35-t79-t80; +float t37 = t2*t5*t36; +float t38 = P[6][1]*t2*t10; +float t39 = P[0][1]*t2*t5; +float t40 = P[2][1]*t2*t19; +float t41 = P[5][1]*t2*t27; +float t81 = P[4][1]*t2*t7; +float t82 = P[3][1]*t2*t22; +float t42 = t31+t38+t39+t40+t41-t81-t82; +float t43 = t2*t15*t42; +float t44 = P[6][2]*t2*t10; +float t45 = P[0][2]*t2*t5; +float t46 = P[1][2]*t2*t15; +float t47 = P[2][2]*t2*t19; +float t48 = P[5][2]*t2*t27; +float t83 = P[4][2]*t2*t7; +float t84 = P[3][2]*t2*t22; +float t49 = t44+t45+t46+t47+t48-t83-t84; +float t50 = t2*t19*t49; +float t51 = P[6][3]*t2*t10; +float t52 = P[0][3]*t2*t5; +float t53 = P[1][3]*t2*t15; +float t54 = P[2][3]*t2*t19; +float t55 = P[5][3]*t2*t27; +float t85 = P[4][3]*t2*t7; +float t86 = P[3][3]*t2*t22; +float t56 = t51+t52+t53+t54+t55-t85-t86; +float t57 = P[6][5]*t2*t10; +float t58 = P[0][5]*t2*t5; +float t59 = P[1][5]*t2*t15; +float t60 = P[2][5]*t2*t19; +float t61 = P[5][5]*t2*t27; +float t88 = P[4][5]*t2*t7; +float t89 = P[3][5]*t2*t22; +float t62 = t57+t58+t59+t60+t61-t88-t89; +float t63 = t2*t27*t62; +float t64 = P[6][4]*t2*t10; +float t65 = P[0][4]*t2*t5; +float t66 = P[1][4]*t2*t15; +float t67 = P[2][4]*t2*t19; +float t68 = P[5][4]*t2*t27; +float t90 = P[4][4]*t2*t7; +float t91 = P[3][4]*t2*t22; +float t69 = t64+t65+t66+t67+t68-t90-t91; +float t70 = P[6][6]*t2*t10; +float t71 = P[0][6]*t2*t5; +float t72 = P[1][6]*t2*t15; +float t73 = P[2][6]*t2*t19; +float t74 = P[5][6]*t2*t27; +float t93 = P[4][6]*t2*t7; +float t94 = P[3][6]*t2*t22; +float t75 = t70+t71+t72+t73+t74-t93-t94; +float t76 = t2*t10*t75; +float t87 = t2*t22*t56; +float t92 = t2*t7*t69; +float t77 = R_LOS+t37+t43+t50+t63+t76-t87-t92; +float t78 = 1.0f / t77; -// calculate Y axis observation jacobian -float t2 = 1.0f/range; -float t3 = q0*q0; -float t4 = q1*q1; -float t5 = q2*q2; -float t6 = q3*q3; -float t7 = q0*q1*2.0f; -float t8 = q0*q3*2.0f; +// calculate Kalman gains for X-axis observation +Kfusion[0][0] = t78*(t12-P[0][4]*t2*t7+P[0][1]*t2*t15+P[0][6]*t2*t10+P[0][2]*t2*t19-P[0][3]*t2*t22+P[0][5]*t2*t27); +Kfusion[1][0] = t78*(t31+P[1][0]*t2*t5-P[1][4]*t2*t7+P[1][6]*t2*t10+P[1][2]*t2*t19-P[1][3]*t2*t22+P[1][5]*t2*t27); +Kfusion[2][0] = t78*(t47+P[2][0]*t2*t5-P[2][4]*t2*t7+P[2][1]*t2*t15+P[2][6]*t2*t10-P[2][3]*t2*t22+P[2][5]*t2*t27); +Kfusion[3][0] = t78*(-t86+P[3][0]*t2*t5-P[3][4]*t2*t7+P[3][1]*t2*t15+P[3][6]*t2*t10+P[3][2]*t2*t19+P[3][5]*t2*t27); +Kfusion[4][0] = t78*(-t90+P[4][0]*t2*t5+P[4][1]*t2*t15+P[4][6]*t2*t10+P[4][2]*t2*t19-P[4][3]*t2*t22+P[4][5]*t2*t27); +Kfusion[5][0] = t78*(t61+P[5][0]*t2*t5-P[5][4]*t2*t7+P[5][1]*t2*t15+P[5][6]*t2*t10+P[5][2]*t2*t19-P[5][3]*t2*t22); +Kfusion[6][0] = t78*(t70+P[6][0]*t2*t5-P[6][4]*t2*t7+P[6][1]*t2*t15+P[6][2]*t2*t19-P[6][3]*t2*t22+P[6][5]*t2*t27); +Kfusion[7][0] = t78*(P[7][0]*t2*t5-P[7][4]*t2*t7+P[7][1]*t2*t15+P[7][6]*t2*t10+P[7][2]*t2*t19-P[7][3]*t2*t22+P[7][5]*t2*t27); +Kfusion[8][0] = t78*(P[8][0]*t2*t5-P[8][4]*t2*t7+P[8][1]*t2*t15+P[8][6]*t2*t10+P[8][2]*t2*t19-P[8][3]*t2*t22+P[8][5]*t2*t27); +Kfusion[9][0] = t78*(P[9][0]*t2*t5-P[9][4]*t2*t7+P[9][1]*t2*t15+P[9][6]*t2*t10+P[9][2]*t2*t19-P[9][3]*t2*t22+P[9][5]*t2*t27); +Kfusion[10][0] = t78*(P[10][0]*t2*t5-P[10][4]*t2*t7+P[10][1]*t2*t15+P[10][6]*t2*t10+P[10][2]*t2*t19-P[10][3]*t2*t22+P[10][5]*t2*t27); +Kfusion[11][0] = t78*(P[11][0]*t2*t5-P[11][4]*t2*t7+P[11][1]*t2*t15+P[11][6]*t2*t10+P[11][2]*t2*t19-P[11][3]*t2*t22+P[11][5]*t2*t27); +Kfusion[12][0] = t78*(P[12][0]*t2*t5-P[12][4]*t2*t7+P[12][1]*t2*t15+P[12][6]*t2*t10+P[12][2]*t2*t19-P[12][3]*t2*t22+P[12][5]*t2*t27); +Kfusion[13][0] = t78*(P[13][0]*t2*t5-P[13][4]*t2*t7+P[13][1]*t2*t15+P[13][6]*t2*t10+P[13][2]*t2*t19-P[13][3]*t2*t22+P[13][5]*t2*t27); +Kfusion[14][0] = t78*(P[14][0]*t2*t5-P[14][4]*t2*t7+P[14][1]*t2*t15+P[14][6]*t2*t10+P[14][2]*t2*t19-P[14][3]*t2*t22+P[14][5]*t2*t27); +Kfusion[15][0] = t78*(P[15][0]*t2*t5-P[15][4]*t2*t7+P[15][1]*t2*t15+P[15][6]*t2*t10+P[15][2]*t2*t19-P[15][3]*t2*t22+P[15][5]*t2*t27); +Kfusion[16][0] = t78*(P[16][0]*t2*t5-P[16][4]*t2*t7+P[16][1]*t2*t15+P[16][6]*t2*t10+P[16][2]*t2*t19-P[16][3]*t2*t22+P[16][5]*t2*t27); +Kfusion[17][0] = t78*(P[17][0]*t2*t5-P[17][4]*t2*t7+P[17][1]*t2*t15+P[17][6]*t2*t10+P[17][2]*t2*t19-P[17][3]*t2*t22+P[17][5]*t2*t27); +Kfusion[18][0] = t78*(P[18][0]*t2*t5-P[18][4]*t2*t7+P[18][1]*t2*t15+P[18][6]*t2*t10+P[18][2]*t2*t19-P[18][3]*t2*t22+P[18][5]*t2*t27); +Kfusion[19][0] = t78*(P[19][0]*t2*t5-P[19][4]*t2*t7+P[19][1]*t2*t15+P[19][6]*t2*t10+P[19][2]*t2*t19-P[19][3]*t2*t22+P[19][5]*t2*t27); +Kfusion[20][0] = t78*(P[20][0]*t2*t5-P[20][4]*t2*t7+P[20][1]*t2*t15+P[20][6]*t2*t10+P[20][2]*t2*t19-P[20][3]*t2*t22+P[20][5]*t2*t27); +Kfusion[21][0] = t78*(P[21][0]*t2*t5-P[21][4]*t2*t7+P[21][1]*t2*t15+P[21][6]*t2*t10+P[21][2]*t2*t19-P[21][3]*t2*t22+P[21][5]*t2*t27); +Kfusion[22][0] = t78*(P[22][0]*t2*t5-P[22][4]*t2*t7+P[22][1]*t2*t15+P[22][6]*t2*t10+P[22][2]*t2*t19-P[22][3]*t2*t22+P[22][5]*t2*t27); +Kfusion[23][0] = t78*(P[23][0]*t2*t5-P[23][4]*t2*t7+P[23][1]*t2*t15+P[23][6]*t2*t10+P[23][2]*t2*t19-P[23][3]*t2*t22+P[23][5]*t2*t27); + +// calculate Y axis observation Jacobian +float t2 = 1.0f / range; +H_LOS[1][0] = -t2*(q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f); +H_LOS[1][1] = -t2*(q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f); +H_LOS[1][2] = t2*(q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f); +H_LOS[1][3] = -t2*(q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f); +H_LOS[1][4] = -t2*(q0*q0+q1*q1-q2*q2-q3*q3); +H_LOS[1][5] = -t2*(q0*q3*2.0f+q1*q2*2.0f); +H_LOS[1][6] = t2*(q0*q2*2.0f-q1*q3*2.0f); + +// calculate intermediate variables for the Y axis observaton innovation variance and Kalman gains +float t3 = q3*ve*2.0f; +float t4 = q0*vn*2.0f; +float t11 = q2*vd*2.0f; +float t5 = t3+t4-t11; +float t6 = q0*q3*2.0f; +float t7 = q1*q2*2.0f; +float t8 = t6+t7; float t9 = q0*q2*2.0f; -float t10 = q1*q3*2.0f; -H_LOS[1][1] = t2*(vn*(t9+t10)+vd*(t3-t4-t5+t6)-ve*(t7-q2*q3*2.0f)); -H_LOS[1][2] = -t2*(ve*(t3-t4+t5-t6)+vd*(t7+q2*q3*2.0f)-vn*(t8-q1*q2*2.0f)); -H_LOS[1][3] = -t2*(t3+t4-t5-t6); -H_LOS[1][4] = -t2*(t8+q1*q2*2.0f); -H_LOS[1][5] = t2*(t9-t10); +float t28 = q1*q3*2.0f; +float t10 = t9-t28; +float t12 = P[0][0]*t2*t5; +float t13 = q3*vd*2.0f; +float t14 = q2*ve*2.0f; +float t15 = q1*vn*2.0f; +float t16 = t13+t14+t15; +float t17 = q0*vd*2.0f; +float t18 = q2*vn*2.0f; +float t29 = q1*ve*2.0f; +float t19 = t17+t18-t29; +float t20 = q1*vd*2.0f; +float t21 = q0*ve*2.0f; +float t30 = q3*vn*2.0f; +float t22 = t20+t21-t30; +float t23 = q0*q0; +float t24 = q1*q1; +float t25 = q2*q2; +float t26 = q3*q3; +float t27 = t23+t24-t25-t26; +float t31 = P[1][1]*t2*t16; +float t32 = P[5][0]*t2*t8; +float t33 = P[1][0]*t2*t16; +float t34 = P[3][0]*t2*t22; +float t35 = P[4][0]*t2*t27; +float t80 = P[6][0]*t2*t10; +float t81 = P[2][0]*t2*t19; +float t36 = t12+t32+t33+t34+t35-t80-t81; +float t37 = t2*t5*t36; +float t38 = P[5][1]*t2*t8; +float t39 = P[0][1]*t2*t5; +float t40 = P[3][1]*t2*t22; +float t41 = P[4][1]*t2*t27; +float t82 = P[6][1]*t2*t10; +float t83 = P[2][1]*t2*t19; +float t42 = t31+t38+t39+t40+t41-t82-t83; +float t43 = t2*t16*t42; +float t44 = P[5][2]*t2*t8; +float t45 = P[0][2]*t2*t5; +float t46 = P[1][2]*t2*t16; +float t47 = P[3][2]*t2*t22; +float t48 = P[4][2]*t2*t27; +float t79 = P[2][2]*t2*t19; +float t84 = P[6][2]*t2*t10; +float t49 = t44+t45+t46+t47+t48-t79-t84; +float t50 = P[5][3]*t2*t8; +float t51 = P[0][3]*t2*t5; +float t52 = P[1][3]*t2*t16; +float t53 = P[3][3]*t2*t22; +float t54 = P[4][3]*t2*t27; +float t86 = P[6][3]*t2*t10; +float t87 = P[2][3]*t2*t19; +float t55 = t50+t51+t52+t53+t54-t86-t87; +float t56 = t2*t22*t55; +float t57 = P[5][4]*t2*t8; +float t58 = P[0][4]*t2*t5; +float t59 = P[1][4]*t2*t16; +float t60 = P[3][4]*t2*t22; +float t61 = P[4][4]*t2*t27; +float t88 = P[6][4]*t2*t10; +float t89 = P[2][4]*t2*t19; +float t62 = t57+t58+t59+t60+t61-t88-t89; +float t63 = t2*t27*t62; +float t64 = P[5][5]*t2*t8; +float t65 = P[0][5]*t2*t5; +float t66 = P[1][5]*t2*t16; +float t67 = P[3][5]*t2*t22; +float t68 = P[4][5]*t2*t27; +float t90 = P[6][5]*t2*t10; +float t91 = P[2][5]*t2*t19; +float t69 = t64+t65+t66+t67+t68-t90-t91; +float t70 = t2*t8*t69; +float t71 = P[5][6]*t2*t8; +float t72 = P[0][6]*t2*t5; +float t73 = P[1][6]*t2*t16; +float t74 = P[3][6]*t2*t22; +float t75 = P[4][6]*t2*t27; +float t92 = P[6][6]*t2*t10; +float t93 = P[2][6]*t2*t19; +float t76 = t71+t72+t73+t74+t75-t92-t93; +float t85 = t2*t19*t49; +float t94 = t2*t10*t76; +float t77 = R_LOS+t37+t43+t56+t63+t70-t85-t94; +float t78 = 1.0f / t77; -// calculate Y axis Kalman gains -t2 = 1.0f/range; -t3 = q0*q2*2.0f; -t4 = q0*q0; -t5 = q1*q1; -t6 = q2*q2; -t7 = q3*q3; -t8 = q0*q1*2.0f; -t9 = q0*q3*2.0f; -t10 = q1*q2*2.0f; -float t11 = t9+t10; -float t12 = q1*q3*2.0f; -float t13 = t4-t5-t6+t7; -float t14 = t13*vd; -float t15 = q2*q3*2.0f; -float t16 = t3+t12; -float t17 = t16*vn; -float t18 = t4-t5+t6-t7; -float t19 = t18*ve; -float t20 = t8+t15; -float t21 = t20*vd; -float t22 = t9-t10; -float t28 = t22*vn; -float t23 = t19+t21-t28; -float t24 = t4+t5-t6-t7; -float t25 = t3-t12; -float t26 = t8-t15; -float t29 = t26*ve; -float t27 = t14+t17-t29; -float t30 = P[4][4]*t2*t11; -float t31 = P[2][4]*t2*t23; -float t32 = P[3][4]*t2*t24; -float t56 = P[5][4]*t2*t25; -float t57 = P[1][4]*t2*t27; -float t33 = t30+t31+t32-t56-t57; -float t34 = t2*t11*t33; -float t35 = P[4][5]*t2*t11; -float t36 = P[2][5]*t2*t23; -float t37 = P[3][5]*t2*t24; -float t58 = P[5][5]*t2*t25; -float t59 = P[1][5]*t2*t27; -float t38 = t35+t36+t37-t58-t59; -float t39 = P[4][1]*t2*t11; -float t40 = P[2][1]*t2*t23; -float t41 = P[3][1]*t2*t24; -float t55 = P[1][1]*t2*t27; -float t61 = P[5][1]*t2*t25; -float t42 = t39+t40+t41-t55-t61; -float t43 = P[4][2]*t2*t11; -float t44 = P[2][2]*t2*t23; -float t45 = P[3][2]*t2*t24; -float t63 = P[5][2]*t2*t25; -float t64 = P[1][2]*t2*t27; -float t46 = t43+t44+t45-t63-t64; -float t47 = t2*t23*t46; -float t48 = P[4][3]*t2*t11; -float t49 = P[2][3]*t2*t23; -float t50 = P[3][3]*t2*t24; -float t65 = P[5][3]*t2*t25; -float t66 = P[1][3]*t2*t27; -float t51 = t48+t49+t50-t65-t66; -float t52 = t2*t24*t51; -float t60 = t2*t25*t38; -float t62 = t2*t27*t42; -float t53 = R_LOS+t34+t47+t52-t60-t62; -float t54 = 1.0f/t53; -Kfusion[0][1] = -t54*(P[0][4]*t2*t11+P[0][2]*t2*t23+P[0][3]*t2*t24-P[0][1]*t2*t27-P[0][5]*t2*t25); -Kfusion[1][1] = -t54*(-t55+P[1][4]*t2*t11+P[1][2]*t2*t23+P[1][3]*t2*t24-P[1][5]*t2*t25); -Kfusion[2][1] = -t54*(t44+P[2][4]*t2*t11+P[2][3]*t2*t24-P[2][1]*t2*t27-P[2][5]*t2*t25); -Kfusion[3][1] = -t54*(t50+P[3][4]*t2*t11+P[3][2]*t2*t23-P[3][1]*t2*t27-P[3][5]*t2*t25); -Kfusion[4][1] = -t54*(t30+P[4][2]*t2*t23+P[4][3]*t2*t24-P[4][1]*t2*t27-P[4][5]*t2*t25); -Kfusion[5][1] = -t54*(-t58+P[5][4]*t2*t11+P[5][2]*t2*t23+P[5][3]*t2*t24-P[5][1]*t2*t27); -Kfusion[6][1] = -t54*(P[6][4]*t2*t11+P[6][2]*t2*t23+P[6][3]*t2*t24-P[6][1]*t2*t27-P[6][5]*t2*t25); -Kfusion[7][1] = -t54*(P[7][4]*t2*t11+P[7][2]*t2*t23+P[7][3]*t2*t24-P[7][1]*t2*t27-P[7][5]*t2*t25); -Kfusion[8][1] = -t54*(P[8][4]*t2*t11+P[8][2]*t2*t23+P[8][3]*t2*t24-P[8][1]*t2*t27-P[8][5]*t2*t25); -Kfusion[9][1] = -t54*(P[9][4]*t2*t11+P[9][2]*t2*t23+P[9][3]*t2*t24-P[9][1]*t2*t27-P[9][5]*t2*t25); -Kfusion[10][1] = -t54*(P[10][4]*t2*t11+P[10][2]*t2*t23+P[10][3]*t2*t24-P[10][1]*t2*t27-P[10][5]*t2*t25); -Kfusion[11][1] = -t54*(P[11][4]*t2*t11+P[11][2]*t2*t23+P[11][3]*t2*t24-P[11][1]*t2*t27-P[11][5]*t2*t25); -Kfusion[12][1] = -t54*(P[12][4]*t2*t11+P[12][2]*t2*t23+P[12][3]*t2*t24-P[12][1]*t2*t27-P[12][5]*t2*t25); -Kfusion[13][1] = -t54*(P[13][4]*t2*t11+P[13][2]*t2*t23+P[13][3]*t2*t24-P[13][1]*t2*t27-P[13][5]*t2*t25); -Kfusion[14][1] = -t54*(P[14][4]*t2*t11+P[14][2]*t2*t23+P[14][3]*t2*t24-P[14][1]*t2*t27-P[14][5]*t2*t25); -Kfusion[15][1] = -t54*(P[15][4]*t2*t11+P[15][2]*t2*t23+P[15][3]*t2*t24-P[15][1]*t2*t27-P[15][5]*t2*t25); -Kfusion[16][1] = -t54*(P[16][4]*t2*t11+P[16][2]*t2*t23+P[16][3]*t2*t24-P[16][1]*t2*t27-P[16][5]*t2*t25); -Kfusion[17][1] = -t54*(P[17][4]*t2*t11+P[17][2]*t2*t23+P[17][3]*t2*t24-P[17][1]*t2*t27-P[17][5]*t2*t25); -Kfusion[18][1] = -t54*(P[18][4]*t2*t11+P[18][2]*t2*t23+P[18][3]*t2*t24-P[18][1]*t2*t27-P[18][5]*t2*t25); -Kfusion[19][1] = -t54*(P[19][4]*t2*t11+P[19][2]*t2*t23+P[19][3]*t2*t24-P[19][1]*t2*t27-P[19][5]*t2*t25); -Kfusion[20][1] = -t54*(P[20][4]*t2*t11+P[20][2]*t2*t23+P[20][3]*t2*t24-P[20][1]*t2*t27-P[20][5]*t2*t25); -Kfusion[21][1] = -t54*(P[21][4]*t2*t11+P[21][2]*t2*t23+P[21][3]*t2*t24-P[21][1]*t2*t27-P[21][5]*t2*t25); -Kfusion[22][1] = -t54*(P[22][4]*t2*t11+P[22][2]*t2*t23+P[22][3]*t2*t24-P[22][1]*t2*t27-P[22][5]*t2*t25); -Kfusion[23][1] = -t54*(P[23][4]*t2*t11+P[23][2]*t2*t23+P[23][3]*t2*t24-P[23][1]*t2*t27-P[23][5]*t2*t25); +// calculate Kalman gains for Y-axis observation +Kfusion[0][1] = -t78*(t12+P[0][5]*t2*t8-P[0][6]*t2*t10+P[0][1]*t2*t16-P[0][2]*t2*t19+P[0][3]*t2*t22+P[0][4]*t2*t27); +Kfusion[1][1] = -t78*(t31+P[1][0]*t2*t5+P[1][5]*t2*t8-P[1][6]*t2*t10-P[1][2]*t2*t19+P[1][3]*t2*t22+P[1][4]*t2*t27); +Kfusion[2][1] = -t78*(-t79+P[2][0]*t2*t5+P[2][5]*t2*t8-P[2][6]*t2*t10+P[2][1]*t2*t16+P[2][3]*t2*t22+P[2][4]*t2*t27); +Kfusion[3][1] = -t78*(t53+P[3][0]*t2*t5+P[3][5]*t2*t8-P[3][6]*t2*t10+P[3][1]*t2*t16-P[3][2]*t2*t19+P[3][4]*t2*t27); +Kfusion[4][1] = -t78*(t61+P[4][0]*t2*t5+P[4][5]*t2*t8-P[4][6]*t2*t10+P[4][1]*t2*t16-P[4][2]*t2*t19+P[4][3]*t2*t22); +Kfusion[5][1] = -t78*(t64+P[5][0]*t2*t5-P[5][6]*t2*t10+P[5][1]*t2*t16-P[5][2]*t2*t19+P[5][3]*t2*t22+P[5][4]*t2*t27); +Kfusion[6][1] = -t78*(-t92+P[6][0]*t2*t5+P[6][5]*t2*t8+P[6][1]*t2*t16-P[6][2]*t2*t19+P[6][3]*t2*t22+P[6][4]*t2*t27); +Kfusion[7][1] = -t78*(P[7][0]*t2*t5+P[7][5]*t2*t8-P[7][6]*t2*t10+P[7][1]*t2*t16-P[7][2]*t2*t19+P[7][3]*t2*t22+P[7][4]*t2*t27); +Kfusion[8][1] = -t78*(P[8][0]*t2*t5+P[8][5]*t2*t8-P[8][6]*t2*t10+P[8][1]*t2*t16-P[8][2]*t2*t19+P[8][3]*t2*t22+P[8][4]*t2*t27); +Kfusion[9][1] = -t78*(P[9][0]*t2*t5+P[9][5]*t2*t8-P[9][6]*t2*t10+P[9][1]*t2*t16-P[9][2]*t2*t19+P[9][3]*t2*t22+P[9][4]*t2*t27); +Kfusion[10][1] = -t78*(P[10][0]*t2*t5+P[10][5]*t2*t8-P[10][6]*t2*t10+P[10][1]*t2*t16-P[10][2]*t2*t19+P[10][3]*t2*t22+P[10][4]*t2*t27); +Kfusion[11][1] = -t78*(P[11][0]*t2*t5+P[11][5]*t2*t8-P[11][6]*t2*t10+P[11][1]*t2*t16-P[11][2]*t2*t19+P[11][3]*t2*t22+P[11][4]*t2*t27); +Kfusion[12][1] = -t78*(P[12][0]*t2*t5+P[12][5]*t2*t8-P[12][6]*t2*t10+P[12][1]*t2*t16-P[12][2]*t2*t19+P[12][3]*t2*t22+P[12][4]*t2*t27); +Kfusion[13][1] = -t78*(P[13][0]*t2*t5+P[13][5]*t2*t8-P[13][6]*t2*t10+P[13][1]*t2*t16-P[13][2]*t2*t19+P[13][3]*t2*t22+P[13][4]*t2*t27); +Kfusion[14][1] = -t78*(P[14][0]*t2*t5+P[14][5]*t2*t8-P[14][6]*t2*t10+P[14][1]*t2*t16-P[14][2]*t2*t19+P[14][3]*t2*t22+P[14][4]*t2*t27); +Kfusion[15][1] = -t78*(P[15][0]*t2*t5+P[15][5]*t2*t8-P[15][6]*t2*t10+P[15][1]*t2*t16-P[15][2]*t2*t19+P[15][3]*t2*t22+P[15][4]*t2*t27); +Kfusion[16][1] = -t78*(P[16][0]*t2*t5+P[16][5]*t2*t8-P[16][6]*t2*t10+P[16][1]*t2*t16-P[16][2]*t2*t19+P[16][3]*t2*t22+P[16][4]*t2*t27); +Kfusion[17][1] = -t78*(P[17][0]*t2*t5+P[17][5]*t2*t8-P[17][6]*t2*t10+P[17][1]*t2*t16-P[17][2]*t2*t19+P[17][3]*t2*t22+P[17][4]*t2*t27); +Kfusion[18][1] = -t78*(P[18][0]*t2*t5+P[18][5]*t2*t8-P[18][6]*t2*t10+P[18][1]*t2*t16-P[18][2]*t2*t19+P[18][3]*t2*t22+P[18][4]*t2*t27); +Kfusion[19][1] = -t78*(P[19][0]*t2*t5+P[19][5]*t2*t8-P[19][6]*t2*t10+P[19][1]*t2*t16-P[19][2]*t2*t19+P[19][3]*t2*t22+P[19][4]*t2*t27); +Kfusion[20][1] = -t78*(P[20][0]*t2*t5+P[20][5]*t2*t8-P[20][6]*t2*t10+P[20][1]*t2*t16-P[20][2]*t2*t19+P[20][3]*t2*t22+P[20][4]*t2*t27); +Kfusion[21][1] = -t78*(P[21][0]*t2*t5+P[21][5]*t2*t8-P[21][6]*t2*t10+P[21][1]*t2*t16-P[21][2]*t2*t19+P[21][3]*t2*t22+P[21][4]*t2*t27); +Kfusion[22][1] = -t78*(P[22][0]*t2*t5+P[22][5]*t2*t8-P[22][6]*t2*t10+P[22][1]*t2*t16-P[22][2]*t2*t19+P[22][3]*t2*t22+P[22][4]*t2*t27); +Kfusion[23][1] = -t78*(P[23][0]*t2*t5+P[23][5]*t2*t8-P[23][6]*t2*t10+P[23][1]*t2*t16-P[23][2]*t2*t19+P[23][3]*t2*t22+P[23][4]*t2*t27); diff --git a/matlab/generated/Inertial Nav EKF/Sideslip Fusion.txt b/matlab/generated/Inertial Nav EKF/Sideslip Fusion.txt index 172bef58c5..3f645fb7fe 100644 --- a/matlab/generated/Inertial Nav EKF/Sideslip Fusion.txt +++ b/matlab/generated/Inertial Nav EKF/Sideslip Fusion.txt @@ -3,61 +3,67 @@ // Calculate the observation jacobian // intermediate variable from algebraic optimisation -float SH_BETA[10]; -SH_BETA[0] = (vn - vwn)*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) - vd*(2*q0*q2 - 2*q1*q3) + (ve - vwe)*(2*q0*q3 + 2*q1*q2); -SH_BETA[1] = (ve - vwe)*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + vd*(2*q0*q1 + 2*q2*q3) - (vn - vwn)*(2*q0*q3 - 2*q1*q2); -SH_BETA[2] = vd*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) - (ve - vwe)*(2*q0*q1 - 2*q2*q3) + (vn - vwn)*(2*q0*q2 + 2*q1*q3); -SH_BETA[3] = 1/sq(SH_BETA[0]); -SH_BETA[4] = (sq(q0) - sq(q1) + sq(q2) - sq(q3))/SH_BETA[0]; -SH_BETA[5] = sq(q0) + sq(q1) - sq(q2) - sq(q3); -SH_BETA[6] = 1/SH_BETA[0]; -SH_BETA[7] = 2*q0*q3; -SH_BETA[8] = SH_BETA[7] + 2*q1*q2; -SH_BETA[9] = SH_BETA[7] - 2*q1*q2; +float SH_BETA[13]; +SH_BETA = (vn - vwn)*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) - vd*(2.0f*q0*q2 - 2.0f*q1*q3) + (ve - vwe)*(2.0f*q0*q3 + 2.0f*q1*q2); +SH_BETA[1] = (ve - vwe)*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + vd*(2.0f*q0*q1 + 2.0f*q2*q3) - (vn - vwn)*(2.0f*q0*q3 - 2.0f*q1*q2); +SH_BETA[2] = vn - vwn; +SH_BETA[3] = ve - vwe; +SH_BETA[4] = 1.0f/sq(SH_BETA); +SH_BETA[5] = 1.0f/SH_BETA; +SH_BETA[6] = SH_BETA[5]*(sq(q0) - sq(q1) + sq(q2) - sq(q3)); +SH_BETA[7] = sq(q0) + sq(q1) - sq(q2) - sq(q3); +SH_BETA[8] = 2.0f*q0*SH_BETA[3] - 2.0f*q3*SH_BETA[2] + 2.0f*q1*vd; +SH_BETA[9] = 2.0f*q0*SH_BETA[2] + 2.0f*q3*SH_BETA[3] - 2.0f*q2*vd; +SH_BETA[10] = 2.0f*q2*SH_BETA[2] - 2.0f*q1*SH_BETA[3] + 2.0f*q0*vd; +SH_BETA[11] = 2.0f*q1*SH_BETA[2] + 2.0f*q2*SH_BETA[3] + 2.0f*q3*vd; +SH_BETA[12] = 2.0f*q0*q3; float H_BETA[24]; -H_BETA[0] = SH_BETA[2]*SH_BETA[6]; -H_BETA[1] = SH_BETA[1]*SH_BETA[2]*SH_BETA[3]; -H_BETA[2] = - sq(SH_BETA[1])*SH_BETA[3] - 1; -H_BETA[3] = - SH_BETA[6]*SH_BETA[9] - SH_BETA[1]*SH_BETA[3]*SH_BETA[5]; -H_BETA[4] = SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]; -H_BETA[5] = SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3); -H_BETA[22] = SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]; -H_BETA[23] = SH_BETA[1]*SH_BETA[3]*SH_BETA[8] - SH_BETA[4]; +H_BETA[0] = SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]; +H_BETA[1] = SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]; +H_BETA[2] = SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]; +H_BETA[3] = - SH_BETA[5]*SH_BETA[9] - SH_BETA[1]*SH_BETA[4]*SH_BETA[8]; +H_BETA[4] = - SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) - SH_BETA[1]*SH_BETA[4]*SH_BETA[7]; +H_BETA[5] = SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2); +H_BETA[6] = SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3); +H_BETA[22] = SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]; +H_BETA[23] = SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2) - SH_BETA[6]; // calculate the Kalman gain matrix -// intermediate variables - note SK_BETA[0] is 1/(innovation variance) -float SK_BETA[6]; -SK_BETA[0] = 1/(R_BETA + (SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8])*(P[22][4]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][4]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][4]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][4]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][4]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][4]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][4]*SH_BETA[2]*SH_BETA[6] + P[1][4]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) - (SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8])*(P[22][23]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][23]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][23]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][23]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][23]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][23]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][23]*SH_BETA[2]*SH_BETA[6] + P[1][23]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) - (SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5])*(P[22][3]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][3]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][3]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][3]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][3]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][3]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][3]*SH_BETA[2]*SH_BETA[6] + P[1][3]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) + (SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5])*(P[22][22]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][22]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][22]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][22]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][22]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][22]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][22]*SH_BETA[2]*SH_BETA[6] + P[1][22]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) - (sq(SH_BETA[1])*SH_BETA[3] + 1)*(P[22][2]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][2]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][2]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][2]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][2]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][2]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][2]*SH_BETA[2]*SH_BETA[6] + P[1][2]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) + (SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3))*(P[22][5]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][5]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][5]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][5]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][5]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][5]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][5]*SH_BETA[2]*SH_BETA[6] + P[1][5]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) + SH_BETA[2]*SH_BETA[6]*(P[22][0]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][0]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][0]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][0]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][0]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][0]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][0]*SH_BETA[2]*SH_BETA[6] + P[1][0]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) + SH_BETA[1]*SH_BETA[2]*SH_BETA[3]*(P[22][1]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][1]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][1]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][1]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][1]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][1]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][1]*SH_BETA[2]*SH_BETA[6] + P[1][1]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3])); -SK_BETA[1] = SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3); -SK_BETA[2] = SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]; -SK_BETA[3] = SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]; -SK_BETA[4] = sq(SH_BETA[1])*SH_BETA[3] + 1; -SK_BETA[5] = SH_BETA[2]; +// intermediate variables - note SK_BETA[0] is 1.0f/(innovation variance) +float SK_BETA[8]; +SK_BETA[0] = 1.0f/(R_BETA - (SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7])*(P[22][4]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][4]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][4]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P[23][4]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P[0][4]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][4]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][4]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][4]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][4]*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) + (SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7])*(P[22][22]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][22]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][22]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P[23][22]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P[0][22]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][22]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][22]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][22]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][22]*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) + (SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2))*(P[22][5]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][5]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][5]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P[23][5]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P[0][5]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][5]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][5]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][5]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][5]*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) - (SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2))*(P[22][23]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][23]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][23]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P[23][23]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P[0][23]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][23]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][23]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][23]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][23]*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) + (SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9])*(P[22][0]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][0]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][0]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P[23][0]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P[0][0]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][0]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][0]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][0]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][0]*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) + (SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11])*(P[22][1]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][1]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][1]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P[23][1]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P[0][1]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][1]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][1]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][1]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][1]*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) + (SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10])*(P[22][2]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][2]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][2]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P[23][2]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P[0][2]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][2]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][2]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][2]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][2]*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) - (SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8])*(P[22][3]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][3]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][3]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P[23][3]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P[0][3]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][3]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][3]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][3]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][3]*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) + (SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))*(P[22][6]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][6]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][6]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P[23][6]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P[0][6]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][6]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][6]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][6]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][6]*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3)))); +SK_BETA[1] = SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]; +SK_BETA[2] = SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2); +SK_BETA[3] = SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3); +SK_BETA[4] = SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]; +SK_BETA[5] = SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]; +SK_BETA[6] = SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]; +SK_BETA[7] = SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]; float Kfusion[24]; -Kfusion[0] = SK_BETA[0]*(P[0][5]*SK_BETA[1] - P[0][2]*SK_BETA[4] - P[0][3]*SK_BETA[2] + P[0][4]*SK_BETA[3] + P[0][22]*SK_BETA[2] - P[0][23]*SK_BETA[3] + P[0][0]*SH_BETA[6]*SK_BETA[5] + P[0][1]*SH_BETA[1]*SH_BETA[3]*SK_BETA[5]); -Kfusion[1] = SK_BETA[0]*(P[1][5]*SK_BETA[1] - P[1][2]*SK_BETA[4] - P[1][3]*SK_BETA[2] + P[1][4]*SK_BETA[3] + P[1][22]*SK_BETA[2] - P[1][23]*SK_BETA[3] + P[1][0]*SH_BETA[6]*SK_BETA[5] + P[1][1]*SH_BETA[1]*SH_BETA[3]*SK_BETA[5]); -Kfusion[2] = SK_BETA[0]*(P[2][5]*SK_BETA[1] - P[2][2]*SK_BETA[4] - P[2][3]*SK_BETA[2] + P[2][4]*SK_BETA[3] + P[2][22]*SK_BETA[2] - P[2][23]*SK_BETA[3] + P[2][0]*SH_BETA[6]*SK_BETA[5] + P[2][1]*SH_BETA[1]*SH_BETA[3]*SK_BETA[5]); -Kfusion[3] = SK_BETA[0]*(P[3][5]*SK_BETA[1] - P[3][2]*SK_BETA[4] - P[3][3]*SK_BETA[2] + P[3][4]*SK_BETA[3] + P[3][22]*SK_BETA[2] - P[3][23]*SK_BETA[3] + P[3][0]*SH_BETA[6]*SK_BETA[5] + P[3][1]*SH_BETA[1]*SH_BETA[3]*SK_BETA[5]); -Kfusion[4] = SK_BETA[0]*(P[4][5]*SK_BETA[1] - P[4][2]*SK_BETA[4] - P[4][3]*SK_BETA[2] + P[4][4]*SK_BETA[3] + P[4][22]*SK_BETA[2] - P[4][23]*SK_BETA[3] + P[4][0]*SH_BETA[6]*SK_BETA[5] + P[4][1]*SH_BETA[1]*SH_BETA[3]*SK_BETA[5]); -Kfusion[5] = SK_BETA[0]*(P[5][5]*SK_BETA[1] - P[5][2]*SK_BETA[4] - P[5][3]*SK_BETA[2] + P[5][4]*SK_BETA[3] + P[5][22]*SK_BETA[2] - P[5][23]*SK_BETA[3] + P[5][0]*SH_BETA[6]*SK_BETA[5] + P[5][1]*SH_BETA[1]*SH_BETA[3]*SK_BETA[5]); -Kfusion[6] = SK_BETA[0]*(P[6][5]*SK_BETA[1] - P[6][2]*SK_BETA[4] - P[6][3]*SK_BETA[2] + P[6][4]*SK_BETA[3] + P[6][22]*SK_BETA[2] - P[6][23]*SK_BETA[3] + P[6][0]*SH_BETA[6]*SK_BETA[5] + P[6][1]*SH_BETA[1]*SH_BETA[3]*SK_BETA[5]); -Kfusion[7] = SK_BETA[0]*(P[7][5]*SK_BETA[1] - P[7][2]*SK_BETA[4] - P[7][3]*SK_BETA[2] + P[7][4]*SK_BETA[3] + P[7][22]*SK_BETA[2] - P[7][23]*SK_BETA[3] + P[7][0]*SH_BETA[6]*SK_BETA[5] + P[7][1]*SH_BETA[1]*SH_BETA[3]*SK_BETA[5]); -Kfusion[8] = SK_BETA[0]*(P[8][5]*SK_BETA[1] - P[8][2]*SK_BETA[4] - P[8][3]*SK_BETA[2] + P[8][4]*SK_BETA[3] + P[8][22]*SK_BETA[2] - P[8][23]*SK_BETA[3] + P[8][0]*SH_BETA[6]*SK_BETA[5] + P[8][1]*SH_BETA[1]*SH_BETA[3]*SK_BETA[5]); -Kfusion[9] = SK_BETA[0]*(P[9][5]*SK_BETA[1] - P[9][2]*SK_BETA[4] - P[9][3]*SK_BETA[2] + P[9][4]*SK_BETA[3] + P[9][22]*SK_BETA[2] - P[9][23]*SK_BETA[3] + P[9][0]*SH_BETA[6]*SK_BETA[5] + P[9][1]*SH_BETA[1]*SH_BETA[3]*SK_BETA[5]); -Kfusion[10] = SK_BETA[0]*(P[10][5]*SK_BETA[1] - P[10][2]*SK_BETA[4] - P[10][3]*SK_BETA[2] + P[10][4]*SK_BETA[3] + P[10][22]*SK_BETA[2] - P[10][23]*SK_BETA[3] + P[10][0]*SH_BETA[6]*SK_BETA[5] + P[10][1]*SH_BETA[1]*SH_BETA[3]*SK_BETA[5]); -Kfusion[11] = SK_BETA[0]*(P[11][5]*SK_BETA[1] - P[11][2]*SK_BETA[4] - P[11][3]*SK_BETA[2] + P[11][4]*SK_BETA[3] + P[11][22]*SK_BETA[2] - P[11][23]*SK_BETA[3] + P[11][0]*SH_BETA[6]*SK_BETA[5] + P[11][1]*SH_BETA[1]*SH_BETA[3]*SK_BETA[5]); -Kfusion[12] = SK_BETA[0]*(P[12][5]*SK_BETA[1] - P[12][2]*SK_BETA[4] - P[12][3]*SK_BETA[2] + P[12][4]*SK_BETA[3] + P[12][22]*SK_BETA[2] - P[12][23]*SK_BETA[3] + P[12][0]*SH_BETA[6]*SK_BETA[5] + P[12][1]*SH_BETA[1]*SH_BETA[3]*SK_BETA[5]); -Kfusion[13] = SK_BETA[0]*(P[13][5]*SK_BETA[1] - P[13][2]*SK_BETA[4] - P[13][3]*SK_BETA[2] + P[13][4]*SK_BETA[3] + P[13][22]*SK_BETA[2] - P[13][23]*SK_BETA[3] + P[13][0]*SH_BETA[6]*SK_BETA[5] + P[13][1]*SH_BETA[1]*SH_BETA[3]*SK_BETA[5]); -Kfusion[14] = SK_BETA[0]*(P[14][5]*SK_BETA[1] - P[14][2]*SK_BETA[4] - P[14][3]*SK_BETA[2] + P[14][4]*SK_BETA[3] + P[14][22]*SK_BETA[2] - P[14][23]*SK_BETA[3] + P[14][0]*SH_BETA[6]*SK_BETA[5] + P[14][1]*SH_BETA[1]*SH_BETA[3]*SK_BETA[5]); -Kfusion[15] = SK_BETA[0]*(P[15][5]*SK_BETA[1] - P[15][2]*SK_BETA[4] - P[15][3]*SK_BETA[2] + P[15][4]*SK_BETA[3] + P[15][22]*SK_BETA[2] - P[15][23]*SK_BETA[3] + P[15][0]*SH_BETA[6]*SK_BETA[5] + P[15][1]*SH_BETA[1]*SH_BETA[3]*SK_BETA[5]); -Kfusion[16] = SK_BETA[0]*(P[16][5]*SK_BETA[1] - P[16][2]*SK_BETA[4] - P[16][3]*SK_BETA[2] + P[16][4]*SK_BETA[3] + P[16][22]*SK_BETA[2] - P[16][23]*SK_BETA[3] + P[16][0]*SH_BETA[6]*SK_BETA[5] + P[16][1]*SH_BETA[1]*SH_BETA[3]*SK_BETA[5]); -Kfusion[17] = SK_BETA[0]*(P[17][5]*SK_BETA[1] - P[17][2]*SK_BETA[4] - P[17][3]*SK_BETA[2] + P[17][4]*SK_BETA[3] + P[17][22]*SK_BETA[2] - P[17][23]*SK_BETA[3] + P[17][0]*SH_BETA[6]*SK_BETA[5] + P[17][1]*SH_BETA[1]*SH_BETA[3]*SK_BETA[5]); -Kfusion[18] = SK_BETA[0]*(P[18][5]*SK_BETA[1] - P[18][2]*SK_BETA[4] - P[18][3]*SK_BETA[2] + P[18][4]*SK_BETA[3] + P[18][22]*SK_BETA[2] - P[18][23]*SK_BETA[3] + P[18][0]*SH_BETA[6]*SK_BETA[5] + P[18][1]*SH_BETA[1]*SH_BETA[3]*SK_BETA[5]); -Kfusion[19] = SK_BETA[0]*(P[19][5]*SK_BETA[1] - P[19][2]*SK_BETA[4] - P[19][3]*SK_BETA[2] + P[19][4]*SK_BETA[3] + P[19][22]*SK_BETA[2] - P[19][23]*SK_BETA[3] + P[19][0]*SH_BETA[6]*SK_BETA[5] + P[19][1]*SH_BETA[1]*SH_BETA[3]*SK_BETA[5]); -Kfusion[20] = SK_BETA[0]*(P[20][5]*SK_BETA[1] - P[20][2]*SK_BETA[4] - P[20][3]*SK_BETA[2] + P[20][4]*SK_BETA[3] + P[20][22]*SK_BETA[2] - P[20][23]*SK_BETA[3] + P[20][0]*SH_BETA[6]*SK_BETA[5] + P[20][1]*SH_BETA[1]*SH_BETA[3]*SK_BETA[5]); -Kfusion[21] = SK_BETA[0]*(P[21][5]*SK_BETA[1] - P[21][2]*SK_BETA[4] - P[21][3]*SK_BETA[2] + P[21][4]*SK_BETA[3] + P[21][22]*SK_BETA[2] - P[21][23]*SK_BETA[3] + P[21][0]*SH_BETA[6]*SK_BETA[5] + P[21][1]*SH_BETA[1]*SH_BETA[3]*SK_BETA[5]); -Kfusion[22] = SK_BETA[0]*(P[22][5]*SK_BETA[1] - P[22][2]*SK_BETA[4] - P[22][3]*SK_BETA[2] + P[22][4]*SK_BETA[3] + P[22][22]*SK_BETA[2] - P[22][23]*SK_BETA[3] + P[22][0]*SH_BETA[6]*SK_BETA[5] + P[22][1]*SH_BETA[1]*SH_BETA[3]*SK_BETA[5]); -Kfusion[23] = SK_BETA[0]*(P[23][5]*SK_BETA[1] - P[23][2]*SK_BETA[4] - P[23][3]*SK_BETA[2] + P[23][4]*SK_BETA[3] + P[23][22]*SK_BETA[2] - P[23][23]*SK_BETA[3] + P[23][0]*SH_BETA[6]*SK_BETA[5] + P[23][1]*SH_BETA[1]*SH_BETA[3]*SK_BETA[5]); \ No newline at end of file +Kfusion[0] = SK_BETA[0]*(P[0][0]*SK_BETA[5] + P[0][1]*SK_BETA[4] - P[0][4]*SK_BETA[1] + P[0][5]*SK_BETA[2] + P[0][2]*SK_BETA[6] + P[0][6]*SK_BETA[3] - P[0][3]*SK_BETA[7] + P[0][22]*SK_BETA[1] - P[0][23]*SK_BETA[2]); +Kfusion[1] = SK_BETA[0]*(P[1][0]*SK_BETA[5] + P[1][1]*SK_BETA[4] - P[1][4]*SK_BETA[1] + P[1][5]*SK_BETA[2] + P[1][2]*SK_BETA[6] + P[1][6]*SK_BETA[3] - P[1][3]*SK_BETA[7] + P[1][22]*SK_BETA[1] - P[1][23]*SK_BETA[2]); +Kfusion[2] = SK_BETA[0]*(P[2][0]*SK_BETA[5] + P[2][1]*SK_BETA[4] - P[2][4]*SK_BETA[1] + P[2][5]*SK_BETA[2] + P[2][2]*SK_BETA[6] + P[2][6]*SK_BETA[3] - P[2][3]*SK_BETA[7] + P[2][22]*SK_BETA[1] - P[2][23]*SK_BETA[2]); +Kfusion[3] = SK_BETA[0]*(P[3][0]*SK_BETA[5] + P[3][1]*SK_BETA[4] - P[3][4]*SK_BETA[1] + P[3][5]*SK_BETA[2] + P[3][2]*SK_BETA[6] + P[3][6]*SK_BETA[3] - P[3][3]*SK_BETA[7] + P[3][22]*SK_BETA[1] - P[3][23]*SK_BETA[2]); +Kfusion[4] = SK_BETA[0]*(P[4][0]*SK_BETA[5] + P[4][1]*SK_BETA[4] - P[4][4]*SK_BETA[1] + P[4][5]*SK_BETA[2] + P[4][2]*SK_BETA[6] + P[4][6]*SK_BETA[3] - P[4][3]*SK_BETA[7] + P[4][22]*SK_BETA[1] - P[4][23]*SK_BETA[2]); +Kfusion[5] = SK_BETA[0]*(P[5][0]*SK_BETA[5] + P[5][1]*SK_BETA[4] - P[5][4]*SK_BETA[1] + P[5][5]*SK_BETA[2] + P[5][2]*SK_BETA[6] + P[5][6]*SK_BETA[3] - P[5][3]*SK_BETA[7] + P[5][22]*SK_BETA[1] - P[5][23]*SK_BETA[2]); +Kfusion[6] = SK_BETA[0]*(P[6][0]*SK_BETA[5] + P[6][1]*SK_BETA[4] - P[6][4]*SK_BETA[1] + P[6][5]*SK_BETA[2] + P[6][2]*SK_BETA[6] + P[6][6]*SK_BETA[3] - P[6][3]*SK_BETA[7] + P[6][22]*SK_BETA[1] - P[6][23]*SK_BETA[2]); +Kfusion[7] = SK_BETA[0]*(P[7][0]*SK_BETA[5] + P[7][1]*SK_BETA[4] - P[7][4]*SK_BETA[1] + P[7][5]*SK_BETA[2] + P[7][2]*SK_BETA[6] + P[7][6]*SK_BETA[3] - P[7][3]*SK_BETA[7] + P[7][22]*SK_BETA[1] - P[7][23]*SK_BETA[2]); +Kfusion[8] = SK_BETA[0]*(P[8][0]*SK_BETA[5] + P[8][1]*SK_BETA[4] - P[8][4]*SK_BETA[1] + P[8][5]*SK_BETA[2] + P[8][2]*SK_BETA[6] + P[8][6]*SK_BETA[3] - P[8][3]*SK_BETA[7] + P[8][22]*SK_BETA[1] - P[8][23]*SK_BETA[2]); +Kfusion[9] = SK_BETA[0]*(P[9][0]*SK_BETA[5] + P[9][1]*SK_BETA[4] - P[9][4]*SK_BETA[1] + P[9][5]*SK_BETA[2] + P[9][2]*SK_BETA[6] + P[9][6]*SK_BETA[3] - P[9][3]*SK_BETA[7] + P[9][22]*SK_BETA[1] - P[9][23]*SK_BETA[2]); +Kfusion[10] = SK_BETA[0]*(P[10][0]*SK_BETA[5] + P[10][1]*SK_BETA[4] - P[10][4]*SK_BETA[1] + P[10][5]*SK_BETA[2] + P[10][2]*SK_BETA[6] + P[10][6]*SK_BETA[3] - P[10][3]*SK_BETA[7] + P[10][22]*SK_BETA[1] - P[10][23]*SK_BETA[2]); +Kfusion[11] = SK_BETA[0]*(P[11][0]*SK_BETA[5] + P[11][1]*SK_BETA[4] - P[11][4]*SK_BETA[1] + P[11][5]*SK_BETA[2] + P[11][2]*SK_BETA[6] + P[11][6]*SK_BETA[3] - P[11][3]*SK_BETA[7] + P[11][22]*SK_BETA[1] - P[11][23]*SK_BETA[2]); +Kfusion[12] = SK_BETA[0]*(P[12][0]*SK_BETA[5] + P[12][1]*SK_BETA[4] - P[12][4]*SK_BETA[1] + P[12][5]*SK_BETA[2] + P[12][2]*SK_BETA[6] + P[12][6]*SK_BETA[3] - P[12][3]*SK_BETA[7] + P[12][22]*SK_BETA[1] - P[12][23]*SK_BETA[2]); +Kfusion[13] = SK_BETA[0]*(P[13][0]*SK_BETA[5] + P[13][1]*SK_BETA[4] - P[13][4]*SK_BETA[1] + P[13][5]*SK_BETA[2] + P[13][2]*SK_BETA[6] + P[13][6]*SK_BETA[3] - P[13][3]*SK_BETA[7] + P[13][22]*SK_BETA[1] - P[13][23]*SK_BETA[2]); +Kfusion[14] = SK_BETA[0]*(P[14][0]*SK_BETA[5] + P[14][1]*SK_BETA[4] - P[14][4]*SK_BETA[1] + P[14][5]*SK_BETA[2] + P[14][2]*SK_BETA[6] + P[14][6]*SK_BETA[3] - P[14][3]*SK_BETA[7] + P[14][22]*SK_BETA[1] - P[14][23]*SK_BETA[2]); +Kfusion[15] = SK_BETA[0]*(P[15][0]*SK_BETA[5] + P[15][1]*SK_BETA[4] - P[15][4]*SK_BETA[1] + P[15][5]*SK_BETA[2] + P[15][2]*SK_BETA[6] + P[15][6]*SK_BETA[3] - P[15][3]*SK_BETA[7] + P[15][22]*SK_BETA[1] - P[15][23]*SK_BETA[2]); +Kfusion[16] = SK_BETA[0]*(P[16][0]*SK_BETA[5] + P[16][1]*SK_BETA[4] - P[16][4]*SK_BETA[1] + P[16][5]*SK_BETA[2] + P[16][2]*SK_BETA[6] + P[16][6]*SK_BETA[3] - P[16][3]*SK_BETA[7] + P[16][22]*SK_BETA[1] - P[16][23]*SK_BETA[2]); +Kfusion[17] = SK_BETA[0]*(P[17][0]*SK_BETA[5] + P[17][1]*SK_BETA[4] - P[17][4]*SK_BETA[1] + P[17][5]*SK_BETA[2] + P[17][2]*SK_BETA[6] + P[17][6]*SK_BETA[3] - P[17][3]*SK_BETA[7] + P[17][22]*SK_BETA[1] - P[17][23]*SK_BETA[2]); +Kfusion[18] = SK_BETA[0]*(P[18][0]*SK_BETA[5] + P[18][1]*SK_BETA[4] - P[18][4]*SK_BETA[1] + P[18][5]*SK_BETA[2] + P[18][2]*SK_BETA[6] + P[18][6]*SK_BETA[3] - P[18][3]*SK_BETA[7] + P[18][22]*SK_BETA[1] - P[18][23]*SK_BETA[2]); +Kfusion[19] = SK_BETA[0]*(P[19][0]*SK_BETA[5] + P[19][1]*SK_BETA[4] - P[19][4]*SK_BETA[1] + P[19][5]*SK_BETA[2] + P[19][2]*SK_BETA[6] + P[19][6]*SK_BETA[3] - P[19][3]*SK_BETA[7] + P[19][22]*SK_BETA[1] - P[19][23]*SK_BETA[2]); +Kfusion[20] = SK_BETA[0]*(P[20][0]*SK_BETA[5] + P[20][1]*SK_BETA[4] - P[20][4]*SK_BETA[1] + P[20][5]*SK_BETA[2] + P[20][2]*SK_BETA[6] + P[20][6]*SK_BETA[3] - P[20][3]*SK_BETA[7] + P[20][22]*SK_BETA[1] - P[20][23]*SK_BETA[2]); +Kfusion[21] = SK_BETA[0]*(P[21][0]*SK_BETA[5] + P[21][1]*SK_BETA[4] - P[21][4]*SK_BETA[1] + P[21][5]*SK_BETA[2] + P[21][2]*SK_BETA[6] + P[21][6]*SK_BETA[3] - P[21][3]*SK_BETA[7] + P[21][22]*SK_BETA[1] - P[21][23]*SK_BETA[2]); +Kfusion[22] = SK_BETA[0]*(P[22][0]*SK_BETA[5] + P[22][1]*SK_BETA[4] - P[22][4]*SK_BETA[1] + P[22][5]*SK_BETA[2] + P[22][2]*SK_BETA[6] + P[22][6]*SK_BETA[3] - P[22][3]*SK_BETA[7] + P[22][22]*SK_BETA[1] - P[22][23]*SK_BETA[2]); +Kfusion[23] = SK_BETA[0]*(P[23][0]*SK_BETA[5] + P[23][1]*SK_BETA[4] - P[23][4]*SK_BETA[1] + P[23][5]*SK_BETA[2] + P[23][2]*SK_BETA[6] + P[23][6]*SK_BETA[3] - P[23][3]*SK_BETA[7] + P[23][22]*SK_BETA[1] - P[23][23]*SK_BETA[2]); diff --git a/matlab/generated/Inertial Nav EKF/Simple Magnetometer Fusion.txt b/matlab/generated/Inertial Nav EKF/Simple Magnetometer Fusion.txt index f098aeab8a..fc4e7ac0ea 100644 --- a/matlab/generated/Inertial Nav EKF/Simple Magnetometer Fusion.txt +++ b/matlab/generated/Inertial Nav EKF/Simple Magnetometer Fusion.txt @@ -10,48 +10,33 @@ Divide by zero protection hs been added */ // calculate intermediate variables for observation jacobian -float t2 = q0*q0; -float t3 = q1*q1; -float t4 = q2*q2; -float t5 = q3*q3; -float t6 = q0*q3*2.0f; -float t8 = t2-t3+t4-t5; -float t9 = q0*q1*2.0f; -float t10 = q2*q3*2.0f; -float t11 = t9-t10; -float t14 = q1*q2*2.0f; -float t21 = magY*t8; -float t22 = t6+t14; -float t23 = magX*t22; -float t24 = magZ*t11; -float t7 = t21+t23-t24; -float t12 = t2+t3-t4-t5; -float t13 = magX*t12; -float t15 = q0*q2*2.0f; -float t16 = q1*q3*2.0f; -float t17 = t15+t16; -float t18 = magZ*t17; -float t19 = t6-t14; -float t25 = magY*t19; -float t20 = t13+t18-t25; -if (fabsf(t20) < 1e-6f) { - return; +float t9 = q0*q3; +float t10 = q1*q2; +float t2 = t9+t10; +float t3 = q0*q0; +float t4 = q1*q1; +float t5 = q2*q2; +float t6 = q3*q3; +float t7 = t3+t4-t5-t6; +float t8 = t7*t7; +if (t8 > 1e-6f) { + t8 = 1.0f/t8; +} else { + return; } -float t26 = 1.0f/(t20*t20); -float t27 = t7*t7; -float t28 = t26*t27; -float t29 = t28+1.0; -if (fabsf(t29) < 1e-12f) { - return; +float t11 = t2*t2; +float t12 = t8*t11*4.0f; +float t13 = t12+1.0f; +float t14; +if (fabsf(t13) > 1e-6f) { + t14 = 1.0f/t13; +} else { + return; } -float t30 = 1.0f/t29; -if (fabsf(t20) < 1e-12f) { - return; -} -float t31 = 1.0f/t20; // calculate observation jacobian -float H_DECL[3] = {}; -H_DECL[0] = -t30*(t31*(magZ*t8+magY*t11)+t7*t26*(magY*t17+magZ*t19)); -H_DECL[1] = t30*(t31*(magX*t11+magZ*t22)-t7*t26*(magZ*t12-magX*t17)); -H_DECL[2] = t30*(t31*(magX*t8-magY*t22)+t7*t26*(magY*t12+magX*t19)); +float H_DECL[4] = {}; +H_YAW[0] = t8*t14*(q3*t3-q3*t4+q3*t5+q3*t6+q0*q1*q2*2.0f)*-2.0f; +H_YAW[1] = t8*t14*(-q2*t3+q2*t4+q2*t5+q2*t6+q0*q1*q3*2.0f)*-2.0f; +H_YAW[2] = t8*t14*(q1*t3+q1*t4+q1*t5-q1*t6+q0*q2*q3*2.0f)*2.0f; +H_YAW[3] = t8*t14*(q0*t3+q0*t4-q0*t5+q0*t6+q1*q2*q3*2.0f)*2.0f; diff --git a/matlab/generated/Inertial Nav EKF/Yaw Angle Fusion.txt b/matlab/generated/Inertial Nav EKF/Yaw Angle Fusion.txt index 0c856be00f..9b0c46ea13 100644 --- a/matlab/generated/Inertial Nav EKF/Yaw Angle Fusion.txt +++ b/matlab/generated/Inertial Nav EKF/Yaw Angle Fusion.txt @@ -2,44 +2,66 @@ Code fragments for fusion of an Euler yaw measurement from a 321 sequence. */ -// calculate observation jacobians -float t2 = q0*q0; -float t3 = q1*q1; -float t4 = q2*q2; -float t5 = q3*q3; -float t6 = t2+t3-t4-t5; -float t7 = q0*q3*2.0f; -float t8 = q1*q2*2.0f; -float t9 = t7+t8; -float t10 = 1.0f/(t6*t6); -float t11 = t9*t9; -float t12 = t10*t11; +// calculate observation jacobian when we are observing the first rotation in a 321 sequence +float t9 = q0*q3; +float t10 = q1*q2; +float t2 = t9+t10; +float t3 = q0*q0; +float t4 = q1*q1; +float t5 = q2*q2; +float t6 = q3*q3; +float t7 = t3+t4-t5-t6; +float t8 = t7*t7; +if (t8 > 1e-6f) { + t8 = 1.0f/t8; +} else { + return; +} +float t11 = t2*t2; +float t12 = t8*t11*4.0f; float t13 = t12+1.0f; -float t14 = 1.0f/t13; -float t15 = 1.0f/t6; -H_YAW[0] = 0.0f; -H_YAW[1] = t14*(t15*(q0*q1*2.0f-q2*q3*2.0f)+t9*t10*(q0*q2*2.0f+q1*q3*2.0f)); -H_YAW[2] = t14*(t15*(t2-t3+t4-t5)+t9*t10*(t7-t8)); +float t14; +if (fabsf(t13) > 1e-6f) { + t14 = 1.0f/t13; +} else { + return; +} + +H_YAW[0] = t8*t14*(q3*t3-q3*t4+q3*t5+q3*t6+q0*q1*q2*2.0f)*-2.0f; +H_YAW[1] = t8*t14*(-q2*t3+q2*t4+q2*t5+q2*t6+q0*q1*q3*2.0f)*-2.0f; +H_YAW[2] = t8*t14*(q1*t3+q1*t4+q1*t5-q1*t6+q0*q2*q3*2.0f)*2.0f; +H_YAW[3] = t8*t14*(q0*t3+q0*t4-q0*t5+q0*t6+q1*q2*q3*2.0f)*2.0f; /* Code fragments for fusion of an Euler yaw measurement from a 312 sequence. */ -// calculate observation jacobian -float t2 = q0*q0; -float t3 = q1*q1; -float t4 = q2*q2; -float t5 = q3*q3; -float t6 = t2-t3+t4-t5; -float t7 = q0*q3*2.0f; -float t10 = q1*q2*2.0f; -float t8 = t7-t10; -float t9 = 1.0f/(t6*t6); -float t11 = t8*t8; -float t12 = t9*t11; +// calculate observaton jacobian when we are observing a rotation in a 312 sequence +float t9 = q0*q3; +float t10 = q1*q2; +float t2 = t9-t10; +float t3 = q0*q0; +float t4 = q1*q1; +float t5 = q2*q2; +float t6 = q3*q3; +float t7 = t3-t4+t5-t6; +float t8 = t7*t7; +if (t8 > 1e-6f) { + t8 = 1.0f/t8; +} else { + return; +} +float t11 = t2*t2; +float t12 = t8*t11*4.0f; float t13 = t12+1.0f; -float t14 = 1.0f/t13; -float t15 = 1.0f/t6; -H_YAW[0] = -t14*(t15*(q0*q2*2.0f+q1*q3*2.0f)-t8*t9*(q0*q1*2.0f-q2*q3*2.0f)); -H_YAW[1] = 0.0f; -H_YAW[2] = t14*(t15*(t2+t3-t4-t5)+t8*t9*(t7+t10)); +float t14; +if (fabsf(t13) > 1e-6f) { + t14 = 1.0f/t13; +} else { + return; +} + +H_YAW[0] = t8*t14*(q3*t3+q3*t4-q3*t5+q3*t6-q0*q1*q2*2.0f)*-2.0f; +H_YAW[1] = t8*t14*(q2*t3+q2*t4+q2*t5-q2*t6-q0*q1*q3*2.0f)*-2.0f; +H_YAW[2] = t8*t14*(-q1*t3+q1*t4+q1*t5+q1*t6-q0*q2*q3*2.0f)*2.0f; +H_YAW[3] = t8*t14*(q0*t3-q0*t4+q0*t5+q0*t6-q1*q2*q3*2.0f)*2.0f; diff --git a/matlab/scripts/Inertial Nav EKF/Airspeed.mat b/matlab/scripts/Inertial Nav EKF/Airspeed.mat index 9dc46ab0fa..fd834d9025 100644 Binary files a/matlab/scripts/Inertial Nav EKF/Airspeed.mat and b/matlab/scripts/Inertial Nav EKF/Airspeed.mat differ diff --git a/matlab/scripts/Inertial Nav EKF/C_code24.txt b/matlab/scripts/Inertial Nav EKF/C_code24.txt index 6572c81d01..7c6f9d1b5f 100644 --- a/matlab/scripts/Inertial Nav EKF/C_code24.txt +++ b/matlab/scripts/Inertial Nav EKF/C_code24.txt @@ -1,217 +1,205 @@ -float SF[25][1]; -SF[0] = daz_b/2 + dazNoise/2 - (daz*daz_s)/2; -SF[1] = day_b/2 + dayNoise/2 - (day*day_s)/2; -SF[2] = dax_b/2 + daxNoise/2 - (dax*dax_s)/2; -SF[3] = q3/2 - (q0*SF[0])/2 + (q1*SF[1])/2 - (q2*SF[2])/2; -SF[4] = q0/2 - (q1*SF[2])/2 - (q2*SF[1])/2 + (q3*SF[0])/2; -SF[5] = q1/2 + (q0*SF[2])/2 - (q2*SF[0])/2 - (q3*SF[1])/2; -SF[6] = q3/2 + (q0*SF[0])/2 - (q1*SF[1])/2 - (q2*SF[2])/2; -SF[7] = q0/2 - (q1*SF[2])/2 + (q2*SF[1])/2 - (q3*SF[0])/2; -SF[8] = q0/2 + (q1*SF[2])/2 - (q2*SF[1])/2 - (q3*SF[0])/2; -SF[9] = q2/2 + (q0*SF[1])/2 + (q1*SF[0])/2 + (q3*SF[2])/2; -SF[10] = q2/2 - (q0*SF[1])/2 - (q1*SF[0])/2 + (q3*SF[2])/2; -SF[11] = q2/2 + (q0*SF[1])/2 - (q1*SF[0])/2 - (q3*SF[2])/2; -SF[12] = q1/2 + (q0*SF[2])/2 + (q2*SF[0])/2 + (q3*SF[1])/2; -SF[13] = q1/2 - (q0*SF[2])/2 + (q2*SF[0])/2 - (q3*SF[1])/2; -SF[14] = q3/2 + (q0*SF[0])/2 + (q1*SF[1])/2 + (q2*SF[2])/2; -SF[15] = - sq(q0) - sq(q1) - sq(q2) - sq(q3); -SF[16] = dvz_b - dvz + dvzNoise; -SF[17] = dvx - dvxNoise; -SF[18] = dvy - dvyNoise; -SF[19] = sq(q2); -SF[20] = SF[19] - sq(q0) + sq(q1) - sq(q3); -SF[21] = SF[19] + sq(q0) - sq(q1) - sq(q3); -SF[22] = 2*q0*q1 - 2*q2*q3; -SF[23] = SF[19] - sq(q0) - sq(q1) + sq(q3); -SF[24] = 2*q1*q2; -float SG[5][1]; -SG[0] = - sq(q0) - sq(q1) - sq(q2) - sq(q3); +float SF[21][1]; +SF[0] = dvz - dvz_b; +SF[1] = dvy - dvy_b; +SF[2] = dvx - dvx_b; +SF[3] = 2*q1*SF[2] + 2*q2*SF[1] + 2*q3*SF[0]; +SF[4] = 2*q0*SF[1] - 2*q1*SF[0] + 2*q3*SF[2]; +SF[5] = 2*q0*SF[2] + 2*q2*SF[0] - 2*q3*SF[1]; +SF[6] = day/2 - day_b/2; +SF[7] = daz/2 - daz_b/2; +SF[8] = dax/2 - dax_b/2; +SF[9] = dax_b/2 - dax/2; +SF[10] = daz_b/2 - daz/2; +SF[11] = day_b/2 - day/2; +SF[12] = 2*q1*SF[1]; +SF[13] = 2*q0*SF[0]; +SF[14] = q1/2; +SF[15] = q2/2; +SF[16] = q3/2; +SF[17] = sq(q3); +SF[18] = sq(q2); +SF[19] = sq(q1); +SF[20] = sq(q0); +float SG[8][1]; +SG[0] = q0/2; SG[1] = sq(q3); SG[2] = sq(q2); SG[3] = sq(q1); SG[4] = sq(q0); -float SQ[10][1]; -SQ[0] = - sq(dvyNoise)*(2*q0*q1 + 2*q2*q3)*(SG[1] - SG[2] + SG[3] - SG[4]) - sq(dvzNoise)*(2*q0*q1 - 2*q2*q3)*(SG[1] - SG[2] - SG[3] + SG[4]) - sq(dvxNoise)*(2*q0*q2 - 2*q1*q3)*(2*q0*q3 + 2*q1*q2); -SQ[1] = sq(dvxNoise)*(2*q0*q2 - 2*q1*q3)*(SG[1] + SG[2] - SG[3] - SG[4]) + sq(dvzNoise)*(2*q0*q2 + 2*q1*q3)*(SG[1] - SG[2] - SG[3] + SG[4]) - sq(dvyNoise)*(2*q0*q1 + 2*q2*q3)*(2*q0*q3 - 2*q1*q2); -SQ[2] = sq(dvyNoise)*(2*q0*q3 - 2*q1*q2)*(SG[1] - SG[2] + SG[3] - SG[4]) - sq(dvxNoise)*(2*q0*q3 + 2*q1*q2)*(SG[1] + SG[2] - SG[3] - SG[4]) - sq(dvzNoise)*(2*q0*q1 - 2*q2*q3)*(2*q0*q2 + 2*q1*q3); -SQ[3] = sq(SG[0]); -SQ[4] = sq(dvyNoise); -SQ[5] = sq(dvzNoise); -SQ[6] = sq(dvxNoise); -SQ[7] = 2*q2*q3; -SQ[8] = 2*q1*q3; -SQ[9] = 2*q1*q2; -float SPP[23][1]; -SPP[0] = SF[17]*(2*q0*q1 + 2*q2*q3) + SF[18]*(2*q0*q2 - 2*q1*q3); -SPP[1] = SF[18]*(2*q0*q2 + 2*q1*q3) + SF[16]*(SF[24] - 2*q0*q3); -SPP[2] = 2*q3*SF[8] + 2*q1*SF[11] - 2*q0*SF[14] - 2*q2*SF[13]; -SPP[3] = 2*q1*SF[7] + 2*q2*SF[6] - 2*q0*SF[12] - 2*q3*SF[10]; -SPP[4] = 2*q0*SF[6] - 2*q3*SF[7] - 2*q1*SF[10] + 2*q2*SF[12]; -SPP[5] = 2*q0*SF[8] + 2*q2*SF[11] + 2*q1*SF[13] + 2*q3*SF[14]; -SPP[6] = 2*q0*SF[7] + 2*q3*SF[6] + 2*q2*SF[10] + 2*q1*SF[12]; -SPP[7] = 2*q1*SF[3] - 2*q2*SF[4] - 2*q3*SF[5] + 2*q0*SF[9]; -SPP[8] = 2*q0*SF[5] - 2*q1*SF[4] - 2*q2*SF[3] + 2*q3*SF[9]; -SPP[9] = SF[18]*SF[20] - SF[16]*(2*q0*q1 + 2*q2*q3); -SPP[10] = SF[17]*SF[20] + SF[16]*(2*q0*q2 - 2*q1*q3); -SPP[11] = SF[17]*SF[21] - SF[18]*(SF[24] + 2*q0*q3); -SPP[12] = SF[17]*SF[22] - SF[16]*(SF[24] + 2*q0*q3); -SPP[13] = 2*q0*SF[4] + 2*q1*SF[5] + 2*q3*SF[3] + 2*q2*SF[9]; -SPP[14] = 2*q2*SF[8] - 2*q0*SF[11] - 2*q1*SF[14] + 2*q3*SF[13]; -SPP[15] = SF[18]*SF[23] + SF[17]*(SF[24] - 2*q0*q3); -SPP[16] = daz*SF[19] + daz*sq(q0) + daz*sq(q1) + daz*sq(q3); -SPP[17] = day*SF[19] + day*sq(q0) + day*sq(q1) + day*sq(q3); -SPP[18] = dax*SF[19] + dax*sq(q0) + dax*sq(q1) + dax*sq(q3); -SPP[19] = SF[16]*SF[23] - SF[17]*(2*q0*q2 + 2*q1*q3); -SPP[20] = SF[16]*SF[21] - SF[18]*SF[22]; -SPP[21] = 2*q0*q2 + 2*q1*q3; -SPP[22] = SF[15]; +SG[5] = 2*q2*q3; +SG[6] = 2*q1*q3; +SG[7] = 2*q1*q2; +float SQ[11][1]; +SQ[0] = dvzVar*(SG[5] - 2*q0*q1)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvyVar*(SG[5] + 2*q0*q1)*(SG[1] - SG[2] + SG[3] - SG[4]) + dvxVar*(SG[6] - 2*q0*q2)*(SG[7] + 2*q0*q3); +SQ[1] = dvzVar*(SG[6] + 2*q0*q2)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvxVar*(SG[6] - 2*q0*q2)*(SG[1] + SG[2] - SG[3] - SG[4]) + dvyVar*(SG[5] + 2*q0*q1)*(SG[7] - 2*q0*q3); +SQ[2] = dvzVar*(SG[5] - 2*q0*q1)*(SG[6] + 2*q0*q2) - dvyVar*(SG[7] - 2*q0*q3)*(SG[1] - SG[2] + SG[3] - SG[4]) - dvxVar*(SG[7] + 2*q0*q3)*(SG[1] + SG[2] - SG[3] - SG[4]); +SQ[3] = (dayVar*q1*SG[0])/2 - (dazVar*q1*SG[0])/2 - (daxVar*q2*q3)/4; +SQ[4] = (dazVar*q2*SG[0])/2 - (daxVar*q2*SG[0])/2 - (dayVar*q1*q3)/4; +SQ[5] = (daxVar*q3*SG[0])/2 - (dayVar*q3*SG[0])/2 - (dazVar*q1*q2)/4; +SQ[6] = (daxVar*q1*q2)/4 - (dazVar*q3*SG[0])/2 - (dayVar*q1*q2)/4; +SQ[7] = (dazVar*q1*q3)/4 - (daxVar*q1*q3)/4 - (dayVar*q2*SG[0])/2; +SQ[8] = (dayVar*q2*q3)/4 - (daxVar*q1*SG[0])/2 - (dazVar*q2*q3)/4; +SQ[9] = sq(SG[0]); +SQ[10] = sq(q1); +float SPP[11][1]; +SPP[0] = SF[12] + SF[13] - 2*q2*SF[2]; +SPP[1] = SF[17] - SF[18] - SF[19] + SF[20]; +SPP[2] = SF[17] - SF[18] + SF[19] - SF[20]; +SPP[3] = SF[17] + SF[18] - SF[19] - SF[20]; +SPP[4] = 2*q0*q2 - 2*q1*q3; +SPP[5] = 2*q0*q1 - 2*q2*q3; +SPP[6] = 2*q0*q3 - 2*q1*q2; +SPP[7] = 2*q0*q1 + 2*q2*q3; +SPP[8] = 2*q0*q3 + 2*q1*q2; +SPP[9] = 2*q0*q2 + 2*q1*q3; +SPP[10] = SF[16]; float nextP[24][24]; -nextP[0][0] = SPP[5]*(P[0][0]*SPP[5] - P[1][0]*SPP[4] + P[2][0]*SPP[7] + P[9][0]*SPP[22] + P[12][0]*SPP[18]) - SPP[4]*(P[0][1]*SPP[5] - P[1][1]*SPP[4] + P[2][1]*SPP[7] + P[9][1]*SPP[22] + P[12][1]*SPP[18]) + SPP[7]*(P[0][2]*SPP[5] - P[1][2]*SPP[4] + P[2][2]*SPP[7] + P[9][2]*SPP[22] + P[12][2]*SPP[18]) + SPP[22]*(P[0][9]*SPP[5] - P[1][9]*SPP[4] + P[2][9]*SPP[7] + P[9][9]*SPP[22] + P[12][9]*SPP[18]) + SPP[18]*(P[0][12]*SPP[5] - P[1][12]*SPP[4] + P[2][12]*SPP[7] + P[9][12]*SPP[22] + P[12][12]*SPP[18]) + sq(daxNoise)*SQ[3]; -nextP[0][1] = SPP[6]*(P[0][1]*SPP[5] - P[1][1]*SPP[4] + P[2][1]*SPP[7] + P[9][1]*SPP[22] + P[12][1]*SPP[18]) - SPP[2]*(P[0][0]*SPP[5] - P[1][0]*SPP[4] + P[2][0]*SPP[7] + P[9][0]*SPP[22] + P[12][0]*SPP[18]) - SPP[8]*(P[0][2]*SPP[5] - P[1][2]*SPP[4] + P[2][2]*SPP[7] + P[9][2]*SPP[22] + P[12][2]*SPP[18]) + SPP[22]*(P[0][10]*SPP[5] - P[1][10]*SPP[4] + P[2][10]*SPP[7] + P[9][10]*SPP[22] + P[12][10]*SPP[18]) + SPP[17]*(P[0][13]*SPP[5] - P[1][13]*SPP[4] + P[2][13]*SPP[7] + P[9][13]*SPP[22] + P[12][13]*SPP[18]); -nextP[1][1] = SPP[6]*(P[1][1]*SPP[6] - P[0][1]*SPP[2] - P[2][1]*SPP[8] + P[10][1]*SPP[22] + P[13][1]*SPP[17]) - SPP[2]*(P[1][0]*SPP[6] - P[0][0]*SPP[2] - P[2][0]*SPP[8] + P[10][0]*SPP[22] + P[13][0]*SPP[17]) - SPP[8]*(P[1][2]*SPP[6] - P[0][2]*SPP[2] - P[2][2]*SPP[8] + P[10][2]*SPP[22] + P[13][2]*SPP[17]) + SPP[22]*(P[1][10]*SPP[6] - P[0][10]*SPP[2] - P[2][10]*SPP[8] + P[10][10]*SPP[22] + P[13][10]*SPP[17]) + SPP[17]*(P[1][13]*SPP[6] - P[0][13]*SPP[2] - P[2][13]*SPP[8] + P[10][13]*SPP[22] + P[13][13]*SPP[17]) + sq(dayNoise)*SQ[3]; -nextP[0][2] = SPP[14]*(P[0][0]*SPP[5] - P[1][0]*SPP[4] + P[2][0]*SPP[7] + P[9][0]*SPP[22] + P[12][0]*SPP[18]) - SPP[3]*(P[0][1]*SPP[5] - P[1][1]*SPP[4] + P[2][1]*SPP[7] + P[9][1]*SPP[22] + P[12][1]*SPP[18]) + SPP[13]*(P[0][2]*SPP[5] - P[1][2]*SPP[4] + P[2][2]*SPP[7] + P[9][2]*SPP[22] + P[12][2]*SPP[18]) + SPP[22]*(P[0][11]*SPP[5] - P[1][11]*SPP[4] + P[2][11]*SPP[7] + P[9][11]*SPP[22] + P[12][11]*SPP[18]) + SPP[16]*(P[0][14]*SPP[5] - P[1][14]*SPP[4] + P[2][14]*SPP[7] + P[9][14]*SPP[22] + P[12][14]*SPP[18]); -nextP[1][2] = SPP[14]*(P[1][0]*SPP[6] - P[0][0]*SPP[2] - P[2][0]*SPP[8] + P[10][0]*SPP[22] + P[13][0]*SPP[17]) - SPP[3]*(P[1][1]*SPP[6] - P[0][1]*SPP[2] - P[2][1]*SPP[8] + P[10][1]*SPP[22] + P[13][1]*SPP[17]) + SPP[13]*(P[1][2]*SPP[6] - P[0][2]*SPP[2] - P[2][2]*SPP[8] + P[10][2]*SPP[22] + P[13][2]*SPP[17]) + SPP[22]*(P[1][11]*SPP[6] - P[0][11]*SPP[2] - P[2][11]*SPP[8] + P[10][11]*SPP[22] + P[13][11]*SPP[17]) + SPP[16]*(P[1][14]*SPP[6] - P[0][14]*SPP[2] - P[2][14]*SPP[8] + P[10][14]*SPP[22] + P[13][14]*SPP[17]); -nextP[2][2] = SPP[14]*(P[0][0]*SPP[14] - P[1][0]*SPP[3] + P[2][0]*SPP[13] + P[11][0]*SPP[22] + P[14][0]*SPP[16]) - SPP[3]*(P[0][1]*SPP[14] - P[1][1]*SPP[3] + P[2][1]*SPP[13] + P[11][1]*SPP[22] + P[14][1]*SPP[16]) + SPP[13]*(P[0][2]*SPP[14] - P[1][2]*SPP[3] + P[2][2]*SPP[13] + P[11][2]*SPP[22] + P[14][2]*SPP[16]) + SPP[22]*(P[0][11]*SPP[14] - P[1][11]*SPP[3] + P[2][11]*SPP[13] + P[11][11]*SPP[22] + P[14][11]*SPP[16]) + SPP[16]*(P[0][14]*SPP[14] - P[1][14]*SPP[3] + P[2][14]*SPP[13] + P[11][14]*SPP[22] + P[14][14]*SPP[16]) + sq(dazNoise)*SQ[3]; -nextP[0][3] = P[0][3]*SPP[5] - P[1][3]*SPP[4] + P[2][3]*SPP[7] + P[9][3]*SPP[22] + P[12][3]*SPP[18] + SPP[1]*(P[0][0]*SPP[5] - P[1][0]*SPP[4] + P[2][0]*SPP[7] + P[9][0]*SPP[22] + P[12][0]*SPP[18]) + SPP[19]*(P[0][1]*SPP[5] - P[1][1]*SPP[4] + P[2][1]*SPP[7] + P[9][1]*SPP[22] + P[12][1]*SPP[18]) + SPP[15]*(P[0][2]*SPP[5] - P[1][2]*SPP[4] + P[2][2]*SPP[7] + P[9][2]*SPP[22] + P[12][2]*SPP[18]) - SPP[21]*(P[0][15]*SPP[5] - P[1][15]*SPP[4] + P[2][15]*SPP[7] + P[9][15]*SPP[22] + P[12][15]*SPP[18]); -nextP[1][3] = P[1][3]*SPP[6] - P[0][3]*SPP[2] - P[2][3]*SPP[8] + P[10][3]*SPP[22] + P[13][3]*SPP[17] + SPP[1]*(P[1][0]*SPP[6] - P[0][0]*SPP[2] - P[2][0]*SPP[8] + P[10][0]*SPP[22] + P[13][0]*SPP[17]) + SPP[19]*(P[1][1]*SPP[6] - P[0][1]*SPP[2] - P[2][1]*SPP[8] + P[10][1]*SPP[22] + P[13][1]*SPP[17]) + SPP[15]*(P[1][2]*SPP[6] - P[0][2]*SPP[2] - P[2][2]*SPP[8] + P[10][2]*SPP[22] + P[13][2]*SPP[17]) - SPP[21]*(P[1][15]*SPP[6] - P[0][15]*SPP[2] - P[2][15]*SPP[8] + P[10][15]*SPP[22] + P[13][15]*SPP[17]); -nextP[2][3] = P[0][3]*SPP[14] - P[1][3]*SPP[3] + P[2][3]*SPP[13] + P[11][3]*SPP[22] + P[14][3]*SPP[16] + SPP[1]*(P[0][0]*SPP[14] - P[1][0]*SPP[3] + P[2][0]*SPP[13] + P[11][0]*SPP[22] + P[14][0]*SPP[16]) + SPP[19]*(P[0][1]*SPP[14] - P[1][1]*SPP[3] + P[2][1]*SPP[13] + P[11][1]*SPP[22] + P[14][1]*SPP[16]) + SPP[15]*(P[0][2]*SPP[14] - P[1][2]*SPP[3] + P[2][2]*SPP[13] + P[11][2]*SPP[22] + P[14][2]*SPP[16]) - SPP[21]*(P[0][15]*SPP[14] - P[1][15]*SPP[3] + P[2][15]*SPP[13] + P[11][15]*SPP[22] + P[14][15]*SPP[16]); -nextP[3][3] = P[3][3] + P[0][3]*SPP[1] + P[1][3]*SPP[19] + P[2][3]*SPP[15] - P[15][3]*SPP[21] + SQ[5]*sq(SQ[8] + 2*q0*q2) + SQ[4]*sq(SQ[9] - 2*q0*q3) + SPP[1]*(P[3][0] + P[0][0]*SPP[1] + P[1][0]*SPP[19] + P[2][0]*SPP[15] - P[15][0]*SPP[21]) + SPP[19]*(P[3][1] + P[0][1]*SPP[1] + P[1][1]*SPP[19] + P[2][1]*SPP[15] - P[15][1]*SPP[21]) + SPP[15]*(P[3][2] + P[0][2]*SPP[1] + P[1][2]*SPP[19] + P[2][2]*SPP[15] - P[15][2]*SPP[21]) - SPP[21]*(P[3][15] + P[0][15]*SPP[1] + P[1][15]*SPP[19] + P[2][15]*SPP[15] - P[15][15]*SPP[21]) + SQ[6]*sq(SG[1] + SG[2] - SG[3] - SG[4]); -nextP[0][4] = P[0][4]*SPP[5] - P[1][4]*SPP[4] + P[2][4]*SPP[7] + P[9][4]*SPP[22] + P[12][4]*SPP[18] + SF[22]*(P[0][15]*SPP[5] - P[1][15]*SPP[4] + P[2][15]*SPP[7] + P[9][15]*SPP[22] + P[12][15]*SPP[18]) + SPP[12]*(P[0][1]*SPP[5] - P[1][1]*SPP[4] + P[2][1]*SPP[7] + P[9][1]*SPP[22] + P[12][1]*SPP[18]) + SPP[20]*(P[0][0]*SPP[5] - P[1][0]*SPP[4] + P[2][0]*SPP[7] + P[9][0]*SPP[22] + P[12][0]*SPP[18]) + SPP[11]*(P[0][2]*SPP[5] - P[1][2]*SPP[4] + P[2][2]*SPP[7] + P[9][2]*SPP[22] + P[12][2]*SPP[18]); -nextP[1][4] = P[1][4]*SPP[6] - P[0][4]*SPP[2] - P[2][4]*SPP[8] + P[10][4]*SPP[22] + P[13][4]*SPP[17] + SF[22]*(P[1][15]*SPP[6] - P[0][15]*SPP[2] - P[2][15]*SPP[8] + P[10][15]*SPP[22] + P[13][15]*SPP[17]) + SPP[12]*(P[1][1]*SPP[6] - P[0][1]*SPP[2] - P[2][1]*SPP[8] + P[10][1]*SPP[22] + P[13][1]*SPP[17]) + SPP[20]*(P[1][0]*SPP[6] - P[0][0]*SPP[2] - P[2][0]*SPP[8] + P[10][0]*SPP[22] + P[13][0]*SPP[17]) + SPP[11]*(P[1][2]*SPP[6] - P[0][2]*SPP[2] - P[2][2]*SPP[8] + P[10][2]*SPP[22] + P[13][2]*SPP[17]); -nextP[2][4] = P[0][4]*SPP[14] - P[1][4]*SPP[3] + P[2][4]*SPP[13] + P[11][4]*SPP[22] + P[14][4]*SPP[16] + SF[22]*(P[0][15]*SPP[14] - P[1][15]*SPP[3] + P[2][15]*SPP[13] + P[11][15]*SPP[22] + P[14][15]*SPP[16]) + SPP[12]*(P[0][1]*SPP[14] - P[1][1]*SPP[3] + P[2][1]*SPP[13] + P[11][1]*SPP[22] + P[14][1]*SPP[16]) + SPP[20]*(P[0][0]*SPP[14] - P[1][0]*SPP[3] + P[2][0]*SPP[13] + P[11][0]*SPP[22] + P[14][0]*SPP[16]) + SPP[11]*(P[0][2]*SPP[14] - P[1][2]*SPP[3] + P[2][2]*SPP[13] + P[11][2]*SPP[22] + P[14][2]*SPP[16]); -nextP[3][4] = P[3][4] + SQ[2] + P[0][4]*SPP[1] + P[1][4]*SPP[19] + P[2][4]*SPP[15] - P[15][4]*SPP[21] + SF[22]*(P[3][15] + P[0][15]*SPP[1] + P[1][15]*SPP[19] + P[2][15]*SPP[15] - P[15][15]*SPP[21]) + SPP[12]*(P[3][1] + P[0][1]*SPP[1] + P[1][1]*SPP[19] + P[2][1]*SPP[15] - P[15][1]*SPP[21]) + SPP[20]*(P[3][0] + P[0][0]*SPP[1] + P[1][0]*SPP[19] + P[2][0]*SPP[15] - P[15][0]*SPP[21]) + SPP[11]*(P[3][2] + P[0][2]*SPP[1] + P[1][2]*SPP[19] + P[2][2]*SPP[15] - P[15][2]*SPP[21]); -nextP[4][4] = P[4][4] + P[15][4]*SF[22] + P[0][4]*SPP[20] + P[1][4]*SPP[12] + P[2][4]*SPP[11] + SQ[5]*sq(SQ[7] - 2*q0*q1) + SQ[6]*sq(SQ[9] + 2*q0*q3) + SF[22]*(P[4][15] + P[15][15]*SF[22] + P[0][15]*SPP[20] + P[1][15]*SPP[12] + P[2][15]*SPP[11]) + SPP[12]*(P[4][1] + P[15][1]*SF[22] + P[0][1]*SPP[20] + P[1][1]*SPP[12] + P[2][1]*SPP[11]) + SPP[20]*(P[4][0] + P[15][0]*SF[22] + P[0][0]*SPP[20] + P[1][0]*SPP[12] + P[2][0]*SPP[11]) + SPP[11]*(P[4][2] + P[15][2]*SF[22] + P[0][2]*SPP[20] + P[1][2]*SPP[12] + P[2][2]*SPP[11]) + SQ[4]*sq(SG[1] - SG[2] + SG[3] - SG[4]); -nextP[0][5] = P[0][5]*SPP[5] - P[1][5]*SPP[4] + P[2][5]*SPP[7] + P[9][5]*SPP[22] + P[12][5]*SPP[18] + SF[20]*(P[0][15]*SPP[5] - P[1][15]*SPP[4] + P[2][15]*SPP[7] + P[9][15]*SPP[22] + P[12][15]*SPP[18]) - SPP[9]*(P[0][0]*SPP[5] - P[1][0]*SPP[4] + P[2][0]*SPP[7] + P[9][0]*SPP[22] + P[12][0]*SPP[18]) + SPP[0]*(P[0][2]*SPP[5] - P[1][2]*SPP[4] + P[2][2]*SPP[7] + P[9][2]*SPP[22] + P[12][2]*SPP[18]) + SPP[10]*(P[0][1]*SPP[5] - P[1][1]*SPP[4] + P[2][1]*SPP[7] + P[9][1]*SPP[22] + P[12][1]*SPP[18]); -nextP[1][5] = P[1][5]*SPP[6] - P[0][5]*SPP[2] - P[2][5]*SPP[8] + P[10][5]*SPP[22] + P[13][5]*SPP[17] + SF[20]*(P[1][15]*SPP[6] - P[0][15]*SPP[2] - P[2][15]*SPP[8] + P[10][15]*SPP[22] + P[13][15]*SPP[17]) - SPP[9]*(P[1][0]*SPP[6] - P[0][0]*SPP[2] - P[2][0]*SPP[8] + P[10][0]*SPP[22] + P[13][0]*SPP[17]) + SPP[0]*(P[1][2]*SPP[6] - P[0][2]*SPP[2] - P[2][2]*SPP[8] + P[10][2]*SPP[22] + P[13][2]*SPP[17]) + SPP[10]*(P[1][1]*SPP[6] - P[0][1]*SPP[2] - P[2][1]*SPP[8] + P[10][1]*SPP[22] + P[13][1]*SPP[17]); -nextP[2][5] = P[0][5]*SPP[14] - P[1][5]*SPP[3] + P[2][5]*SPP[13] + P[11][5]*SPP[22] + P[14][5]*SPP[16] + SF[20]*(P[0][15]*SPP[14] - P[1][15]*SPP[3] + P[2][15]*SPP[13] + P[11][15]*SPP[22] + P[14][15]*SPP[16]) - SPP[9]*(P[0][0]*SPP[14] - P[1][0]*SPP[3] + P[2][0]*SPP[13] + P[11][0]*SPP[22] + P[14][0]*SPP[16]) + SPP[0]*(P[0][2]*SPP[14] - P[1][2]*SPP[3] + P[2][2]*SPP[13] + P[11][2]*SPP[22] + P[14][2]*SPP[16]) + SPP[10]*(P[0][1]*SPP[14] - P[1][1]*SPP[3] + P[2][1]*SPP[13] + P[11][1]*SPP[22] + P[14][1]*SPP[16]); -nextP[3][5] = P[3][5] + SQ[1] + P[0][5]*SPP[1] + P[1][5]*SPP[19] + P[2][5]*SPP[15] - P[15][5]*SPP[21] + SF[20]*(P[3][15] + P[0][15]*SPP[1] + P[1][15]*SPP[19] + P[2][15]*SPP[15] - P[15][15]*SPP[21]) - SPP[9]*(P[3][0] + P[0][0]*SPP[1] + P[1][0]*SPP[19] + P[2][0]*SPP[15] - P[15][0]*SPP[21]) + SPP[0]*(P[3][2] + P[0][2]*SPP[1] + P[1][2]*SPP[19] + P[2][2]*SPP[15] - P[15][2]*SPP[21]) + SPP[10]*(P[3][1] + P[0][1]*SPP[1] + P[1][1]*SPP[19] + P[2][1]*SPP[15] - P[15][1]*SPP[21]); -nextP[4][5] = P[4][5] + SQ[0] + P[15][5]*SF[22] + P[0][5]*SPP[20] + P[1][5]*SPP[12] + P[2][5]*SPP[11] + SF[20]*(P[4][15] + P[15][15]*SF[22] + P[0][15]*SPP[20] + P[1][15]*SPP[12] + P[2][15]*SPP[11]) - SPP[9]*(P[4][0] + P[15][0]*SF[22] + P[0][0]*SPP[20] + P[1][0]*SPP[12] + P[2][0]*SPP[11]) + SPP[0]*(P[4][2] + P[15][2]*SF[22] + P[0][2]*SPP[20] + P[1][2]*SPP[12] + P[2][2]*SPP[11]) + SPP[10]*(P[4][1] + P[15][1]*SF[22] + P[0][1]*SPP[20] + P[1][1]*SPP[12] + P[2][1]*SPP[11]); -nextP[5][5] = P[5][5] + P[15][5]*SF[20] - P[0][5]*SPP[9] + P[1][5]*SPP[10] + P[2][5]*SPP[0] + SQ[4]*sq(SQ[7] + 2*q0*q1) + SQ[6]*sq(SQ[8] - 2*q0*q2) + SF[20]*(P[5][15] + P[15][15]*SF[20] - P[0][15]*SPP[9] + P[1][15]*SPP[10] + P[2][15]*SPP[0]) - SPP[9]*(P[5][0] + P[15][0]*SF[20] - P[0][0]*SPP[9] + P[1][0]*SPP[10] + P[2][0]*SPP[0]) + SPP[0]*(P[5][2] + P[15][2]*SF[20] - P[0][2]*SPP[9] + P[1][2]*SPP[10] + P[2][2]*SPP[0]) + SPP[10]*(P[5][1] + P[15][1]*SF[20] - P[0][1]*SPP[9] + P[1][1]*SPP[10] + P[2][1]*SPP[0]) + SQ[5]*sq(SG[1] - SG[2] - SG[3] + SG[4]); -nextP[0][6] = P[0][6]*SPP[5] - P[1][6]*SPP[4] + P[2][6]*SPP[7] + P[9][6]*SPP[22] + P[12][6]*SPP[18] + dt*(P[0][3]*SPP[5] - P[1][3]*SPP[4] + P[2][3]*SPP[7] + P[9][3]*SPP[22] + P[12][3]*SPP[18]); -nextP[1][6] = P[1][6]*SPP[6] - P[0][6]*SPP[2] - P[2][6]*SPP[8] + P[10][6]*SPP[22] + P[13][6]*SPP[17] + dt*(P[1][3]*SPP[6] - P[0][3]*SPP[2] - P[2][3]*SPP[8] + P[10][3]*SPP[22] + P[13][3]*SPP[17]); -nextP[2][6] = P[0][6]*SPP[14] - P[1][6]*SPP[3] + P[2][6]*SPP[13] + P[11][6]*SPP[22] + P[14][6]*SPP[16] + dt*(P[0][3]*SPP[14] - P[1][3]*SPP[3] + P[2][3]*SPP[13] + P[11][3]*SPP[22] + P[14][3]*SPP[16]); -nextP[3][6] = P[3][6] + P[0][6]*SPP[1] + P[1][6]*SPP[19] + P[2][6]*SPP[15] - P[15][6]*SPP[21] + dt*(P[3][3] + P[0][3]*SPP[1] + P[1][3]*SPP[19] + P[2][3]*SPP[15] - P[15][3]*SPP[21]); -nextP[4][6] = P[4][6] + P[15][6]*SF[22] + P[0][6]*SPP[20] + P[1][6]*SPP[12] + P[2][6]*SPP[11] + dt*(P[4][3] + P[15][3]*SF[22] + P[0][3]*SPP[20] + P[1][3]*SPP[12] + P[2][3]*SPP[11]); -nextP[5][6] = P[5][6] + P[15][6]*SF[20] - P[0][6]*SPP[9] + P[1][6]*SPP[10] + P[2][6]*SPP[0] + dt*(P[5][3] + P[15][3]*SF[20] - P[0][3]*SPP[9] + P[1][3]*SPP[10] + P[2][3]*SPP[0]); -nextP[6][6] = P[6][6] + P[3][6]*dt + dt*(P[6][3] + P[3][3]*dt); -nextP[0][7] = P[0][7]*SPP[5] - P[1][7]*SPP[4] + P[2][7]*SPP[7] + P[9][7]*SPP[22] + P[12][7]*SPP[18] + dt*(P[0][4]*SPP[5] - P[1][4]*SPP[4] + P[2][4]*SPP[7] + P[9][4]*SPP[22] + P[12][4]*SPP[18]); -nextP[1][7] = P[1][7]*SPP[6] - P[0][7]*SPP[2] - P[2][7]*SPP[8] + P[10][7]*SPP[22] + P[13][7]*SPP[17] + dt*(P[1][4]*SPP[6] - P[0][4]*SPP[2] - P[2][4]*SPP[8] + P[10][4]*SPP[22] + P[13][4]*SPP[17]); -nextP[2][7] = P[0][7]*SPP[14] - P[1][7]*SPP[3] + P[2][7]*SPP[13] + P[11][7]*SPP[22] + P[14][7]*SPP[16] + dt*(P[0][4]*SPP[14] - P[1][4]*SPP[3] + P[2][4]*SPP[13] + P[11][4]*SPP[22] + P[14][4]*SPP[16]); -nextP[3][7] = P[3][7] + P[0][7]*SPP[1] + P[1][7]*SPP[19] + P[2][7]*SPP[15] - P[15][7]*SPP[21] + dt*(P[3][4] + P[0][4]*SPP[1] + P[1][4]*SPP[19] + P[2][4]*SPP[15] - P[15][4]*SPP[21]); -nextP[4][7] = P[4][7] + P[15][7]*SF[22] + P[0][7]*SPP[20] + P[1][7]*SPP[12] + P[2][7]*SPP[11] + dt*(P[4][4] + P[15][4]*SF[22] + P[0][4]*SPP[20] + P[1][4]*SPP[12] + P[2][4]*SPP[11]); -nextP[5][7] = P[5][7] + P[15][7]*SF[20] - P[0][7]*SPP[9] + P[1][7]*SPP[10] + P[2][7]*SPP[0] + dt*(P[5][4] + P[15][4]*SF[20] - P[0][4]*SPP[9] + P[1][4]*SPP[10] + P[2][4]*SPP[0]); -nextP[6][7] = P[6][7] + P[3][7]*dt + dt*(P[6][4] + P[3][4]*dt); +nextP[0][0] = P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10] + (daxVar*SQ[10])/4 + SF[9]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) + SF[11]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SF[10]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) + SF[14]*(P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10]) + SF[15]*(P[0][11] + P[1][11]*SF[9] + P[2][11]*SF[11] + P[3][11]*SF[10] + P[10][11]*SF[14] + P[11][11]*SF[15] + P[12][11]*SPP[10]) + SPP[10]*(P[0][12] + P[1][12]*SF[9] + P[2][12]*SF[11] + P[3][12]*SF[10] + P[10][12]*SF[14] + P[11][12]*SF[15] + P[12][12]*SPP[10]) + (dayVar*sq(q2))/4 + (dazVar*sq(q3))/4; +nextP[0][1] = P[0][1] + SQ[8] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10] + SF[8]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SF[7]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SF[11]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) - SF[15]*(P[0][12] + P[1][12]*SF[9] + P[2][12]*SF[11] + P[3][12]*SF[10] + P[10][12]*SF[14] + P[11][12]*SF[15] + P[12][12]*SPP[10]) + SPP[10]*(P[0][11] + P[1][11]*SF[9] + P[2][11]*SF[11] + P[3][11]*SF[10] + P[10][11]*SF[14] + P[11][11]*SF[15] + P[12][11]*SPP[10]) - (q0*(P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10]))/2; +nextP[1][1] = P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] + daxVar*SQ[9] - (P[10][1]*q0)/2 + SF[8]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SF[7]*(P[1][2] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2) + SF[11]*(P[1][3] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2) - SF[15]*(P[1][12] + P[0][12]*SF[8] + P[2][12]*SF[7] + P[3][12]*SF[11] - P[12][12]*SF[15] + P[11][12]*SPP[10] - (P[10][12]*q0)/2) + SPP[10]*(P[1][11] + P[0][11]*SF[8] + P[2][11]*SF[7] + P[3][11]*SF[11] - P[12][11]*SF[15] + P[11][11]*SPP[10] - (P[10][11]*q0)/2) + (dayVar*sq(q3))/4 + (dazVar*sq(q2))/4 - (q0*(P[1][10] + P[0][10]*SF[8] + P[2][10]*SF[7] + P[3][10]*SF[11] - P[12][10]*SF[15] + P[11][10]*SPP[10] - (P[10][10]*q0)/2))/2; +nextP[0][2] = P[0][2] + SQ[7] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10] + SF[6]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SF[10]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) + SF[8]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) + SF[14]*(P[0][12] + P[1][12]*SF[9] + P[2][12]*SF[11] + P[3][12]*SF[10] + P[10][12]*SF[14] + P[11][12]*SF[15] + P[12][12]*SPP[10]) - SPP[10]*(P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10]) - (q0*(P[0][11] + P[1][11]*SF[9] + P[2][11]*SF[11] + P[3][11]*SF[10] + P[10][11]*SF[14] + P[11][11]*SF[15] + P[12][11]*SPP[10]))/2; +nextP[1][2] = P[1][2] + SQ[5] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2 + SF[6]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SF[10]*(P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] - (P[10][1]*q0)/2) + SF[8]*(P[1][3] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2) + SF[14]*(P[1][12] + P[0][12]*SF[8] + P[2][12]*SF[7] + P[3][12]*SF[11] - P[12][12]*SF[15] + P[11][12]*SPP[10] - (P[10][12]*q0)/2) - SPP[10]*(P[1][10] + P[0][10]*SF[8] + P[2][10]*SF[7] + P[3][10]*SF[11] - P[12][10]*SF[15] + P[11][10]*SPP[10] - (P[10][10]*q0)/2) - (q0*(P[1][11] + P[0][11]*SF[8] + P[2][11]*SF[7] + P[3][11]*SF[11] - P[12][11]*SF[15] + P[11][11]*SPP[10] - (P[10][11]*q0)/2))/2; +nextP[2][2] = P[2][2] + P[0][2]*SF[6] + P[1][2]*SF[10] + P[3][2]*SF[8] + P[12][2]*SF[14] - P[10][2]*SPP[10] + dayVar*SQ[9] + (dazVar*SQ[10])/4 - (P[11][2]*q0)/2 + SF[6]*(P[2][0] + P[0][0]*SF[6] + P[1][0]*SF[10] + P[3][0]*SF[8] + P[12][0]*SF[14] - P[10][0]*SPP[10] - (P[11][0]*q0)/2) + SF[10]*(P[2][1] + P[0][1]*SF[6] + P[1][1]*SF[10] + P[3][1]*SF[8] + P[12][1]*SF[14] - P[10][1]*SPP[10] - (P[11][1]*q0)/2) + SF[8]*(P[2][3] + P[0][3]*SF[6] + P[1][3]*SF[10] + P[3][3]*SF[8] + P[12][3]*SF[14] - P[10][3]*SPP[10] - (P[11][3]*q0)/2) + SF[14]*(P[2][12] + P[0][12]*SF[6] + P[1][12]*SF[10] + P[3][12]*SF[8] + P[12][12]*SF[14] - P[10][12]*SPP[10] - (P[11][12]*q0)/2) - SPP[10]*(P[2][10] + P[0][10]*SF[6] + P[1][10]*SF[10] + P[3][10]*SF[8] + P[12][10]*SF[14] - P[10][10]*SPP[10] - (P[11][10]*q0)/2) + (daxVar*sq(q3))/4 - (q0*(P[2][11] + P[0][11]*SF[6] + P[1][11]*SF[10] + P[3][11]*SF[8] + P[12][11]*SF[14] - P[10][11]*SPP[10] - (P[11][11]*q0)/2))/2; +nextP[0][3] = P[0][3] + SQ[6] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10] + SF[7]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SF[6]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) + SF[9]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SF[15]*(P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10]) - SF[14]*(P[0][11] + P[1][11]*SF[9] + P[2][11]*SF[11] + P[3][11]*SF[10] + P[10][11]*SF[14] + P[11][11]*SF[15] + P[12][11]*SPP[10]) - (q0*(P[0][12] + P[1][12]*SF[9] + P[2][12]*SF[11] + P[3][12]*SF[10] + P[10][12]*SF[14] + P[11][12]*SF[15] + P[12][12]*SPP[10]))/2; +nextP[1][3] = P[1][3] + SQ[4] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2 + SF[7]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SF[6]*(P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] - (P[10][1]*q0)/2) + SF[9]*(P[1][2] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2) + SF[15]*(P[1][10] + P[0][10]*SF[8] + P[2][10]*SF[7] + P[3][10]*SF[11] - P[12][10]*SF[15] + P[11][10]*SPP[10] - (P[10][10]*q0)/2) - SF[14]*(P[1][11] + P[0][11]*SF[8] + P[2][11]*SF[7] + P[3][11]*SF[11] - P[12][11]*SF[15] + P[11][11]*SPP[10] - (P[10][11]*q0)/2) - (q0*(P[1][12] + P[0][12]*SF[8] + P[2][12]*SF[7] + P[3][12]*SF[11] - P[12][12]*SF[15] + P[11][12]*SPP[10] - (P[10][12]*q0)/2))/2; +nextP[2][3] = P[2][3] + SQ[3] + P[0][3]*SF[6] + P[1][3]*SF[10] + P[3][3]*SF[8] + P[12][3]*SF[14] - P[10][3]*SPP[10] - (P[11][3]*q0)/2 + SF[7]*(P[2][0] + P[0][0]*SF[6] + P[1][0]*SF[10] + P[3][0]*SF[8] + P[12][0]*SF[14] - P[10][0]*SPP[10] - (P[11][0]*q0)/2) + SF[6]*(P[2][1] + P[0][1]*SF[6] + P[1][1]*SF[10] + P[3][1]*SF[8] + P[12][1]*SF[14] - P[10][1]*SPP[10] - (P[11][1]*q0)/2) + SF[9]*(P[2][2] + P[0][2]*SF[6] + P[1][2]*SF[10] + P[3][2]*SF[8] + P[12][2]*SF[14] - P[10][2]*SPP[10] - (P[11][2]*q0)/2) + SF[15]*(P[2][10] + P[0][10]*SF[6] + P[1][10]*SF[10] + P[3][10]*SF[8] + P[12][10]*SF[14] - P[10][10]*SPP[10] - (P[11][10]*q0)/2) - SF[14]*(P[2][11] + P[0][11]*SF[6] + P[1][11]*SF[10] + P[3][11]*SF[8] + P[12][11]*SF[14] - P[10][11]*SPP[10] - (P[11][11]*q0)/2) - (q0*(P[2][12] + P[0][12]*SF[6] + P[1][12]*SF[10] + P[3][12]*SF[8] + P[12][12]*SF[14] - P[10][12]*SPP[10] - (P[11][12]*q0)/2))/2; +nextP[3][3] = P[3][3] + P[0][3]*SF[7] + P[1][3]*SF[6] + P[2][3]*SF[9] + P[10][3]*SF[15] - P[11][3]*SF[14] + (dayVar*SQ[10])/4 + dazVar*SQ[9] - (P[12][3]*q0)/2 + SF[7]*(P[3][0] + P[0][0]*SF[7] + P[1][0]*SF[6] + P[2][0]*SF[9] + P[10][0]*SF[15] - P[11][0]*SF[14] - (P[12][0]*q0)/2) + SF[6]*(P[3][1] + P[0][1]*SF[7] + P[1][1]*SF[6] + P[2][1]*SF[9] + P[10][1]*SF[15] - P[11][1]*SF[14] - (P[12][1]*q0)/2) + SF[9]*(P[3][2] + P[0][2]*SF[7] + P[1][2]*SF[6] + P[2][2]*SF[9] + P[10][2]*SF[15] - P[11][2]*SF[14] - (P[12][2]*q0)/2) + SF[15]*(P[3][10] + P[0][10]*SF[7] + P[1][10]*SF[6] + P[2][10]*SF[9] + P[10][10]*SF[15] - P[11][10]*SF[14] - (P[12][10]*q0)/2) - SF[14]*(P[3][11] + P[0][11]*SF[7] + P[1][11]*SF[6] + P[2][11]*SF[9] + P[10][11]*SF[15] - P[11][11]*SF[14] - (P[12][11]*q0)/2) + (daxVar*sq(q2))/4 - (q0*(P[3][12] + P[0][12]*SF[7] + P[1][12]*SF[6] + P[2][12]*SF[9] + P[10][12]*SF[15] - P[11][12]*SF[14] - (P[12][12]*q0)/2))/2; +nextP[0][4] = P[0][4] + P[1][4]*SF[9] + P[2][4]*SF[11] + P[3][4]*SF[10] + P[10][4]*SF[14] + P[11][4]*SF[15] + P[12][4]*SPP[10] + SF[5]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SF[3]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) - SF[4]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) + SPP[0]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SPP[3]*(P[0][13] + P[1][13]*SF[9] + P[2][13]*SF[11] + P[3][13]*SF[10] + P[10][13]*SF[14] + P[11][13]*SF[15] + P[12][13]*SPP[10]) + SPP[6]*(P[0][14] + P[1][14]*SF[9] + P[2][14]*SF[11] + P[3][14]*SF[10] + P[10][14]*SF[14] + P[11][14]*SF[15] + P[12][14]*SPP[10]) - SPP[9]*(P[0][15] + P[1][15]*SF[9] + P[2][15]*SF[11] + P[3][15]*SF[10] + P[10][15]*SF[14] + P[11][15]*SF[15] + P[12][15]*SPP[10]); +nextP[1][4] = P[1][4] + P[0][4]*SF[8] + P[2][4]*SF[7] + P[3][4]*SF[11] - P[12][4]*SF[15] + P[11][4]*SPP[10] - (P[10][4]*q0)/2 + SF[5]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SF[3]*(P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] - (P[10][1]*q0)/2) - SF[4]*(P[1][3] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2) + SPP[0]*(P[1][2] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2) + SPP[3]*(P[1][13] + P[0][13]*SF[8] + P[2][13]*SF[7] + P[3][13]*SF[11] - P[12][13]*SF[15] + P[11][13]*SPP[10] - (P[10][13]*q0)/2) + SPP[6]*(P[1][14] + P[0][14]*SF[8] + P[2][14]*SF[7] + P[3][14]*SF[11] - P[12][14]*SF[15] + P[11][14]*SPP[10] - (P[10][14]*q0)/2) - SPP[9]*(P[1][15] + P[0][15]*SF[8] + P[2][15]*SF[7] + P[3][15]*SF[11] - P[12][15]*SF[15] + P[11][15]*SPP[10] - (P[10][15]*q0)/2); +nextP[2][4] = P[2][4] + P[0][4]*SF[6] + P[1][4]*SF[10] + P[3][4]*SF[8] + P[12][4]*SF[14] - P[10][4]*SPP[10] - (P[11][4]*q0)/2 + SF[5]*(P[2][0] + P[0][0]*SF[6] + P[1][0]*SF[10] + P[3][0]*SF[8] + P[12][0]*SF[14] - P[10][0]*SPP[10] - (P[11][0]*q0)/2) + SF[3]*(P[2][1] + P[0][1]*SF[6] + P[1][1]*SF[10] + P[3][1]*SF[8] + P[12][1]*SF[14] - P[10][1]*SPP[10] - (P[11][1]*q0)/2) - SF[4]*(P[2][3] + P[0][3]*SF[6] + P[1][3]*SF[10] + P[3][3]*SF[8] + P[12][3]*SF[14] - P[10][3]*SPP[10] - (P[11][3]*q0)/2) + SPP[0]*(P[2][2] + P[0][2]*SF[6] + P[1][2]*SF[10] + P[3][2]*SF[8] + P[12][2]*SF[14] - P[10][2]*SPP[10] - (P[11][2]*q0)/2) + SPP[3]*(P[2][13] + P[0][13]*SF[6] + P[1][13]*SF[10] + P[3][13]*SF[8] + P[12][13]*SF[14] - P[10][13]*SPP[10] - (P[11][13]*q0)/2) + SPP[6]*(P[2][14] + P[0][14]*SF[6] + P[1][14]*SF[10] + P[3][14]*SF[8] + P[12][14]*SF[14] - P[10][14]*SPP[10] - (P[11][14]*q0)/2) - SPP[9]*(P[2][15] + P[0][15]*SF[6] + P[1][15]*SF[10] + P[3][15]*SF[8] + P[12][15]*SF[14] - P[10][15]*SPP[10] - (P[11][15]*q0)/2); +nextP[3][4] = P[3][4] + P[0][4]*SF[7] + P[1][4]*SF[6] + P[2][4]*SF[9] + P[10][4]*SF[15] - P[11][4]*SF[14] - (P[12][4]*q0)/2 + SF[5]*(P[3][0] + P[0][0]*SF[7] + P[1][0]*SF[6] + P[2][0]*SF[9] + P[10][0]*SF[15] - P[11][0]*SF[14] - (P[12][0]*q0)/2) + SF[3]*(P[3][1] + P[0][1]*SF[7] + P[1][1]*SF[6] + P[2][1]*SF[9] + P[10][1]*SF[15] - P[11][1]*SF[14] - (P[12][1]*q0)/2) - SF[4]*(P[3][3] + P[0][3]*SF[7] + P[1][3]*SF[6] + P[2][3]*SF[9] + P[10][3]*SF[15] - P[11][3]*SF[14] - (P[12][3]*q0)/2) + SPP[0]*(P[3][2] + P[0][2]*SF[7] + P[1][2]*SF[6] + P[2][2]*SF[9] + P[10][2]*SF[15] - P[11][2]*SF[14] - (P[12][2]*q0)/2) + SPP[3]*(P[3][13] + P[0][13]*SF[7] + P[1][13]*SF[6] + P[2][13]*SF[9] + P[10][13]*SF[15] - P[11][13]*SF[14] - (P[12][13]*q0)/2) + SPP[6]*(P[3][14] + P[0][14]*SF[7] + P[1][14]*SF[6] + P[2][14]*SF[9] + P[10][14]*SF[15] - P[11][14]*SF[14] - (P[12][14]*q0)/2) - SPP[9]*(P[3][15] + P[0][15]*SF[7] + P[1][15]*SF[6] + P[2][15]*SF[9] + P[10][15]*SF[15] - P[11][15]*SF[14] - (P[12][15]*q0)/2); +nextP[4][4] = P[4][4] + P[0][4]*SF[5] + P[1][4]*SF[3] - P[3][4]*SF[4] + P[2][4]*SPP[0] + P[13][4]*SPP[3] + P[14][4]*SPP[6] - P[15][4]*SPP[9] + dvyVar*sq(SG[7] - 2*q0*q3) + dvzVar*sq(SG[6] + 2*q0*q2) + SF[5]*(P[4][0] + P[0][0]*SF[5] + P[1][0]*SF[3] - P[3][0]*SF[4] + P[2][0]*SPP[0] + P[13][0]*SPP[3] + P[14][0]*SPP[6] - P[15][0]*SPP[9]) + SF[3]*(P[4][1] + P[0][1]*SF[5] + P[1][1]*SF[3] - P[3][1]*SF[4] + P[2][1]*SPP[0] + P[13][1]*SPP[3] + P[14][1]*SPP[6] - P[15][1]*SPP[9]) - SF[4]*(P[4][3] + P[0][3]*SF[5] + P[1][3]*SF[3] - P[3][3]*SF[4] + P[2][3]*SPP[0] + P[13][3]*SPP[3] + P[14][3]*SPP[6] - P[15][3]*SPP[9]) + SPP[0]*(P[4][2] + P[0][2]*SF[5] + P[1][2]*SF[3] - P[3][2]*SF[4] + P[2][2]*SPP[0] + P[13][2]*SPP[3] + P[14][2]*SPP[6] - P[15][2]*SPP[9]) + SPP[3]*(P[4][13] + P[0][13]*SF[5] + P[1][13]*SF[3] - P[3][13]*SF[4] + P[2][13]*SPP[0] + P[13][13]*SPP[3] + P[14][13]*SPP[6] - P[15][13]*SPP[9]) + SPP[6]*(P[4][14] + P[0][14]*SF[5] + P[1][14]*SF[3] - P[3][14]*SF[4] + P[2][14]*SPP[0] + P[13][14]*SPP[3] + P[14][14]*SPP[6] - P[15][14]*SPP[9]) - SPP[9]*(P[4][15] + P[0][15]*SF[5] + P[1][15]*SF[3] - P[3][15]*SF[4] + P[2][15]*SPP[0] + P[13][15]*SPP[3] + P[14][15]*SPP[6] - P[15][15]*SPP[9]) + dvxVar*sq(SG[1] + SG[2] - SG[3] - SG[4]); +nextP[0][5] = P[0][5] + P[1][5]*SF[9] + P[2][5]*SF[11] + P[3][5]*SF[10] + P[10][5]*SF[14] + P[11][5]*SF[15] + P[12][5]*SPP[10] + SF[4]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SF[3]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SF[5]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) - SPP[0]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) - SPP[8]*(P[0][13] + P[1][13]*SF[9] + P[2][13]*SF[11] + P[3][13]*SF[10] + P[10][13]*SF[14] + P[11][13]*SF[15] + P[12][13]*SPP[10]) + SPP[2]*(P[0][14] + P[1][14]*SF[9] + P[2][14]*SF[11] + P[3][14]*SF[10] + P[10][14]*SF[14] + P[11][14]*SF[15] + P[12][14]*SPP[10]) + SPP[5]*(P[0][15] + P[1][15]*SF[9] + P[2][15]*SF[11] + P[3][15]*SF[10] + P[10][15]*SF[14] + P[11][15]*SF[15] + P[12][15]*SPP[10]); +nextP[1][5] = P[1][5] + P[0][5]*SF[8] + P[2][5]*SF[7] + P[3][5]*SF[11] - P[12][5]*SF[15] + P[11][5]*SPP[10] - (P[10][5]*q0)/2 + SF[4]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SF[3]*(P[1][2] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2) + SF[5]*(P[1][3] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2) - SPP[0]*(P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] - (P[10][1]*q0)/2) - SPP[8]*(P[1][13] + P[0][13]*SF[8] + P[2][13]*SF[7] + P[3][13]*SF[11] - P[12][13]*SF[15] + P[11][13]*SPP[10] - (P[10][13]*q0)/2) + SPP[2]*(P[1][14] + P[0][14]*SF[8] + P[2][14]*SF[7] + P[3][14]*SF[11] - P[12][14]*SF[15] + P[11][14]*SPP[10] - (P[10][14]*q0)/2) + SPP[5]*(P[1][15] + P[0][15]*SF[8] + P[2][15]*SF[7] + P[3][15]*SF[11] - P[12][15]*SF[15] + P[11][15]*SPP[10] - (P[10][15]*q0)/2); +nextP[2][5] = P[2][5] + P[0][5]*SF[6] + P[1][5]*SF[10] + P[3][5]*SF[8] + P[12][5]*SF[14] - P[10][5]*SPP[10] - (P[11][5]*q0)/2 + SF[4]*(P[2][0] + P[0][0]*SF[6] + P[1][0]*SF[10] + P[3][0]*SF[8] + P[12][0]*SF[14] - P[10][0]*SPP[10] - (P[11][0]*q0)/2) + SF[3]*(P[2][2] + P[0][2]*SF[6] + P[1][2]*SF[10] + P[3][2]*SF[8] + P[12][2]*SF[14] - P[10][2]*SPP[10] - (P[11][2]*q0)/2) + SF[5]*(P[2][3] + P[0][3]*SF[6] + P[1][3]*SF[10] + P[3][3]*SF[8] + P[12][3]*SF[14] - P[10][3]*SPP[10] - (P[11][3]*q0)/2) - SPP[0]*(P[2][1] + P[0][1]*SF[6] + P[1][1]*SF[10] + P[3][1]*SF[8] + P[12][1]*SF[14] - P[10][1]*SPP[10] - (P[11][1]*q0)/2) - SPP[8]*(P[2][13] + P[0][13]*SF[6] + P[1][13]*SF[10] + P[3][13]*SF[8] + P[12][13]*SF[14] - P[10][13]*SPP[10] - (P[11][13]*q0)/2) + SPP[2]*(P[2][14] + P[0][14]*SF[6] + P[1][14]*SF[10] + P[3][14]*SF[8] + P[12][14]*SF[14] - P[10][14]*SPP[10] - (P[11][14]*q0)/2) + SPP[5]*(P[2][15] + P[0][15]*SF[6] + P[1][15]*SF[10] + P[3][15]*SF[8] + P[12][15]*SF[14] - P[10][15]*SPP[10] - (P[11][15]*q0)/2); +nextP[3][5] = P[3][5] + P[0][5]*SF[7] + P[1][5]*SF[6] + P[2][5]*SF[9] + P[10][5]*SF[15] - P[11][5]*SF[14] - (P[12][5]*q0)/2 + SF[4]*(P[3][0] + P[0][0]*SF[7] + P[1][0]*SF[6] + P[2][0]*SF[9] + P[10][0]*SF[15] - P[11][0]*SF[14] - (P[12][0]*q0)/2) + SF[3]*(P[3][2] + P[0][2]*SF[7] + P[1][2]*SF[6] + P[2][2]*SF[9] + P[10][2]*SF[15] - P[11][2]*SF[14] - (P[12][2]*q0)/2) + SF[5]*(P[3][3] + P[0][3]*SF[7] + P[1][3]*SF[6] + P[2][3]*SF[9] + P[10][3]*SF[15] - P[11][3]*SF[14] - (P[12][3]*q0)/2) - SPP[0]*(P[3][1] + P[0][1]*SF[7] + P[1][1]*SF[6] + P[2][1]*SF[9] + P[10][1]*SF[15] - P[11][1]*SF[14] - (P[12][1]*q0)/2) - SPP[8]*(P[3][13] + P[0][13]*SF[7] + P[1][13]*SF[6] + P[2][13]*SF[9] + P[10][13]*SF[15] - P[11][13]*SF[14] - (P[12][13]*q0)/2) + SPP[2]*(P[3][14] + P[0][14]*SF[7] + P[1][14]*SF[6] + P[2][14]*SF[9] + P[10][14]*SF[15] - P[11][14]*SF[14] - (P[12][14]*q0)/2) + SPP[5]*(P[3][15] + P[0][15]*SF[7] + P[1][15]*SF[6] + P[2][15]*SF[9] + P[10][15]*SF[15] - P[11][15]*SF[14] - (P[12][15]*q0)/2); +nextP[4][5] = P[4][5] + SQ[2] + P[0][5]*SF[5] + P[1][5]*SF[3] - P[3][5]*SF[4] + P[2][5]*SPP[0] + P[13][5]*SPP[3] + P[14][5]*SPP[6] - P[15][5]*SPP[9] + SF[4]*(P[4][0] + P[0][0]*SF[5] + P[1][0]*SF[3] - P[3][0]*SF[4] + P[2][0]*SPP[0] + P[13][0]*SPP[3] + P[14][0]*SPP[6] - P[15][0]*SPP[9]) + SF[3]*(P[4][2] + P[0][2]*SF[5] + P[1][2]*SF[3] - P[3][2]*SF[4] + P[2][2]*SPP[0] + P[13][2]*SPP[3] + P[14][2]*SPP[6] - P[15][2]*SPP[9]) + SF[5]*(P[4][3] + P[0][3]*SF[5] + P[1][3]*SF[3] - P[3][3]*SF[4] + P[2][3]*SPP[0] + P[13][3]*SPP[3] + P[14][3]*SPP[6] - P[15][3]*SPP[9]) - SPP[0]*(P[4][1] + P[0][1]*SF[5] + P[1][1]*SF[3] - P[3][1]*SF[4] + P[2][1]*SPP[0] + P[13][1]*SPP[3] + P[14][1]*SPP[6] - P[15][1]*SPP[9]) - SPP[8]*(P[4][13] + P[0][13]*SF[5] + P[1][13]*SF[3] - P[3][13]*SF[4] + P[2][13]*SPP[0] + P[13][13]*SPP[3] + P[14][13]*SPP[6] - P[15][13]*SPP[9]) + SPP[2]*(P[4][14] + P[0][14]*SF[5] + P[1][14]*SF[3] - P[3][14]*SF[4] + P[2][14]*SPP[0] + P[13][14]*SPP[3] + P[14][14]*SPP[6] - P[15][14]*SPP[9]) + SPP[5]*(P[4][15] + P[0][15]*SF[5] + P[1][15]*SF[3] - P[3][15]*SF[4] + P[2][15]*SPP[0] + P[13][15]*SPP[3] + P[14][15]*SPP[6] - P[15][15]*SPP[9]); +nextP[5][5] = P[5][5] + P[0][5]*SF[4] + P[2][5]*SF[3] + P[3][5]*SF[5] - P[1][5]*SPP[0] - P[13][5]*SPP[8] + P[14][5]*SPP[2] + P[15][5]*SPP[5] + dvxVar*sq(SG[7] + 2*q0*q3) + dvzVar*sq(SG[5] - 2*q0*q1) + SF[4]*(P[5][0] + P[0][0]*SF[4] + P[2][0]*SF[3] + P[3][0]*SF[5] - P[1][0]*SPP[0] - P[13][0]*SPP[8] + P[14][0]*SPP[2] + P[15][0]*SPP[5]) + SF[3]*(P[5][2] + P[0][2]*SF[4] + P[2][2]*SF[3] + P[3][2]*SF[5] - P[1][2]*SPP[0] - P[13][2]*SPP[8] + P[14][2]*SPP[2] + P[15][2]*SPP[5]) + SF[5]*(P[5][3] + P[0][3]*SF[4] + P[2][3]*SF[3] + P[3][3]*SF[5] - P[1][3]*SPP[0] - P[13][3]*SPP[8] + P[14][3]*SPP[2] + P[15][3]*SPP[5]) - SPP[0]*(P[5][1] + P[0][1]*SF[4] + P[2][1]*SF[3] + P[3][1]*SF[5] - P[1][1]*SPP[0] - P[13][1]*SPP[8] + P[14][1]*SPP[2] + P[15][1]*SPP[5]) - SPP[8]*(P[5][13] + P[0][13]*SF[4] + P[2][13]*SF[3] + P[3][13]*SF[5] - P[1][13]*SPP[0] - P[13][13]*SPP[8] + P[14][13]*SPP[2] + P[15][13]*SPP[5]) + SPP[2]*(P[5][14] + P[0][14]*SF[4] + P[2][14]*SF[3] + P[3][14]*SF[5] - P[1][14]*SPP[0] - P[13][14]*SPP[8] + P[14][14]*SPP[2] + P[15][14]*SPP[5]) + SPP[5]*(P[5][15] + P[0][15]*SF[4] + P[2][15]*SF[3] + P[3][15]*SF[5] - P[1][15]*SPP[0] - P[13][15]*SPP[8] + P[14][15]*SPP[2] + P[15][15]*SPP[5]) + dvyVar*sq(SG[1] - SG[2] + SG[3] - SG[4]); +nextP[0][6] = P[0][6] + P[1][6]*SF[9] + P[2][6]*SF[11] + P[3][6]*SF[10] + P[10][6]*SF[14] + P[11][6]*SF[15] + P[12][6]*SPP[10] + SF[4]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) - SF[5]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SF[3]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) + SPP[0]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SPP[4]*(P[0][13] + P[1][13]*SF[9] + P[2][13]*SF[11] + P[3][13]*SF[10] + P[10][13]*SF[14] + P[11][13]*SF[15] + P[12][13]*SPP[10]) - SPP[7]*(P[0][14] + P[1][14]*SF[9] + P[2][14]*SF[11] + P[3][14]*SF[10] + P[10][14]*SF[14] + P[11][14]*SF[15] + P[12][14]*SPP[10]) - SPP[1]*(P[0][15] + P[1][15]*SF[9] + P[2][15]*SF[11] + P[3][15]*SF[10] + P[10][15]*SF[14] + P[11][15]*SF[15] + P[12][15]*SPP[10]); +nextP[1][6] = P[1][6] + P[0][6]*SF[8] + P[2][6]*SF[7] + P[3][6]*SF[11] - P[12][6]*SF[15] + P[11][6]*SPP[10] - (P[10][6]*q0)/2 + SF[4]*(P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] - (P[10][1]*q0)/2) - SF[5]*(P[1][2] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2) + SF[3]*(P[1][3] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2) + SPP[0]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SPP[4]*(P[1][13] + P[0][13]*SF[8] + P[2][13]*SF[7] + P[3][13]*SF[11] - P[12][13]*SF[15] + P[11][13]*SPP[10] - (P[10][13]*q0)/2) - SPP[7]*(P[1][14] + P[0][14]*SF[8] + P[2][14]*SF[7] + P[3][14]*SF[11] - P[12][14]*SF[15] + P[11][14]*SPP[10] - (P[10][14]*q0)/2) - SPP[1]*(P[1][15] + P[0][15]*SF[8] + P[2][15]*SF[7] + P[3][15]*SF[11] - P[12][15]*SF[15] + P[11][15]*SPP[10] - (P[10][15]*q0)/2); +nextP[2][6] = P[2][6] + P[0][6]*SF[6] + P[1][6]*SF[10] + P[3][6]*SF[8] + P[12][6]*SF[14] - P[10][6]*SPP[10] - (P[11][6]*q0)/2 + SF[4]*(P[2][1] + P[0][1]*SF[6] + P[1][1]*SF[10] + P[3][1]*SF[8] + P[12][1]*SF[14] - P[10][1]*SPP[10] - (P[11][1]*q0)/2) - SF[5]*(P[2][2] + P[0][2]*SF[6] + P[1][2]*SF[10] + P[3][2]*SF[8] + P[12][2]*SF[14] - P[10][2]*SPP[10] - (P[11][2]*q0)/2) + SF[3]*(P[2][3] + P[0][3]*SF[6] + P[1][3]*SF[10] + P[3][3]*SF[8] + P[12][3]*SF[14] - P[10][3]*SPP[10] - (P[11][3]*q0)/2) + SPP[0]*(P[2][0] + P[0][0]*SF[6] + P[1][0]*SF[10] + P[3][0]*SF[8] + P[12][0]*SF[14] - P[10][0]*SPP[10] - (P[11][0]*q0)/2) + SPP[4]*(P[2][13] + P[0][13]*SF[6] + P[1][13]*SF[10] + P[3][13]*SF[8] + P[12][13]*SF[14] - P[10][13]*SPP[10] - (P[11][13]*q0)/2) - SPP[7]*(P[2][14] + P[0][14]*SF[6] + P[1][14]*SF[10] + P[3][14]*SF[8] + P[12][14]*SF[14] - P[10][14]*SPP[10] - (P[11][14]*q0)/2) - SPP[1]*(P[2][15] + P[0][15]*SF[6] + P[1][15]*SF[10] + P[3][15]*SF[8] + P[12][15]*SF[14] - P[10][15]*SPP[10] - (P[11][15]*q0)/2); +nextP[3][6] = P[3][6] + P[0][6]*SF[7] + P[1][6]*SF[6] + P[2][6]*SF[9] + P[10][6]*SF[15] - P[11][6]*SF[14] - (P[12][6]*q0)/2 + SF[4]*(P[3][1] + P[0][1]*SF[7] + P[1][1]*SF[6] + P[2][1]*SF[9] + P[10][1]*SF[15] - P[11][1]*SF[14] - (P[12][1]*q0)/2) - SF[5]*(P[3][2] + P[0][2]*SF[7] + P[1][2]*SF[6] + P[2][2]*SF[9] + P[10][2]*SF[15] - P[11][2]*SF[14] - (P[12][2]*q0)/2) + SF[3]*(P[3][3] + P[0][3]*SF[7] + P[1][3]*SF[6] + P[2][3]*SF[9] + P[10][3]*SF[15] - P[11][3]*SF[14] - (P[12][3]*q0)/2) + SPP[0]*(P[3][0] + P[0][0]*SF[7] + P[1][0]*SF[6] + P[2][0]*SF[9] + P[10][0]*SF[15] - P[11][0]*SF[14] - (P[12][0]*q0)/2) + SPP[4]*(P[3][13] + P[0][13]*SF[7] + P[1][13]*SF[6] + P[2][13]*SF[9] + P[10][13]*SF[15] - P[11][13]*SF[14] - (P[12][13]*q0)/2) - SPP[7]*(P[3][14] + P[0][14]*SF[7] + P[1][14]*SF[6] + P[2][14]*SF[9] + P[10][14]*SF[15] - P[11][14]*SF[14] - (P[12][14]*q0)/2) - SPP[1]*(P[3][15] + P[0][15]*SF[7] + P[1][15]*SF[6] + P[2][15]*SF[9] + P[10][15]*SF[15] - P[11][15]*SF[14] - (P[12][15]*q0)/2); +nextP[4][6] = P[4][6] + SQ[1] + P[0][6]*SF[5] + P[1][6]*SF[3] - P[3][6]*SF[4] + P[2][6]*SPP[0] + P[13][6]*SPP[3] + P[14][6]*SPP[6] - P[15][6]*SPP[9] + SF[4]*(P[4][1] + P[0][1]*SF[5] + P[1][1]*SF[3] - P[3][1]*SF[4] + P[2][1]*SPP[0] + P[13][1]*SPP[3] + P[14][1]*SPP[6] - P[15][1]*SPP[9]) - SF[5]*(P[4][2] + P[0][2]*SF[5] + P[1][2]*SF[3] - P[3][2]*SF[4] + P[2][2]*SPP[0] + P[13][2]*SPP[3] + P[14][2]*SPP[6] - P[15][2]*SPP[9]) + SF[3]*(P[4][3] + P[0][3]*SF[5] + P[1][3]*SF[3] - P[3][3]*SF[4] + P[2][3]*SPP[0] + P[13][3]*SPP[3] + P[14][3]*SPP[6] - P[15][3]*SPP[9]) + SPP[0]*(P[4][0] + P[0][0]*SF[5] + P[1][0]*SF[3] - P[3][0]*SF[4] + P[2][0]*SPP[0] + P[13][0]*SPP[3] + P[14][0]*SPP[6] - P[15][0]*SPP[9]) + SPP[4]*(P[4][13] + P[0][13]*SF[5] + P[1][13]*SF[3] - P[3][13]*SF[4] + P[2][13]*SPP[0] + P[13][13]*SPP[3] + P[14][13]*SPP[6] - P[15][13]*SPP[9]) - SPP[7]*(P[4][14] + P[0][14]*SF[5] + P[1][14]*SF[3] - P[3][14]*SF[4] + P[2][14]*SPP[0] + P[13][14]*SPP[3] + P[14][14]*SPP[6] - P[15][14]*SPP[9]) - SPP[1]*(P[4][15] + P[0][15]*SF[5] + P[1][15]*SF[3] - P[3][15]*SF[4] + P[2][15]*SPP[0] + P[13][15]*SPP[3] + P[14][15]*SPP[6] - P[15][15]*SPP[9]); +nextP[5][6] = P[5][6] + SQ[0] + P[0][6]*SF[4] + P[2][6]*SF[3] + P[3][6]*SF[5] - P[1][6]*SPP[0] - P[13][6]*SPP[8] + P[14][6]*SPP[2] + P[15][6]*SPP[5] + SF[4]*(P[5][1] + P[0][1]*SF[4] + P[2][1]*SF[3] + P[3][1]*SF[5] - P[1][1]*SPP[0] - P[13][1]*SPP[8] + P[14][1]*SPP[2] + P[15][1]*SPP[5]) - SF[5]*(P[5][2] + P[0][2]*SF[4] + P[2][2]*SF[3] + P[3][2]*SF[5] - P[1][2]*SPP[0] - P[13][2]*SPP[8] + P[14][2]*SPP[2] + P[15][2]*SPP[5]) + SF[3]*(P[5][3] + P[0][3]*SF[4] + P[2][3]*SF[3] + P[3][3]*SF[5] - P[1][3]*SPP[0] - P[13][3]*SPP[8] + P[14][3]*SPP[2] + P[15][3]*SPP[5]) + SPP[0]*(P[5][0] + P[0][0]*SF[4] + P[2][0]*SF[3] + P[3][0]*SF[5] - P[1][0]*SPP[0] - P[13][0]*SPP[8] + P[14][0]*SPP[2] + P[15][0]*SPP[5]) + SPP[4]*(P[5][13] + P[0][13]*SF[4] + P[2][13]*SF[3] + P[3][13]*SF[5] - P[1][13]*SPP[0] - P[13][13]*SPP[8] + P[14][13]*SPP[2] + P[15][13]*SPP[5]) - SPP[7]*(P[5][14] + P[0][14]*SF[4] + P[2][14]*SF[3] + P[3][14]*SF[5] - P[1][14]*SPP[0] - P[13][14]*SPP[8] + P[14][14]*SPP[2] + P[15][14]*SPP[5]) - SPP[1]*(P[5][15] + P[0][15]*SF[4] + P[2][15]*SF[3] + P[3][15]*SF[5] - P[1][15]*SPP[0] - P[13][15]*SPP[8] + P[14][15]*SPP[2] + P[15][15]*SPP[5]); +nextP[6][6] = P[6][6] + P[1][6]*SF[4] - P[2][6]*SF[5] + P[3][6]*SF[3] + P[0][6]*SPP[0] + P[13][6]*SPP[4] - P[14][6]*SPP[7] - P[15][6]*SPP[1] + dvxVar*sq(SG[6] - 2*q0*q2) + dvyVar*sq(SG[5] + 2*q0*q1) + SF[4]*(P[6][1] + P[1][1]*SF[4] - P[2][1]*SF[5] + P[3][1]*SF[3] + P[0][1]*SPP[0] + P[13][1]*SPP[4] - P[14][1]*SPP[7] - P[15][1]*SPP[1]) - SF[5]*(P[6][2] + P[1][2]*SF[4] - P[2][2]*SF[5] + P[3][2]*SF[3] + P[0][2]*SPP[0] + P[13][2]*SPP[4] - P[14][2]*SPP[7] - P[15][2]*SPP[1]) + SF[3]*(P[6][3] + P[1][3]*SF[4] - P[2][3]*SF[5] + P[3][3]*SF[3] + P[0][3]*SPP[0] + P[13][3]*SPP[4] - P[14][3]*SPP[7] - P[15][3]*SPP[1]) + SPP[0]*(P[6][0] + P[1][0]*SF[4] - P[2][0]*SF[5] + P[3][0]*SF[3] + P[0][0]*SPP[0] + P[13][0]*SPP[4] - P[14][0]*SPP[7] - P[15][0]*SPP[1]) + SPP[4]*(P[6][13] + P[1][13]*SF[4] - P[2][13]*SF[5] + P[3][13]*SF[3] + P[0][13]*SPP[0] + P[13][13]*SPP[4] - P[14][13]*SPP[7] - P[15][13]*SPP[1]) - SPP[7]*(P[6][14] + P[1][14]*SF[4] - P[2][14]*SF[5] + P[3][14]*SF[3] + P[0][14]*SPP[0] + P[13][14]*SPP[4] - P[14][14]*SPP[7] - P[15][14]*SPP[1]) - SPP[1]*(P[6][15] + P[1][15]*SF[4] - P[2][15]*SF[5] + P[3][15]*SF[3] + P[0][15]*SPP[0] + P[13][15]*SPP[4] - P[14][15]*SPP[7] - P[15][15]*SPP[1]) + dvzVar*sq(SG[1] - SG[2] - SG[3] + SG[4]); +nextP[0][7] = P[0][7] + P[1][7]*SF[9] + P[2][7]*SF[11] + P[3][7]*SF[10] + P[10][7]*SF[14] + P[11][7]*SF[15] + P[12][7]*SPP[10] + dt*(P[0][4] + P[1][4]*SF[9] + P[2][4]*SF[11] + P[3][4]*SF[10] + P[10][4]*SF[14] + P[11][4]*SF[15] + P[12][4]*SPP[10]); +nextP[1][7] = P[1][7] + P[0][7]*SF[8] + P[2][7]*SF[7] + P[3][7]*SF[11] - P[12][7]*SF[15] + P[11][7]*SPP[10] - (P[10][7]*q0)/2 + dt*(P[1][4] + P[0][4]*SF[8] + P[2][4]*SF[7] + P[3][4]*SF[11] - P[12][4]*SF[15] + P[11][4]*SPP[10] - (P[10][4]*q0)/2); +nextP[2][7] = P[2][7] + P[0][7]*SF[6] + P[1][7]*SF[10] + P[3][7]*SF[8] + P[12][7]*SF[14] - P[10][7]*SPP[10] - (P[11][7]*q0)/2 + dt*(P[2][4] + P[0][4]*SF[6] + P[1][4]*SF[10] + P[3][4]*SF[8] + P[12][4]*SF[14] - P[10][4]*SPP[10] - (P[11][4]*q0)/2); +nextP[3][7] = P[3][7] + P[0][7]*SF[7] + P[1][7]*SF[6] + P[2][7]*SF[9] + P[10][7]*SF[15] - P[11][7]*SF[14] - (P[12][7]*q0)/2 + dt*(P[3][4] + P[0][4]*SF[7] + P[1][4]*SF[6] + P[2][4]*SF[9] + P[10][4]*SF[15] - P[11][4]*SF[14] - (P[12][4]*q0)/2); +nextP[4][7] = P[4][7] + P[0][7]*SF[5] + P[1][7]*SF[3] - P[3][7]*SF[4] + P[2][7]*SPP[0] + P[13][7]*SPP[3] + P[14][7]*SPP[6] - P[15][7]*SPP[9] + dt*(P[4][4] + P[0][4]*SF[5] + P[1][4]*SF[3] - P[3][4]*SF[4] + P[2][4]*SPP[0] + P[13][4]*SPP[3] + P[14][4]*SPP[6] - P[15][4]*SPP[9]); +nextP[5][7] = P[5][7] + P[0][7]*SF[4] + P[2][7]*SF[3] + P[3][7]*SF[5] - P[1][7]*SPP[0] - P[13][7]*SPP[8] + P[14][7]*SPP[2] + P[15][7]*SPP[5] + dt*(P[5][4] + P[0][4]*SF[4] + P[2][4]*SF[3] + P[3][4]*SF[5] - P[1][4]*SPP[0] - P[13][4]*SPP[8] + P[14][4]*SPP[2] + P[15][4]*SPP[5]); +nextP[6][7] = P[6][7] + P[1][7]*SF[4] - P[2][7]*SF[5] + P[3][7]*SF[3] + P[0][7]*SPP[0] + P[13][7]*SPP[4] - P[14][7]*SPP[7] - P[15][7]*SPP[1] + dt*(P[6][4] + P[1][4]*SF[4] - P[2][4]*SF[5] + P[3][4]*SF[3] + P[0][4]*SPP[0] + P[13][4]*SPP[4] - P[14][4]*SPP[7] - P[15][4]*SPP[1]); nextP[7][7] = P[7][7] + P[4][7]*dt + dt*(P[7][4] + P[4][4]*dt); -nextP[0][8] = P[0][8]*SPP[5] - P[1][8]*SPP[4] + P[2][8]*SPP[7] + P[9][8]*SPP[22] + P[12][8]*SPP[18] + dt*(P[0][5]*SPP[5] - P[1][5]*SPP[4] + P[2][5]*SPP[7] + P[9][5]*SPP[22] + P[12][5]*SPP[18]); -nextP[1][8] = P[1][8]*SPP[6] - P[0][8]*SPP[2] - P[2][8]*SPP[8] + P[10][8]*SPP[22] + P[13][8]*SPP[17] + dt*(P[1][5]*SPP[6] - P[0][5]*SPP[2] - P[2][5]*SPP[8] + P[10][5]*SPP[22] + P[13][5]*SPP[17]); -nextP[2][8] = P[0][8]*SPP[14] - P[1][8]*SPP[3] + P[2][8]*SPP[13] + P[11][8]*SPP[22] + P[14][8]*SPP[16] + dt*(P[0][5]*SPP[14] - P[1][5]*SPP[3] + P[2][5]*SPP[13] + P[11][5]*SPP[22] + P[14][5]*SPP[16]); -nextP[3][8] = P[3][8] + P[0][8]*SPP[1] + P[1][8]*SPP[19] + P[2][8]*SPP[15] - P[15][8]*SPP[21] + dt*(P[3][5] + P[0][5]*SPP[1] + P[1][5]*SPP[19] + P[2][5]*SPP[15] - P[15][5]*SPP[21]); -nextP[4][8] = P[4][8] + P[15][8]*SF[22] + P[0][8]*SPP[20] + P[1][8]*SPP[12] + P[2][8]*SPP[11] + dt*(P[4][5] + P[15][5]*SF[22] + P[0][5]*SPP[20] + P[1][5]*SPP[12] + P[2][5]*SPP[11]); -nextP[5][8] = P[5][8] + P[15][8]*SF[20] - P[0][8]*SPP[9] + P[1][8]*SPP[10] + P[2][8]*SPP[0] + dt*(P[5][5] + P[15][5]*SF[20] - P[0][5]*SPP[9] + P[1][5]*SPP[10] + P[2][5]*SPP[0]); -nextP[6][8] = P[6][8] + P[3][8]*dt + dt*(P[6][5] + P[3][5]*dt); +nextP[0][8] = P[0][8] + P[1][8]*SF[9] + P[2][8]*SF[11] + P[3][8]*SF[10] + P[10][8]*SF[14] + P[11][8]*SF[15] + P[12][8]*SPP[10] + dt*(P[0][5] + P[1][5]*SF[9] + P[2][5]*SF[11] + P[3][5]*SF[10] + P[10][5]*SF[14] + P[11][5]*SF[15] + P[12][5]*SPP[10]); +nextP[1][8] = P[1][8] + P[0][8]*SF[8] + P[2][8]*SF[7] + P[3][8]*SF[11] - P[12][8]*SF[15] + P[11][8]*SPP[10] - (P[10][8]*q0)/2 + dt*(P[1][5] + P[0][5]*SF[8] + P[2][5]*SF[7] + P[3][5]*SF[11] - P[12][5]*SF[15] + P[11][5]*SPP[10] - (P[10][5]*q0)/2); +nextP[2][8] = P[2][8] + P[0][8]*SF[6] + P[1][8]*SF[10] + P[3][8]*SF[8] + P[12][8]*SF[14] - P[10][8]*SPP[10] - (P[11][8]*q0)/2 + dt*(P[2][5] + P[0][5]*SF[6] + P[1][5]*SF[10] + P[3][5]*SF[8] + P[12][5]*SF[14] - P[10][5]*SPP[10] - (P[11][5]*q0)/2); +nextP[3][8] = P[3][8] + P[0][8]*SF[7] + P[1][8]*SF[6] + P[2][8]*SF[9] + P[10][8]*SF[15] - P[11][8]*SF[14] - (P[12][8]*q0)/2 + dt*(P[3][5] + P[0][5]*SF[7] + P[1][5]*SF[6] + P[2][5]*SF[9] + P[10][5]*SF[15] - P[11][5]*SF[14] - (P[12][5]*q0)/2); +nextP[4][8] = P[4][8] + P[0][8]*SF[5] + P[1][8]*SF[3] - P[3][8]*SF[4] + P[2][8]*SPP[0] + P[13][8]*SPP[3] + P[14][8]*SPP[6] - P[15][8]*SPP[9] + dt*(P[4][5] + P[0][5]*SF[5] + P[1][5]*SF[3] - P[3][5]*SF[4] + P[2][5]*SPP[0] + P[13][5]*SPP[3] + P[14][5]*SPP[6] - P[15][5]*SPP[9]); +nextP[5][8] = P[5][8] + P[0][8]*SF[4] + P[2][8]*SF[3] + P[3][8]*SF[5] - P[1][8]*SPP[0] - P[13][8]*SPP[8] + P[14][8]*SPP[2] + P[15][8]*SPP[5] + dt*(P[5][5] + P[0][5]*SF[4] + P[2][5]*SF[3] + P[3][5]*SF[5] - P[1][5]*SPP[0] - P[13][5]*SPP[8] + P[14][5]*SPP[2] + P[15][5]*SPP[5]); +nextP[6][8] = P[6][8] + P[1][8]*SF[4] - P[2][8]*SF[5] + P[3][8]*SF[3] + P[0][8]*SPP[0] + P[13][8]*SPP[4] - P[14][8]*SPP[7] - P[15][8]*SPP[1] + dt*(P[6][5] + P[1][5]*SF[4] - P[2][5]*SF[5] + P[3][5]*SF[3] + P[0][5]*SPP[0] + P[13][5]*SPP[4] - P[14][5]*SPP[7] - P[15][5]*SPP[1]); nextP[7][8] = P[7][8] + P[4][8]*dt + dt*(P[7][5] + P[4][5]*dt); nextP[8][8] = P[8][8] + P[5][8]*dt + dt*(P[8][5] + P[5][5]*dt); -nextP[0][9] = P[0][9]*SPP[5] - P[1][9]*SPP[4] + P[2][9]*SPP[7] + P[9][9]*SPP[22] + P[12][9]*SPP[18]; -nextP[1][9] = P[1][9]*SPP[6] - P[0][9]*SPP[2] - P[2][9]*SPP[8] + P[10][9]*SPP[22] + P[13][9]*SPP[17]; -nextP[2][9] = P[0][9]*SPP[14] - P[1][9]*SPP[3] + P[2][9]*SPP[13] + P[11][9]*SPP[22] + P[14][9]*SPP[16]; -nextP[3][9] = P[3][9] + P[0][9]*SPP[1] + P[1][9]*SPP[19] + P[2][9]*SPP[15] - P[15][9]*SPP[21]; -nextP[4][9] = P[4][9] + P[15][9]*SF[22] + P[0][9]*SPP[20] + P[1][9]*SPP[12] + P[2][9]*SPP[11]; -nextP[5][9] = P[5][9] + P[15][9]*SF[20] - P[0][9]*SPP[9] + P[1][9]*SPP[10] + P[2][9]*SPP[0]; -nextP[6][9] = P[6][9] + P[3][9]*dt; -nextP[7][9] = P[7][9] + P[4][9]*dt; -nextP[8][9] = P[8][9] + P[5][9]*dt; -nextP[9][9] = P[9][9]; -nextP[0][10] = P[0][10]*SPP[5] - P[1][10]*SPP[4] + P[2][10]*SPP[7] + P[9][10]*SPP[22] + P[12][10]*SPP[18]; -nextP[1][10] = P[1][10]*SPP[6] - P[0][10]*SPP[2] - P[2][10]*SPP[8] + P[10][10]*SPP[22] + P[13][10]*SPP[17]; -nextP[2][10] = P[0][10]*SPP[14] - P[1][10]*SPP[3] + P[2][10]*SPP[13] + P[11][10]*SPP[22] + P[14][10]*SPP[16]; -nextP[3][10] = P[3][10] + P[0][10]*SPP[1] + P[1][10]*SPP[19] + P[2][10]*SPP[15] - P[15][10]*SPP[21]; -nextP[4][10] = P[4][10] + P[15][10]*SF[22] + P[0][10]*SPP[20] + P[1][10]*SPP[12] + P[2][10]*SPP[11]; -nextP[5][10] = P[5][10] + P[15][10]*SF[20] - P[0][10]*SPP[9] + P[1][10]*SPP[10] + P[2][10]*SPP[0]; -nextP[6][10] = P[6][10] + P[3][10]*dt; +nextP[0][9] = P[0][9] + P[1][9]*SF[9] + P[2][9]*SF[11] + P[3][9]*SF[10] + P[10][9]*SF[14] + P[11][9]*SF[15] + P[12][9]*SPP[10] + dt*(P[0][6] + P[1][6]*SF[9] + P[2][6]*SF[11] + P[3][6]*SF[10] + P[10][6]*SF[14] + P[11][6]*SF[15] + P[12][6]*SPP[10]); +nextP[1][9] = P[1][9] + P[0][9]*SF[8] + P[2][9]*SF[7] + P[3][9]*SF[11] - P[12][9]*SF[15] + P[11][9]*SPP[10] - (P[10][9]*q0)/2 + dt*(P[1][6] + P[0][6]*SF[8] + P[2][6]*SF[7] + P[3][6]*SF[11] - P[12][6]*SF[15] + P[11][6]*SPP[10] - (P[10][6]*q0)/2); +nextP[2][9] = P[2][9] + P[0][9]*SF[6] + P[1][9]*SF[10] + P[3][9]*SF[8] + P[12][9]*SF[14] - P[10][9]*SPP[10] - (P[11][9]*q0)/2 + dt*(P[2][6] + P[0][6]*SF[6] + P[1][6]*SF[10] + P[3][6]*SF[8] + P[12][6]*SF[14] - P[10][6]*SPP[10] - (P[11][6]*q0)/2); +nextP[3][9] = P[3][9] + P[0][9]*SF[7] + P[1][9]*SF[6] + P[2][9]*SF[9] + P[10][9]*SF[15] - P[11][9]*SF[14] - (P[12][9]*q0)/2 + dt*(P[3][6] + P[0][6]*SF[7] + P[1][6]*SF[6] + P[2][6]*SF[9] + P[10][6]*SF[15] - P[11][6]*SF[14] - (P[12][6]*q0)/2); +nextP[4][9] = P[4][9] + P[0][9]*SF[5] + P[1][9]*SF[3] - P[3][9]*SF[4] + P[2][9]*SPP[0] + P[13][9]*SPP[3] + P[14][9]*SPP[6] - P[15][9]*SPP[9] + dt*(P[4][6] + P[0][6]*SF[5] + P[1][6]*SF[3] - P[3][6]*SF[4] + P[2][6]*SPP[0] + P[13][6]*SPP[3] + P[14][6]*SPP[6] - P[15][6]*SPP[9]); +nextP[5][9] = P[5][9] + P[0][9]*SF[4] + P[2][9]*SF[3] + P[3][9]*SF[5] - P[1][9]*SPP[0] - P[13][9]*SPP[8] + P[14][9]*SPP[2] + P[15][9]*SPP[5] + dt*(P[5][6] + P[0][6]*SF[4] + P[2][6]*SF[3] + P[3][6]*SF[5] - P[1][6]*SPP[0] - P[13][6]*SPP[8] + P[14][6]*SPP[2] + P[15][6]*SPP[5]); +nextP[6][9] = P[6][9] + P[1][9]*SF[4] - P[2][9]*SF[5] + P[3][9]*SF[3] + P[0][9]*SPP[0] + P[13][9]*SPP[4] - P[14][9]*SPP[7] - P[15][9]*SPP[1] + dt*(P[6][6] + P[1][6]*SF[4] - P[2][6]*SF[5] + P[3][6]*SF[3] + P[0][6]*SPP[0] + P[13][6]*SPP[4] - P[14][6]*SPP[7] - P[15][6]*SPP[1]); +nextP[7][9] = P[7][9] + P[4][9]*dt + dt*(P[7][6] + P[4][6]*dt); +nextP[8][9] = P[8][9] + P[5][9]*dt + dt*(P[8][6] + P[5][6]*dt); +nextP[9][9] = P[9][9] + P[6][9]*dt + dt*(P[9][6] + P[6][6]*dt); +nextP[0][10] = P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10]; +nextP[1][10] = P[1][10] + P[0][10]*SF[8] + P[2][10]*SF[7] + P[3][10]*SF[11] - P[12][10]*SF[15] + P[11][10]*SPP[10] - (P[10][10]*q0)/2; +nextP[2][10] = P[2][10] + P[0][10]*SF[6] + P[1][10]*SF[10] + P[3][10]*SF[8] + P[12][10]*SF[14] - P[10][10]*SPP[10] - (P[11][10]*q0)/2; +nextP[3][10] = P[3][10] + P[0][10]*SF[7] + P[1][10]*SF[6] + P[2][10]*SF[9] + P[10][10]*SF[15] - P[11][10]*SF[14] - (P[12][10]*q0)/2; +nextP[4][10] = P[4][10] + P[0][10]*SF[5] + P[1][10]*SF[3] - P[3][10]*SF[4] + P[2][10]*SPP[0] + P[13][10]*SPP[3] + P[14][10]*SPP[6] - P[15][10]*SPP[9]; +nextP[5][10] = P[5][10] + P[0][10]*SF[4] + P[2][10]*SF[3] + P[3][10]*SF[5] - P[1][10]*SPP[0] - P[13][10]*SPP[8] + P[14][10]*SPP[2] + P[15][10]*SPP[5]; +nextP[6][10] = P[6][10] + P[1][10]*SF[4] - P[2][10]*SF[5] + P[3][10]*SF[3] + P[0][10]*SPP[0] + P[13][10]*SPP[4] - P[14][10]*SPP[7] - P[15][10]*SPP[1]; nextP[7][10] = P[7][10] + P[4][10]*dt; nextP[8][10] = P[8][10] + P[5][10]*dt; -nextP[9][10] = P[9][10]; +nextP[9][10] = P[9][10] + P[6][10]*dt; nextP[10][10] = P[10][10]; -nextP[0][11] = P[0][11]*SPP[5] - P[1][11]*SPP[4] + P[2][11]*SPP[7] + P[9][11]*SPP[22] + P[12][11]*SPP[18]; -nextP[1][11] = P[1][11]*SPP[6] - P[0][11]*SPP[2] - P[2][11]*SPP[8] + P[10][11]*SPP[22] + P[13][11]*SPP[17]; -nextP[2][11] = P[0][11]*SPP[14] - P[1][11]*SPP[3] + P[2][11]*SPP[13] + P[11][11]*SPP[22] + P[14][11]*SPP[16]; -nextP[3][11] = P[3][11] + P[0][11]*SPP[1] + P[1][11]*SPP[19] + P[2][11]*SPP[15] - P[15][11]*SPP[21]; -nextP[4][11] = P[4][11] + P[15][11]*SF[22] + P[0][11]*SPP[20] + P[1][11]*SPP[12] + P[2][11]*SPP[11]; -nextP[5][11] = P[5][11] + P[15][11]*SF[20] - P[0][11]*SPP[9] + P[1][11]*SPP[10] + P[2][11]*SPP[0]; -nextP[6][11] = P[6][11] + P[3][11]*dt; +nextP[0][11] = P[0][11] + P[1][11]*SF[9] + P[2][11]*SF[11] + P[3][11]*SF[10] + P[10][11]*SF[14] + P[11][11]*SF[15] + P[12][11]*SPP[10]; +nextP[1][11] = P[1][11] + P[0][11]*SF[8] + P[2][11]*SF[7] + P[3][11]*SF[11] - P[12][11]*SF[15] + P[11][11]*SPP[10] - (P[10][11]*q0)/2; +nextP[2][11] = P[2][11] + P[0][11]*SF[6] + P[1][11]*SF[10] + P[3][11]*SF[8] + P[12][11]*SF[14] - P[10][11]*SPP[10] - (P[11][11]*q0)/2; +nextP[3][11] = P[3][11] + P[0][11]*SF[7] + P[1][11]*SF[6] + P[2][11]*SF[9] + P[10][11]*SF[15] - P[11][11]*SF[14] - (P[12][11]*q0)/2; +nextP[4][11] = P[4][11] + P[0][11]*SF[5] + P[1][11]*SF[3] - P[3][11]*SF[4] + P[2][11]*SPP[0] + P[13][11]*SPP[3] + P[14][11]*SPP[6] - P[15][11]*SPP[9]; +nextP[5][11] = P[5][11] + P[0][11]*SF[4] + P[2][11]*SF[3] + P[3][11]*SF[5] - P[1][11]*SPP[0] - P[13][11]*SPP[8] + P[14][11]*SPP[2] + P[15][11]*SPP[5]; +nextP[6][11] = P[6][11] + P[1][11]*SF[4] - P[2][11]*SF[5] + P[3][11]*SF[3] + P[0][11]*SPP[0] + P[13][11]*SPP[4] - P[14][11]*SPP[7] - P[15][11]*SPP[1]; nextP[7][11] = P[7][11] + P[4][11]*dt; nextP[8][11] = P[8][11] + P[5][11]*dt; -nextP[9][11] = P[9][11]; +nextP[9][11] = P[9][11] + P[6][11]*dt; nextP[10][11] = P[10][11]; nextP[11][11] = P[11][11]; -nextP[0][12] = P[0][12]*SPP[5] - P[1][12]*SPP[4] + P[2][12]*SPP[7] + P[9][12]*SPP[22] + P[12][12]*SPP[18]; -nextP[1][12] = P[1][12]*SPP[6] - P[0][12]*SPP[2] - P[2][12]*SPP[8] + P[10][12]*SPP[22] + P[13][12]*SPP[17]; -nextP[2][12] = P[0][12]*SPP[14] - P[1][12]*SPP[3] + P[2][12]*SPP[13] + P[11][12]*SPP[22] + P[14][12]*SPP[16]; -nextP[3][12] = P[3][12] + P[0][12]*SPP[1] + P[1][12]*SPP[19] + P[2][12]*SPP[15] - P[15][12]*SPP[21]; -nextP[4][12] = P[4][12] + P[15][12]*SF[22] + P[0][12]*SPP[20] + P[1][12]*SPP[12] + P[2][12]*SPP[11]; -nextP[5][12] = P[5][12] + P[15][12]*SF[20] - P[0][12]*SPP[9] + P[1][12]*SPP[10] + P[2][12]*SPP[0]; -nextP[6][12] = P[6][12] + P[3][12]*dt; +nextP[0][12] = P[0][12] + P[1][12]*SF[9] + P[2][12]*SF[11] + P[3][12]*SF[10] + P[10][12]*SF[14] + P[11][12]*SF[15] + P[12][12]*SPP[10]; +nextP[1][12] = P[1][12] + P[0][12]*SF[8] + P[2][12]*SF[7] + P[3][12]*SF[11] - P[12][12]*SF[15] + P[11][12]*SPP[10] - (P[10][12]*q0)/2; +nextP[2][12] = P[2][12] + P[0][12]*SF[6] + P[1][12]*SF[10] + P[3][12]*SF[8] + P[12][12]*SF[14] - P[10][12]*SPP[10] - (P[11][12]*q0)/2; +nextP[3][12] = P[3][12] + P[0][12]*SF[7] + P[1][12]*SF[6] + P[2][12]*SF[9] + P[10][12]*SF[15] - P[11][12]*SF[14] - (P[12][12]*q0)/2; +nextP[4][12] = P[4][12] + P[0][12]*SF[5] + P[1][12]*SF[3] - P[3][12]*SF[4] + P[2][12]*SPP[0] + P[13][12]*SPP[3] + P[14][12]*SPP[6] - P[15][12]*SPP[9]; +nextP[5][12] = P[5][12] + P[0][12]*SF[4] + P[2][12]*SF[3] + P[3][12]*SF[5] - P[1][12]*SPP[0] - P[13][12]*SPP[8] + P[14][12]*SPP[2] + P[15][12]*SPP[5]; +nextP[6][12] = P[6][12] + P[1][12]*SF[4] - P[2][12]*SF[5] + P[3][12]*SF[3] + P[0][12]*SPP[0] + P[13][12]*SPP[4] - P[14][12]*SPP[7] - P[15][12]*SPP[1]; nextP[7][12] = P[7][12] + P[4][12]*dt; nextP[8][12] = P[8][12] + P[5][12]*dt; -nextP[9][12] = P[9][12]; +nextP[9][12] = P[9][12] + P[6][12]*dt; nextP[10][12] = P[10][12]; nextP[11][12] = P[11][12]; nextP[12][12] = P[12][12]; -nextP[0][13] = P[0][13]*SPP[5] - P[1][13]*SPP[4] + P[2][13]*SPP[7] + P[9][13]*SPP[22] + P[12][13]*SPP[18]; -nextP[1][13] = P[1][13]*SPP[6] - P[0][13]*SPP[2] - P[2][13]*SPP[8] + P[10][13]*SPP[22] + P[13][13]*SPP[17]; -nextP[2][13] = P[0][13]*SPP[14] - P[1][13]*SPP[3] + P[2][13]*SPP[13] + P[11][13]*SPP[22] + P[14][13]*SPP[16]; -nextP[3][13] = P[3][13] + P[0][13]*SPP[1] + P[1][13]*SPP[19] + P[2][13]*SPP[15] - P[15][13]*SPP[21]; -nextP[4][13] = P[4][13] + P[15][13]*SF[22] + P[0][13]*SPP[20] + P[1][13]*SPP[12] + P[2][13]*SPP[11]; -nextP[5][13] = P[5][13] + P[15][13]*SF[20] - P[0][13]*SPP[9] + P[1][13]*SPP[10] + P[2][13]*SPP[0]; -nextP[6][13] = P[6][13] + P[3][13]*dt; +nextP[0][13] = P[0][13] + P[1][13]*SF[9] + P[2][13]*SF[11] + P[3][13]*SF[10] + P[10][13]*SF[14] + P[11][13]*SF[15] + P[12][13]*SPP[10]; +nextP[1][13] = P[1][13] + P[0][13]*SF[8] + P[2][13]*SF[7] + P[3][13]*SF[11] - P[12][13]*SF[15] + P[11][13]*SPP[10] - (P[10][13]*q0)/2; +nextP[2][13] = P[2][13] + P[0][13]*SF[6] + P[1][13]*SF[10] + P[3][13]*SF[8] + P[12][13]*SF[14] - P[10][13]*SPP[10] - (P[11][13]*q0)/2; +nextP[3][13] = P[3][13] + P[0][13]*SF[7] + P[1][13]*SF[6] + P[2][13]*SF[9] + P[10][13]*SF[15] - P[11][13]*SF[14] - (P[12][13]*q0)/2; +nextP[4][13] = P[4][13] + P[0][13]*SF[5] + P[1][13]*SF[3] - P[3][13]*SF[4] + P[2][13]*SPP[0] + P[13][13]*SPP[3] + P[14][13]*SPP[6] - P[15][13]*SPP[9]; +nextP[5][13] = P[5][13] + P[0][13]*SF[4] + P[2][13]*SF[3] + P[3][13]*SF[5] - P[1][13]*SPP[0] - P[13][13]*SPP[8] + P[14][13]*SPP[2] + P[15][13]*SPP[5]; +nextP[6][13] = P[6][13] + P[1][13]*SF[4] - P[2][13]*SF[5] + P[3][13]*SF[3] + P[0][13]*SPP[0] + P[13][13]*SPP[4] - P[14][13]*SPP[7] - P[15][13]*SPP[1]; nextP[7][13] = P[7][13] + P[4][13]*dt; nextP[8][13] = P[8][13] + P[5][13]*dt; -nextP[9][13] = P[9][13]; +nextP[9][13] = P[9][13] + P[6][13]*dt; nextP[10][13] = P[10][13]; nextP[11][13] = P[11][13]; nextP[12][13] = P[12][13]; nextP[13][13] = P[13][13]; -nextP[0][14] = P[0][14]*SPP[5] - P[1][14]*SPP[4] + P[2][14]*SPP[7] + P[9][14]*SPP[22] + P[12][14]*SPP[18]; -nextP[1][14] = P[1][14]*SPP[6] - P[0][14]*SPP[2] - P[2][14]*SPP[8] + P[10][14]*SPP[22] + P[13][14]*SPP[17]; -nextP[2][14] = P[0][14]*SPP[14] - P[1][14]*SPP[3] + P[2][14]*SPP[13] + P[11][14]*SPP[22] + P[14][14]*SPP[16]; -nextP[3][14] = P[3][14] + P[0][14]*SPP[1] + P[1][14]*SPP[19] + P[2][14]*SPP[15] - P[15][14]*SPP[21]; -nextP[4][14] = P[4][14] + P[15][14]*SF[22] + P[0][14]*SPP[20] + P[1][14]*SPP[12] + P[2][14]*SPP[11]; -nextP[5][14] = P[5][14] + P[15][14]*SF[20] - P[0][14]*SPP[9] + P[1][14]*SPP[10] + P[2][14]*SPP[0]; -nextP[6][14] = P[6][14] + P[3][14]*dt; +nextP[0][14] = P[0][14] + P[1][14]*SF[9] + P[2][14]*SF[11] + P[3][14]*SF[10] + P[10][14]*SF[14] + P[11][14]*SF[15] + P[12][14]*SPP[10]; +nextP[1][14] = P[1][14] + P[0][14]*SF[8] + P[2][14]*SF[7] + P[3][14]*SF[11] - P[12][14]*SF[15] + P[11][14]*SPP[10] - (P[10][14]*q0)/2; +nextP[2][14] = P[2][14] + P[0][14]*SF[6] + P[1][14]*SF[10] + P[3][14]*SF[8] + P[12][14]*SF[14] - P[10][14]*SPP[10] - (P[11][14]*q0)/2; +nextP[3][14] = P[3][14] + P[0][14]*SF[7] + P[1][14]*SF[6] + P[2][14]*SF[9] + P[10][14]*SF[15] - P[11][14]*SF[14] - (P[12][14]*q0)/2; +nextP[4][14] = P[4][14] + P[0][14]*SF[5] + P[1][14]*SF[3] - P[3][14]*SF[4] + P[2][14]*SPP[0] + P[13][14]*SPP[3] + P[14][14]*SPP[6] - P[15][14]*SPP[9]; +nextP[5][14] = P[5][14] + P[0][14]*SF[4] + P[2][14]*SF[3] + P[3][14]*SF[5] - P[1][14]*SPP[0] - P[13][14]*SPP[8] + P[14][14]*SPP[2] + P[15][14]*SPP[5]; +nextP[6][14] = P[6][14] + P[1][14]*SF[4] - P[2][14]*SF[5] + P[3][14]*SF[3] + P[0][14]*SPP[0] + P[13][14]*SPP[4] - P[14][14]*SPP[7] - P[15][14]*SPP[1]; nextP[7][14] = P[7][14] + P[4][14]*dt; nextP[8][14] = P[8][14] + P[5][14]*dt; -nextP[9][14] = P[9][14]; +nextP[9][14] = P[9][14] + P[6][14]*dt; nextP[10][14] = P[10][14]; nextP[11][14] = P[11][14]; nextP[12][14] = P[12][14]; nextP[13][14] = P[13][14]; nextP[14][14] = P[14][14]; -nextP[0][15] = P[0][15]*SPP[5] - P[1][15]*SPP[4] + P[2][15]*SPP[7] + P[9][15]*SPP[22] + P[12][15]*SPP[18]; -nextP[1][15] = P[1][15]*SPP[6] - P[0][15]*SPP[2] - P[2][15]*SPP[8] + P[10][15]*SPP[22] + P[13][15]*SPP[17]; -nextP[2][15] = P[0][15]*SPP[14] - P[1][15]*SPP[3] + P[2][15]*SPP[13] + P[11][15]*SPP[22] + P[14][15]*SPP[16]; -nextP[3][15] = P[3][15] + P[0][15]*SPP[1] + P[1][15]*SPP[19] + P[2][15]*SPP[15] - P[15][15]*SPP[21]; -nextP[4][15] = P[4][15] + P[15][15]*SF[22] + P[0][15]*SPP[20] + P[1][15]*SPP[12] + P[2][15]*SPP[11]; -nextP[5][15] = P[5][15] + P[15][15]*SF[20] - P[0][15]*SPP[9] + P[1][15]*SPP[10] + P[2][15]*SPP[0]; -nextP[6][15] = P[6][15] + P[3][15]*dt; +nextP[0][15] = P[0][15] + P[1][15]*SF[9] + P[2][15]*SF[11] + P[3][15]*SF[10] + P[10][15]*SF[14] + P[11][15]*SF[15] + P[12][15]*SPP[10]; +nextP[1][15] = P[1][15] + P[0][15]*SF[8] + P[2][15]*SF[7] + P[3][15]*SF[11] - P[12][15]*SF[15] + P[11][15]*SPP[10] - (P[10][15]*q0)/2; +nextP[2][15] = P[2][15] + P[0][15]*SF[6] + P[1][15]*SF[10] + P[3][15]*SF[8] + P[12][15]*SF[14] - P[10][15]*SPP[10] - (P[11][15]*q0)/2; +nextP[3][15] = P[3][15] + P[0][15]*SF[7] + P[1][15]*SF[6] + P[2][15]*SF[9] + P[10][15]*SF[15] - P[11][15]*SF[14] - (P[12][15]*q0)/2; +nextP[4][15] = P[4][15] + P[0][15]*SF[5] + P[1][15]*SF[3] - P[3][15]*SF[4] + P[2][15]*SPP[0] + P[13][15]*SPP[3] + P[14][15]*SPP[6] - P[15][15]*SPP[9]; +nextP[5][15] = P[5][15] + P[0][15]*SF[4] + P[2][15]*SF[3] + P[3][15]*SF[5] - P[1][15]*SPP[0] - P[13][15]*SPP[8] + P[14][15]*SPP[2] + P[15][15]*SPP[5]; +nextP[6][15] = P[6][15] + P[1][15]*SF[4] - P[2][15]*SF[5] + P[3][15]*SF[3] + P[0][15]*SPP[0] + P[13][15]*SPP[4] - P[14][15]*SPP[7] - P[15][15]*SPP[1]; nextP[7][15] = P[7][15] + P[4][15]*dt; nextP[8][15] = P[8][15] + P[5][15]*dt; -nextP[9][15] = P[9][15]; +nextP[9][15] = P[9][15] + P[6][15]*dt; nextP[10][15] = P[10][15]; nextP[11][15] = P[11][15]; nextP[12][15] = P[12][15]; nextP[13][15] = P[13][15]; nextP[14][15] = P[14][15]; nextP[15][15] = P[15][15]; -nextP[0][16] = P[0][16]*SPP[5] - P[1][16]*SPP[4] + P[2][16]*SPP[7] + P[9][16]*SPP[22] + P[12][16]*SPP[18]; -nextP[1][16] = P[1][16]*SPP[6] - P[0][16]*SPP[2] - P[2][16]*SPP[8] + P[10][16]*SPP[22] + P[13][16]*SPP[17]; -nextP[2][16] = P[0][16]*SPP[14] - P[1][16]*SPP[3] + P[2][16]*SPP[13] + P[11][16]*SPP[22] + P[14][16]*SPP[16]; -nextP[3][16] = P[3][16] + P[0][16]*SPP[1] + P[1][16]*SPP[19] + P[2][16]*SPP[15] - P[15][16]*SPP[21]; -nextP[4][16] = P[4][16] + P[15][16]*SF[22] + P[0][16]*SPP[20] + P[1][16]*SPP[12] + P[2][16]*SPP[11]; -nextP[5][16] = P[5][16] + P[15][16]*SF[20] - P[0][16]*SPP[9] + P[1][16]*SPP[10] + P[2][16]*SPP[0]; -nextP[6][16] = P[6][16] + P[3][16]*dt; +nextP[0][16] = P[0][16] + P[1][16]*SF[9] + P[2][16]*SF[11] + P[3][16]*SF[10] + P[10][16]*SF[14] + P[11][16]*SF[15] + P[12][16]*SPP[10]; +nextP[1][16] = P[1][16] + P[0][16]*SF[8] + P[2][16]*SF[7] + P[3][16]*SF[11] - P[12][16]*SF[15] + P[11][16]*SPP[10] - (P[10][16]*q0)/2; +nextP[2][16] = P[2][16] + P[0][16]*SF[6] + P[1][16]*SF[10] + P[3][16]*SF[8] + P[12][16]*SF[14] - P[10][16]*SPP[10] - (P[11][16]*q0)/2; +nextP[3][16] = P[3][16] + P[0][16]*SF[7] + P[1][16]*SF[6] + P[2][16]*SF[9] + P[10][16]*SF[15] - P[11][16]*SF[14] - (P[12][16]*q0)/2; +nextP[4][16] = P[4][16] + P[0][16]*SF[5] + P[1][16]*SF[3] - P[3][16]*SF[4] + P[2][16]*SPP[0] + P[13][16]*SPP[3] + P[14][16]*SPP[6] - P[15][16]*SPP[9]; +nextP[5][16] = P[5][16] + P[0][16]*SF[4] + P[2][16]*SF[3] + P[3][16]*SF[5] - P[1][16]*SPP[0] - P[13][16]*SPP[8] + P[14][16]*SPP[2] + P[15][16]*SPP[5]; +nextP[6][16] = P[6][16] + P[1][16]*SF[4] - P[2][16]*SF[5] + P[3][16]*SF[3] + P[0][16]*SPP[0] + P[13][16]*SPP[4] - P[14][16]*SPP[7] - P[15][16]*SPP[1]; nextP[7][16] = P[7][16] + P[4][16]*dt; nextP[8][16] = P[8][16] + P[5][16]*dt; -nextP[9][16] = P[9][16]; +nextP[9][16] = P[9][16] + P[6][16]*dt; nextP[10][16] = P[10][16]; nextP[11][16] = P[11][16]; nextP[12][16] = P[12][16]; @@ -219,16 +207,16 @@ nextP[13][16] = P[13][16]; nextP[14][16] = P[14][16]; nextP[15][16] = P[15][16]; nextP[16][16] = P[16][16]; -nextP[0][17] = P[0][17]*SPP[5] - P[1][17]*SPP[4] + P[2][17]*SPP[7] + P[9][17]*SPP[22] + P[12][17]*SPP[18]; -nextP[1][17] = P[1][17]*SPP[6] - P[0][17]*SPP[2] - P[2][17]*SPP[8] + P[10][17]*SPP[22] + P[13][17]*SPP[17]; -nextP[2][17] = P[0][17]*SPP[14] - P[1][17]*SPP[3] + P[2][17]*SPP[13] + P[11][17]*SPP[22] + P[14][17]*SPP[16]; -nextP[3][17] = P[3][17] + P[0][17]*SPP[1] + P[1][17]*SPP[19] + P[2][17]*SPP[15] - P[15][17]*SPP[21]; -nextP[4][17] = P[4][17] + P[15][17]*SF[22] + P[0][17]*SPP[20] + P[1][17]*SPP[12] + P[2][17]*SPP[11]; -nextP[5][17] = P[5][17] + P[15][17]*SF[20] - P[0][17]*SPP[9] + P[1][17]*SPP[10] + P[2][17]*SPP[0]; -nextP[6][17] = P[6][17] + P[3][17]*dt; +nextP[0][17] = P[0][17] + P[1][17]*SF[9] + P[2][17]*SF[11] + P[3][17]*SF[10] + P[10][17]*SF[14] + P[11][17]*SF[15] + P[12][17]*SPP[10]; +nextP[1][17] = P[1][17] + P[0][17]*SF[8] + P[2][17]*SF[7] + P[3][17]*SF[11] - P[12][17]*SF[15] + P[11][17]*SPP[10] - (P[10][17]*q0)/2; +nextP[2][17] = P[2][17] + P[0][17]*SF[6] + P[1][17]*SF[10] + P[3][17]*SF[8] + P[12][17]*SF[14] - P[10][17]*SPP[10] - (P[11][17]*q0)/2; +nextP[3][17] = P[3][17] + P[0][17]*SF[7] + P[1][17]*SF[6] + P[2][17]*SF[9] + P[10][17]*SF[15] - P[11][17]*SF[14] - (P[12][17]*q0)/2; +nextP[4][17] = P[4][17] + P[0][17]*SF[5] + P[1][17]*SF[3] - P[3][17]*SF[4] + P[2][17]*SPP[0] + P[13][17]*SPP[3] + P[14][17]*SPP[6] - P[15][17]*SPP[9]; +nextP[5][17] = P[5][17] + P[0][17]*SF[4] + P[2][17]*SF[3] + P[3][17]*SF[5] - P[1][17]*SPP[0] - P[13][17]*SPP[8] + P[14][17]*SPP[2] + P[15][17]*SPP[5]; +nextP[6][17] = P[6][17] + P[1][17]*SF[4] - P[2][17]*SF[5] + P[3][17]*SF[3] + P[0][17]*SPP[0] + P[13][17]*SPP[4] - P[14][17]*SPP[7] - P[15][17]*SPP[1]; nextP[7][17] = P[7][17] + P[4][17]*dt; nextP[8][17] = P[8][17] + P[5][17]*dt; -nextP[9][17] = P[9][17]; +nextP[9][17] = P[9][17] + P[6][17]*dt; nextP[10][17] = P[10][17]; nextP[11][17] = P[11][17]; nextP[12][17] = P[12][17]; @@ -237,16 +225,16 @@ nextP[14][17] = P[14][17]; nextP[15][17] = P[15][17]; nextP[16][17] = P[16][17]; nextP[17][17] = P[17][17]; -nextP[0][18] = P[0][18]*SPP[5] - P[1][18]*SPP[4] + P[2][18]*SPP[7] + P[9][18]*SPP[22] + P[12][18]*SPP[18]; -nextP[1][18] = P[1][18]*SPP[6] - P[0][18]*SPP[2] - P[2][18]*SPP[8] + P[10][18]*SPP[22] + P[13][18]*SPP[17]; -nextP[2][18] = P[0][18]*SPP[14] - P[1][18]*SPP[3] + P[2][18]*SPP[13] + P[11][18]*SPP[22] + P[14][18]*SPP[16]; -nextP[3][18] = P[3][18] + P[0][18]*SPP[1] + P[1][18]*SPP[19] + P[2][18]*SPP[15] - P[15][18]*SPP[21]; -nextP[4][18] = P[4][18] + P[15][18]*SF[22] + P[0][18]*SPP[20] + P[1][18]*SPP[12] + P[2][18]*SPP[11]; -nextP[5][18] = P[5][18] + P[15][18]*SF[20] - P[0][18]*SPP[9] + P[1][18]*SPP[10] + P[2][18]*SPP[0]; -nextP[6][18] = P[6][18] + P[3][18]*dt; +nextP[0][18] = P[0][18] + P[1][18]*SF[9] + P[2][18]*SF[11] + P[3][18]*SF[10] + P[10][18]*SF[14] + P[11][18]*SF[15] + P[12][18]*SPP[10]; +nextP[1][18] = P[1][18] + P[0][18]*SF[8] + P[2][18]*SF[7] + P[3][18]*SF[11] - P[12][18]*SF[15] + P[11][18]*SPP[10] - (P[10][18]*q0)/2; +nextP[2][18] = P[2][18] + P[0][18]*SF[6] + P[1][18]*SF[10] + P[3][18]*SF[8] + P[12][18]*SF[14] - P[10][18]*SPP[10] - (P[11][18]*q0)/2; +nextP[3][18] = P[3][18] + P[0][18]*SF[7] + P[1][18]*SF[6] + P[2][18]*SF[9] + P[10][18]*SF[15] - P[11][18]*SF[14] - (P[12][18]*q0)/2; +nextP[4][18] = P[4][18] + P[0][18]*SF[5] + P[1][18]*SF[3] - P[3][18]*SF[4] + P[2][18]*SPP[0] + P[13][18]*SPP[3] + P[14][18]*SPP[6] - P[15][18]*SPP[9]; +nextP[5][18] = P[5][18] + P[0][18]*SF[4] + P[2][18]*SF[3] + P[3][18]*SF[5] - P[1][18]*SPP[0] - P[13][18]*SPP[8] + P[14][18]*SPP[2] + P[15][18]*SPP[5]; +nextP[6][18] = P[6][18] + P[1][18]*SF[4] - P[2][18]*SF[5] + P[3][18]*SF[3] + P[0][18]*SPP[0] + P[13][18]*SPP[4] - P[14][18]*SPP[7] - P[15][18]*SPP[1]; nextP[7][18] = P[7][18] + P[4][18]*dt; nextP[8][18] = P[8][18] + P[5][18]*dt; -nextP[9][18] = P[9][18]; +nextP[9][18] = P[9][18] + P[6][18]*dt; nextP[10][18] = P[10][18]; nextP[11][18] = P[11][18]; nextP[12][18] = P[12][18]; @@ -256,16 +244,16 @@ nextP[15][18] = P[15][18]; nextP[16][18] = P[16][18]; nextP[17][18] = P[17][18]; nextP[18][18] = P[18][18]; -nextP[0][19] = P[0][19]*SPP[5] - P[1][19]*SPP[4] + P[2][19]*SPP[7] + P[9][19]*SPP[22] + P[12][19]*SPP[18]; -nextP[1][19] = P[1][19]*SPP[6] - P[0][19]*SPP[2] - P[2][19]*SPP[8] + P[10][19]*SPP[22] + P[13][19]*SPP[17]; -nextP[2][19] = P[0][19]*SPP[14] - P[1][19]*SPP[3] + P[2][19]*SPP[13] + P[11][19]*SPP[22] + P[14][19]*SPP[16]; -nextP[3][19] = P[3][19] + P[0][19]*SPP[1] + P[1][19]*SPP[19] + P[2][19]*SPP[15] - P[15][19]*SPP[21]; -nextP[4][19] = P[4][19] + P[15][19]*SF[22] + P[0][19]*SPP[20] + P[1][19]*SPP[12] + P[2][19]*SPP[11]; -nextP[5][19] = P[5][19] + P[15][19]*SF[20] - P[0][19]*SPP[9] + P[1][19]*SPP[10] + P[2][19]*SPP[0]; -nextP[6][19] = P[6][19] + P[3][19]*dt; +nextP[0][19] = P[0][19] + P[1][19]*SF[9] + P[2][19]*SF[11] + P[3][19]*SF[10] + P[10][19]*SF[14] + P[11][19]*SF[15] + P[12][19]*SPP[10]; +nextP[1][19] = P[1][19] + P[0][19]*SF[8] + P[2][19]*SF[7] + P[3][19]*SF[11] - P[12][19]*SF[15] + P[11][19]*SPP[10] - (P[10][19]*q0)/2; +nextP[2][19] = P[2][19] + P[0][19]*SF[6] + P[1][19]*SF[10] + P[3][19]*SF[8] + P[12][19]*SF[14] - P[10][19]*SPP[10] - (P[11][19]*q0)/2; +nextP[3][19] = P[3][19] + P[0][19]*SF[7] + P[1][19]*SF[6] + P[2][19]*SF[9] + P[10][19]*SF[15] - P[11][19]*SF[14] - (P[12][19]*q0)/2; +nextP[4][19] = P[4][19] + P[0][19]*SF[5] + P[1][19]*SF[3] - P[3][19]*SF[4] + P[2][19]*SPP[0] + P[13][19]*SPP[3] + P[14][19]*SPP[6] - P[15][19]*SPP[9]; +nextP[5][19] = P[5][19] + P[0][19]*SF[4] + P[2][19]*SF[3] + P[3][19]*SF[5] - P[1][19]*SPP[0] - P[13][19]*SPP[8] + P[14][19]*SPP[2] + P[15][19]*SPP[5]; +nextP[6][19] = P[6][19] + P[1][19]*SF[4] - P[2][19]*SF[5] + P[3][19]*SF[3] + P[0][19]*SPP[0] + P[13][19]*SPP[4] - P[14][19]*SPP[7] - P[15][19]*SPP[1]; nextP[7][19] = P[7][19] + P[4][19]*dt; nextP[8][19] = P[8][19] + P[5][19]*dt; -nextP[9][19] = P[9][19]; +nextP[9][19] = P[9][19] + P[6][19]*dt; nextP[10][19] = P[10][19]; nextP[11][19] = P[11][19]; nextP[12][19] = P[12][19]; @@ -276,16 +264,16 @@ nextP[16][19] = P[16][19]; nextP[17][19] = P[17][19]; nextP[18][19] = P[18][19]; nextP[19][19] = P[19][19]; -nextP[0][20] = P[0][20]*SPP[5] - P[1][20]*SPP[4] + P[2][20]*SPP[7] + P[9][20]*SPP[22] + P[12][20]*SPP[18]; -nextP[1][20] = P[1][20]*SPP[6] - P[0][20]*SPP[2] - P[2][20]*SPP[8] + P[10][20]*SPP[22] + P[13][20]*SPP[17]; -nextP[2][20] = P[0][20]*SPP[14] - P[1][20]*SPP[3] + P[2][20]*SPP[13] + P[11][20]*SPP[22] + P[14][20]*SPP[16]; -nextP[3][20] = P[3][20] + P[0][20]*SPP[1] + P[1][20]*SPP[19] + P[2][20]*SPP[15] - P[15][20]*SPP[21]; -nextP[4][20] = P[4][20] + P[15][20]*SF[22] + P[0][20]*SPP[20] + P[1][20]*SPP[12] + P[2][20]*SPP[11]; -nextP[5][20] = P[5][20] + P[15][20]*SF[20] - P[0][20]*SPP[9] + P[1][20]*SPP[10] + P[2][20]*SPP[0]; -nextP[6][20] = P[6][20] + P[3][20]*dt; +nextP[0][20] = P[0][20] + P[1][20]*SF[9] + P[2][20]*SF[11] + P[3][20]*SF[10] + P[10][20]*SF[14] + P[11][20]*SF[15] + P[12][20]*SPP[10]; +nextP[1][20] = P[1][20] + P[0][20]*SF[8] + P[2][20]*SF[7] + P[3][20]*SF[11] - P[12][20]*SF[15] + P[11][20]*SPP[10] - (P[10][20]*q0)/2; +nextP[2][20] = P[2][20] + P[0][20]*SF[6] + P[1][20]*SF[10] + P[3][20]*SF[8] + P[12][20]*SF[14] - P[10][20]*SPP[10] - (P[11][20]*q0)/2; +nextP[3][20] = P[3][20] + P[0][20]*SF[7] + P[1][20]*SF[6] + P[2][20]*SF[9] + P[10][20]*SF[15] - P[11][20]*SF[14] - (P[12][20]*q0)/2; +nextP[4][20] = P[4][20] + P[0][20]*SF[5] + P[1][20]*SF[3] - P[3][20]*SF[4] + P[2][20]*SPP[0] + P[13][20]*SPP[3] + P[14][20]*SPP[6] - P[15][20]*SPP[9]; +nextP[5][20] = P[5][20] + P[0][20]*SF[4] + P[2][20]*SF[3] + P[3][20]*SF[5] - P[1][20]*SPP[0] - P[13][20]*SPP[8] + P[14][20]*SPP[2] + P[15][20]*SPP[5]; +nextP[6][20] = P[6][20] + P[1][20]*SF[4] - P[2][20]*SF[5] + P[3][20]*SF[3] + P[0][20]*SPP[0] + P[13][20]*SPP[4] - P[14][20]*SPP[7] - P[15][20]*SPP[1]; nextP[7][20] = P[7][20] + P[4][20]*dt; nextP[8][20] = P[8][20] + P[5][20]*dt; -nextP[9][20] = P[9][20]; +nextP[9][20] = P[9][20] + P[6][20]*dt; nextP[10][20] = P[10][20]; nextP[11][20] = P[11][20]; nextP[12][20] = P[12][20]; @@ -297,16 +285,16 @@ nextP[17][20] = P[17][20]; nextP[18][20] = P[18][20]; nextP[19][20] = P[19][20]; nextP[20][20] = P[20][20]; -nextP[0][21] = P[0][21]*SPP[5] - P[1][21]*SPP[4] + P[2][21]*SPP[7] + P[9][21]*SPP[22] + P[12][21]*SPP[18]; -nextP[1][21] = P[1][21]*SPP[6] - P[0][21]*SPP[2] - P[2][21]*SPP[8] + P[10][21]*SPP[22] + P[13][21]*SPP[17]; -nextP[2][21] = P[0][21]*SPP[14] - P[1][21]*SPP[3] + P[2][21]*SPP[13] + P[11][21]*SPP[22] + P[14][21]*SPP[16]; -nextP[3][21] = P[3][21] + P[0][21]*SPP[1] + P[1][21]*SPP[19] + P[2][21]*SPP[15] - P[15][21]*SPP[21]; -nextP[4][21] = P[4][21] + P[15][21]*SF[22] + P[0][21]*SPP[20] + P[1][21]*SPP[12] + P[2][21]*SPP[11]; -nextP[5][21] = P[5][21] + P[15][21]*SF[20] - P[0][21]*SPP[9] + P[1][21]*SPP[10] + P[2][21]*SPP[0]; -nextP[6][21] = P[6][21] + P[3][21]*dt; +nextP[0][21] = P[0][21] + P[1][21]*SF[9] + P[2][21]*SF[11] + P[3][21]*SF[10] + P[10][21]*SF[14] + P[11][21]*SF[15] + P[12][21]*SPP[10]; +nextP[1][21] = P[1][21] + P[0][21]*SF[8] + P[2][21]*SF[7] + P[3][21]*SF[11] - P[12][21]*SF[15] + P[11][21]*SPP[10] - (P[10][21]*q0)/2; +nextP[2][21] = P[2][21] + P[0][21]*SF[6] + P[1][21]*SF[10] + P[3][21]*SF[8] + P[12][21]*SF[14] - P[10][21]*SPP[10] - (P[11][21]*q0)/2; +nextP[3][21] = P[3][21] + P[0][21]*SF[7] + P[1][21]*SF[6] + P[2][21]*SF[9] + P[10][21]*SF[15] - P[11][21]*SF[14] - (P[12][21]*q0)/2; +nextP[4][21] = P[4][21] + P[0][21]*SF[5] + P[1][21]*SF[3] - P[3][21]*SF[4] + P[2][21]*SPP[0] + P[13][21]*SPP[3] + P[14][21]*SPP[6] - P[15][21]*SPP[9]; +nextP[5][21] = P[5][21] + P[0][21]*SF[4] + P[2][21]*SF[3] + P[3][21]*SF[5] - P[1][21]*SPP[0] - P[13][21]*SPP[8] + P[14][21]*SPP[2] + P[15][21]*SPP[5]; +nextP[6][21] = P[6][21] + P[1][21]*SF[4] - P[2][21]*SF[5] + P[3][21]*SF[3] + P[0][21]*SPP[0] + P[13][21]*SPP[4] - P[14][21]*SPP[7] - P[15][21]*SPP[1]; nextP[7][21] = P[7][21] + P[4][21]*dt; nextP[8][21] = P[8][21] + P[5][21]*dt; -nextP[9][21] = P[9][21]; +nextP[9][21] = P[9][21] + P[6][21]*dt; nextP[10][21] = P[10][21]; nextP[11][21] = P[11][21]; nextP[12][21] = P[12][21]; @@ -319,16 +307,16 @@ nextP[18][21] = P[18][21]; nextP[19][21] = P[19][21]; nextP[20][21] = P[20][21]; nextP[21][21] = P[21][21]; -nextP[0][22] = P[0][22]*SPP[5] - P[1][22]*SPP[4] + P[2][22]*SPP[7] + P[9][22]*SPP[22] + P[12][22]*SPP[18]; -nextP[1][22] = P[1][22]*SPP[6] - P[0][22]*SPP[2] - P[2][22]*SPP[8] + P[10][22]*SPP[22] + P[13][22]*SPP[17]; -nextP[2][22] = P[0][22]*SPP[14] - P[1][22]*SPP[3] + P[2][22]*SPP[13] + P[11][22]*SPP[22] + P[14][22]*SPP[16]; -nextP[3][22] = P[3][22] + P[0][22]*SPP[1] + P[1][22]*SPP[19] + P[2][22]*SPP[15] - P[15][22]*SPP[21]; -nextP[4][22] = P[4][22] + P[15][22]*SF[22] + P[0][22]*SPP[20] + P[1][22]*SPP[12] + P[2][22]*SPP[11]; -nextP[5][22] = P[5][22] + P[15][22]*SF[20] - P[0][22]*SPP[9] + P[1][22]*SPP[10] + P[2][22]*SPP[0]; -nextP[6][22] = P[6][22] + P[3][22]*dt; +nextP[0][22] = P[0][22] + P[1][22]*SF[9] + P[2][22]*SF[11] + P[3][22]*SF[10] + P[10][22]*SF[14] + P[11][22]*SF[15] + P[12][22]*SPP[10]; +nextP[1][22] = P[1][22] + P[0][22]*SF[8] + P[2][22]*SF[7] + P[3][22]*SF[11] - P[12][22]*SF[15] + P[11][22]*SPP[10] - (P[10][22]*q0)/2; +nextP[2][22] = P[2][22] + P[0][22]*SF[6] + P[1][22]*SF[10] + P[3][22]*SF[8] + P[12][22]*SF[14] - P[10][22]*SPP[10] - (P[11][22]*q0)/2; +nextP[3][22] = P[3][22] + P[0][22]*SF[7] + P[1][22]*SF[6] + P[2][22]*SF[9] + P[10][22]*SF[15] - P[11][22]*SF[14] - (P[12][22]*q0)/2; +nextP[4][22] = P[4][22] + P[0][22]*SF[5] + P[1][22]*SF[3] - P[3][22]*SF[4] + P[2][22]*SPP[0] + P[13][22]*SPP[3] + P[14][22]*SPP[6] - P[15][22]*SPP[9]; +nextP[5][22] = P[5][22] + P[0][22]*SF[4] + P[2][22]*SF[3] + P[3][22]*SF[5] - P[1][22]*SPP[0] - P[13][22]*SPP[8] + P[14][22]*SPP[2] + P[15][22]*SPP[5]; +nextP[6][22] = P[6][22] + P[1][22]*SF[4] - P[2][22]*SF[5] + P[3][22]*SF[3] + P[0][22]*SPP[0] + P[13][22]*SPP[4] - P[14][22]*SPP[7] - P[15][22]*SPP[1]; nextP[7][22] = P[7][22] + P[4][22]*dt; nextP[8][22] = P[8][22] + P[5][22]*dt; -nextP[9][22] = P[9][22]; +nextP[9][22] = P[9][22] + P[6][22]*dt; nextP[10][22] = P[10][22]; nextP[11][22] = P[11][22]; nextP[12][22] = P[12][22]; @@ -342,16 +330,16 @@ nextP[19][22] = P[19][22]; nextP[20][22] = P[20][22]; nextP[21][22] = P[21][22]; nextP[22][22] = P[22][22]; -nextP[0][23] = P[0][23]*SPP[5] - P[1][23]*SPP[4] + P[2][23]*SPP[7] + P[9][23]*SPP[22] + P[12][23]*SPP[18]; -nextP[1][23] = P[1][23]*SPP[6] - P[0][23]*SPP[2] - P[2][23]*SPP[8] + P[10][23]*SPP[22] + P[13][23]*SPP[17]; -nextP[2][23] = P[0][23]*SPP[14] - P[1][23]*SPP[3] + P[2][23]*SPP[13] + P[11][23]*SPP[22] + P[14][23]*SPP[16]; -nextP[3][23] = P[3][23] + P[0][23]*SPP[1] + P[1][23]*SPP[19] + P[2][23]*SPP[15] - P[15][23]*SPP[21]; -nextP[4][23] = P[4][23] + P[15][23]*SF[22] + P[0][23]*SPP[20] + P[1][23]*SPP[12] + P[2][23]*SPP[11]; -nextP[5][23] = P[5][23] + P[15][23]*SF[20] - P[0][23]*SPP[9] + P[1][23]*SPP[10] + P[2][23]*SPP[0]; -nextP[6][23] = P[6][23] + P[3][23]*dt; +nextP[0][23] = P[0][23] + P[1][23]*SF[9] + P[2][23]*SF[11] + P[3][23]*SF[10] + P[10][23]*SF[14] + P[11][23]*SF[15] + P[12][23]*SPP[10]; +nextP[1][23] = P[1][23] + P[0][23]*SF[8] + P[2][23]*SF[7] + P[3][23]*SF[11] - P[12][23]*SF[15] + P[11][23]*SPP[10] - (P[10][23]*q0)/2; +nextP[2][23] = P[2][23] + P[0][23]*SF[6] + P[1][23]*SF[10] + P[3][23]*SF[8] + P[12][23]*SF[14] - P[10][23]*SPP[10] - (P[11][23]*q0)/2; +nextP[3][23] = P[3][23] + P[0][23]*SF[7] + P[1][23]*SF[6] + P[2][23]*SF[9] + P[10][23]*SF[15] - P[11][23]*SF[14] - (P[12][23]*q0)/2; +nextP[4][23] = P[4][23] + P[0][23]*SF[5] + P[1][23]*SF[3] - P[3][23]*SF[4] + P[2][23]*SPP[0] + P[13][23]*SPP[3] + P[14][23]*SPP[6] - P[15][23]*SPP[9]; +nextP[5][23] = P[5][23] + P[0][23]*SF[4] + P[2][23]*SF[3] + P[3][23]*SF[5] - P[1][23]*SPP[0] - P[13][23]*SPP[8] + P[14][23]*SPP[2] + P[15][23]*SPP[5]; +nextP[6][23] = P[6][23] + P[1][23]*SF[4] - P[2][23]*SF[5] + P[3][23]*SF[3] + P[0][23]*SPP[0] + P[13][23]*SPP[4] - P[14][23]*SPP[7] - P[15][23]*SPP[1]; nextP[7][23] = P[7][23] + P[4][23]*dt; nextP[8][23] = P[8][23] + P[5][23]*dt; -nextP[9][23] = P[9][23]; +nextP[9][23] = P[9][23] + P[6][23]*dt; nextP[10][23] = P[10][23]; nextP[11][23] = P[11][23]; nextP[12][23] = P[12][23]; @@ -371,311 +359,328 @@ SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd))); SH_TAS[1] = (SH_TAS[0]*(2*ve - 2*vwe))/2; SH_TAS[2] = (SH_TAS[0]*(2*vn - 2*vwn))/2; float H_TAS[1][24]; -H_TAS[0][3] = SH_TAS[2]; -H_TAS[0][4] = SH_TAS[1]; -H_TAS[0][5] = vd*SH_TAS[0]; +H_TAS[0][4] = SH_TAS[2]; +H_TAS[0][5] = SH_TAS[1]; +H_TAS[0][6] = vd*SH_TAS[0]; H_TAS[0][22] = -SH_TAS[2]; H_TAS[0][23] = -SH_TAS[1]; float SK_TAS[2][1]; -SK_TAS[0] = 1/(R_TAS + SH_TAS[2]*(P[3][3]*SH_TAS[2] + P[4][3]*SH_TAS[1] - P[22][3]*SH_TAS[2] - P[23][3]*SH_TAS[1] + P[5][3]*vd*SH_TAS[0]) + SH_TAS[1]*(P[3][4]*SH_TAS[2] + P[4][4]*SH_TAS[1] - P[22][4]*SH_TAS[2] - P[23][4]*SH_TAS[1] + P[5][4]*vd*SH_TAS[0]) - SH_TAS[2]*(P[3][22]*SH_TAS[2] + P[4][22]*SH_TAS[1] - P[22][22]*SH_TAS[2] - P[23][22]*SH_TAS[1] + P[5][22]*vd*SH_TAS[0]) - SH_TAS[1]*(P[3][23]*SH_TAS[2] + P[4][23]*SH_TAS[1] - P[22][23]*SH_TAS[2] - P[23][23]*SH_TAS[1] + P[5][23]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[3][5]*SH_TAS[2] + P[4][5]*SH_TAS[1] - P[22][5]*SH_TAS[2] - P[23][5]*SH_TAS[1] + P[5][5]*vd*SH_TAS[0])); +SK_TAS[0] = 1/(R_TAS + SH_TAS[2]*(P[4][4]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[22][4]*SH_TAS[2] - P[23][4]*SH_TAS[1] + P[6][4]*vd*SH_TAS[0]) + SH_TAS[1]*(P[4][5]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[22][5]*SH_TAS[2] - P[23][5]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]) - SH_TAS[2]*(P[4][22]*SH_TAS[2] + P[5][22]*SH_TAS[1] - P[22][22]*SH_TAS[2] - P[23][22]*SH_TAS[1] + P[6][22]*vd*SH_TAS[0]) - SH_TAS[1]*(P[4][23]*SH_TAS[2] + P[5][23]*SH_TAS[1] - P[22][23]*SH_TAS[2] - P[23][23]*SH_TAS[1] + P[6][23]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[4][6]*SH_TAS[2] + P[5][6]*SH_TAS[1] - P[22][6]*SH_TAS[2] - P[23][6]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0])); SK_TAS[1] = SH_TAS[1]; float Kfusion[24][1]; float Kfusion[1][1]; -Kfusion[0] = SK_TAS[0]*(P[0][3]*SH_TAS[2] - P[0][22]*SH_TAS[2] + P[0][4]*SK_TAS[1] - P[0][23]*SK_TAS[1] + P[0][5]*vd*SH_TAS[0]); -Kfusion[1] = SK_TAS[0]*(P[1][3]*SH_TAS[2] - P[1][22]*SH_TAS[2] + P[1][4]*SK_TAS[1] - P[1][23]*SK_TAS[1] + P[1][5]*vd*SH_TAS[0]); -Kfusion[2] = SK_TAS[0]*(P[2][3]*SH_TAS[2] - P[2][22]*SH_TAS[2] + P[2][4]*SK_TAS[1] - P[2][23]*SK_TAS[1] + P[2][5]*vd*SH_TAS[0]); -Kfusion[3] = SK_TAS[0]*(P[3][3]*SH_TAS[2] - P[3][22]*SH_TAS[2] + P[3][4]*SK_TAS[1] - P[3][23]*SK_TAS[1] + P[3][5]*vd*SH_TAS[0]); -Kfusion[4] = SK_TAS[0]*(P[4][3]*SH_TAS[2] - P[4][22]*SH_TAS[2] + P[4][4]*SK_TAS[1] - P[4][23]*SK_TAS[1] + P[4][5]*vd*SH_TAS[0]); -Kfusion[5] = SK_TAS[0]*(P[5][3]*SH_TAS[2] - P[5][22]*SH_TAS[2] + P[5][4]*SK_TAS[1] - P[5][23]*SK_TAS[1] + P[5][5]*vd*SH_TAS[0]); -Kfusion[6] = SK_TAS[0]*(P[6][3]*SH_TAS[2] - P[6][22]*SH_TAS[2] + P[6][4]*SK_TAS[1] - P[6][23]*SK_TAS[1] + P[6][5]*vd*SH_TAS[0]); -Kfusion[7] = SK_TAS[0]*(P[7][3]*SH_TAS[2] - P[7][22]*SH_TAS[2] + P[7][4]*SK_TAS[1] - P[7][23]*SK_TAS[1] + P[7][5]*vd*SH_TAS[0]); -Kfusion[8] = SK_TAS[0]*(P[8][3]*SH_TAS[2] - P[8][22]*SH_TAS[2] + P[8][4]*SK_TAS[1] - P[8][23]*SK_TAS[1] + P[8][5]*vd*SH_TAS[0]); -Kfusion[9] = SK_TAS[0]*(P[9][3]*SH_TAS[2] - P[9][22]*SH_TAS[2] + P[9][4]*SK_TAS[1] - P[9][23]*SK_TAS[1] + P[9][5]*vd*SH_TAS[0]); -Kfusion[10] = SK_TAS[0]*(P[10][3]*SH_TAS[2] - P[10][22]*SH_TAS[2] + P[10][4]*SK_TAS[1] - P[10][23]*SK_TAS[1] + P[10][5]*vd*SH_TAS[0]); -Kfusion[11] = SK_TAS[0]*(P[11][3]*SH_TAS[2] - P[11][22]*SH_TAS[2] + P[11][4]*SK_TAS[1] - P[11][23]*SK_TAS[1] + P[11][5]*vd*SH_TAS[0]); -Kfusion[12] = SK_TAS[0]*(P[12][3]*SH_TAS[2] - P[12][22]*SH_TAS[2] + P[12][4]*SK_TAS[1] - P[12][23]*SK_TAS[1] + P[12][5]*vd*SH_TAS[0]); -Kfusion[13] = SK_TAS[0]*(P[13][3]*SH_TAS[2] - P[13][22]*SH_TAS[2] + P[13][4]*SK_TAS[1] - P[13][23]*SK_TAS[1] + P[13][5]*vd*SH_TAS[0]); -Kfusion[14] = SK_TAS[0]*(P[14][3]*SH_TAS[2] - P[14][22]*SH_TAS[2] + P[14][4]*SK_TAS[1] - P[14][23]*SK_TAS[1] + P[14][5]*vd*SH_TAS[0]); -Kfusion[15] = SK_TAS[0]*(P[15][3]*SH_TAS[2] - P[15][22]*SH_TAS[2] + P[15][4]*SK_TAS[1] - P[15][23]*SK_TAS[1] + P[15][5]*vd*SH_TAS[0]); -Kfusion[16] = SK_TAS[0]*(P[16][3]*SH_TAS[2] - P[16][22]*SH_TAS[2] + P[16][4]*SK_TAS[1] - P[16][23]*SK_TAS[1] + P[16][5]*vd*SH_TAS[0]); -Kfusion[17] = SK_TAS[0]*(P[17][3]*SH_TAS[2] - P[17][22]*SH_TAS[2] + P[17][4]*SK_TAS[1] - P[17][23]*SK_TAS[1] + P[17][5]*vd*SH_TAS[0]); -Kfusion[18] = SK_TAS[0]*(P[18][3]*SH_TAS[2] - P[18][22]*SH_TAS[2] + P[18][4]*SK_TAS[1] - P[18][23]*SK_TAS[1] + P[18][5]*vd*SH_TAS[0]); -Kfusion[19] = SK_TAS[0]*(P[19][3]*SH_TAS[2] - P[19][22]*SH_TAS[2] + P[19][4]*SK_TAS[1] - P[19][23]*SK_TAS[1] + P[19][5]*vd*SH_TAS[0]); -Kfusion[20] = SK_TAS[0]*(P[20][3]*SH_TAS[2] - P[20][22]*SH_TAS[2] + P[20][4]*SK_TAS[1] - P[20][23]*SK_TAS[1] + P[20][5]*vd*SH_TAS[0]); -Kfusion[21] = SK_TAS[0]*(P[21][3]*SH_TAS[2] - P[21][22]*SH_TAS[2] + P[21][4]*SK_TAS[1] - P[21][23]*SK_TAS[1] + P[21][5]*vd*SH_TAS[0]); -Kfusion[22] = SK_TAS[0]*(P[22][3]*SH_TAS[2] - P[22][22]*SH_TAS[2] + P[22][4]*SK_TAS[1] - P[22][23]*SK_TAS[1] + P[22][5]*vd*SH_TAS[0]); -Kfusion[23] = SK_TAS[0]*(P[23][3]*SH_TAS[2] - P[23][22]*SH_TAS[2] + P[23][4]*SK_TAS[1] - P[23][23]*SK_TAS[1] + P[23][5]*vd*SH_TAS[0]); -float SH_BETA[10][1]; -SH_BETA[0] = (2*conj(q0)*conj(q3) + 2*conj(q1)*conj(q2))*(ve - vwe) - vd*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3)) + (vn - vwn)*(conjsq(q0) + conjsq(q1) - conjsq(q2) - conjsq(q3)); -SH_BETA[1] = vd*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) - (2*conj(q0)*conj(q3) - 2*conj(q1)*conj(q2))*(vn - vwn) + (ve - vwe)*(conjsq(q0) - conjsq(q1) + conjsq(q2) - conjsq(q3)); -SH_BETA[2] = (2*conj(q0)*conj(q2) + 2*conj(q1)*conj(q3))*(vn - vwn) - (2*conj(q0)*conj(q1) - 2*conj(q2)*conj(q3))*(ve - vwe) + vd*(conjsq(q0) - conjsq(q1) - conjsq(q2) + conjsq(q3)); -SH_BETA[3] = (conjsq(q0) - conjsq(q1) + conjsq(q2) - conjsq(q3))/SH_BETA[0]; +Kfusion[0] = SK_TAS[0]*(P[0][4]*SH_TAS[2] - P[0][22]*SH_TAS[2] + P[0][5]*SK_TAS[1] - P[0][23]*SK_TAS[1] + P[0][6]*vd*SH_TAS[0]); +Kfusion[1] = SK_TAS[0]*(P[1][4]*SH_TAS[2] - P[1][22]*SH_TAS[2] + P[1][5]*SK_TAS[1] - P[1][23]*SK_TAS[1] + P[1][6]*vd*SH_TAS[0]); +Kfusion[2] = SK_TAS[0]*(P[2][4]*SH_TAS[2] - P[2][22]*SH_TAS[2] + P[2][5]*SK_TAS[1] - P[2][23]*SK_TAS[1] + P[2][6]*vd*SH_TAS[0]); +Kfusion[3] = SK_TAS[0]*(P[3][4]*SH_TAS[2] - P[3][22]*SH_TAS[2] + P[3][5]*SK_TAS[1] - P[3][23]*SK_TAS[1] + P[3][6]*vd*SH_TAS[0]); +Kfusion[4] = SK_TAS[0]*(P[4][4]*SH_TAS[2] - P[4][22]*SH_TAS[2] + P[4][5]*SK_TAS[1] - P[4][23]*SK_TAS[1] + P[4][6]*vd*SH_TAS[0]); +Kfusion[5] = SK_TAS[0]*(P[5][4]*SH_TAS[2] - P[5][22]*SH_TAS[2] + P[5][5]*SK_TAS[1] - P[5][23]*SK_TAS[1] + P[5][6]*vd*SH_TAS[0]); +Kfusion[6] = SK_TAS[0]*(P[6][4]*SH_TAS[2] - P[6][22]*SH_TAS[2] + P[6][5]*SK_TAS[1] - P[6][23]*SK_TAS[1] + P[6][6]*vd*SH_TAS[0]); +Kfusion[7] = SK_TAS[0]*(P[7][4]*SH_TAS[2] - P[7][22]*SH_TAS[2] + P[7][5]*SK_TAS[1] - P[7][23]*SK_TAS[1] + P[7][6]*vd*SH_TAS[0]); +Kfusion[8] = SK_TAS[0]*(P[8][4]*SH_TAS[2] - P[8][22]*SH_TAS[2] + P[8][5]*SK_TAS[1] - P[8][23]*SK_TAS[1] + P[8][6]*vd*SH_TAS[0]); +Kfusion[9] = SK_TAS[0]*(P[9][4]*SH_TAS[2] - P[9][22]*SH_TAS[2] + P[9][5]*SK_TAS[1] - P[9][23]*SK_TAS[1] + P[9][6]*vd*SH_TAS[0]); +Kfusion[10] = SK_TAS[0]*(P[10][4]*SH_TAS[2] - P[10][22]*SH_TAS[2] + P[10][5]*SK_TAS[1] - P[10][23]*SK_TAS[1] + P[10][6]*vd*SH_TAS[0]); +Kfusion[11] = SK_TAS[0]*(P[11][4]*SH_TAS[2] - P[11][22]*SH_TAS[2] + P[11][5]*SK_TAS[1] - P[11][23]*SK_TAS[1] + P[11][6]*vd*SH_TAS[0]); +Kfusion[12] = SK_TAS[0]*(P[12][4]*SH_TAS[2] - P[12][22]*SH_TAS[2] + P[12][5]*SK_TAS[1] - P[12][23]*SK_TAS[1] + P[12][6]*vd*SH_TAS[0]); +Kfusion[13] = SK_TAS[0]*(P[13][4]*SH_TAS[2] - P[13][22]*SH_TAS[2] + P[13][5]*SK_TAS[1] - P[13][23]*SK_TAS[1] + P[13][6]*vd*SH_TAS[0]); +Kfusion[14] = SK_TAS[0]*(P[14][4]*SH_TAS[2] - P[14][22]*SH_TAS[2] + P[14][5]*SK_TAS[1] - P[14][23]*SK_TAS[1] + P[14][6]*vd*SH_TAS[0]); +Kfusion[15] = SK_TAS[0]*(P[15][4]*SH_TAS[2] - P[15][22]*SH_TAS[2] + P[15][5]*SK_TAS[1] - P[15][23]*SK_TAS[1] + P[15][6]*vd*SH_TAS[0]); +Kfusion[16] = SK_TAS[0]*(P[16][4]*SH_TAS[2] - P[16][22]*SH_TAS[2] + P[16][5]*SK_TAS[1] - P[16][23]*SK_TAS[1] + P[16][6]*vd*SH_TAS[0]); +Kfusion[17] = SK_TAS[0]*(P[17][4]*SH_TAS[2] - P[17][22]*SH_TAS[2] + P[17][5]*SK_TAS[1] - P[17][23]*SK_TAS[1] + P[17][6]*vd*SH_TAS[0]); +Kfusion[18] = SK_TAS[0]*(P[18][4]*SH_TAS[2] - P[18][22]*SH_TAS[2] + P[18][5]*SK_TAS[1] - P[18][23]*SK_TAS[1] + P[18][6]*vd*SH_TAS[0]); +Kfusion[19] = SK_TAS[0]*(P[19][4]*SH_TAS[2] - P[19][22]*SH_TAS[2] + P[19][5]*SK_TAS[1] - P[19][23]*SK_TAS[1] + P[19][6]*vd*SH_TAS[0]); +Kfusion[20] = SK_TAS[0]*(P[20][4]*SH_TAS[2] - P[20][22]*SH_TAS[2] + P[20][5]*SK_TAS[1] - P[20][23]*SK_TAS[1] + P[20][6]*vd*SH_TAS[0]); +Kfusion[21] = SK_TAS[0]*(P[21][4]*SH_TAS[2] - P[21][22]*SH_TAS[2] + P[21][5]*SK_TAS[1] - P[21][23]*SK_TAS[1] + P[21][6]*vd*SH_TAS[0]); +Kfusion[22] = SK_TAS[0]*(P[22][4]*SH_TAS[2] - P[22][22]*SH_TAS[2] + P[22][5]*SK_TAS[1] - P[22][23]*SK_TAS[1] + P[22][6]*vd*SH_TAS[0]); +Kfusion[23] = SK_TAS[0]*(P[23][4]*SH_TAS[2] - P[23][22]*SH_TAS[2] + P[23][5]*SK_TAS[1] - P[23][23]*SK_TAS[1] + P[23][6]*vd*SH_TAS[0]); +float SH_BETA[13][1]; +SH_BETA[0] = (vn - vwn)*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) - vd*(2*q0*q2 - 2*q1*q3) + (ve - vwe)*(2*q0*q3 + 2*q1*q2); +SH_BETA[1] = (ve - vwe)*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + vd*(2*q0*q1 + 2*q2*q3) - (vn - vwn)*(2*q0*q3 - 2*q1*q2); +SH_BETA[2] = vn - vwn; +SH_BETA[3] = ve - vwe; SH_BETA[4] = 1/sq(SH_BETA[0]); -SH_BETA[5] = conjsq(q0) + conjsq(q1) - conjsq(q2) - conjsq(q3); -SH_BETA[6] = 2*conj(q0)*conj(q3); -SH_BETA[7] = 1/SH_BETA[0]; -SH_BETA[8] = SH_BETA[6] + 2*conj(q1)*conj(q2); -SH_BETA[9] = SH_BETA[6] - 2*conj(q1)*conj(q2); +SH_BETA[5] = 1/SH_BETA[0]; +SH_BETA[6] = SH_BETA[5]*(sq(q0) - sq(q1) + sq(q2) - sq(q3)); +SH_BETA[7] = sq(q0) + sq(q1) - sq(q2) - sq(q3); +SH_BETA[8] = 2*q0*SH_BETA[3] - 2*q3*SH_BETA[2] + 2*q1*vd; +SH_BETA[9] = 2*q0*SH_BETA[2] + 2*q3*SH_BETA[3] - 2*q2*vd; +SH_BETA[10] = 2*q2*SH_BETA[2] - 2*q1*SH_BETA[3] + 2*q0*vd; +SH_BETA[11] = 2*q1*SH_BETA[2] + 2*q2*SH_BETA[3] + 2*q3*vd; +SH_BETA[12] = 2*q0*q3; float H_BETA[1][24]; -H_BETA[0][0] = SH_BETA[2]*SH_BETA[7]; -H_BETA[0][1] = SH_BETA[1]*SH_BETA[2]*SH_BETA[4]; -H_BETA[0][2] = - sq(SH_BETA[1])*SH_BETA[4] - 1; -H_BETA[0][3] = - SH_BETA[7]*SH_BETA[9] - SH_BETA[1]*SH_BETA[4]*SH_BETA[5]; -H_BETA[0][4] = SH_BETA[3] - SH_BETA[1]*SH_BETA[4]*SH_BETA[8]; -H_BETA[0][5] = SH_BETA[7]*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA[1]*SH_BETA[4]*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3)); -H_BETA[0][22] = SH_BETA[7]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[5]; -H_BETA[0][23] = SH_BETA[1]*SH_BETA[4]*SH_BETA[8] - SH_BETA[3]; -float SK_BETA[6][1]; -SK_BETA[0] = 1/(R_BETA + (SH_BETA[7]*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA[1]*SH_BETA[4]*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3)))*(P[22][5]*(SH_BETA[7]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[5]) - P[3][5]*(SH_BETA[7]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[5]) - P[2][5]*(sq(SH_BETA[1])*SH_BETA[4] + 1) + P[5][5]*(SH_BETA[7]*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA[1]*SH_BETA[4]*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + P[4][5]*(SH_BETA[3] - SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) - P[23][5]*(SH_BETA[3] - SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[0][5]*SH_BETA[2]*SH_BETA[7] + P[1][5]*SH_BETA[1]*SH_BETA[2]*SH_BETA[4]) + (SH_BETA[3] - SH_BETA[1]*SH_BETA[4]*SH_BETA[8])*(P[22][4]*(SH_BETA[7]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[5]) - P[3][4]*(SH_BETA[7]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[5]) - P[2][4]*(sq(SH_BETA[1])*SH_BETA[4] + 1) + P[5][4]*(SH_BETA[7]*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA[1]*SH_BETA[4]*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + P[4][4]*(SH_BETA[3] - SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) - P[23][4]*(SH_BETA[3] - SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[0][4]*SH_BETA[2]*SH_BETA[7] + P[1][4]*SH_BETA[1]*SH_BETA[2]*SH_BETA[4]) - (SH_BETA[3] - SH_BETA[1]*SH_BETA[4]*SH_BETA[8])*(P[22][23]*(SH_BETA[7]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[5]) - P[3][23]*(SH_BETA[7]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[5]) - P[2][23]*(sq(SH_BETA[1])*SH_BETA[4] + 1) + P[5][23]*(SH_BETA[7]*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA[1]*SH_BETA[4]*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + P[4][23]*(SH_BETA[3] - SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) - P[23][23]*(SH_BETA[3] - SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[0][23]*SH_BETA[2]*SH_BETA[7] + P[1][23]*SH_BETA[1]*SH_BETA[2]*SH_BETA[4]) - (SH_BETA[7]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[5])*(P[22][3]*(SH_BETA[7]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[5]) - P[3][3]*(SH_BETA[7]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[5]) - P[2][3]*(sq(SH_BETA[1])*SH_BETA[4] + 1) + P[5][3]*(SH_BETA[7]*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA[1]*SH_BETA[4]*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + P[4][3]*(SH_BETA[3] - SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) - P[23][3]*(SH_BETA[3] - SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[0][3]*SH_BETA[2]*SH_BETA[7] + P[1][3]*SH_BETA[1]*SH_BETA[2]*SH_BETA[4]) + (SH_BETA[7]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[5])*(P[22][22]*(SH_BETA[7]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[5]) - P[3][22]*(SH_BETA[7]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[5]) - P[2][22]*(sq(SH_BETA[1])*SH_BETA[4] + 1) + P[5][22]*(SH_BETA[7]*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA[1]*SH_BETA[4]*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + P[4][22]*(SH_BETA[3] - SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) - P[23][22]*(SH_BETA[3] - SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[0][22]*SH_BETA[2]*SH_BETA[7] + P[1][22]*SH_BETA[1]*SH_BETA[2]*SH_BETA[4]) - (sq(SH_BETA[1])*SH_BETA[4] + 1)*(P[22][2]*(SH_BETA[7]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[5]) - P[3][2]*(SH_BETA[7]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[5]) - P[2][2]*(sq(SH_BETA[1])*SH_BETA[4] + 1) + P[5][2]*(SH_BETA[7]*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA[1]*SH_BETA[4]*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + P[4][2]*(SH_BETA[3] - SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) - P[23][2]*(SH_BETA[3] - SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[0][2]*SH_BETA[2]*SH_BETA[7] + P[1][2]*SH_BETA[1]*SH_BETA[2]*SH_BETA[4]) + SH_BETA[2]*SH_BETA[7]*(P[22][0]*(SH_BETA[7]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[5]) - P[3][0]*(SH_BETA[7]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[5]) - P[2][0]*(sq(SH_BETA[1])*SH_BETA[4] + 1) + P[5][0]*(SH_BETA[7]*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA[1]*SH_BETA[4]*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + P[4][0]*(SH_BETA[3] - SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) - P[23][0]*(SH_BETA[3] - SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[0][0]*SH_BETA[2]*SH_BETA[7] + P[1][0]*SH_BETA[1]*SH_BETA[2]*SH_BETA[4]) + SH_BETA[1]*SH_BETA[2]*SH_BETA[4]*(P[22][1]*(SH_BETA[7]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[5]) - P[3][1]*(SH_BETA[7]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[5]) - P[2][1]*(sq(SH_BETA[1])*SH_BETA[4] + 1) + P[5][1]*(SH_BETA[7]*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA[1]*SH_BETA[4]*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + P[4][1]*(SH_BETA[3] - SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) - P[23][1]*(SH_BETA[3] - SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[0][1]*SH_BETA[2]*SH_BETA[7] + P[1][1]*SH_BETA[1]*SH_BETA[2]*SH_BETA[4])); -SK_BETA[1] = SH_BETA[7]*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA[1]*SH_BETA[4]*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3)); -SK_BETA[2] = SH_BETA[7]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[5]; -SK_BETA[3] = SH_BETA[3] - SH_BETA[1]*SH_BETA[4]*SH_BETA[8]; -SK_BETA[4] = sq(SH_BETA[1])*SH_BETA[4] + 1; -SK_BETA[5] = SH_BETA[2]; +H_BETA[0][0] = SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]; +H_BETA[0][1] = SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]; +H_BETA[0][2] = SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]; +H_BETA[0][3] = - SH_BETA[5]*SH_BETA[9] - SH_BETA[1]*SH_BETA[4]*SH_BETA[8]; +H_BETA[0][4] = - SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) - SH_BETA[1]*SH_BETA[4]*SH_BETA[7]; +H_BETA[0][5] = SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2); +H_BETA[0][6] = SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3); +H_BETA[0][22] = SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]; +H_BETA[0][23] = SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2) - SH_BETA[6]; +float SK_BETA[8][1]; +SK_BETA[0] = 1/(R_BETA - (SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7])*(P[22][4]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][4]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][4]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][4]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][4]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][4]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][4]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][4]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][4]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) + (SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7])*(P[22][22]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][22]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][22]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][22]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][22]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][22]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][22]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][22]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][22]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) + (SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2))*(P[22][5]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][5]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][5]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][5]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][5]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][5]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][5]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][5]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][5]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) - (SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2))*(P[22][23]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][23]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][23]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][23]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][23]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][23]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][23]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][23]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][23]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) + (SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9])*(P[22][0]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][0]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][0]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][0]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][0]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][0]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][0]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][0]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][0]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) + (SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11])*(P[22][1]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][1]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][1]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][1]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][1]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][1]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][1]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][1]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][1]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) + (SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10])*(P[22][2]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][2]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][2]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][2]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][2]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][2]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][2]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][2]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][2]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) - (SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8])*(P[22][3]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][3]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][3]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][3]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][3]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][3]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][3]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][3]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][3]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) + (SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))*(P[22][6]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][6]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][6]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][6]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][6]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][6]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][6]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][6]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][6]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3)))); +SK_BETA[1] = SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]; +SK_BETA[2] = SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2); +SK_BETA[3] = SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3); +SK_BETA[4] = SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]; +SK_BETA[5] = SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]; +SK_BETA[6] = SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]; +SK_BETA[7] = SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]; float Kfusion[24][1]; float Kfusion[1][1]; -Kfusion[0] = SK_BETA[0]*(P[0][5]*SK_BETA[1] - P[0][2]*SK_BETA[4] - P[0][3]*SK_BETA[2] + P[0][4]*SK_BETA[3] + P[0][22]*SK_BETA[2] - P[0][23]*SK_BETA[3] + P[0][0]*SH_BETA[7]*SK_BETA[5] + P[0][1]*SH_BETA[1]*SH_BETA[4]*SK_BETA[5]); -Kfusion[1] = SK_BETA[0]*(P[1][5]*SK_BETA[1] - P[1][2]*SK_BETA[4] - P[1][3]*SK_BETA[2] + P[1][4]*SK_BETA[3] + P[1][22]*SK_BETA[2] - P[1][23]*SK_BETA[3] + P[1][0]*SH_BETA[7]*SK_BETA[5] + P[1][1]*SH_BETA[1]*SH_BETA[4]*SK_BETA[5]); -Kfusion[2] = SK_BETA[0]*(P[2][5]*SK_BETA[1] - P[2][2]*SK_BETA[4] - P[2][3]*SK_BETA[2] + P[2][4]*SK_BETA[3] + P[2][22]*SK_BETA[2] - P[2][23]*SK_BETA[3] + P[2][0]*SH_BETA[7]*SK_BETA[5] + P[2][1]*SH_BETA[1]*SH_BETA[4]*SK_BETA[5]); -Kfusion[3] = SK_BETA[0]*(P[3][5]*SK_BETA[1] - P[3][2]*SK_BETA[4] - P[3][3]*SK_BETA[2] + P[3][4]*SK_BETA[3] + P[3][22]*SK_BETA[2] - P[3][23]*SK_BETA[3] + P[3][0]*SH_BETA[7]*SK_BETA[5] + P[3][1]*SH_BETA[1]*SH_BETA[4]*SK_BETA[5]); -Kfusion[4] = SK_BETA[0]*(P[4][5]*SK_BETA[1] - P[4][2]*SK_BETA[4] - P[4][3]*SK_BETA[2] + P[4][4]*SK_BETA[3] + P[4][22]*SK_BETA[2] - P[4][23]*SK_BETA[3] + P[4][0]*SH_BETA[7]*SK_BETA[5] + P[4][1]*SH_BETA[1]*SH_BETA[4]*SK_BETA[5]); -Kfusion[5] = SK_BETA[0]*(P[5][5]*SK_BETA[1] - P[5][2]*SK_BETA[4] - P[5][3]*SK_BETA[2] + P[5][4]*SK_BETA[3] + P[5][22]*SK_BETA[2] - P[5][23]*SK_BETA[3] + P[5][0]*SH_BETA[7]*SK_BETA[5] + P[5][1]*SH_BETA[1]*SH_BETA[4]*SK_BETA[5]); -Kfusion[6] = SK_BETA[0]*(P[6][5]*SK_BETA[1] - P[6][2]*SK_BETA[4] - P[6][3]*SK_BETA[2] + P[6][4]*SK_BETA[3] + P[6][22]*SK_BETA[2] - P[6][23]*SK_BETA[3] + P[6][0]*SH_BETA[7]*SK_BETA[5] + P[6][1]*SH_BETA[1]*SH_BETA[4]*SK_BETA[5]); -Kfusion[7] = SK_BETA[0]*(P[7][5]*SK_BETA[1] - P[7][2]*SK_BETA[4] - P[7][3]*SK_BETA[2] + P[7][4]*SK_BETA[3] + P[7][22]*SK_BETA[2] - P[7][23]*SK_BETA[3] + P[7][0]*SH_BETA[7]*SK_BETA[5] + P[7][1]*SH_BETA[1]*SH_BETA[4]*SK_BETA[5]); -Kfusion[8] = SK_BETA[0]*(P[8][5]*SK_BETA[1] - P[8][2]*SK_BETA[4] - P[8][3]*SK_BETA[2] + P[8][4]*SK_BETA[3] + P[8][22]*SK_BETA[2] - P[8][23]*SK_BETA[3] + P[8][0]*SH_BETA[7]*SK_BETA[5] + P[8][1]*SH_BETA[1]*SH_BETA[4]*SK_BETA[5]); -Kfusion[9] = SK_BETA[0]*(P[9][5]*SK_BETA[1] - P[9][2]*SK_BETA[4] - P[9][3]*SK_BETA[2] + P[9][4]*SK_BETA[3] + P[9][22]*SK_BETA[2] - P[9][23]*SK_BETA[3] + P[9][0]*SH_BETA[7]*SK_BETA[5] + P[9][1]*SH_BETA[1]*SH_BETA[4]*SK_BETA[5]); -Kfusion[10] = SK_BETA[0]*(P[10][5]*SK_BETA[1] - P[10][2]*SK_BETA[4] - P[10][3]*SK_BETA[2] + P[10][4]*SK_BETA[3] + P[10][22]*SK_BETA[2] - P[10][23]*SK_BETA[3] + P[10][0]*SH_BETA[7]*SK_BETA[5] + P[10][1]*SH_BETA[1]*SH_BETA[4]*SK_BETA[5]); -Kfusion[11] = SK_BETA[0]*(P[11][5]*SK_BETA[1] - P[11][2]*SK_BETA[4] - P[11][3]*SK_BETA[2] + P[11][4]*SK_BETA[3] + P[11][22]*SK_BETA[2] - P[11][23]*SK_BETA[3] + P[11][0]*SH_BETA[7]*SK_BETA[5] + P[11][1]*SH_BETA[1]*SH_BETA[4]*SK_BETA[5]); -Kfusion[12] = SK_BETA[0]*(P[12][5]*SK_BETA[1] - P[12][2]*SK_BETA[4] - P[12][3]*SK_BETA[2] + P[12][4]*SK_BETA[3] + P[12][22]*SK_BETA[2] - P[12][23]*SK_BETA[3] + P[12][0]*SH_BETA[7]*SK_BETA[5] + P[12][1]*SH_BETA[1]*SH_BETA[4]*SK_BETA[5]); -Kfusion[13] = SK_BETA[0]*(P[13][5]*SK_BETA[1] - P[13][2]*SK_BETA[4] - P[13][3]*SK_BETA[2] + P[13][4]*SK_BETA[3] + P[13][22]*SK_BETA[2] - P[13][23]*SK_BETA[3] + P[13][0]*SH_BETA[7]*SK_BETA[5] + P[13][1]*SH_BETA[1]*SH_BETA[4]*SK_BETA[5]); -Kfusion[14] = SK_BETA[0]*(P[14][5]*SK_BETA[1] - P[14][2]*SK_BETA[4] - P[14][3]*SK_BETA[2] + P[14][4]*SK_BETA[3] + P[14][22]*SK_BETA[2] - P[14][23]*SK_BETA[3] + P[14][0]*SH_BETA[7]*SK_BETA[5] + P[14][1]*SH_BETA[1]*SH_BETA[4]*SK_BETA[5]); -Kfusion[15] = SK_BETA[0]*(P[15][5]*SK_BETA[1] - P[15][2]*SK_BETA[4] - P[15][3]*SK_BETA[2] + P[15][4]*SK_BETA[3] + P[15][22]*SK_BETA[2] - P[15][23]*SK_BETA[3] + P[15][0]*SH_BETA[7]*SK_BETA[5] + P[15][1]*SH_BETA[1]*SH_BETA[4]*SK_BETA[5]); -Kfusion[16] = SK_BETA[0]*(P[16][5]*SK_BETA[1] - P[16][2]*SK_BETA[4] - P[16][3]*SK_BETA[2] + P[16][4]*SK_BETA[3] + P[16][22]*SK_BETA[2] - P[16][23]*SK_BETA[3] + P[16][0]*SH_BETA[7]*SK_BETA[5] + P[16][1]*SH_BETA[1]*SH_BETA[4]*SK_BETA[5]); -Kfusion[17] = SK_BETA[0]*(P[17][5]*SK_BETA[1] - P[17][2]*SK_BETA[4] - P[17][3]*SK_BETA[2] + P[17][4]*SK_BETA[3] + P[17][22]*SK_BETA[2] - P[17][23]*SK_BETA[3] + P[17][0]*SH_BETA[7]*SK_BETA[5] + P[17][1]*SH_BETA[1]*SH_BETA[4]*SK_BETA[5]); -Kfusion[18] = SK_BETA[0]*(P[18][5]*SK_BETA[1] - P[18][2]*SK_BETA[4] - P[18][3]*SK_BETA[2] + P[18][4]*SK_BETA[3] + P[18][22]*SK_BETA[2] - P[18][23]*SK_BETA[3] + P[18][0]*SH_BETA[7]*SK_BETA[5] + P[18][1]*SH_BETA[1]*SH_BETA[4]*SK_BETA[5]); -Kfusion[19] = SK_BETA[0]*(P[19][5]*SK_BETA[1] - P[19][2]*SK_BETA[4] - P[19][3]*SK_BETA[2] + P[19][4]*SK_BETA[3] + P[19][22]*SK_BETA[2] - P[19][23]*SK_BETA[3] + P[19][0]*SH_BETA[7]*SK_BETA[5] + P[19][1]*SH_BETA[1]*SH_BETA[4]*SK_BETA[5]); -Kfusion[20] = SK_BETA[0]*(P[20][5]*SK_BETA[1] - P[20][2]*SK_BETA[4] - P[20][3]*SK_BETA[2] + P[20][4]*SK_BETA[3] + P[20][22]*SK_BETA[2] - P[20][23]*SK_BETA[3] + P[20][0]*SH_BETA[7]*SK_BETA[5] + P[20][1]*SH_BETA[1]*SH_BETA[4]*SK_BETA[5]); -Kfusion[21] = SK_BETA[0]*(P[21][5]*SK_BETA[1] - P[21][2]*SK_BETA[4] - P[21][3]*SK_BETA[2] + P[21][4]*SK_BETA[3] + P[21][22]*SK_BETA[2] - P[21][23]*SK_BETA[3] + P[21][0]*SH_BETA[7]*SK_BETA[5] + P[21][1]*SH_BETA[1]*SH_BETA[4]*SK_BETA[5]); -Kfusion[22] = SK_BETA[0]*(P[22][5]*SK_BETA[1] - P[22][2]*SK_BETA[4] - P[22][3]*SK_BETA[2] + P[22][4]*SK_BETA[3] + P[22][22]*SK_BETA[2] - P[22][23]*SK_BETA[3] + P[22][0]*SH_BETA[7]*SK_BETA[5] + P[22][1]*SH_BETA[1]*SH_BETA[4]*SK_BETA[5]); -Kfusion[23] = SK_BETA[0]*(P[23][5]*SK_BETA[1] - P[23][2]*SK_BETA[4] - P[23][3]*SK_BETA[2] + P[23][4]*SK_BETA[3] + P[23][22]*SK_BETA[2] - P[23][23]*SK_BETA[3] + P[23][0]*SH_BETA[7]*SK_BETA[5] + P[23][1]*SH_BETA[1]*SH_BETA[4]*SK_BETA[5]); +Kfusion[0] = SK_BETA[0]*(P[0][0]*SK_BETA[5] + P[0][1]*SK_BETA[4] - P[0][4]*SK_BETA[1] + P[0][5]*SK_BETA[2] + P[0][2]*SK_BETA[6] + P[0][6]*SK_BETA[3] - P[0][3]*SK_BETA[7] + P[0][22]*SK_BETA[1] - P[0][23]*SK_BETA[2]); +Kfusion[1] = SK_BETA[0]*(P[1][0]*SK_BETA[5] + P[1][1]*SK_BETA[4] - P[1][4]*SK_BETA[1] + P[1][5]*SK_BETA[2] + P[1][2]*SK_BETA[6] + P[1][6]*SK_BETA[3] - P[1][3]*SK_BETA[7] + P[1][22]*SK_BETA[1] - P[1][23]*SK_BETA[2]); +Kfusion[2] = SK_BETA[0]*(P[2][0]*SK_BETA[5] + P[2][1]*SK_BETA[4] - P[2][4]*SK_BETA[1] + P[2][5]*SK_BETA[2] + P[2][2]*SK_BETA[6] + P[2][6]*SK_BETA[3] - P[2][3]*SK_BETA[7] + P[2][22]*SK_BETA[1] - P[2][23]*SK_BETA[2]); +Kfusion[3] = SK_BETA[0]*(P[3][0]*SK_BETA[5] + P[3][1]*SK_BETA[4] - P[3][4]*SK_BETA[1] + P[3][5]*SK_BETA[2] + P[3][2]*SK_BETA[6] + P[3][6]*SK_BETA[3] - P[3][3]*SK_BETA[7] + P[3][22]*SK_BETA[1] - P[3][23]*SK_BETA[2]); +Kfusion[4] = SK_BETA[0]*(P[4][0]*SK_BETA[5] + P[4][1]*SK_BETA[4] - P[4][4]*SK_BETA[1] + P[4][5]*SK_BETA[2] + P[4][2]*SK_BETA[6] + P[4][6]*SK_BETA[3] - P[4][3]*SK_BETA[7] + P[4][22]*SK_BETA[1] - P[4][23]*SK_BETA[2]); +Kfusion[5] = SK_BETA[0]*(P[5][0]*SK_BETA[5] + P[5][1]*SK_BETA[4] - P[5][4]*SK_BETA[1] + P[5][5]*SK_BETA[2] + P[5][2]*SK_BETA[6] + P[5][6]*SK_BETA[3] - P[5][3]*SK_BETA[7] + P[5][22]*SK_BETA[1] - P[5][23]*SK_BETA[2]); +Kfusion[6] = SK_BETA[0]*(P[6][0]*SK_BETA[5] + P[6][1]*SK_BETA[4] - P[6][4]*SK_BETA[1] + P[6][5]*SK_BETA[2] + P[6][2]*SK_BETA[6] + P[6][6]*SK_BETA[3] - P[6][3]*SK_BETA[7] + P[6][22]*SK_BETA[1] - P[6][23]*SK_BETA[2]); +Kfusion[7] = SK_BETA[0]*(P[7][0]*SK_BETA[5] + P[7][1]*SK_BETA[4] - P[7][4]*SK_BETA[1] + P[7][5]*SK_BETA[2] + P[7][2]*SK_BETA[6] + P[7][6]*SK_BETA[3] - P[7][3]*SK_BETA[7] + P[7][22]*SK_BETA[1] - P[7][23]*SK_BETA[2]); +Kfusion[8] = SK_BETA[0]*(P[8][0]*SK_BETA[5] + P[8][1]*SK_BETA[4] - P[8][4]*SK_BETA[1] + P[8][5]*SK_BETA[2] + P[8][2]*SK_BETA[6] + P[8][6]*SK_BETA[3] - P[8][3]*SK_BETA[7] + P[8][22]*SK_BETA[1] - P[8][23]*SK_BETA[2]); +Kfusion[9] = SK_BETA[0]*(P[9][0]*SK_BETA[5] + P[9][1]*SK_BETA[4] - P[9][4]*SK_BETA[1] + P[9][5]*SK_BETA[2] + P[9][2]*SK_BETA[6] + P[9][6]*SK_BETA[3] - P[9][3]*SK_BETA[7] + P[9][22]*SK_BETA[1] - P[9][23]*SK_BETA[2]); +Kfusion[10] = SK_BETA[0]*(P[10][0]*SK_BETA[5] + P[10][1]*SK_BETA[4] - P[10][4]*SK_BETA[1] + P[10][5]*SK_BETA[2] + P[10][2]*SK_BETA[6] + P[10][6]*SK_BETA[3] - P[10][3]*SK_BETA[7] + P[10][22]*SK_BETA[1] - P[10][23]*SK_BETA[2]); +Kfusion[11] = SK_BETA[0]*(P[11][0]*SK_BETA[5] + P[11][1]*SK_BETA[4] - P[11][4]*SK_BETA[1] + P[11][5]*SK_BETA[2] + P[11][2]*SK_BETA[6] + P[11][6]*SK_BETA[3] - P[11][3]*SK_BETA[7] + P[11][22]*SK_BETA[1] - P[11][23]*SK_BETA[2]); +Kfusion[12] = SK_BETA[0]*(P[12][0]*SK_BETA[5] + P[12][1]*SK_BETA[4] - P[12][4]*SK_BETA[1] + P[12][5]*SK_BETA[2] + P[12][2]*SK_BETA[6] + P[12][6]*SK_BETA[3] - P[12][3]*SK_BETA[7] + P[12][22]*SK_BETA[1] - P[12][23]*SK_BETA[2]); +Kfusion[13] = SK_BETA[0]*(P[13][0]*SK_BETA[5] + P[13][1]*SK_BETA[4] - P[13][4]*SK_BETA[1] + P[13][5]*SK_BETA[2] + P[13][2]*SK_BETA[6] + P[13][6]*SK_BETA[3] - P[13][3]*SK_BETA[7] + P[13][22]*SK_BETA[1] - P[13][23]*SK_BETA[2]); +Kfusion[14] = SK_BETA[0]*(P[14][0]*SK_BETA[5] + P[14][1]*SK_BETA[4] - P[14][4]*SK_BETA[1] + P[14][5]*SK_BETA[2] + P[14][2]*SK_BETA[6] + P[14][6]*SK_BETA[3] - P[14][3]*SK_BETA[7] + P[14][22]*SK_BETA[1] - P[14][23]*SK_BETA[2]); +Kfusion[15] = SK_BETA[0]*(P[15][0]*SK_BETA[5] + P[15][1]*SK_BETA[4] - P[15][4]*SK_BETA[1] + P[15][5]*SK_BETA[2] + P[15][2]*SK_BETA[6] + P[15][6]*SK_BETA[3] - P[15][3]*SK_BETA[7] + P[15][22]*SK_BETA[1] - P[15][23]*SK_BETA[2]); +Kfusion[16] = SK_BETA[0]*(P[16][0]*SK_BETA[5] + P[16][1]*SK_BETA[4] - P[16][4]*SK_BETA[1] + P[16][5]*SK_BETA[2] + P[16][2]*SK_BETA[6] + P[16][6]*SK_BETA[3] - P[16][3]*SK_BETA[7] + P[16][22]*SK_BETA[1] - P[16][23]*SK_BETA[2]); +Kfusion[17] = SK_BETA[0]*(P[17][0]*SK_BETA[5] + P[17][1]*SK_BETA[4] - P[17][4]*SK_BETA[1] + P[17][5]*SK_BETA[2] + P[17][2]*SK_BETA[6] + P[17][6]*SK_BETA[3] - P[17][3]*SK_BETA[7] + P[17][22]*SK_BETA[1] - P[17][23]*SK_BETA[2]); +Kfusion[18] = SK_BETA[0]*(P[18][0]*SK_BETA[5] + P[18][1]*SK_BETA[4] - P[18][4]*SK_BETA[1] + P[18][5]*SK_BETA[2] + P[18][2]*SK_BETA[6] + P[18][6]*SK_BETA[3] - P[18][3]*SK_BETA[7] + P[18][22]*SK_BETA[1] - P[18][23]*SK_BETA[2]); +Kfusion[19] = SK_BETA[0]*(P[19][0]*SK_BETA[5] + P[19][1]*SK_BETA[4] - P[19][4]*SK_BETA[1] + P[19][5]*SK_BETA[2] + P[19][2]*SK_BETA[6] + P[19][6]*SK_BETA[3] - P[19][3]*SK_BETA[7] + P[19][22]*SK_BETA[1] - P[19][23]*SK_BETA[2]); +Kfusion[20] = SK_BETA[0]*(P[20][0]*SK_BETA[5] + P[20][1]*SK_BETA[4] - P[20][4]*SK_BETA[1] + P[20][5]*SK_BETA[2] + P[20][2]*SK_BETA[6] + P[20][6]*SK_BETA[3] - P[20][3]*SK_BETA[7] + P[20][22]*SK_BETA[1] - P[20][23]*SK_BETA[2]); +Kfusion[21] = SK_BETA[0]*(P[21][0]*SK_BETA[5] + P[21][1]*SK_BETA[4] - P[21][4]*SK_BETA[1] + P[21][5]*SK_BETA[2] + P[21][2]*SK_BETA[6] + P[21][6]*SK_BETA[3] - P[21][3]*SK_BETA[7] + P[21][22]*SK_BETA[1] - P[21][23]*SK_BETA[2]); +Kfusion[22] = SK_BETA[0]*(P[22][0]*SK_BETA[5] + P[22][1]*SK_BETA[4] - P[22][4]*SK_BETA[1] + P[22][5]*SK_BETA[2] + P[22][2]*SK_BETA[6] + P[22][6]*SK_BETA[3] - P[22][3]*SK_BETA[7] + P[22][22]*SK_BETA[1] - P[22][23]*SK_BETA[2]); +Kfusion[23] = SK_BETA[0]*(P[23][0]*SK_BETA[5] + P[23][1]*SK_BETA[4] - P[23][4]*SK_BETA[1] + P[23][5]*SK_BETA[2] + P[23][2]*SK_BETA[6] + P[23][6]*SK_BETA[3] - P[23][3]*SK_BETA[7] + P[23][22]*SK_BETA[1] - P[23][23]*SK_BETA[2]); float SH_MAG[9][1]; -SH_MAG[0] = sq(q0) - sq(q1) + sq(q2) - sq(q3); -SH_MAG[1] = sq(q0) + sq(q1) - sq(q2) - sq(q3); -SH_MAG[2] = sq(q0) - sq(q1) - sq(q2) + sq(q3); -SH_MAG[3] = 2*q0*q1 + 2*q2*q3; -SH_MAG[4] = 2*q0*q3 + 2*q1*q2; -SH_MAG[5] = 2*q0*q2 + 2*q1*q3; -SH_MAG[6] = magE*(2*q0*q1 - 2*q2*q3); -SH_MAG[7] = 2*q1*q3 - 2*q0*q2; -SH_MAG[8] = 2*q0*q3; +SH_MAG[0] = 2*magD*q3 + 2*magE*q2 + 2*magN*q1; +SH_MAG[1] = 2*magD*q0 - 2*magE*q1 + 2*magN*q2; +SH_MAG[2] = 2*magD*q1 + 2*magE*q0 - 2*magN*q3; +SH_MAG[3] = sq(q3); +SH_MAG[4] = sq(q2); +SH_MAG[5] = sq(q1); +SH_MAG[6] = sq(q0); +SH_MAG[7] = 2*magN*q0; +SH_MAG[8] = 2*magE*q3; float H_MAG[1][24]; -H_MAG[1] = SH_MAG[6] - magD*SH_MAG[2] - magN*SH_MAG[5]; -H_MAG[2] = magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2); -H_MAG[16] = SH_MAG[1]; -H_MAG[17] = SH_MAG[4]; -H_MAG[18] = SH_MAG[7]; +H_MAG[0] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; +H_MAG[1] = SH_MAG[0]; +H_MAG[2] = -SH_MAG[1]; +H_MAG[3] = SH_MAG[2]; +H_MAG[16] = SH_MAG[5] - SH_MAG[4] - SH_MAG[3] + SH_MAG[6]; +H_MAG[17] = 2*q0*q3 + 2*q1*q2; +H_MAG[18] = 2*q1*q3 - 2*q0*q2; H_MAG[19] = 1; -float SK_MX[4][1]; -SK_MX[0] = 1/(P[19][19] + R_MAG - P[1][19]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][19]*SH_MAG[1] + P[17][19]*SH_MAG[4] + P[18][19]*SH_MAG[7] + P[2][19]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2)) - (magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5])*(P[19][1] - P[1][1]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][1]*SH_MAG[1] + P[17][1]*SH_MAG[4] + P[18][1]*SH_MAG[7] + P[2][1]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2))) + SH_MAG[1]*(P[19][16] - P[1][16]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][16]*SH_MAG[1] + P[17][16]*SH_MAG[4] + P[18][16]*SH_MAG[7] + P[2][16]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2))) + SH_MAG[4]*(P[19][17] - P[1][17]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][17]*SH_MAG[1] + P[17][17]*SH_MAG[4] + P[18][17]*SH_MAG[7] + P[2][17]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2))) + SH_MAG[7]*(P[19][18] - P[1][18]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][18]*SH_MAG[1] + P[17][18]*SH_MAG[4] + P[18][18]*SH_MAG[7] + P[2][18]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2))) + (magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2))*(P[19][2] - P[1][2]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][2]*SH_MAG[1] + P[17][2]*SH_MAG[4] + P[18][2]*SH_MAG[7] + P[2][2]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2)))); -SK_MX[1] = magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2); -SK_MX[2] = magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]; -SK_MX[3] = SH_MAG[7]; +float SK_MX[5][1]; +SK_MX[0] = 1/(P[19][19] + R_MAG + P[1][19]*SH_MAG[0] - P[2][19]*SH_MAG[1] + P[3][19]*SH_MAG[2] - P[16][19]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + (2*q0*q3 + 2*q1*q2)*(P[19][17] + P[1][17]*SH_MAG[0] - P[2][17]*SH_MAG[1] + P[3][17]*SH_MAG[2] - P[16][17]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][17]*(2*q0*q3 + 2*q1*q2) - P[18][17]*(2*q0*q2 - 2*q1*q3) + P[0][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q2 - 2*q1*q3)*(P[19][18] + P[1][18]*SH_MAG[0] - P[2][18]*SH_MAG[1] + P[3][18]*SH_MAG[2] - P[16][18]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][18]*(2*q0*q3 + 2*q1*q2) - P[18][18]*(2*q0*q2 - 2*q1*q3) + P[0][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[19][0] + P[1][0]*SH_MAG[0] - P[2][0]*SH_MAG[1] + P[3][0]*SH_MAG[2] - P[16][0]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][0]*(2*q0*q3 + 2*q1*q2) - P[18][0]*(2*q0*q2 - 2*q1*q3) + P[0][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[17][19]*(2*q0*q3 + 2*q1*q2) - P[18][19]*(2*q0*q2 - 2*q1*q3) + SH_MAG[0]*(P[19][1] + P[1][1]*SH_MAG[0] - P[2][1]*SH_MAG[1] + P[3][1]*SH_MAG[2] - P[16][1]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][1]*(2*q0*q3 + 2*q1*q2) - P[18][1]*(2*q0*q2 - 2*q1*q3) + P[0][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - SH_MAG[1]*(P[19][2] + P[1][2]*SH_MAG[0] - P[2][2]*SH_MAG[1] + P[3][2]*SH_MAG[2] - P[16][2]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][2]*(2*q0*q3 + 2*q1*q2) - P[18][2]*(2*q0*q2 - 2*q1*q3) + P[0][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[2]*(P[19][3] + P[1][3]*SH_MAG[0] - P[2][3]*SH_MAG[1] + P[3][3]*SH_MAG[2] - P[16][3]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][3]*(2*q0*q3 + 2*q1*q2) - P[18][3]*(2*q0*q2 - 2*q1*q3) + P[0][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P[19][16] + P[1][16]*SH_MAG[0] - P[2][16]*SH_MAG[1] + P[3][16]*SH_MAG[2] - P[16][16]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][16]*(2*q0*q3 + 2*q1*q2) - P[18][16]*(2*q0*q2 - 2*q1*q3) + P[0][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[0][19]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); +SK_MX[1] = SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]; +SK_MX[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; +SK_MX[3] = 2*q0*q2 - 2*q1*q3; +SK_MX[4] = 2*q0*q3 + 2*q1*q2; float Kfusion[24][1]; float Kfusion[1][1]; -Kfusion[0] = SK_MX[0]*(P[0][19] + P[0][16]*SH_MAG[1] + P[0][17]*SH_MAG[4] - P[0][1]*SK_MX[2] + P[0][2]*SK_MX[1] + P[0][18]*SK_MX[3]); -Kfusion[1] = SK_MX[0]*(P[1][19] + P[1][16]*SH_MAG[1] + P[1][17]*SH_MAG[4] - P[1][1]*SK_MX[2] + P[1][2]*SK_MX[1] + P[1][18]*SK_MX[3]); -Kfusion[2] = SK_MX[0]*(P[2][19] + P[2][16]*SH_MAG[1] + P[2][17]*SH_MAG[4] - P[2][1]*SK_MX[2] + P[2][2]*SK_MX[1] + P[2][18]*SK_MX[3]); -Kfusion[3] = SK_MX[0]*(P[3][19] + P[3][16]*SH_MAG[1] + P[3][17]*SH_MAG[4] - P[3][1]*SK_MX[2] + P[3][2]*SK_MX[1] + P[3][18]*SK_MX[3]); -Kfusion[4] = SK_MX[0]*(P[4][19] + P[4][16]*SH_MAG[1] + P[4][17]*SH_MAG[4] - P[4][1]*SK_MX[2] + P[4][2]*SK_MX[1] + P[4][18]*SK_MX[3]); -Kfusion[5] = SK_MX[0]*(P[5][19] + P[5][16]*SH_MAG[1] + P[5][17]*SH_MAG[4] - P[5][1]*SK_MX[2] + P[5][2]*SK_MX[1] + P[5][18]*SK_MX[3]); -Kfusion[6] = SK_MX[0]*(P[6][19] + P[6][16]*SH_MAG[1] + P[6][17]*SH_MAG[4] - P[6][1]*SK_MX[2] + P[6][2]*SK_MX[1] + P[6][18]*SK_MX[3]); -Kfusion[7] = SK_MX[0]*(P[7][19] + P[7][16]*SH_MAG[1] + P[7][17]*SH_MAG[4] - P[7][1]*SK_MX[2] + P[7][2]*SK_MX[1] + P[7][18]*SK_MX[3]); -Kfusion[8] = SK_MX[0]*(P[8][19] + P[8][16]*SH_MAG[1] + P[8][17]*SH_MAG[4] - P[8][1]*SK_MX[2] + P[8][2]*SK_MX[1] + P[8][18]*SK_MX[3]); -Kfusion[9] = SK_MX[0]*(P[9][19] + P[9][16]*SH_MAG[1] + P[9][17]*SH_MAG[4] - P[9][1]*SK_MX[2] + P[9][2]*SK_MX[1] + P[9][18]*SK_MX[3]); -Kfusion[10] = SK_MX[0]*(P[10][19] + P[10][16]*SH_MAG[1] + P[10][17]*SH_MAG[4] - P[10][1]*SK_MX[2] + P[10][2]*SK_MX[1] + P[10][18]*SK_MX[3]); -Kfusion[11] = SK_MX[0]*(P[11][19] + P[11][16]*SH_MAG[1] + P[11][17]*SH_MAG[4] - P[11][1]*SK_MX[2] + P[11][2]*SK_MX[1] + P[11][18]*SK_MX[3]); -Kfusion[12] = SK_MX[0]*(P[12][19] + P[12][16]*SH_MAG[1] + P[12][17]*SH_MAG[4] - P[12][1]*SK_MX[2] + P[12][2]*SK_MX[1] + P[12][18]*SK_MX[3]); -Kfusion[13] = SK_MX[0]*(P[13][19] + P[13][16]*SH_MAG[1] + P[13][17]*SH_MAG[4] - P[13][1]*SK_MX[2] + P[13][2]*SK_MX[1] + P[13][18]*SK_MX[3]); -Kfusion[14] = SK_MX[0]*(P[14][19] + P[14][16]*SH_MAG[1] + P[14][17]*SH_MAG[4] - P[14][1]*SK_MX[2] + P[14][2]*SK_MX[1] + P[14][18]*SK_MX[3]); -Kfusion[15] = SK_MX[0]*(P[15][19] + P[15][16]*SH_MAG[1] + P[15][17]*SH_MAG[4] - P[15][1]*SK_MX[2] + P[15][2]*SK_MX[1] + P[15][18]*SK_MX[3]); -Kfusion[16] = SK_MX[0]*(P[16][19] + P[16][16]*SH_MAG[1] + P[16][17]*SH_MAG[4] - P[16][1]*SK_MX[2] + P[16][2]*SK_MX[1] + P[16][18]*SK_MX[3]); -Kfusion[17] = SK_MX[0]*(P[17][19] + P[17][16]*SH_MAG[1] + P[17][17]*SH_MAG[4] - P[17][1]*SK_MX[2] + P[17][2]*SK_MX[1] + P[17][18]*SK_MX[3]); -Kfusion[18] = SK_MX[0]*(P[18][19] + P[18][16]*SH_MAG[1] + P[18][17]*SH_MAG[4] - P[18][1]*SK_MX[2] + P[18][2]*SK_MX[1] + P[18][18]*SK_MX[3]); -Kfusion[19] = SK_MX[0]*(P[19][19] + P[19][16]*SH_MAG[1] + P[19][17]*SH_MAG[4] - P[19][1]*SK_MX[2] + P[19][2]*SK_MX[1] + P[19][18]*SK_MX[3]); -Kfusion[20] = SK_MX[0]*(P[20][19] + P[20][16]*SH_MAG[1] + P[20][17]*SH_MAG[4] - P[20][1]*SK_MX[2] + P[20][2]*SK_MX[1] + P[20][18]*SK_MX[3]); -Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][16]*SH_MAG[1] + P[21][17]*SH_MAG[4] - P[21][1]*SK_MX[2] + P[21][2]*SK_MX[1] + P[21][18]*SK_MX[3]); -Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][16]*SH_MAG[1] + P[22][17]*SH_MAG[4] - P[22][1]*SK_MX[2] + P[22][2]*SK_MX[1] + P[22][18]*SK_MX[3]); -Kfusion[23] = SK_MX[0]*(P[23][19] + P[23][16]*SH_MAG[1] + P[23][17]*SH_MAG[4] - P[23][1]*SK_MX[2] + P[23][2]*SK_MX[1] + P[23][18]*SK_MX[3]); +Kfusion[0] = SK_MX[0]*(P[0][19] + P[0][1]*SH_MAG[0] - P[0][2]*SH_MAG[1] + P[0][3]*SH_MAG[2] + P[0][0]*SK_MX[2] - P[0][16]*SK_MX[1] + P[0][17]*SK_MX[4] - P[0][18]*SK_MX[3]); +Kfusion[1] = SK_MX[0]*(P[1][19] + P[1][1]*SH_MAG[0] - P[1][2]*SH_MAG[1] + P[1][3]*SH_MAG[2] + P[1][0]*SK_MX[2] - P[1][16]*SK_MX[1] + P[1][17]*SK_MX[4] - P[1][18]*SK_MX[3]); +Kfusion[2] = SK_MX[0]*(P[2][19] + P[2][1]*SH_MAG[0] - P[2][2]*SH_MAG[1] + P[2][3]*SH_MAG[2] + P[2][0]*SK_MX[2] - P[2][16]*SK_MX[1] + P[2][17]*SK_MX[4] - P[2][18]*SK_MX[3]); +Kfusion[3] = SK_MX[0]*(P[3][19] + P[3][1]*SH_MAG[0] - P[3][2]*SH_MAG[1] + P[3][3]*SH_MAG[2] + P[3][0]*SK_MX[2] - P[3][16]*SK_MX[1] + P[3][17]*SK_MX[4] - P[3][18]*SK_MX[3]); +Kfusion[4] = SK_MX[0]*(P[4][19] + P[4][1]*SH_MAG[0] - P[4][2]*SH_MAG[1] + P[4][3]*SH_MAG[2] + P[4][0]*SK_MX[2] - P[4][16]*SK_MX[1] + P[4][17]*SK_MX[4] - P[4][18]*SK_MX[3]); +Kfusion[5] = SK_MX[0]*(P[5][19] + P[5][1]*SH_MAG[0] - P[5][2]*SH_MAG[1] + P[5][3]*SH_MAG[2] + P[5][0]*SK_MX[2] - P[5][16]*SK_MX[1] + P[5][17]*SK_MX[4] - P[5][18]*SK_MX[3]); +Kfusion[6] = SK_MX[0]*(P[6][19] + P[6][1]*SH_MAG[0] - P[6][2]*SH_MAG[1] + P[6][3]*SH_MAG[2] + P[6][0]*SK_MX[2] - P[6][16]*SK_MX[1] + P[6][17]*SK_MX[4] - P[6][18]*SK_MX[3]); +Kfusion[7] = SK_MX[0]*(P[7][19] + P[7][1]*SH_MAG[0] - P[7][2]*SH_MAG[1] + P[7][3]*SH_MAG[2] + P[7][0]*SK_MX[2] - P[7][16]*SK_MX[1] + P[7][17]*SK_MX[4] - P[7][18]*SK_MX[3]); +Kfusion[8] = SK_MX[0]*(P[8][19] + P[8][1]*SH_MAG[0] - P[8][2]*SH_MAG[1] + P[8][3]*SH_MAG[2] + P[8][0]*SK_MX[2] - P[8][16]*SK_MX[1] + P[8][17]*SK_MX[4] - P[8][18]*SK_MX[3]); +Kfusion[9] = SK_MX[0]*(P[9][19] + P[9][1]*SH_MAG[0] - P[9][2]*SH_MAG[1] + P[9][3]*SH_MAG[2] + P[9][0]*SK_MX[2] - P[9][16]*SK_MX[1] + P[9][17]*SK_MX[4] - P[9][18]*SK_MX[3]); +Kfusion[10] = SK_MX[0]*(P[10][19] + P[10][1]*SH_MAG[0] - P[10][2]*SH_MAG[1] + P[10][3]*SH_MAG[2] + P[10][0]*SK_MX[2] - P[10][16]*SK_MX[1] + P[10][17]*SK_MX[4] - P[10][18]*SK_MX[3]); +Kfusion[11] = SK_MX[0]*(P[11][19] + P[11][1]*SH_MAG[0] - P[11][2]*SH_MAG[1] + P[11][3]*SH_MAG[2] + P[11][0]*SK_MX[2] - P[11][16]*SK_MX[1] + P[11][17]*SK_MX[4] - P[11][18]*SK_MX[3]); +Kfusion[12] = SK_MX[0]*(P[12][19] + P[12][1]*SH_MAG[0] - P[12][2]*SH_MAG[1] + P[12][3]*SH_MAG[2] + P[12][0]*SK_MX[2] - P[12][16]*SK_MX[1] + P[12][17]*SK_MX[4] - P[12][18]*SK_MX[3]); +Kfusion[13] = SK_MX[0]*(P[13][19] + P[13][1]*SH_MAG[0] - P[13][2]*SH_MAG[1] + P[13][3]*SH_MAG[2] + P[13][0]*SK_MX[2] - P[13][16]*SK_MX[1] + P[13][17]*SK_MX[4] - P[13][18]*SK_MX[3]); +Kfusion[14] = SK_MX[0]*(P[14][19] + P[14][1]*SH_MAG[0] - P[14][2]*SH_MAG[1] + P[14][3]*SH_MAG[2] + P[14][0]*SK_MX[2] - P[14][16]*SK_MX[1] + P[14][17]*SK_MX[4] - P[14][18]*SK_MX[3]); +Kfusion[15] = SK_MX[0]*(P[15][19] + P[15][1]*SH_MAG[0] - P[15][2]*SH_MAG[1] + P[15][3]*SH_MAG[2] + P[15][0]*SK_MX[2] - P[15][16]*SK_MX[1] + P[15][17]*SK_MX[4] - P[15][18]*SK_MX[3]); +Kfusion[16] = SK_MX[0]*(P[16][19] + P[16][1]*SH_MAG[0] - P[16][2]*SH_MAG[1] + P[16][3]*SH_MAG[2] + P[16][0]*SK_MX[2] - P[16][16]*SK_MX[1] + P[16][17]*SK_MX[4] - P[16][18]*SK_MX[3]); +Kfusion[17] = SK_MX[0]*(P[17][19] + P[17][1]*SH_MAG[0] - P[17][2]*SH_MAG[1] + P[17][3]*SH_MAG[2] + P[17][0]*SK_MX[2] - P[17][16]*SK_MX[1] + P[17][17]*SK_MX[4] - P[17][18]*SK_MX[3]); +Kfusion[18] = SK_MX[0]*(P[18][19] + P[18][1]*SH_MAG[0] - P[18][2]*SH_MAG[1] + P[18][3]*SH_MAG[2] + P[18][0]*SK_MX[2] - P[18][16]*SK_MX[1] + P[18][17]*SK_MX[4] - P[18][18]*SK_MX[3]); +Kfusion[19] = SK_MX[0]*(P[19][19] + P[19][1]*SH_MAG[0] - P[19][2]*SH_MAG[1] + P[19][3]*SH_MAG[2] + P[19][0]*SK_MX[2] - P[19][16]*SK_MX[1] + P[19][17]*SK_MX[4] - P[19][18]*SK_MX[3]); +Kfusion[20] = SK_MX[0]*(P[20][19] + P[20][1]*SH_MAG[0] - P[20][2]*SH_MAG[1] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[2] - P[20][16]*SK_MX[1] + P[20][17]*SK_MX[4] - P[20][18]*SK_MX[3]); +Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][1]*SH_MAG[0] - P[21][2]*SH_MAG[1] + P[21][3]*SH_MAG[2] + P[21][0]*SK_MX[2] - P[21][16]*SK_MX[1] + P[21][17]*SK_MX[4] - P[21][18]*SK_MX[3]); +Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] - P[22][2]*SH_MAG[1] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[4] - P[22][18]*SK_MX[3]); +Kfusion[23] = SK_MX[0]*(P[23][19] + P[23][1]*SH_MAG[0] - P[23][2]*SH_MAG[1] + P[23][3]*SH_MAG[2] + P[23][0]*SK_MX[2] - P[23][16]*SK_MX[1] + P[23][17]*SK_MX[4] - P[23][18]*SK_MX[3]); float H_MAG[1][24]; -H_MAG[0] = magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]; -H_MAG[2] = - magE*SH_MAG[4] - magD*SH_MAG[7] - magN*SH_MAG[1]; -H_MAG[16] = 2*q1*q2 - SH_MAG[8]; -H_MAG[17] = SH_MAG[0]; -H_MAG[18] = SH_MAG[3]; +H_MAG[0] = SH_MAG[2]; +H_MAG[1] = SH_MAG[1]; +H_MAG[2] = SH_MAG[0]; +H_MAG[3] = 2*magD*q2 - SH_MAG[8] - SH_MAG[7]; +H_MAG[16] = 2*q1*q2 - 2*q0*q3; +H_MAG[17] = SH_MAG[4] - SH_MAG[3] - SH_MAG[5] + SH_MAG[6]; +H_MAG[18] = 2*q0*q1 + 2*q2*q3; H_MAG[20] = 1; -float SK_MY[4][1]; -SK_MY[0] = 1/(P[20][20] + R_MAG + P[0][20]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][20]*SH_MAG[0] + P[18][20]*SH_MAG[3] - (SH_MAG[8] - 2*q1*q2)*(P[20][16] + P[0][16]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][16]*SH_MAG[0] + P[18][16]*SH_MAG[3] - P[2][16]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][16]*(SH_MAG[8] - 2*q1*q2)) - P[2][20]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) + (magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5])*(P[20][0] + P[0][0]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][0]*SH_MAG[0] + P[18][0]*SH_MAG[3] - P[2][0]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][0]*(SH_MAG[8] - 2*q1*q2)) + SH_MAG[0]*(P[20][17] + P[0][17]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][17]*SH_MAG[0] + P[18][17]*SH_MAG[3] - P[2][17]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][17]*(SH_MAG[8] - 2*q1*q2)) + SH_MAG[3]*(P[20][18] + P[0][18]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][18]*SH_MAG[0] + P[18][18]*SH_MAG[3] - P[2][18]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][18]*(SH_MAG[8] - 2*q1*q2)) - P[16][20]*(SH_MAG[8] - 2*q1*q2) - (magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1])*(P[20][2] + P[0][2]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][2]*SH_MAG[0] + P[18][2]*SH_MAG[3] - P[2][2]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][2]*(SH_MAG[8] - 2*q1*q2))); -SK_MY[1] = magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]; -SK_MY[2] = magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]; -SK_MY[3] = SH_MAG[8] - 2*q1*q2; +float SK_MY[5][1]; +SK_MY[0] = 1/(P[20][20] + R_MAG + P[0][20]*SH_MAG[2] + P[1][20]*SH_MAG[1] + P[2][20]*SH_MAG[0] - P[17][20]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - (2*q0*q3 - 2*q1*q2)*(P[20][16] + P[0][16]*SH_MAG[2] + P[1][16]*SH_MAG[1] + P[2][16]*SH_MAG[0] - P[17][16]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][16]*(2*q0*q3 - 2*q1*q2) + P[18][16]*(2*q0*q1 + 2*q2*q3) - P[3][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (2*q0*q1 + 2*q2*q3)*(P[20][18] + P[0][18]*SH_MAG[2] + P[1][18]*SH_MAG[1] + P[2][18]*SH_MAG[0] - P[17][18]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][18]*(2*q0*q3 - 2*q1*q2) + P[18][18]*(2*q0*q1 + 2*q2*q3) - P[3][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[20][3] + P[0][3]*SH_MAG[2] + P[1][3]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[17][3]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][3]*(2*q0*q3 - 2*q1*q2) + P[18][3]*(2*q0*q1 + 2*q2*q3) - P[3][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[16][20]*(2*q0*q3 - 2*q1*q2) + P[18][20]*(2*q0*q1 + 2*q2*q3) + SH_MAG[2]*(P[20][0] + P[0][0]*SH_MAG[2] + P[1][0]*SH_MAG[1] + P[2][0]*SH_MAG[0] - P[17][0]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][0]*(2*q0*q3 - 2*q1*q2) + P[18][0]*(2*q0*q1 + 2*q2*q3) - P[3][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[20][1] + P[0][1]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[2][1]*SH_MAG[0] - P[17][1]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][1]*(2*q0*q3 - 2*q1*q2) + P[18][1]*(2*q0*q1 + 2*q2*q3) - P[3][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[20][2] + P[0][2]*SH_MAG[2] + P[1][2]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[17][2]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][2]*(2*q0*q3 - 2*q1*q2) + P[18][2]*(2*q0*q1 + 2*q2*q3) - P[3][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6])*(P[20][17] + P[0][17]*SH_MAG[2] + P[1][17]*SH_MAG[1] + P[2][17]*SH_MAG[0] - P[17][17]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][17]*(2*q0*q3 - 2*q1*q2) + P[18][17]*(2*q0*q1 + 2*q2*q3) - P[3][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[3][20]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); +SK_MY[1] = SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]; +SK_MY[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; +SK_MY[3] = 2*q0*q3 - 2*q1*q2; +SK_MY[4] = 2*q0*q1 + 2*q2*q3; float Kfusion[24][1]; float Kfusion[1][1]; -Kfusion[0] = SK_MY[0]*(P[0][20] + P[0][17]*SH_MAG[0] + P[0][18]*SH_MAG[3] + P[0][0]*SK_MY[2] - P[0][2]*SK_MY[1] - P[0][16]*SK_MY[3]); -Kfusion[1] = SK_MY[0]*(P[1][20] + P[1][17]*SH_MAG[0] + P[1][18]*SH_MAG[3] + P[1][0]*SK_MY[2] - P[1][2]*SK_MY[1] - P[1][16]*SK_MY[3]); -Kfusion[2] = SK_MY[0]*(P[2][20] + P[2][17]*SH_MAG[0] + P[2][18]*SH_MAG[3] + P[2][0]*SK_MY[2] - P[2][2]*SK_MY[1] - P[2][16]*SK_MY[3]); -Kfusion[3] = SK_MY[0]*(P[3][20] + P[3][17]*SH_MAG[0] + P[3][18]*SH_MAG[3] + P[3][0]*SK_MY[2] - P[3][2]*SK_MY[1] - P[3][16]*SK_MY[3]); -Kfusion[4] = SK_MY[0]*(P[4][20] + P[4][17]*SH_MAG[0] + P[4][18]*SH_MAG[3] + P[4][0]*SK_MY[2] - P[4][2]*SK_MY[1] - P[4][16]*SK_MY[3]); -Kfusion[5] = SK_MY[0]*(P[5][20] + P[5][17]*SH_MAG[0] + P[5][18]*SH_MAG[3] + P[5][0]*SK_MY[2] - P[5][2]*SK_MY[1] - P[5][16]*SK_MY[3]); -Kfusion[6] = SK_MY[0]*(P[6][20] + P[6][17]*SH_MAG[0] + P[6][18]*SH_MAG[3] + P[6][0]*SK_MY[2] - P[6][2]*SK_MY[1] - P[6][16]*SK_MY[3]); -Kfusion[7] = SK_MY[0]*(P[7][20] + P[7][17]*SH_MAG[0] + P[7][18]*SH_MAG[3] + P[7][0]*SK_MY[2] - P[7][2]*SK_MY[1] - P[7][16]*SK_MY[3]); -Kfusion[8] = SK_MY[0]*(P[8][20] + P[8][17]*SH_MAG[0] + P[8][18]*SH_MAG[3] + P[8][0]*SK_MY[2] - P[8][2]*SK_MY[1] - P[8][16]*SK_MY[3]); -Kfusion[9] = SK_MY[0]*(P[9][20] + P[9][17]*SH_MAG[0] + P[9][18]*SH_MAG[3] + P[9][0]*SK_MY[2] - P[9][2]*SK_MY[1] - P[9][16]*SK_MY[3]); -Kfusion[10] = SK_MY[0]*(P[10][20] + P[10][17]*SH_MAG[0] + P[10][18]*SH_MAG[3] + P[10][0]*SK_MY[2] - P[10][2]*SK_MY[1] - P[10][16]*SK_MY[3]); -Kfusion[11] = SK_MY[0]*(P[11][20] + P[11][17]*SH_MAG[0] + P[11][18]*SH_MAG[3] + P[11][0]*SK_MY[2] - P[11][2]*SK_MY[1] - P[11][16]*SK_MY[3]); -Kfusion[12] = SK_MY[0]*(P[12][20] + P[12][17]*SH_MAG[0] + P[12][18]*SH_MAG[3] + P[12][0]*SK_MY[2] - P[12][2]*SK_MY[1] - P[12][16]*SK_MY[3]); -Kfusion[13] = SK_MY[0]*(P[13][20] + P[13][17]*SH_MAG[0] + P[13][18]*SH_MAG[3] + P[13][0]*SK_MY[2] - P[13][2]*SK_MY[1] - P[13][16]*SK_MY[3]); -Kfusion[14] = SK_MY[0]*(P[14][20] + P[14][17]*SH_MAG[0] + P[14][18]*SH_MAG[3] + P[14][0]*SK_MY[2] - P[14][2]*SK_MY[1] - P[14][16]*SK_MY[3]); -Kfusion[15] = SK_MY[0]*(P[15][20] + P[15][17]*SH_MAG[0] + P[15][18]*SH_MAG[3] + P[15][0]*SK_MY[2] - P[15][2]*SK_MY[1] - P[15][16]*SK_MY[3]); -Kfusion[16] = SK_MY[0]*(P[16][20] + P[16][17]*SH_MAG[0] + P[16][18]*SH_MAG[3] + P[16][0]*SK_MY[2] - P[16][2]*SK_MY[1] - P[16][16]*SK_MY[3]); -Kfusion[17] = SK_MY[0]*(P[17][20] + P[17][17]*SH_MAG[0] + P[17][18]*SH_MAG[3] + P[17][0]*SK_MY[2] - P[17][2]*SK_MY[1] - P[17][16]*SK_MY[3]); -Kfusion[18] = SK_MY[0]*(P[18][20] + P[18][17]*SH_MAG[0] + P[18][18]*SH_MAG[3] + P[18][0]*SK_MY[2] - P[18][2]*SK_MY[1] - P[18][16]*SK_MY[3]); -Kfusion[19] = SK_MY[0]*(P[19][20] + P[19][17]*SH_MAG[0] + P[19][18]*SH_MAG[3] + P[19][0]*SK_MY[2] - P[19][2]*SK_MY[1] - P[19][16]*SK_MY[3]); -Kfusion[20] = SK_MY[0]*(P[20][20] + P[20][17]*SH_MAG[0] + P[20][18]*SH_MAG[3] + P[20][0]*SK_MY[2] - P[20][2]*SK_MY[1] - P[20][16]*SK_MY[3]); -Kfusion[21] = SK_MY[0]*(P[21][20] + P[21][17]*SH_MAG[0] + P[21][18]*SH_MAG[3] + P[21][0]*SK_MY[2] - P[21][2]*SK_MY[1] - P[21][16]*SK_MY[3]); -Kfusion[22] = SK_MY[0]*(P[22][20] + P[22][17]*SH_MAG[0] + P[22][18]*SH_MAG[3] + P[22][0]*SK_MY[2] - P[22][2]*SK_MY[1] - P[22][16]*SK_MY[3]); -Kfusion[23] = SK_MY[0]*(P[23][20] + P[23][17]*SH_MAG[0] + P[23][18]*SH_MAG[3] + P[23][0]*SK_MY[2] - P[23][2]*SK_MY[1] - P[23][16]*SK_MY[3]); +Kfusion[0] = SK_MY[0]*(P[0][20] + P[0][0]*SH_MAG[2] + P[0][1]*SH_MAG[1] + P[0][2]*SH_MAG[0] - P[0][3]*SK_MY[2] - P[0][17]*SK_MY[1] - P[0][16]*SK_MY[3] + P[0][18]*SK_MY[4]); +Kfusion[1] = SK_MY[0]*(P[1][20] + P[1][0]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[1][2]*SH_MAG[0] - P[1][3]*SK_MY[2] - P[1][17]*SK_MY[1] - P[1][16]*SK_MY[3] + P[1][18]*SK_MY[4]); +Kfusion[2] = SK_MY[0]*(P[2][20] + P[2][0]*SH_MAG[2] + P[2][1]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[2][3]*SK_MY[2] - P[2][17]*SK_MY[1] - P[2][16]*SK_MY[3] + P[2][18]*SK_MY[4]); +Kfusion[3] = SK_MY[0]*(P[3][20] + P[3][0]*SH_MAG[2] + P[3][1]*SH_MAG[1] + P[3][2]*SH_MAG[0] - P[3][3]*SK_MY[2] - P[3][17]*SK_MY[1] - P[3][16]*SK_MY[3] + P[3][18]*SK_MY[4]); +Kfusion[4] = SK_MY[0]*(P[4][20] + P[4][0]*SH_MAG[2] + P[4][1]*SH_MAG[1] + P[4][2]*SH_MAG[0] - P[4][3]*SK_MY[2] - P[4][17]*SK_MY[1] - P[4][16]*SK_MY[3] + P[4][18]*SK_MY[4]); +Kfusion[5] = SK_MY[0]*(P[5][20] + P[5][0]*SH_MAG[2] + P[5][1]*SH_MAG[1] + P[5][2]*SH_MAG[0] - P[5][3]*SK_MY[2] - P[5][17]*SK_MY[1] - P[5][16]*SK_MY[3] + P[5][18]*SK_MY[4]); +Kfusion[6] = SK_MY[0]*(P[6][20] + P[6][0]*SH_MAG[2] + P[6][1]*SH_MAG[1] + P[6][2]*SH_MAG[0] - P[6][3]*SK_MY[2] - P[6][17]*SK_MY[1] - P[6][16]*SK_MY[3] + P[6][18]*SK_MY[4]); +Kfusion[7] = SK_MY[0]*(P[7][20] + P[7][0]*SH_MAG[2] + P[7][1]*SH_MAG[1] + P[7][2]*SH_MAG[0] - P[7][3]*SK_MY[2] - P[7][17]*SK_MY[1] - P[7][16]*SK_MY[3] + P[7][18]*SK_MY[4]); +Kfusion[8] = SK_MY[0]*(P[8][20] + P[8][0]*SH_MAG[2] + P[8][1]*SH_MAG[1] + P[8][2]*SH_MAG[0] - P[8][3]*SK_MY[2] - P[8][17]*SK_MY[1] - P[8][16]*SK_MY[3] + P[8][18]*SK_MY[4]); +Kfusion[9] = SK_MY[0]*(P[9][20] + P[9][0]*SH_MAG[2] + P[9][1]*SH_MAG[1] + P[9][2]*SH_MAG[0] - P[9][3]*SK_MY[2] - P[9][17]*SK_MY[1] - P[9][16]*SK_MY[3] + P[9][18]*SK_MY[4]); +Kfusion[10] = SK_MY[0]*(P[10][20] + P[10][0]*SH_MAG[2] + P[10][1]*SH_MAG[1] + P[10][2]*SH_MAG[0] - P[10][3]*SK_MY[2] - P[10][17]*SK_MY[1] - P[10][16]*SK_MY[3] + P[10][18]*SK_MY[4]); +Kfusion[11] = SK_MY[0]*(P[11][20] + P[11][0]*SH_MAG[2] + P[11][1]*SH_MAG[1] + P[11][2]*SH_MAG[0] - P[11][3]*SK_MY[2] - P[11][17]*SK_MY[1] - P[11][16]*SK_MY[3] + P[11][18]*SK_MY[4]); +Kfusion[12] = SK_MY[0]*(P[12][20] + P[12][0]*SH_MAG[2] + P[12][1]*SH_MAG[1] + P[12][2]*SH_MAG[0] - P[12][3]*SK_MY[2] - P[12][17]*SK_MY[1] - P[12][16]*SK_MY[3] + P[12][18]*SK_MY[4]); +Kfusion[13] = SK_MY[0]*(P[13][20] + P[13][0]*SH_MAG[2] + P[13][1]*SH_MAG[1] + P[13][2]*SH_MAG[0] - P[13][3]*SK_MY[2] - P[13][17]*SK_MY[1] - P[13][16]*SK_MY[3] + P[13][18]*SK_MY[4]); +Kfusion[14] = SK_MY[0]*(P[14][20] + P[14][0]*SH_MAG[2] + P[14][1]*SH_MAG[1] + P[14][2]*SH_MAG[0] - P[14][3]*SK_MY[2] - P[14][17]*SK_MY[1] - P[14][16]*SK_MY[3] + P[14][18]*SK_MY[4]); +Kfusion[15] = SK_MY[0]*(P[15][20] + P[15][0]*SH_MAG[2] + P[15][1]*SH_MAG[1] + P[15][2]*SH_MAG[0] - P[15][3]*SK_MY[2] - P[15][17]*SK_MY[1] - P[15][16]*SK_MY[3] + P[15][18]*SK_MY[4]); +Kfusion[16] = SK_MY[0]*(P[16][20] + P[16][0]*SH_MAG[2] + P[16][1]*SH_MAG[1] + P[16][2]*SH_MAG[0] - P[16][3]*SK_MY[2] - P[16][17]*SK_MY[1] - P[16][16]*SK_MY[3] + P[16][18]*SK_MY[4]); +Kfusion[17] = SK_MY[0]*(P[17][20] + P[17][0]*SH_MAG[2] + P[17][1]*SH_MAG[1] + P[17][2]*SH_MAG[0] - P[17][3]*SK_MY[2] - P[17][17]*SK_MY[1] - P[17][16]*SK_MY[3] + P[17][18]*SK_MY[4]); +Kfusion[18] = SK_MY[0]*(P[18][20] + P[18][0]*SH_MAG[2] + P[18][1]*SH_MAG[1] + P[18][2]*SH_MAG[0] - P[18][3]*SK_MY[2] - P[18][17]*SK_MY[1] - P[18][16]*SK_MY[3] + P[18][18]*SK_MY[4]); +Kfusion[19] = SK_MY[0]*(P[19][20] + P[19][0]*SH_MAG[2] + P[19][1]*SH_MAG[1] + P[19][2]*SH_MAG[0] - P[19][3]*SK_MY[2] - P[19][17]*SK_MY[1] - P[19][16]*SK_MY[3] + P[19][18]*SK_MY[4]); +Kfusion[20] = SK_MY[0]*(P[20][20] + P[20][0]*SH_MAG[2] + P[20][1]*SH_MAG[1] + P[20][2]*SH_MAG[0] - P[20][3]*SK_MY[2] - P[20][17]*SK_MY[1] - P[20][16]*SK_MY[3] + P[20][18]*SK_MY[4]); +Kfusion[21] = SK_MY[0]*(P[21][20] + P[21][0]*SH_MAG[2] + P[21][1]*SH_MAG[1] + P[21][2]*SH_MAG[0] - P[21][3]*SK_MY[2] - P[21][17]*SK_MY[1] - P[21][16]*SK_MY[3] + P[21][18]*SK_MY[4]); +Kfusion[22] = SK_MY[0]*(P[22][20] + P[22][0]*SH_MAG[2] + P[22][1]*SH_MAG[1] + P[22][2]*SH_MAG[0] - P[22][3]*SK_MY[2] - P[22][17]*SK_MY[1] - P[22][16]*SK_MY[3] + P[22][18]*SK_MY[4]); +Kfusion[23] = SK_MY[0]*(P[23][20] + P[23][0]*SH_MAG[2] + P[23][1]*SH_MAG[1] + P[23][2]*SH_MAG[0] - P[23][3]*SK_MY[2] - P[23][17]*SK_MY[1] - P[23][16]*SK_MY[3] + P[23][18]*SK_MY[4]); float H_MAG[1][24]; -H_MAG[0] = magN*(SH_MAG[8] - 2*q1*q2) - magD*SH_MAG[3] - magE*SH_MAG[0]; -H_MAG[1] = magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]; -H_MAG[16] = SH_MAG[5]; +H_MAG[0] = SH_MAG[1]; +H_MAG[1] = -SH_MAG[2]; +H_MAG[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; +H_MAG[3] = SH_MAG[0]; +H_MAG[16] = 2*q0*q2 + 2*q1*q3; H_MAG[17] = 2*q2*q3 - 2*q0*q1; -H_MAG[18] = SH_MAG[2]; +H_MAG[18] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]; H_MAG[21] = 1; -float SK_MZ[4][1]; -SK_MZ[0] = 1/(P[21][21] + R_MAG + P[16][21]*SH_MAG[5] + P[18][21]*SH_MAG[2] - (2*q0*q1 - 2*q2*q3)*(P[21][17] + P[16][17]*SH_MAG[5] + P[18][17]*SH_MAG[2] - P[0][17]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2)) + P[1][17]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][17]*(2*q0*q1 - 2*q2*q3)) - P[0][21]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2)) + P[1][21]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) + SH_MAG[5]*(P[21][16] + P[16][16]*SH_MAG[5] + P[18][16]*SH_MAG[2] - P[0][16]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2)) + P[1][16]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][16]*(2*q0*q1 - 2*q2*q3)) + SH_MAG[2]*(P[21][18] + P[16][18]*SH_MAG[5] + P[18][18]*SH_MAG[2] - P[0][18]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2)) + P[1][18]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][18]*(2*q0*q1 - 2*q2*q3)) - (magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2))*(P[21][0] + P[16][0]*SH_MAG[5] + P[18][0]*SH_MAG[2] - P[0][0]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2)) + P[1][0]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][0]*(2*q0*q1 - 2*q2*q3)) - P[17][21]*(2*q0*q1 - 2*q2*q3) + (magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1])*(P[21][1] + P[16][1]*SH_MAG[5] + P[18][1]*SH_MAG[2] - P[0][1]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2)) + P[1][1]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][1]*(2*q0*q1 - 2*q2*q3))); -SK_MZ[1] = magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2); -SK_MZ[2] = magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]; +float SK_MZ[5][1]; +SK_MZ[0] = 1/(P[21][21] + R_MAG + P[0][21]*SH_MAG[1] - P[1][21]*SH_MAG[2] + P[3][21]*SH_MAG[0] + P[18][21]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + (2*q0*q2 + 2*q1*q3)*(P[21][16] + P[0][16]*SH_MAG[1] - P[1][16]*SH_MAG[2] + P[3][16]*SH_MAG[0] + P[18][16]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][16]*(2*q0*q2 + 2*q1*q3) - P[17][16]*(2*q0*q1 - 2*q2*q3) + P[2][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q1 - 2*q2*q3)*(P[21][17] + P[0][17]*SH_MAG[1] - P[1][17]*SH_MAG[2] + P[3][17]*SH_MAG[0] + P[18][17]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][17]*(2*q0*q2 + 2*q1*q3) - P[17][17]*(2*q0*q1 - 2*q2*q3) + P[2][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[21][2] + P[0][2]*SH_MAG[1] - P[1][2]*SH_MAG[2] + P[3][2]*SH_MAG[0] + P[18][2]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][2]*(2*q0*q2 + 2*q1*q3) - P[17][2]*(2*q0*q1 - 2*q2*q3) + P[2][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[16][21]*(2*q0*q2 + 2*q1*q3) - P[17][21]*(2*q0*q1 - 2*q2*q3) + SH_MAG[1]*(P[21][0] + P[0][0]*SH_MAG[1] - P[1][0]*SH_MAG[2] + P[3][0]*SH_MAG[0] + P[18][0]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][0]*(2*q0*q2 + 2*q1*q3) - P[17][0]*(2*q0*q1 - 2*q2*q3) + P[2][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - SH_MAG[2]*(P[21][1] + P[0][1]*SH_MAG[1] - P[1][1]*SH_MAG[2] + P[3][1]*SH_MAG[0] + P[18][1]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][1]*(2*q0*q2 + 2*q1*q3) - P[17][1]*(2*q0*q1 - 2*q2*q3) + P[2][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[21][3] + P[0][3]*SH_MAG[1] - P[1][3]*SH_MAG[2] + P[3][3]*SH_MAG[0] + P[18][3]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][3]*(2*q0*q2 + 2*q1*q3) - P[17][3]*(2*q0*q1 - 2*q2*q3) + P[2][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6])*(P[21][18] + P[0][18]*SH_MAG[1] - P[1][18]*SH_MAG[2] + P[3][18]*SH_MAG[0] + P[18][18]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][18]*(2*q0*q2 + 2*q1*q3) - P[17][18]*(2*q0*q1 - 2*q2*q3) + P[2][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[2][21]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); +SK_MZ[1] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]; +SK_MZ[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; SK_MZ[3] = 2*q0*q1 - 2*q2*q3; +SK_MZ[4] = 2*q0*q2 + 2*q1*q3; float Kfusion[24][1]; float Kfusion[1][1]; -Kfusion[0] = SK_MZ[0]*(P[0][21] + P[0][18]*SH_MAG[2] + P[0][16]*SH_MAG[5] - P[0][0]*SK_MZ[1] + P[0][1]*SK_MZ[2] - P[0][17]*SK_MZ[3]); -Kfusion[1] = SK_MZ[0]*(P[1][21] + P[1][18]*SH_MAG[2] + P[1][16]*SH_MAG[5] - P[1][0]*SK_MZ[1] + P[1][1]*SK_MZ[2] - P[1][17]*SK_MZ[3]); -Kfusion[2] = SK_MZ[0]*(P[2][21] + P[2][18]*SH_MAG[2] + P[2][16]*SH_MAG[5] - P[2][0]*SK_MZ[1] + P[2][1]*SK_MZ[2] - P[2][17]*SK_MZ[3]); -Kfusion[3] = SK_MZ[0]*(P[3][21] + P[3][18]*SH_MAG[2] + P[3][16]*SH_MAG[5] - P[3][0]*SK_MZ[1] + P[3][1]*SK_MZ[2] - P[3][17]*SK_MZ[3]); -Kfusion[4] = SK_MZ[0]*(P[4][21] + P[4][18]*SH_MAG[2] + P[4][16]*SH_MAG[5] - P[4][0]*SK_MZ[1] + P[4][1]*SK_MZ[2] - P[4][17]*SK_MZ[3]); -Kfusion[5] = SK_MZ[0]*(P[5][21] + P[5][18]*SH_MAG[2] + P[5][16]*SH_MAG[5] - P[5][0]*SK_MZ[1] + P[5][1]*SK_MZ[2] - P[5][17]*SK_MZ[3]); -Kfusion[6] = SK_MZ[0]*(P[6][21] + P[6][18]*SH_MAG[2] + P[6][16]*SH_MAG[5] - P[6][0]*SK_MZ[1] + P[6][1]*SK_MZ[2] - P[6][17]*SK_MZ[3]); -Kfusion[7] = SK_MZ[0]*(P[7][21] + P[7][18]*SH_MAG[2] + P[7][16]*SH_MAG[5] - P[7][0]*SK_MZ[1] + P[7][1]*SK_MZ[2] - P[7][17]*SK_MZ[3]); -Kfusion[8] = SK_MZ[0]*(P[8][21] + P[8][18]*SH_MAG[2] + P[8][16]*SH_MAG[5] - P[8][0]*SK_MZ[1] + P[8][1]*SK_MZ[2] - P[8][17]*SK_MZ[3]); -Kfusion[9] = SK_MZ[0]*(P[9][21] + P[9][18]*SH_MAG[2] + P[9][16]*SH_MAG[5] - P[9][0]*SK_MZ[1] + P[9][1]*SK_MZ[2] - P[9][17]*SK_MZ[3]); -Kfusion[10] = SK_MZ[0]*(P[10][21] + P[10][18]*SH_MAG[2] + P[10][16]*SH_MAG[5] - P[10][0]*SK_MZ[1] + P[10][1]*SK_MZ[2] - P[10][17]*SK_MZ[3]); -Kfusion[11] = SK_MZ[0]*(P[11][21] + P[11][18]*SH_MAG[2] + P[11][16]*SH_MAG[5] - P[11][0]*SK_MZ[1] + P[11][1]*SK_MZ[2] - P[11][17]*SK_MZ[3]); -Kfusion[12] = SK_MZ[0]*(P[12][21] + P[12][18]*SH_MAG[2] + P[12][16]*SH_MAG[5] - P[12][0]*SK_MZ[1] + P[12][1]*SK_MZ[2] - P[12][17]*SK_MZ[3]); -Kfusion[13] = SK_MZ[0]*(P[13][21] + P[13][18]*SH_MAG[2] + P[13][16]*SH_MAG[5] - P[13][0]*SK_MZ[1] + P[13][1]*SK_MZ[2] - P[13][17]*SK_MZ[3]); -Kfusion[14] = SK_MZ[0]*(P[14][21] + P[14][18]*SH_MAG[2] + P[14][16]*SH_MAG[5] - P[14][0]*SK_MZ[1] + P[14][1]*SK_MZ[2] - P[14][17]*SK_MZ[3]); -Kfusion[15] = SK_MZ[0]*(P[15][21] + P[15][18]*SH_MAG[2] + P[15][16]*SH_MAG[5] - P[15][0]*SK_MZ[1] + P[15][1]*SK_MZ[2] - P[15][17]*SK_MZ[3]); -Kfusion[16] = SK_MZ[0]*(P[16][21] + P[16][18]*SH_MAG[2] + P[16][16]*SH_MAG[5] - P[16][0]*SK_MZ[1] + P[16][1]*SK_MZ[2] - P[16][17]*SK_MZ[3]); -Kfusion[17] = SK_MZ[0]*(P[17][21] + P[17][18]*SH_MAG[2] + P[17][16]*SH_MAG[5] - P[17][0]*SK_MZ[1] + P[17][1]*SK_MZ[2] - P[17][17]*SK_MZ[3]); -Kfusion[18] = SK_MZ[0]*(P[18][21] + P[18][18]*SH_MAG[2] + P[18][16]*SH_MAG[5] - P[18][0]*SK_MZ[1] + P[18][1]*SK_MZ[2] - P[18][17]*SK_MZ[3]); -Kfusion[19] = SK_MZ[0]*(P[19][21] + P[19][18]*SH_MAG[2] + P[19][16]*SH_MAG[5] - P[19][0]*SK_MZ[1] + P[19][1]*SK_MZ[2] - P[19][17]*SK_MZ[3]); -Kfusion[20] = SK_MZ[0]*(P[20][21] + P[20][18]*SH_MAG[2] + P[20][16]*SH_MAG[5] - P[20][0]*SK_MZ[1] + P[20][1]*SK_MZ[2] - P[20][17]*SK_MZ[3]); -Kfusion[21] = SK_MZ[0]*(P[21][21] + P[21][18]*SH_MAG[2] + P[21][16]*SH_MAG[5] - P[21][0]*SK_MZ[1] + P[21][1]*SK_MZ[2] - P[21][17]*SK_MZ[3]); -Kfusion[22] = SK_MZ[0]*(P[22][21] + P[22][18]*SH_MAG[2] + P[22][16]*SH_MAG[5] - P[22][0]*SK_MZ[1] + P[22][1]*SK_MZ[2] - P[22][17]*SK_MZ[3]); -Kfusion[23] = SK_MZ[0]*(P[23][21] + P[23][18]*SH_MAG[2] + P[23][16]*SH_MAG[5] - P[23][0]*SK_MZ[1] + P[23][1]*SK_MZ[2] - P[23][17]*SK_MZ[3]); -float SH_ACCX[7][1]; +Kfusion[0] = SK_MZ[0]*(P[0][21] + P[0][0]*SH_MAG[1] - P[0][1]*SH_MAG[2] + P[0][3]*SH_MAG[0] + P[0][2]*SK_MZ[2] + P[0][18]*SK_MZ[1] + P[0][16]*SK_MZ[4] - P[0][17]*SK_MZ[3]); +Kfusion[1] = SK_MZ[0]*(P[1][21] + P[1][0]*SH_MAG[1] - P[1][1]*SH_MAG[2] + P[1][3]*SH_MAG[0] + P[1][2]*SK_MZ[2] + P[1][18]*SK_MZ[1] + P[1][16]*SK_MZ[4] - P[1][17]*SK_MZ[3]); +Kfusion[2] = SK_MZ[0]*(P[2][21] + P[2][0]*SH_MAG[1] - P[2][1]*SH_MAG[2] + P[2][3]*SH_MAG[0] + P[2][2]*SK_MZ[2] + P[2][18]*SK_MZ[1] + P[2][16]*SK_MZ[4] - P[2][17]*SK_MZ[3]); +Kfusion[3] = SK_MZ[0]*(P[3][21] + P[3][0]*SH_MAG[1] - P[3][1]*SH_MAG[2] + P[3][3]*SH_MAG[0] + P[3][2]*SK_MZ[2] + P[3][18]*SK_MZ[1] + P[3][16]*SK_MZ[4] - P[3][17]*SK_MZ[3]); +Kfusion[4] = SK_MZ[0]*(P[4][21] + P[4][0]*SH_MAG[1] - P[4][1]*SH_MAG[2] + P[4][3]*SH_MAG[0] + P[4][2]*SK_MZ[2] + P[4][18]*SK_MZ[1] + P[4][16]*SK_MZ[4] - P[4][17]*SK_MZ[3]); +Kfusion[5] = SK_MZ[0]*(P[5][21] + P[5][0]*SH_MAG[1] - P[5][1]*SH_MAG[2] + P[5][3]*SH_MAG[0] + P[5][2]*SK_MZ[2] + P[5][18]*SK_MZ[1] + P[5][16]*SK_MZ[4] - P[5][17]*SK_MZ[3]); +Kfusion[6] = SK_MZ[0]*(P[6][21] + P[6][0]*SH_MAG[1] - P[6][1]*SH_MAG[2] + P[6][3]*SH_MAG[0] + P[6][2]*SK_MZ[2] + P[6][18]*SK_MZ[1] + P[6][16]*SK_MZ[4] - P[6][17]*SK_MZ[3]); +Kfusion[7] = SK_MZ[0]*(P[7][21] + P[7][0]*SH_MAG[1] - P[7][1]*SH_MAG[2] + P[7][3]*SH_MAG[0] + P[7][2]*SK_MZ[2] + P[7][18]*SK_MZ[1] + P[7][16]*SK_MZ[4] - P[7][17]*SK_MZ[3]); +Kfusion[8] = SK_MZ[0]*(P[8][21] + P[8][0]*SH_MAG[1] - P[8][1]*SH_MAG[2] + P[8][3]*SH_MAG[0] + P[8][2]*SK_MZ[2] + P[8][18]*SK_MZ[1] + P[8][16]*SK_MZ[4] - P[8][17]*SK_MZ[3]); +Kfusion[9] = SK_MZ[0]*(P[9][21] + P[9][0]*SH_MAG[1] - P[9][1]*SH_MAG[2] + P[9][3]*SH_MAG[0] + P[9][2]*SK_MZ[2] + P[9][18]*SK_MZ[1] + P[9][16]*SK_MZ[4] - P[9][17]*SK_MZ[3]); +Kfusion[10] = SK_MZ[0]*(P[10][21] + P[10][0]*SH_MAG[1] - P[10][1]*SH_MAG[2] + P[10][3]*SH_MAG[0] + P[10][2]*SK_MZ[2] + P[10][18]*SK_MZ[1] + P[10][16]*SK_MZ[4] - P[10][17]*SK_MZ[3]); +Kfusion[11] = SK_MZ[0]*(P[11][21] + P[11][0]*SH_MAG[1] - P[11][1]*SH_MAG[2] + P[11][3]*SH_MAG[0] + P[11][2]*SK_MZ[2] + P[11][18]*SK_MZ[1] + P[11][16]*SK_MZ[4] - P[11][17]*SK_MZ[3]); +Kfusion[12] = SK_MZ[0]*(P[12][21] + P[12][0]*SH_MAG[1] - P[12][1]*SH_MAG[2] + P[12][3]*SH_MAG[0] + P[12][2]*SK_MZ[2] + P[12][18]*SK_MZ[1] + P[12][16]*SK_MZ[4] - P[12][17]*SK_MZ[3]); +Kfusion[13] = SK_MZ[0]*(P[13][21] + P[13][0]*SH_MAG[1] - P[13][1]*SH_MAG[2] + P[13][3]*SH_MAG[0] + P[13][2]*SK_MZ[2] + P[13][18]*SK_MZ[1] + P[13][16]*SK_MZ[4] - P[13][17]*SK_MZ[3]); +Kfusion[14] = SK_MZ[0]*(P[14][21] + P[14][0]*SH_MAG[1] - P[14][1]*SH_MAG[2] + P[14][3]*SH_MAG[0] + P[14][2]*SK_MZ[2] + P[14][18]*SK_MZ[1] + P[14][16]*SK_MZ[4] - P[14][17]*SK_MZ[3]); +Kfusion[15] = SK_MZ[0]*(P[15][21] + P[15][0]*SH_MAG[1] - P[15][1]*SH_MAG[2] + P[15][3]*SH_MAG[0] + P[15][2]*SK_MZ[2] + P[15][18]*SK_MZ[1] + P[15][16]*SK_MZ[4] - P[15][17]*SK_MZ[3]); +Kfusion[16] = SK_MZ[0]*(P[16][21] + P[16][0]*SH_MAG[1] - P[16][1]*SH_MAG[2] + P[16][3]*SH_MAG[0] + P[16][2]*SK_MZ[2] + P[16][18]*SK_MZ[1] + P[16][16]*SK_MZ[4] - P[16][17]*SK_MZ[3]); +Kfusion[17] = SK_MZ[0]*(P[17][21] + P[17][0]*SH_MAG[1] - P[17][1]*SH_MAG[2] + P[17][3]*SH_MAG[0] + P[17][2]*SK_MZ[2] + P[17][18]*SK_MZ[1] + P[17][16]*SK_MZ[4] - P[17][17]*SK_MZ[3]); +Kfusion[18] = SK_MZ[0]*(P[18][21] + P[18][0]*SH_MAG[1] - P[18][1]*SH_MAG[2] + P[18][3]*SH_MAG[0] + P[18][2]*SK_MZ[2] + P[18][18]*SK_MZ[1] + P[18][16]*SK_MZ[4] - P[18][17]*SK_MZ[3]); +Kfusion[19] = SK_MZ[0]*(P[19][21] + P[19][0]*SH_MAG[1] - P[19][1]*SH_MAG[2] + P[19][3]*SH_MAG[0] + P[19][2]*SK_MZ[2] + P[19][18]*SK_MZ[1] + P[19][16]*SK_MZ[4] - P[19][17]*SK_MZ[3]); +Kfusion[20] = SK_MZ[0]*(P[20][21] + P[20][0]*SH_MAG[1] - P[20][1]*SH_MAG[2] + P[20][3]*SH_MAG[0] + P[20][2]*SK_MZ[2] + P[20][18]*SK_MZ[1] + P[20][16]*SK_MZ[4] - P[20][17]*SK_MZ[3]); +Kfusion[21] = SK_MZ[0]*(P[21][21] + P[21][0]*SH_MAG[1] - P[21][1]*SH_MAG[2] + P[21][3]*SH_MAG[0] + P[21][2]*SK_MZ[2] + P[21][18]*SK_MZ[1] + P[21][16]*SK_MZ[4] - P[21][17]*SK_MZ[3]); +Kfusion[22] = SK_MZ[0]*(P[22][21] + P[22][0]*SH_MAG[1] - P[22][1]*SH_MAG[2] + P[22][3]*SH_MAG[0] + P[22][2]*SK_MZ[2] + P[22][18]*SK_MZ[1] + P[22][16]*SK_MZ[4] - P[22][17]*SK_MZ[3]); +Kfusion[23] = SK_MZ[0]*(P[23][21] + P[23][0]*SH_MAG[1] - P[23][1]*SH_MAG[2] + P[23][3]*SH_MAG[0] + P[23][2]*SK_MZ[2] + P[23][18]*SK_MZ[1] + P[23][16]*SK_MZ[4] - P[23][17]*SK_MZ[3]); +float SH_ACCX[4][1]; SH_ACCX[0] = sq(q0) + sq(q1) - sq(q2) - sq(q3); -SH_ACCX[1] = 2*q0*q3 + 2*q1*q2; -SH_ACCX[2] = vn - vwn; -SH_ACCX[3] = ve - vwe; -SH_ACCX[4] = sq(q3); -SH_ACCX[5] = 2*q0*q2; -SH_ACCX[6] = 2*q0*q1; +SH_ACCX[1] = vn - vwn; +SH_ACCX[2] = ve - vwe; +SH_ACCX[3] = 2*q0*q3 + 2*q1*q2; float H_ACCX[1][24]; -H_ACCX[0][1] = Kaccx*(SH_ACCX[2]*(SH_ACCX[5] + 2*q1*q3) - SH_ACCX[3]*(SH_ACCX[6] - 2*q2*q3) + vd*(SH_ACCX[4] + sq(q0) - sq(q1) - sq(q2))); -H_ACCX[0][2] = Kaccx*(SH_ACCX[2]*(2*q0*q3 - 2*q1*q2) + SH_ACCX[3]*(SH_ACCX[4] - sq(q0) + sq(q1) - sq(q2)) - vd*(SH_ACCX[6] + 2*q2*q3)); -H_ACCX[0][3] = -Kaccx*SH_ACCX[0]; -H_ACCX[0][4] = -Kaccx*SH_ACCX[1]; -H_ACCX[0][5] = Kaccx*(SH_ACCX[5] - 2*q1*q3); +H_ACCX[0][0] = -Kaccx*(2*q0*SH_ACCX[1] + 2*q3*SH_ACCX[2] - 2*q2*vd); +H_ACCX[0][1] = -Kaccx*(2*q1*SH_ACCX[1] + 2*q2*SH_ACCX[2] + 2*q3*vd); +H_ACCX[0][2] = Kaccx*(2*q2*SH_ACCX[1] - 2*q1*SH_ACCX[2] + 2*q0*vd); +H_ACCX[0][3] = -Kaccx*(2*q0*SH_ACCX[2] - 2*q3*SH_ACCX[1] + 2*q1*vd); +H_ACCX[0][4] = -Kaccx*SH_ACCX[0]; +H_ACCX[0][5] = -Kaccx*SH_ACCX[3]; +H_ACCX[0][6] = Kaccx*(2*q0*q2 - 2*q1*q3); H_ACCX[0][22] = Kaccx*SH_ACCX[0]; -H_ACCX[0][23] = Kaccx*SH_ACCX[1]; -float SK_ACCX[5][1]; -SK_ACCX[0] = 1/(R_ACC - Kaccx*SH_ACCX[0]*(Kaccx*P[22][3]*SH_ACCX[0] - Kaccx*P[4][3]*SH_ACCX[1] - Kaccx*P[3][3]*SH_ACCX[0] + Kaccx*P[23][3]*SH_ACCX[1] + Kaccx*P[2][3]*(SH_ACCX[2]*(2*q0*q3 - 2*q1*q2) + SH_ACCX[3]*(SH_ACCX[4] - sq(q0) + sq(q1) - sq(q2)) - vd*(SH_ACCX[6] + 2*q2*q3)) + Kaccx*P[1][3]*(SH_ACCX[2]*(SH_ACCX[5] + 2*q1*q3) - SH_ACCX[3]*(SH_ACCX[6] - 2*q2*q3) + vd*(SH_ACCX[4] + sq(q0) - sq(q1) - sq(q2))) + Kaccx*P[5][3]*(SH_ACCX[5] - 2*q1*q3)) - Kaccx*SH_ACCX[1]*(Kaccx*P[22][4]*SH_ACCX[0] - Kaccx*P[4][4]*SH_ACCX[1] - Kaccx*P[3][4]*SH_ACCX[0] + Kaccx*P[23][4]*SH_ACCX[1] + Kaccx*P[2][4]*(SH_ACCX[2]*(2*q0*q3 - 2*q1*q2) + SH_ACCX[3]*(SH_ACCX[4] - sq(q0) + sq(q1) - sq(q2)) - vd*(SH_ACCX[6] + 2*q2*q3)) + Kaccx*P[1][4]*(SH_ACCX[2]*(SH_ACCX[5] + 2*q1*q3) - SH_ACCX[3]*(SH_ACCX[6] - 2*q2*q3) + vd*(SH_ACCX[4] + sq(q0) - sq(q1) - sq(q2))) + Kaccx*P[5][4]*(SH_ACCX[5] - 2*q1*q3)) + Kaccx*SH_ACCX[0]*(Kaccx*P[22][22]*SH_ACCX[0] - Kaccx*P[4][22]*SH_ACCX[1] - Kaccx*P[3][22]*SH_ACCX[0] + Kaccx*P[23][22]*SH_ACCX[1] + Kaccx*P[2][22]*(SH_ACCX[2]*(2*q0*q3 - 2*q1*q2) + SH_ACCX[3]*(SH_ACCX[4] - sq(q0) + sq(q1) - sq(q2)) - vd*(SH_ACCX[6] + 2*q2*q3)) + Kaccx*P[1][22]*(SH_ACCX[2]*(SH_ACCX[5] + 2*q1*q3) - SH_ACCX[3]*(SH_ACCX[6] - 2*q2*q3) + vd*(SH_ACCX[4] + sq(q0) - sq(q1) - sq(q2))) + Kaccx*P[5][22]*(SH_ACCX[5] - 2*q1*q3)) + Kaccx*SH_ACCX[1]*(Kaccx*P[22][23]*SH_ACCX[0] - Kaccx*P[4][23]*SH_ACCX[1] - Kaccx*P[3][23]*SH_ACCX[0] + Kaccx*P[23][23]*SH_ACCX[1] + Kaccx*P[2][23]*(SH_ACCX[2]*(2*q0*q3 - 2*q1*q2) + SH_ACCX[3]*(SH_ACCX[4] - sq(q0) + sq(q1) - sq(q2)) - vd*(SH_ACCX[6] + 2*q2*q3)) + Kaccx*P[1][23]*(SH_ACCX[2]*(SH_ACCX[5] + 2*q1*q3) - SH_ACCX[3]*(SH_ACCX[6] - 2*q2*q3) + vd*(SH_ACCX[4] + sq(q0) - sq(q1) - sq(q2))) + Kaccx*P[5][23]*(SH_ACCX[5] - 2*q1*q3)) + Kaccx*(SH_ACCX[2]*(2*q0*q3 - 2*q1*q2) + SH_ACCX[3]*(SH_ACCX[4] - sq(q0) + sq(q1) - sq(q2)) - vd*(SH_ACCX[6] + 2*q2*q3))*(Kaccx*P[22][2]*SH_ACCX[0] - Kaccx*P[4][2]*SH_ACCX[1] - Kaccx*P[3][2]*SH_ACCX[0] + Kaccx*P[23][2]*SH_ACCX[1] + Kaccx*P[2][2]*(SH_ACCX[2]*(2*q0*q3 - 2*q1*q2) + SH_ACCX[3]*(SH_ACCX[4] - sq(q0) + sq(q1) - sq(q2)) - vd*(SH_ACCX[6] + 2*q2*q3)) + Kaccx*P[1][2]*(SH_ACCX[2]*(SH_ACCX[5] + 2*q1*q3) - SH_ACCX[3]*(SH_ACCX[6] - 2*q2*q3) + vd*(SH_ACCX[4] + sq(q0) - sq(q1) - sq(q2))) + Kaccx*P[5][2]*(SH_ACCX[5] - 2*q1*q3)) + Kaccx*(SH_ACCX[2]*(SH_ACCX[5] + 2*q1*q3) - SH_ACCX[3]*(SH_ACCX[6] - 2*q2*q3) + vd*(SH_ACCX[4] + sq(q0) - sq(q1) - sq(q2)))*(Kaccx*P[22][1]*SH_ACCX[0] - Kaccx*P[4][1]*SH_ACCX[1] - Kaccx*P[3][1]*SH_ACCX[0] + Kaccx*P[23][1]*SH_ACCX[1] + Kaccx*P[2][1]*(SH_ACCX[2]*(2*q0*q3 - 2*q1*q2) + SH_ACCX[3]*(SH_ACCX[4] - sq(q0) + sq(q1) - sq(q2)) - vd*(SH_ACCX[6] + 2*q2*q3)) + Kaccx*P[1][1]*(SH_ACCX[2]*(SH_ACCX[5] + 2*q1*q3) - SH_ACCX[3]*(SH_ACCX[6] - 2*q2*q3) + vd*(SH_ACCX[4] + sq(q0) - sq(q1) - sq(q2))) + Kaccx*P[5][1]*(SH_ACCX[5] - 2*q1*q3)) + Kaccx*(SH_ACCX[5] - 2*q1*q3)*(Kaccx*P[22][5]*SH_ACCX[0] - Kaccx*P[4][5]*SH_ACCX[1] - Kaccx*P[3][5]*SH_ACCX[0] + Kaccx*P[23][5]*SH_ACCX[1] + Kaccx*P[2][5]*(SH_ACCX[2]*(2*q0*q3 - 2*q1*q2) + SH_ACCX[3]*(SH_ACCX[4] - sq(q0) + sq(q1) - sq(q2)) - vd*(SH_ACCX[6] + 2*q2*q3)) + Kaccx*P[1][5]*(SH_ACCX[2]*(SH_ACCX[5] + 2*q1*q3) - SH_ACCX[3]*(SH_ACCX[6] - 2*q2*q3) + vd*(SH_ACCX[4] + sq(q0) - sq(q1) - sq(q2))) + Kaccx*P[5][5]*(SH_ACCX[5] - 2*q1*q3))); -SK_ACCX[1] = SH_ACCX[2]*(2*q0*q3 - 2*q1*q2) + SH_ACCX[3]*(SH_ACCX[4] - sq(q0) + sq(q1) - sq(q2)) - vd*(SH_ACCX[6] + 2*q2*q3); -SK_ACCX[2] = SH_ACCX[2]*(SH_ACCX[5] + 2*q1*q3) - SH_ACCX[3]*(SH_ACCX[6] - 2*q2*q3) + vd*(SH_ACCX[4] + sq(q0) - sq(q1) - sq(q2)); -SK_ACCX[3] = SH_ACCX[5] - 2*q1*q3; -SK_ACCX[4] = SH_ACCX[1]; +H_ACCX[0][23] = Kaccx*SH_ACCX[3]; +float SK_ACCX[7][1]; +SK_ACCX[0] = 1/(R_ACC + Kaccx*SH_ACCX[0]*(Kaccx*P[4][4]*SH_ACCX[0] + Kaccx*P[5][4]*SH_ACCX[3] - Kaccx*P[22][4]*SH_ACCX[0] - Kaccx*P[23][4]*SH_ACCX[3] - Kaccx*P[6][4]*(2*q0*q2 - 2*q1*q3) + Kaccx*P[0][4]*(2*q0*SH_ACCX[1] + 2*q3*SH_ACCX[2] - 2*q2*vd) + Kaccx*P[1][4]*(2*q1*SH_ACCX[1] + 2*q2*SH_ACCX[2] + 2*q3*vd) - Kaccx*P[2][4]*(2*q2*SH_ACCX[1] - 2*q1*SH_ACCX[2] + 2*q0*vd) + Kaccx*P[3][4]*(2*q0*SH_ACCX[2] - 2*q3*SH_ACCX[1] + 2*q1*vd)) + Kaccx*SH_ACCX[3]*(Kaccx*P[4][5]*SH_ACCX[0] + Kaccx*P[5][5]*SH_ACCX[3] - Kaccx*P[22][5]*SH_ACCX[0] - Kaccx*P[23][5]*SH_ACCX[3] - Kaccx*P[6][5]*(2*q0*q2 - 2*q1*q3) + Kaccx*P[0][5]*(2*q0*SH_ACCX[1] + 2*q3*SH_ACCX[2] - 2*q2*vd) + Kaccx*P[1][5]*(2*q1*SH_ACCX[1] + 2*q2*SH_ACCX[2] + 2*q3*vd) - Kaccx*P[2][5]*(2*q2*SH_ACCX[1] - 2*q1*SH_ACCX[2] + 2*q0*vd) + Kaccx*P[3][5]*(2*q0*SH_ACCX[2] - 2*q3*SH_ACCX[1] + 2*q1*vd)) - Kaccx*SH_ACCX[0]*(Kaccx*P[4][22]*SH_ACCX[0] + Kaccx*P[5][22]*SH_ACCX[3] - Kaccx*P[22][22]*SH_ACCX[0] - Kaccx*P[23][22]*SH_ACCX[3] - Kaccx*P[6][22]*(2*q0*q2 - 2*q1*q3) + Kaccx*P[0][22]*(2*q0*SH_ACCX[1] + 2*q3*SH_ACCX[2] - 2*q2*vd) + Kaccx*P[1][22]*(2*q1*SH_ACCX[1] + 2*q2*SH_ACCX[2] + 2*q3*vd) - Kaccx*P[2][22]*(2*q2*SH_ACCX[1] - 2*q1*SH_ACCX[2] + 2*q0*vd) + Kaccx*P[3][22]*(2*q0*SH_ACCX[2] - 2*q3*SH_ACCX[1] + 2*q1*vd)) - Kaccx*SH_ACCX[3]*(Kaccx*P[4][23]*SH_ACCX[0] + Kaccx*P[5][23]*SH_ACCX[3] - Kaccx*P[22][23]*SH_ACCX[0] - Kaccx*P[23][23]*SH_ACCX[3] - Kaccx*P[6][23]*(2*q0*q2 - 2*q1*q3) + Kaccx*P[0][23]*(2*q0*SH_ACCX[1] + 2*q3*SH_ACCX[2] - 2*q2*vd) + Kaccx*P[1][23]*(2*q1*SH_ACCX[1] + 2*q2*SH_ACCX[2] + 2*q3*vd) - Kaccx*P[2][23]*(2*q2*SH_ACCX[1] - 2*q1*SH_ACCX[2] + 2*q0*vd) + Kaccx*P[3][23]*(2*q0*SH_ACCX[2] - 2*q3*SH_ACCX[1] + 2*q1*vd)) - Kaccx*(2*q0*q2 - 2*q1*q3)*(Kaccx*P[4][6]*SH_ACCX[0] + Kaccx*P[5][6]*SH_ACCX[3] - Kaccx*P[22][6]*SH_ACCX[0] - Kaccx*P[23][6]*SH_ACCX[3] - Kaccx*P[6][6]*(2*q0*q2 - 2*q1*q3) + Kaccx*P[0][6]*(2*q0*SH_ACCX[1] + 2*q3*SH_ACCX[2] - 2*q2*vd) + Kaccx*P[1][6]*(2*q1*SH_ACCX[1] + 2*q2*SH_ACCX[2] + 2*q3*vd) - Kaccx*P[2][6]*(2*q2*SH_ACCX[1] - 2*q1*SH_ACCX[2] + 2*q0*vd) + Kaccx*P[3][6]*(2*q0*SH_ACCX[2] - 2*q3*SH_ACCX[1] + 2*q1*vd)) + Kaccx*(2*q0*SH_ACCX[1] + 2*q3*SH_ACCX[2] - 2*q2*vd)*(Kaccx*P[4][0]*SH_ACCX[0] + Kaccx*P[5][0]*SH_ACCX[3] - Kaccx*P[22][0]*SH_ACCX[0] - Kaccx*P[23][0]*SH_ACCX[3] - Kaccx*P[6][0]*(2*q0*q2 - 2*q1*q3) + Kaccx*P[0][0]*(2*q0*SH_ACCX[1] + 2*q3*SH_ACCX[2] - 2*q2*vd) + Kaccx*P[1][0]*(2*q1*SH_ACCX[1] + 2*q2*SH_ACCX[2] + 2*q3*vd) - Kaccx*P[2][0]*(2*q2*SH_ACCX[1] - 2*q1*SH_ACCX[2] + 2*q0*vd) + Kaccx*P[3][0]*(2*q0*SH_ACCX[2] - 2*q3*SH_ACCX[1] + 2*q1*vd)) + Kaccx*(2*q1*SH_ACCX[1] + 2*q2*SH_ACCX[2] + 2*q3*vd)*(Kaccx*P[4][1]*SH_ACCX[0] + Kaccx*P[5][1]*SH_ACCX[3] - Kaccx*P[22][1]*SH_ACCX[0] - Kaccx*P[23][1]*SH_ACCX[3] - Kaccx*P[6][1]*(2*q0*q2 - 2*q1*q3) + Kaccx*P[0][1]*(2*q0*SH_ACCX[1] + 2*q3*SH_ACCX[2] - 2*q2*vd) + Kaccx*P[1][1]*(2*q1*SH_ACCX[1] + 2*q2*SH_ACCX[2] + 2*q3*vd) - Kaccx*P[2][1]*(2*q2*SH_ACCX[1] - 2*q1*SH_ACCX[2] + 2*q0*vd) + Kaccx*P[3][1]*(2*q0*SH_ACCX[2] - 2*q3*SH_ACCX[1] + 2*q1*vd)) - Kaccx*(2*q2*SH_ACCX[1] - 2*q1*SH_ACCX[2] + 2*q0*vd)*(Kaccx*P[4][2]*SH_ACCX[0] + Kaccx*P[5][2]*SH_ACCX[3] - Kaccx*P[22][2]*SH_ACCX[0] - Kaccx*P[23][2]*SH_ACCX[3] - Kaccx*P[6][2]*(2*q0*q2 - 2*q1*q3) + Kaccx*P[0][2]*(2*q0*SH_ACCX[1] + 2*q3*SH_ACCX[2] - 2*q2*vd) + Kaccx*P[1][2]*(2*q1*SH_ACCX[1] + 2*q2*SH_ACCX[2] + 2*q3*vd) - Kaccx*P[2][2]*(2*q2*SH_ACCX[1] - 2*q1*SH_ACCX[2] + 2*q0*vd) + Kaccx*P[3][2]*(2*q0*SH_ACCX[2] - 2*q3*SH_ACCX[1] + 2*q1*vd)) + Kaccx*(2*q0*SH_ACCX[2] - 2*q3*SH_ACCX[1] + 2*q1*vd)*(Kaccx*P[4][3]*SH_ACCX[0] + Kaccx*P[5][3]*SH_ACCX[3] - Kaccx*P[22][3]*SH_ACCX[0] - Kaccx*P[23][3]*SH_ACCX[3] - Kaccx*P[6][3]*(2*q0*q2 - 2*q1*q3) + Kaccx*P[0][3]*(2*q0*SH_ACCX[1] + 2*q3*SH_ACCX[2] - 2*q2*vd) + Kaccx*P[1][3]*(2*q1*SH_ACCX[1] + 2*q2*SH_ACCX[2] + 2*q3*vd) - Kaccx*P[2][3]*(2*q2*SH_ACCX[1] - 2*q1*SH_ACCX[2] + 2*q0*vd) + Kaccx*P[3][3]*(2*q0*SH_ACCX[2] - 2*q3*SH_ACCX[1] + 2*q1*vd))); +SK_ACCX[1] = 2*q0*SH_ACCX[2] - 2*q3*SH_ACCX[1] + 2*q1*vd; +SK_ACCX[2] = 2*q2*SH_ACCX[1] - 2*q1*SH_ACCX[2] + 2*q0*vd; +SK_ACCX[3] = 2*q0*SH_ACCX[1] + 2*q3*SH_ACCX[2] - 2*q2*vd; +SK_ACCX[4] = 2*q1*SH_ACCX[1] + 2*q2*SH_ACCX[2] + 2*q3*vd; +SK_ACCX[5] = 2*q0*q2 - 2*q1*q3; +SK_ACCX[6] = SH_ACCX[3]; float Kfusion[24][1]; float Kfusion[1][1]; -Kfusion[0] = SK_ACCX[0]*(Kaccx*P[0][22]*SH_ACCX[0] - Kaccx*P[0][3]*SH_ACCX[0] + Kaccx*P[0][1]*SK_ACCX[2] + Kaccx*P[0][2]*SK_ACCX[1] - Kaccx*P[0][4]*SK_ACCX[4] + Kaccx*P[0][5]*SK_ACCX[3] + Kaccx*P[0][23]*SK_ACCX[4]); -Kfusion[1] = SK_ACCX[0]*(Kaccx*P[1][22]*SH_ACCX[0] - Kaccx*P[1][3]*SH_ACCX[0] + Kaccx*P[1][1]*SK_ACCX[2] + Kaccx*P[1][2]*SK_ACCX[1] - Kaccx*P[1][4]*SK_ACCX[4] + Kaccx*P[1][5]*SK_ACCX[3] + Kaccx*P[1][23]*SK_ACCX[4]); -Kfusion[2] = SK_ACCX[0]*(Kaccx*P[2][22]*SH_ACCX[0] - Kaccx*P[2][3]*SH_ACCX[0] + Kaccx*P[2][1]*SK_ACCX[2] + Kaccx*P[2][2]*SK_ACCX[1] - Kaccx*P[2][4]*SK_ACCX[4] + Kaccx*P[2][5]*SK_ACCX[3] + Kaccx*P[2][23]*SK_ACCX[4]); -Kfusion[3] = SK_ACCX[0]*(Kaccx*P[3][22]*SH_ACCX[0] - Kaccx*P[3][3]*SH_ACCX[0] + Kaccx*P[3][1]*SK_ACCX[2] + Kaccx*P[3][2]*SK_ACCX[1] - Kaccx*P[3][4]*SK_ACCX[4] + Kaccx*P[3][5]*SK_ACCX[3] + Kaccx*P[3][23]*SK_ACCX[4]); -Kfusion[4] = SK_ACCX[0]*(Kaccx*P[4][22]*SH_ACCX[0] - Kaccx*P[4][3]*SH_ACCX[0] + Kaccx*P[4][1]*SK_ACCX[2] + Kaccx*P[4][2]*SK_ACCX[1] - Kaccx*P[4][4]*SK_ACCX[4] + Kaccx*P[4][5]*SK_ACCX[3] + Kaccx*P[4][23]*SK_ACCX[4]); -Kfusion[5] = SK_ACCX[0]*(Kaccx*P[5][22]*SH_ACCX[0] - Kaccx*P[5][3]*SH_ACCX[0] + Kaccx*P[5][1]*SK_ACCX[2] + Kaccx*P[5][2]*SK_ACCX[1] - Kaccx*P[5][4]*SK_ACCX[4] + Kaccx*P[5][5]*SK_ACCX[3] + Kaccx*P[5][23]*SK_ACCX[4]); -Kfusion[6] = SK_ACCX[0]*(Kaccx*P[6][22]*SH_ACCX[0] - Kaccx*P[6][3]*SH_ACCX[0] + Kaccx*P[6][1]*SK_ACCX[2] + Kaccx*P[6][2]*SK_ACCX[1] - Kaccx*P[6][4]*SK_ACCX[4] + Kaccx*P[6][5]*SK_ACCX[3] + Kaccx*P[6][23]*SK_ACCX[4]); -Kfusion[7] = SK_ACCX[0]*(Kaccx*P[7][22]*SH_ACCX[0] - Kaccx*P[7][3]*SH_ACCX[0] + Kaccx*P[7][1]*SK_ACCX[2] + Kaccx*P[7][2]*SK_ACCX[1] - Kaccx*P[7][4]*SK_ACCX[4] + Kaccx*P[7][5]*SK_ACCX[3] + Kaccx*P[7][23]*SK_ACCX[4]); -Kfusion[8] = SK_ACCX[0]*(Kaccx*P[8][22]*SH_ACCX[0] - Kaccx*P[8][3]*SH_ACCX[0] + Kaccx*P[8][1]*SK_ACCX[2] + Kaccx*P[8][2]*SK_ACCX[1] - Kaccx*P[8][4]*SK_ACCX[4] + Kaccx*P[8][5]*SK_ACCX[3] + Kaccx*P[8][23]*SK_ACCX[4]); -Kfusion[9] = SK_ACCX[0]*(Kaccx*P[9][22]*SH_ACCX[0] - Kaccx*P[9][3]*SH_ACCX[0] + Kaccx*P[9][1]*SK_ACCX[2] + Kaccx*P[9][2]*SK_ACCX[1] - Kaccx*P[9][4]*SK_ACCX[4] + Kaccx*P[9][5]*SK_ACCX[3] + Kaccx*P[9][23]*SK_ACCX[4]); -Kfusion[10] = SK_ACCX[0]*(Kaccx*P[10][22]*SH_ACCX[0] - Kaccx*P[10][3]*SH_ACCX[0] + Kaccx*P[10][1]*SK_ACCX[2] + Kaccx*P[10][2]*SK_ACCX[1] - Kaccx*P[10][4]*SK_ACCX[4] + Kaccx*P[10][5]*SK_ACCX[3] + Kaccx*P[10][23]*SK_ACCX[4]); -Kfusion[11] = SK_ACCX[0]*(Kaccx*P[11][22]*SH_ACCX[0] - Kaccx*P[11][3]*SH_ACCX[0] + Kaccx*P[11][1]*SK_ACCX[2] + Kaccx*P[11][2]*SK_ACCX[1] - Kaccx*P[11][4]*SK_ACCX[4] + Kaccx*P[11][5]*SK_ACCX[3] + Kaccx*P[11][23]*SK_ACCX[4]); -Kfusion[12] = SK_ACCX[0]*(Kaccx*P[12][22]*SH_ACCX[0] - Kaccx*P[12][3]*SH_ACCX[0] + Kaccx*P[12][1]*SK_ACCX[2] + Kaccx*P[12][2]*SK_ACCX[1] - Kaccx*P[12][4]*SK_ACCX[4] + Kaccx*P[12][5]*SK_ACCX[3] + Kaccx*P[12][23]*SK_ACCX[4]); -Kfusion[13] = SK_ACCX[0]*(Kaccx*P[13][22]*SH_ACCX[0] - Kaccx*P[13][3]*SH_ACCX[0] + Kaccx*P[13][1]*SK_ACCX[2] + Kaccx*P[13][2]*SK_ACCX[1] - Kaccx*P[13][4]*SK_ACCX[4] + Kaccx*P[13][5]*SK_ACCX[3] + Kaccx*P[13][23]*SK_ACCX[4]); -Kfusion[14] = SK_ACCX[0]*(Kaccx*P[14][22]*SH_ACCX[0] - Kaccx*P[14][3]*SH_ACCX[0] + Kaccx*P[14][1]*SK_ACCX[2] + Kaccx*P[14][2]*SK_ACCX[1] - Kaccx*P[14][4]*SK_ACCX[4] + Kaccx*P[14][5]*SK_ACCX[3] + Kaccx*P[14][23]*SK_ACCX[4]); -Kfusion[15] = SK_ACCX[0]*(Kaccx*P[15][22]*SH_ACCX[0] - Kaccx*P[15][3]*SH_ACCX[0] + Kaccx*P[15][1]*SK_ACCX[2] + Kaccx*P[15][2]*SK_ACCX[1] - Kaccx*P[15][4]*SK_ACCX[4] + Kaccx*P[15][5]*SK_ACCX[3] + Kaccx*P[15][23]*SK_ACCX[4]); -Kfusion[16] = SK_ACCX[0]*(Kaccx*P[16][22]*SH_ACCX[0] - Kaccx*P[16][3]*SH_ACCX[0] + Kaccx*P[16][1]*SK_ACCX[2] + Kaccx*P[16][2]*SK_ACCX[1] - Kaccx*P[16][4]*SK_ACCX[4] + Kaccx*P[16][5]*SK_ACCX[3] + Kaccx*P[16][23]*SK_ACCX[4]); -Kfusion[17] = SK_ACCX[0]*(Kaccx*P[17][22]*SH_ACCX[0] - Kaccx*P[17][3]*SH_ACCX[0] + Kaccx*P[17][1]*SK_ACCX[2] + Kaccx*P[17][2]*SK_ACCX[1] - Kaccx*P[17][4]*SK_ACCX[4] + Kaccx*P[17][5]*SK_ACCX[3] + Kaccx*P[17][23]*SK_ACCX[4]); -Kfusion[18] = SK_ACCX[0]*(Kaccx*P[18][22]*SH_ACCX[0] - Kaccx*P[18][3]*SH_ACCX[0] + Kaccx*P[18][1]*SK_ACCX[2] + Kaccx*P[18][2]*SK_ACCX[1] - Kaccx*P[18][4]*SK_ACCX[4] + Kaccx*P[18][5]*SK_ACCX[3] + Kaccx*P[18][23]*SK_ACCX[4]); -Kfusion[19] = SK_ACCX[0]*(Kaccx*P[19][22]*SH_ACCX[0] - Kaccx*P[19][3]*SH_ACCX[0] + Kaccx*P[19][1]*SK_ACCX[2] + Kaccx*P[19][2]*SK_ACCX[1] - Kaccx*P[19][4]*SK_ACCX[4] + Kaccx*P[19][5]*SK_ACCX[3] + Kaccx*P[19][23]*SK_ACCX[4]); -Kfusion[20] = SK_ACCX[0]*(Kaccx*P[20][22]*SH_ACCX[0] - Kaccx*P[20][3]*SH_ACCX[0] + Kaccx*P[20][1]*SK_ACCX[2] + Kaccx*P[20][2]*SK_ACCX[1] - Kaccx*P[20][4]*SK_ACCX[4] + Kaccx*P[20][5]*SK_ACCX[3] + Kaccx*P[20][23]*SK_ACCX[4]); -Kfusion[21] = SK_ACCX[0]*(Kaccx*P[21][22]*SH_ACCX[0] - Kaccx*P[21][3]*SH_ACCX[0] + Kaccx*P[21][1]*SK_ACCX[2] + Kaccx*P[21][2]*SK_ACCX[1] - Kaccx*P[21][4]*SK_ACCX[4] + Kaccx*P[21][5]*SK_ACCX[3] + Kaccx*P[21][23]*SK_ACCX[4]); -Kfusion[22] = SK_ACCX[0]*(Kaccx*P[22][22]*SH_ACCX[0] - Kaccx*P[22][3]*SH_ACCX[0] + Kaccx*P[22][1]*SK_ACCX[2] + Kaccx*P[22][2]*SK_ACCX[1] - Kaccx*P[22][4]*SK_ACCX[4] + Kaccx*P[22][5]*SK_ACCX[3] + Kaccx*P[22][23]*SK_ACCX[4]); -Kfusion[23] = SK_ACCX[0]*(Kaccx*P[23][22]*SH_ACCX[0] - Kaccx*P[23][3]*SH_ACCX[0] + Kaccx*P[23][1]*SK_ACCX[2] + Kaccx*P[23][2]*SK_ACCX[1] - Kaccx*P[23][4]*SK_ACCX[4] + Kaccx*P[23][5]*SK_ACCX[3] + Kaccx*P[23][23]*SK_ACCX[4]); -float SH_ACCY[6][1]; +Kfusion[0] = -SK_ACCX[0]*(Kaccx*P[0][4]*SH_ACCX[0] - Kaccx*P[0][22]*SH_ACCX[0] + Kaccx*P[0][0]*SK_ACCX[3] - Kaccx*P[0][2]*SK_ACCX[2] + Kaccx*P[0][3]*SK_ACCX[1] + Kaccx*P[0][1]*SK_ACCX[4] + Kaccx*P[0][5]*SK_ACCX[6] - Kaccx*P[0][6]*SK_ACCX[5] - Kaccx*P[0][23]*SK_ACCX[6]); +Kfusion[1] = -SK_ACCX[0]*(Kaccx*P[1][4]*SH_ACCX[0] - Kaccx*P[1][22]*SH_ACCX[0] + Kaccx*P[1][0]*SK_ACCX[3] - Kaccx*P[1][2]*SK_ACCX[2] + Kaccx*P[1][3]*SK_ACCX[1] + Kaccx*P[1][1]*SK_ACCX[4] + Kaccx*P[1][5]*SK_ACCX[6] - Kaccx*P[1][6]*SK_ACCX[5] - Kaccx*P[1][23]*SK_ACCX[6]); +Kfusion[2] = -SK_ACCX[0]*(Kaccx*P[2][4]*SH_ACCX[0] - Kaccx*P[2][22]*SH_ACCX[0] + Kaccx*P[2][0]*SK_ACCX[3] - Kaccx*P[2][2]*SK_ACCX[2] + Kaccx*P[2][3]*SK_ACCX[1] + Kaccx*P[2][1]*SK_ACCX[4] + Kaccx*P[2][5]*SK_ACCX[6] - Kaccx*P[2][6]*SK_ACCX[5] - Kaccx*P[2][23]*SK_ACCX[6]); +Kfusion[3] = -SK_ACCX[0]*(Kaccx*P[3][4]*SH_ACCX[0] - Kaccx*P[3][22]*SH_ACCX[0] + Kaccx*P[3][0]*SK_ACCX[3] - Kaccx*P[3][2]*SK_ACCX[2] + Kaccx*P[3][3]*SK_ACCX[1] + Kaccx*P[3][1]*SK_ACCX[4] + Kaccx*P[3][5]*SK_ACCX[6] - Kaccx*P[3][6]*SK_ACCX[5] - Kaccx*P[3][23]*SK_ACCX[6]); +Kfusion[4] = -SK_ACCX[0]*(Kaccx*P[4][4]*SH_ACCX[0] - Kaccx*P[4][22]*SH_ACCX[0] + Kaccx*P[4][0]*SK_ACCX[3] - Kaccx*P[4][2]*SK_ACCX[2] + Kaccx*P[4][3]*SK_ACCX[1] + Kaccx*P[4][1]*SK_ACCX[4] + Kaccx*P[4][5]*SK_ACCX[6] - Kaccx*P[4][6]*SK_ACCX[5] - Kaccx*P[4][23]*SK_ACCX[6]); +Kfusion[5] = -SK_ACCX[0]*(Kaccx*P[5][4]*SH_ACCX[0] - Kaccx*P[5][22]*SH_ACCX[0] + Kaccx*P[5][0]*SK_ACCX[3] - Kaccx*P[5][2]*SK_ACCX[2] + Kaccx*P[5][3]*SK_ACCX[1] + Kaccx*P[5][1]*SK_ACCX[4] + Kaccx*P[5][5]*SK_ACCX[6] - Kaccx*P[5][6]*SK_ACCX[5] - Kaccx*P[5][23]*SK_ACCX[6]); +Kfusion[6] = -SK_ACCX[0]*(Kaccx*P[6][4]*SH_ACCX[0] - Kaccx*P[6][22]*SH_ACCX[0] + Kaccx*P[6][0]*SK_ACCX[3] - Kaccx*P[6][2]*SK_ACCX[2] + Kaccx*P[6][3]*SK_ACCX[1] + Kaccx*P[6][1]*SK_ACCX[4] + Kaccx*P[6][5]*SK_ACCX[6] - Kaccx*P[6][6]*SK_ACCX[5] - Kaccx*P[6][23]*SK_ACCX[6]); +Kfusion[7] = -SK_ACCX[0]*(Kaccx*P[7][4]*SH_ACCX[0] - Kaccx*P[7][22]*SH_ACCX[0] + Kaccx*P[7][0]*SK_ACCX[3] - Kaccx*P[7][2]*SK_ACCX[2] + Kaccx*P[7][3]*SK_ACCX[1] + Kaccx*P[7][1]*SK_ACCX[4] + Kaccx*P[7][5]*SK_ACCX[6] - Kaccx*P[7][6]*SK_ACCX[5] - Kaccx*P[7][23]*SK_ACCX[6]); +Kfusion[8] = -SK_ACCX[0]*(Kaccx*P[8][4]*SH_ACCX[0] - Kaccx*P[8][22]*SH_ACCX[0] + Kaccx*P[8][0]*SK_ACCX[3] - Kaccx*P[8][2]*SK_ACCX[2] + Kaccx*P[8][3]*SK_ACCX[1] + Kaccx*P[8][1]*SK_ACCX[4] + Kaccx*P[8][5]*SK_ACCX[6] - Kaccx*P[8][6]*SK_ACCX[5] - Kaccx*P[8][23]*SK_ACCX[6]); +Kfusion[9] = -SK_ACCX[0]*(Kaccx*P[9][4]*SH_ACCX[0] - Kaccx*P[9][22]*SH_ACCX[0] + Kaccx*P[9][0]*SK_ACCX[3] - Kaccx*P[9][2]*SK_ACCX[2] + Kaccx*P[9][3]*SK_ACCX[1] + Kaccx*P[9][1]*SK_ACCX[4] + Kaccx*P[9][5]*SK_ACCX[6] - Kaccx*P[9][6]*SK_ACCX[5] - Kaccx*P[9][23]*SK_ACCX[6]); +Kfusion[10] = -SK_ACCX[0]*(Kaccx*P[10][4]*SH_ACCX[0] - Kaccx*P[10][22]*SH_ACCX[0] + Kaccx*P[10][0]*SK_ACCX[3] - Kaccx*P[10][2]*SK_ACCX[2] + Kaccx*P[10][3]*SK_ACCX[1] + Kaccx*P[10][1]*SK_ACCX[4] + Kaccx*P[10][5]*SK_ACCX[6] - Kaccx*P[10][6]*SK_ACCX[5] - Kaccx*P[10][23]*SK_ACCX[6]); +Kfusion[11] = -SK_ACCX[0]*(Kaccx*P[11][4]*SH_ACCX[0] - Kaccx*P[11][22]*SH_ACCX[0] + Kaccx*P[11][0]*SK_ACCX[3] - Kaccx*P[11][2]*SK_ACCX[2] + Kaccx*P[11][3]*SK_ACCX[1] + Kaccx*P[11][1]*SK_ACCX[4] + Kaccx*P[11][5]*SK_ACCX[6] - Kaccx*P[11][6]*SK_ACCX[5] - Kaccx*P[11][23]*SK_ACCX[6]); +Kfusion[12] = -SK_ACCX[0]*(Kaccx*P[12][4]*SH_ACCX[0] - Kaccx*P[12][22]*SH_ACCX[0] + Kaccx*P[12][0]*SK_ACCX[3] - Kaccx*P[12][2]*SK_ACCX[2] + Kaccx*P[12][3]*SK_ACCX[1] + Kaccx*P[12][1]*SK_ACCX[4] + Kaccx*P[12][5]*SK_ACCX[6] - Kaccx*P[12][6]*SK_ACCX[5] - Kaccx*P[12][23]*SK_ACCX[6]); +Kfusion[13] = -SK_ACCX[0]*(Kaccx*P[13][4]*SH_ACCX[0] - Kaccx*P[13][22]*SH_ACCX[0] + Kaccx*P[13][0]*SK_ACCX[3] - Kaccx*P[13][2]*SK_ACCX[2] + Kaccx*P[13][3]*SK_ACCX[1] + Kaccx*P[13][1]*SK_ACCX[4] + Kaccx*P[13][5]*SK_ACCX[6] - Kaccx*P[13][6]*SK_ACCX[5] - Kaccx*P[13][23]*SK_ACCX[6]); +Kfusion[14] = -SK_ACCX[0]*(Kaccx*P[14][4]*SH_ACCX[0] - Kaccx*P[14][22]*SH_ACCX[0] + Kaccx*P[14][0]*SK_ACCX[3] - Kaccx*P[14][2]*SK_ACCX[2] + Kaccx*P[14][3]*SK_ACCX[1] + Kaccx*P[14][1]*SK_ACCX[4] + Kaccx*P[14][5]*SK_ACCX[6] - Kaccx*P[14][6]*SK_ACCX[5] - Kaccx*P[14][23]*SK_ACCX[6]); +Kfusion[15] = -SK_ACCX[0]*(Kaccx*P[15][4]*SH_ACCX[0] - Kaccx*P[15][22]*SH_ACCX[0] + Kaccx*P[15][0]*SK_ACCX[3] - Kaccx*P[15][2]*SK_ACCX[2] + Kaccx*P[15][3]*SK_ACCX[1] + Kaccx*P[15][1]*SK_ACCX[4] + Kaccx*P[15][5]*SK_ACCX[6] - Kaccx*P[15][6]*SK_ACCX[5] - Kaccx*P[15][23]*SK_ACCX[6]); +Kfusion[16] = -SK_ACCX[0]*(Kaccx*P[16][4]*SH_ACCX[0] - Kaccx*P[16][22]*SH_ACCX[0] + Kaccx*P[16][0]*SK_ACCX[3] - Kaccx*P[16][2]*SK_ACCX[2] + Kaccx*P[16][3]*SK_ACCX[1] + Kaccx*P[16][1]*SK_ACCX[4] + Kaccx*P[16][5]*SK_ACCX[6] - Kaccx*P[16][6]*SK_ACCX[5] - Kaccx*P[16][23]*SK_ACCX[6]); +Kfusion[17] = -SK_ACCX[0]*(Kaccx*P[17][4]*SH_ACCX[0] - Kaccx*P[17][22]*SH_ACCX[0] + Kaccx*P[17][0]*SK_ACCX[3] - Kaccx*P[17][2]*SK_ACCX[2] + Kaccx*P[17][3]*SK_ACCX[1] + Kaccx*P[17][1]*SK_ACCX[4] + Kaccx*P[17][5]*SK_ACCX[6] - Kaccx*P[17][6]*SK_ACCX[5] - Kaccx*P[17][23]*SK_ACCX[6]); +Kfusion[18] = -SK_ACCX[0]*(Kaccx*P[18][4]*SH_ACCX[0] - Kaccx*P[18][22]*SH_ACCX[0] + Kaccx*P[18][0]*SK_ACCX[3] - Kaccx*P[18][2]*SK_ACCX[2] + Kaccx*P[18][3]*SK_ACCX[1] + Kaccx*P[18][1]*SK_ACCX[4] + Kaccx*P[18][5]*SK_ACCX[6] - Kaccx*P[18][6]*SK_ACCX[5] - Kaccx*P[18][23]*SK_ACCX[6]); +Kfusion[19] = -SK_ACCX[0]*(Kaccx*P[19][4]*SH_ACCX[0] - Kaccx*P[19][22]*SH_ACCX[0] + Kaccx*P[19][0]*SK_ACCX[3] - Kaccx*P[19][2]*SK_ACCX[2] + Kaccx*P[19][3]*SK_ACCX[1] + Kaccx*P[19][1]*SK_ACCX[4] + Kaccx*P[19][5]*SK_ACCX[6] - Kaccx*P[19][6]*SK_ACCX[5] - Kaccx*P[19][23]*SK_ACCX[6]); +Kfusion[20] = -SK_ACCX[0]*(Kaccx*P[20][4]*SH_ACCX[0] - Kaccx*P[20][22]*SH_ACCX[0] + Kaccx*P[20][0]*SK_ACCX[3] - Kaccx*P[20][2]*SK_ACCX[2] + Kaccx*P[20][3]*SK_ACCX[1] + Kaccx*P[20][1]*SK_ACCX[4] + Kaccx*P[20][5]*SK_ACCX[6] - Kaccx*P[20][6]*SK_ACCX[5] - Kaccx*P[20][23]*SK_ACCX[6]); +Kfusion[21] = -SK_ACCX[0]*(Kaccx*P[21][4]*SH_ACCX[0] - Kaccx*P[21][22]*SH_ACCX[0] + Kaccx*P[21][0]*SK_ACCX[3] - Kaccx*P[21][2]*SK_ACCX[2] + Kaccx*P[21][3]*SK_ACCX[1] + Kaccx*P[21][1]*SK_ACCX[4] + Kaccx*P[21][5]*SK_ACCX[6] - Kaccx*P[21][6]*SK_ACCX[5] - Kaccx*P[21][23]*SK_ACCX[6]); +Kfusion[22] = -SK_ACCX[0]*(Kaccx*P[22][4]*SH_ACCX[0] - Kaccx*P[22][22]*SH_ACCX[0] + Kaccx*P[22][0]*SK_ACCX[3] - Kaccx*P[22][2]*SK_ACCX[2] + Kaccx*P[22][3]*SK_ACCX[1] + Kaccx*P[22][1]*SK_ACCX[4] + Kaccx*P[22][5]*SK_ACCX[6] - Kaccx*P[22][6]*SK_ACCX[5] - Kaccx*P[22][23]*SK_ACCX[6]); +Kfusion[23] = -SK_ACCX[0]*(Kaccx*P[23][4]*SH_ACCX[0] - Kaccx*P[23][22]*SH_ACCX[0] + Kaccx*P[23][0]*SK_ACCX[3] - Kaccx*P[23][2]*SK_ACCX[2] + Kaccx*P[23][3]*SK_ACCX[1] + Kaccx*P[23][1]*SK_ACCX[4] + Kaccx*P[23][5]*SK_ACCX[6] - Kaccx*P[23][6]*SK_ACCX[5] - Kaccx*P[23][23]*SK_ACCX[6]); +float SH_ACCY[3][1]; SH_ACCY[0] = sq(q0) - sq(q1) + sq(q2) - sq(q3); -SH_ACCY[1] = ve - vwe; -SH_ACCY[2] = vn - vwn; -SH_ACCY[3] = sq(q1); -SH_ACCY[4] = 2*q0*q1; -SH_ACCY[5] = 2*q0*q3; +SH_ACCY[1] = vn - vwn; +SH_ACCY[2] = ve - vwe; float H_ACCY[1][24]; -H_ACCY[0][0] = Kaccy*(SH_ACCY[1]*(SH_ACCY[4] - 2*q2*q3) - SH_ACCY[2]*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY[3] - sq(q0) + sq(q2) - sq(q3))); -H_ACCY[0][2] = Kaccy*(SH_ACCY[2]*(SH_ACCY[3] + sq(q0) - sq(q2) - sq(q3)) + SH_ACCY[1]*(SH_ACCY[5] + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)); -H_ACCY[0][3] = Kaccy*(SH_ACCY[5] - 2*q1*q2); -H_ACCY[0][4] = -Kaccy*SH_ACCY[0]; -H_ACCY[0][5] = -Kaccy*(SH_ACCY[4] + 2*q2*q3); +H_ACCY[0][0] = -Kaccy*(2*q0*SH_ACCY[2] - 2*q3*SH_ACCY[1] + 2*q1*vd); +H_ACCY[0][1] = -Kaccy*(2*q2*SH_ACCY[1] - 2*q1*SH_ACCY[2] + 2*q0*vd); +H_ACCY[0][2] = -Kaccy*(2*q1*SH_ACCY[1] + 2*q2*SH_ACCY[2] + 2*q3*vd); +H_ACCY[0][3] = Kaccy*(2*q0*SH_ACCY[1] + 2*q3*SH_ACCY[2] - 2*q2*vd); +H_ACCY[0][4] = Kaccy*(2*q0*q3 - 2*q1*q2); +H_ACCY[0][5] = -Kaccy*SH_ACCY[0]; +H_ACCY[0][6] = -Kaccy*(2*q0*q1 + 2*q2*q3); H_ACCY[0][22] = -2*Kaccy*(q0*q3 - q1*q2); H_ACCY[0][23] = Kaccy*SH_ACCY[0]; -float SK_ACCY[7][1]; -SK_ACCY[0] = 1/(R_ACC - Kaccy*SH_ACCY[0]*(Kaccy*P[23][4]*SH_ACCY[0] - Kaccy*P[4][4]*SH_ACCY[0] + Kaccy*P[0][4]*(SH_ACCY[1]*(SH_ACCY[4] - 2*q2*q3) - SH_ACCY[2]*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY[3] - sq(q0) + sq(q2) - sq(q3))) + Kaccy*P[2][4]*(SH_ACCY[2]*(SH_ACCY[3] + sq(q0) - sq(q2) - sq(q3)) + SH_ACCY[1]*(SH_ACCY[5] + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)) - 2*Kaccy*P[22][4]*(q0*q3 - q1*q2) + Kaccy*P[3][4]*(SH_ACCY[5] - 2*q1*q2) - Kaccy*P[5][4]*(SH_ACCY[4] + 2*q2*q3)) + Kaccy*SH_ACCY[0]*(Kaccy*P[23][23]*SH_ACCY[0] - Kaccy*P[4][23]*SH_ACCY[0] + Kaccy*P[0][23]*(SH_ACCY[1]*(SH_ACCY[4] - 2*q2*q3) - SH_ACCY[2]*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY[3] - sq(q0) + sq(q2) - sq(q3))) + Kaccy*P[2][23]*(SH_ACCY[2]*(SH_ACCY[3] + sq(q0) - sq(q2) - sq(q3)) + SH_ACCY[1]*(SH_ACCY[5] + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)) - 2*Kaccy*P[22][23]*(q0*q3 - q1*q2) + Kaccy*P[3][23]*(SH_ACCY[5] - 2*q1*q2) - Kaccy*P[5][23]*(SH_ACCY[4] + 2*q2*q3)) + Kaccy*(SH_ACCY[1]*(SH_ACCY[4] - 2*q2*q3) - SH_ACCY[2]*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY[3] - sq(q0) + sq(q2) - sq(q3)))*(Kaccy*P[23][0]*SH_ACCY[0] - Kaccy*P[4][0]*SH_ACCY[0] + Kaccy*P[0][0]*(SH_ACCY[1]*(SH_ACCY[4] - 2*q2*q3) - SH_ACCY[2]*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY[3] - sq(q0) + sq(q2) - sq(q3))) + Kaccy*P[2][0]*(SH_ACCY[2]*(SH_ACCY[3] + sq(q0) - sq(q2) - sq(q3)) + SH_ACCY[1]*(SH_ACCY[5] + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)) - 2*Kaccy*P[22][0]*(q0*q3 - q1*q2) + Kaccy*P[3][0]*(SH_ACCY[5] - 2*q1*q2) - Kaccy*P[5][0]*(SH_ACCY[4] + 2*q2*q3)) + Kaccy*(SH_ACCY[2]*(SH_ACCY[3] + sq(q0) - sq(q2) - sq(q3)) + SH_ACCY[1]*(SH_ACCY[5] + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3))*(Kaccy*P[23][2]*SH_ACCY[0] - Kaccy*P[4][2]*SH_ACCY[0] + Kaccy*P[0][2]*(SH_ACCY[1]*(SH_ACCY[4] - 2*q2*q3) - SH_ACCY[2]*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY[3] - sq(q0) + sq(q2) - sq(q3))) + Kaccy*P[2][2]*(SH_ACCY[2]*(SH_ACCY[3] + sq(q0) - sq(q2) - sq(q3)) + SH_ACCY[1]*(SH_ACCY[5] + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)) - 2*Kaccy*P[22][2]*(q0*q3 - q1*q2) + Kaccy*P[3][2]*(SH_ACCY[5] - 2*q1*q2) - Kaccy*P[5][2]*(SH_ACCY[4] + 2*q2*q3)) - 2*Kaccy*(q0*q3 - q1*q2)*(Kaccy*P[23][22]*SH_ACCY[0] - Kaccy*P[4][22]*SH_ACCY[0] + Kaccy*P[0][22]*(SH_ACCY[1]*(SH_ACCY[4] - 2*q2*q3) - SH_ACCY[2]*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY[3] - sq(q0) + sq(q2) - sq(q3))) + Kaccy*P[2][22]*(SH_ACCY[2]*(SH_ACCY[3] + sq(q0) - sq(q2) - sq(q3)) + SH_ACCY[1]*(SH_ACCY[5] + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)) - 2*Kaccy*P[22][22]*(q0*q3 - q1*q2) + Kaccy*P[3][22]*(SH_ACCY[5] - 2*q1*q2) - Kaccy*P[5][22]*(SH_ACCY[4] + 2*q2*q3)) + Kaccy*(SH_ACCY[5] - 2*q1*q2)*(Kaccy*P[23][3]*SH_ACCY[0] - Kaccy*P[4][3]*SH_ACCY[0] + Kaccy*P[0][3]*(SH_ACCY[1]*(SH_ACCY[4] - 2*q2*q3) - SH_ACCY[2]*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY[3] - sq(q0) + sq(q2) - sq(q3))) + Kaccy*P[2][3]*(SH_ACCY[2]*(SH_ACCY[3] + sq(q0) - sq(q2) - sq(q3)) + SH_ACCY[1]*(SH_ACCY[5] + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)) - 2*Kaccy*P[22][3]*(q0*q3 - q1*q2) + Kaccy*P[3][3]*(SH_ACCY[5] - 2*q1*q2) - Kaccy*P[5][3]*(SH_ACCY[4] + 2*q2*q3)) - Kaccy*(SH_ACCY[4] + 2*q2*q3)*(Kaccy*P[23][5]*SH_ACCY[0] - Kaccy*P[4][5]*SH_ACCY[0] + Kaccy*P[0][5]*(SH_ACCY[1]*(SH_ACCY[4] - 2*q2*q3) - SH_ACCY[2]*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY[3] - sq(q0) + sq(q2) - sq(q3))) + Kaccy*P[2][5]*(SH_ACCY[2]*(SH_ACCY[3] + sq(q0) - sq(q2) - sq(q3)) + SH_ACCY[1]*(SH_ACCY[5] + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)) - 2*Kaccy*P[22][5]*(q0*q3 - q1*q2) + Kaccy*P[3][5]*(SH_ACCY[5] - 2*q1*q2) - Kaccy*P[5][5]*(SH_ACCY[4] + 2*q2*q3))); -SK_ACCY[1] = SH_ACCY[2]*(SH_ACCY[3] + sq(q0) - sq(q2) - sq(q3)) + SH_ACCY[1]*(SH_ACCY[5] + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3); -SK_ACCY[2] = SH_ACCY[1]*(SH_ACCY[4] - 2*q2*q3) - SH_ACCY[2]*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY[3] - sq(q0) + sq(q2) - sq(q3)); -SK_ACCY[3] = q0*q3 - q1*q2; -SK_ACCY[4] = SH_ACCY[5] - 2*q1*q2; -SK_ACCY[5] = SH_ACCY[4] + 2*q2*q3; -SK_ACCY[6] = SH_ACCY[0]; +float SK_ACCY[9][1]; +SK_ACCY[0] = 1/(R_ACC + Kaccy*SH_ACCY[0]*(Kaccy*P[5][5]*SH_ACCY[0] - Kaccy*P[23][5]*SH_ACCY[0] - Kaccy*P[4][5]*(2*q0*q3 - 2*q1*q2) + Kaccy*P[6][5]*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*P[22][5]*(q0*q3 - q1*q2) + Kaccy*P[0][5]*(2*q0*SH_ACCY[2] - 2*q3*SH_ACCY[1] + 2*q1*vd) + Kaccy*P[1][5]*(2*q2*SH_ACCY[1] - 2*q1*SH_ACCY[2] + 2*q0*vd) + Kaccy*P[2][5]*(2*q1*SH_ACCY[1] + 2*q2*SH_ACCY[2] + 2*q3*vd) - Kaccy*P[3][5]*(2*q0*SH_ACCY[1] + 2*q3*SH_ACCY[2] - 2*q2*vd)) - Kaccy*SH_ACCY[0]*(Kaccy*P[5][23]*SH_ACCY[0] - Kaccy*P[23][23]*SH_ACCY[0] - Kaccy*P[4][23]*(2*q0*q3 - 2*q1*q2) + Kaccy*P[6][23]*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*P[22][23]*(q0*q3 - q1*q2) + Kaccy*P[0][23]*(2*q0*SH_ACCY[2] - 2*q3*SH_ACCY[1] + 2*q1*vd) + Kaccy*P[1][23]*(2*q2*SH_ACCY[1] - 2*q1*SH_ACCY[2] + 2*q0*vd) + Kaccy*P[2][23]*(2*q1*SH_ACCY[1] + 2*q2*SH_ACCY[2] + 2*q3*vd) - Kaccy*P[3][23]*(2*q0*SH_ACCY[1] + 2*q3*SH_ACCY[2] - 2*q2*vd)) - Kaccy*(2*q0*q3 - 2*q1*q2)*(Kaccy*P[5][4]*SH_ACCY[0] - Kaccy*P[23][4]*SH_ACCY[0] - Kaccy*P[4][4]*(2*q0*q3 - 2*q1*q2) + Kaccy*P[6][4]*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*P[22][4]*(q0*q3 - q1*q2) + Kaccy*P[0][4]*(2*q0*SH_ACCY[2] - 2*q3*SH_ACCY[1] + 2*q1*vd) + Kaccy*P[1][4]*(2*q2*SH_ACCY[1] - 2*q1*SH_ACCY[2] + 2*q0*vd) + Kaccy*P[2][4]*(2*q1*SH_ACCY[1] + 2*q2*SH_ACCY[2] + 2*q3*vd) - Kaccy*P[3][4]*(2*q0*SH_ACCY[1] + 2*q3*SH_ACCY[2] - 2*q2*vd)) + Kaccy*(2*q0*q1 + 2*q2*q3)*(Kaccy*P[5][6]*SH_ACCY[0] - Kaccy*P[23][6]*SH_ACCY[0] - Kaccy*P[4][6]*(2*q0*q3 - 2*q1*q2) + Kaccy*P[6][6]*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*P[22][6]*(q0*q3 - q1*q2) + Kaccy*P[0][6]*(2*q0*SH_ACCY[2] - 2*q3*SH_ACCY[1] + 2*q1*vd) + Kaccy*P[1][6]*(2*q2*SH_ACCY[1] - 2*q1*SH_ACCY[2] + 2*q0*vd) + Kaccy*P[2][6]*(2*q1*SH_ACCY[1] + 2*q2*SH_ACCY[2] + 2*q3*vd) - Kaccy*P[3][6]*(2*q0*SH_ACCY[1] + 2*q3*SH_ACCY[2] - 2*q2*vd)) + 2*Kaccy*(q0*q3 - q1*q2)*(Kaccy*P[5][22]*SH_ACCY[0] - Kaccy*P[23][22]*SH_ACCY[0] - Kaccy*P[4][22]*(2*q0*q3 - 2*q1*q2) + Kaccy*P[6][22]*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*P[22][22]*(q0*q3 - q1*q2) + Kaccy*P[0][22]*(2*q0*SH_ACCY[2] - 2*q3*SH_ACCY[1] + 2*q1*vd) + Kaccy*P[1][22]*(2*q2*SH_ACCY[1] - 2*q1*SH_ACCY[2] + 2*q0*vd) + Kaccy*P[2][22]*(2*q1*SH_ACCY[1] + 2*q2*SH_ACCY[2] + 2*q3*vd) - Kaccy*P[3][22]*(2*q0*SH_ACCY[1] + 2*q3*SH_ACCY[2] - 2*q2*vd)) + Kaccy*(2*q0*SH_ACCY[2] - 2*q3*SH_ACCY[1] + 2*q1*vd)*(Kaccy*P[5][0]*SH_ACCY[0] - Kaccy*P[23][0]*SH_ACCY[0] - Kaccy*P[4][0]*(2*q0*q3 - 2*q1*q2) + Kaccy*P[6][0]*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*P[22][0]*(q0*q3 - q1*q2) + Kaccy*P[0][0]*(2*q0*SH_ACCY[2] - 2*q3*SH_ACCY[1] + 2*q1*vd) + Kaccy*P[1][0]*(2*q2*SH_ACCY[1] - 2*q1*SH_ACCY[2] + 2*q0*vd) + Kaccy*P[2][0]*(2*q1*SH_ACCY[1] + 2*q2*SH_ACCY[2] + 2*q3*vd) - Kaccy*P[3][0]*(2*q0*SH_ACCY[1] + 2*q3*SH_ACCY[2] - 2*q2*vd)) + Kaccy*(2*q2*SH_ACCY[1] - 2*q1*SH_ACCY[2] + 2*q0*vd)*(Kaccy*P[5][1]*SH_ACCY[0] - Kaccy*P[23][1]*SH_ACCY[0] - Kaccy*P[4][1]*(2*q0*q3 - 2*q1*q2) + Kaccy*P[6][1]*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*P[22][1]*(q0*q3 - q1*q2) + Kaccy*P[0][1]*(2*q0*SH_ACCY[2] - 2*q3*SH_ACCY[1] + 2*q1*vd) + Kaccy*P[1][1]*(2*q2*SH_ACCY[1] - 2*q1*SH_ACCY[2] + 2*q0*vd) + Kaccy*P[2][1]*(2*q1*SH_ACCY[1] + 2*q2*SH_ACCY[2] + 2*q3*vd) - Kaccy*P[3][1]*(2*q0*SH_ACCY[1] + 2*q3*SH_ACCY[2] - 2*q2*vd)) + Kaccy*(2*q1*SH_ACCY[1] + 2*q2*SH_ACCY[2] + 2*q3*vd)*(Kaccy*P[5][2]*SH_ACCY[0] - Kaccy*P[23][2]*SH_ACCY[0] - Kaccy*P[4][2]*(2*q0*q3 - 2*q1*q2) + Kaccy*P[6][2]*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*P[22][2]*(q0*q3 - q1*q2) + Kaccy*P[0][2]*(2*q0*SH_ACCY[2] - 2*q3*SH_ACCY[1] + 2*q1*vd) + Kaccy*P[1][2]*(2*q2*SH_ACCY[1] - 2*q1*SH_ACCY[2] + 2*q0*vd) + Kaccy*P[2][2]*(2*q1*SH_ACCY[1] + 2*q2*SH_ACCY[2] + 2*q3*vd) - Kaccy*P[3][2]*(2*q0*SH_ACCY[1] + 2*q3*SH_ACCY[2] - 2*q2*vd)) - Kaccy*(2*q0*SH_ACCY[1] + 2*q3*SH_ACCY[2] - 2*q2*vd)*(Kaccy*P[5][3]*SH_ACCY[0] - Kaccy*P[23][3]*SH_ACCY[0] - Kaccy*P[4][3]*(2*q0*q3 - 2*q1*q2) + Kaccy*P[6][3]*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*P[22][3]*(q0*q3 - q1*q2) + Kaccy*P[0][3]*(2*q0*SH_ACCY[2] - 2*q3*SH_ACCY[1] + 2*q1*vd) + Kaccy*P[1][3]*(2*q2*SH_ACCY[1] - 2*q1*SH_ACCY[2] + 2*q0*vd) + Kaccy*P[2][3]*(2*q1*SH_ACCY[1] + 2*q2*SH_ACCY[2] + 2*q3*vd) - Kaccy*P[3][3]*(2*q0*SH_ACCY[1] + 2*q3*SH_ACCY[2] - 2*q2*vd))); +SK_ACCY[1] = 2*q0*SH_ACCY[1] + 2*q3*SH_ACCY[2] - 2*q2*vd; +SK_ACCY[2] = 2*q2*SH_ACCY[1] - 2*q1*SH_ACCY[2] + 2*q0*vd; +SK_ACCY[3] = 2*q0*SH_ACCY[2] - 2*q3*SH_ACCY[1] + 2*q1*vd; +SK_ACCY[4] = 2*q1*SH_ACCY[1] + 2*q2*SH_ACCY[2] + 2*q3*vd; +SK_ACCY[5] = 2*q0*q3 - 2*q1*q2; +SK_ACCY[6] = q0*q3 - q1*q2; +SK_ACCY[7] = 2*q0*q1 + 2*q2*q3; +SK_ACCY[8] = SH_ACCY[0]; float Kfusion[24][1]; float Kfusion[1][1]; -Kfusion[0] = SK_ACCY[0]*(Kaccy*P[0][0]*SK_ACCY[2] + Kaccy*P[0][2]*SK_ACCY[1] + Kaccy*P[0][3]*SK_ACCY[4] - Kaccy*P[0][4]*SK_ACCY[6] - Kaccy*P[0][5]*SK_ACCY[5] - 2*Kaccy*P[0][22]*SK_ACCY[3] + Kaccy*P[0][23]*SK_ACCY[6]); -Kfusion[1] = SK_ACCY[0]*(Kaccy*P[1][0]*SK_ACCY[2] + Kaccy*P[1][2]*SK_ACCY[1] + Kaccy*P[1][3]*SK_ACCY[4] - Kaccy*P[1][4]*SK_ACCY[6] - Kaccy*P[1][5]*SK_ACCY[5] - 2*Kaccy*P[1][22]*SK_ACCY[3] + Kaccy*P[1][23]*SK_ACCY[6]); -Kfusion[2] = SK_ACCY[0]*(Kaccy*P[2][0]*SK_ACCY[2] + Kaccy*P[2][2]*SK_ACCY[1] + Kaccy*P[2][3]*SK_ACCY[4] - Kaccy*P[2][4]*SK_ACCY[6] - Kaccy*P[2][5]*SK_ACCY[5] - 2*Kaccy*P[2][22]*SK_ACCY[3] + Kaccy*P[2][23]*SK_ACCY[6]); -Kfusion[3] = SK_ACCY[0]*(Kaccy*P[3][0]*SK_ACCY[2] + Kaccy*P[3][2]*SK_ACCY[1] + Kaccy*P[3][3]*SK_ACCY[4] - Kaccy*P[3][4]*SK_ACCY[6] - Kaccy*P[3][5]*SK_ACCY[5] - 2*Kaccy*P[3][22]*SK_ACCY[3] + Kaccy*P[3][23]*SK_ACCY[6]); -Kfusion[4] = SK_ACCY[0]*(Kaccy*P[4][0]*SK_ACCY[2] + Kaccy*P[4][2]*SK_ACCY[1] + Kaccy*P[4][3]*SK_ACCY[4] - Kaccy*P[4][4]*SK_ACCY[6] - Kaccy*P[4][5]*SK_ACCY[5] - 2*Kaccy*P[4][22]*SK_ACCY[3] + Kaccy*P[4][23]*SK_ACCY[6]); -Kfusion[5] = SK_ACCY[0]*(Kaccy*P[5][0]*SK_ACCY[2] + Kaccy*P[5][2]*SK_ACCY[1] + Kaccy*P[5][3]*SK_ACCY[4] - Kaccy*P[5][4]*SK_ACCY[6] - Kaccy*P[5][5]*SK_ACCY[5] - 2*Kaccy*P[5][22]*SK_ACCY[3] + Kaccy*P[5][23]*SK_ACCY[6]); -Kfusion[6] = SK_ACCY[0]*(Kaccy*P[6][0]*SK_ACCY[2] + Kaccy*P[6][2]*SK_ACCY[1] + Kaccy*P[6][3]*SK_ACCY[4] - Kaccy*P[6][4]*SK_ACCY[6] - Kaccy*P[6][5]*SK_ACCY[5] - 2*Kaccy*P[6][22]*SK_ACCY[3] + Kaccy*P[6][23]*SK_ACCY[6]); -Kfusion[7] = SK_ACCY[0]*(Kaccy*P[7][0]*SK_ACCY[2] + Kaccy*P[7][2]*SK_ACCY[1] + Kaccy*P[7][3]*SK_ACCY[4] - Kaccy*P[7][4]*SK_ACCY[6] - Kaccy*P[7][5]*SK_ACCY[5] - 2*Kaccy*P[7][22]*SK_ACCY[3] + Kaccy*P[7][23]*SK_ACCY[6]); -Kfusion[8] = SK_ACCY[0]*(Kaccy*P[8][0]*SK_ACCY[2] + Kaccy*P[8][2]*SK_ACCY[1] + Kaccy*P[8][3]*SK_ACCY[4] - Kaccy*P[8][4]*SK_ACCY[6] - Kaccy*P[8][5]*SK_ACCY[5] - 2*Kaccy*P[8][22]*SK_ACCY[3] + Kaccy*P[8][23]*SK_ACCY[6]); -Kfusion[9] = SK_ACCY[0]*(Kaccy*P[9][0]*SK_ACCY[2] + Kaccy*P[9][2]*SK_ACCY[1] + Kaccy*P[9][3]*SK_ACCY[4] - Kaccy*P[9][4]*SK_ACCY[6] - Kaccy*P[9][5]*SK_ACCY[5] - 2*Kaccy*P[9][22]*SK_ACCY[3] + Kaccy*P[9][23]*SK_ACCY[6]); -Kfusion[10] = SK_ACCY[0]*(Kaccy*P[10][0]*SK_ACCY[2] + Kaccy*P[10][2]*SK_ACCY[1] + Kaccy*P[10][3]*SK_ACCY[4] - Kaccy*P[10][4]*SK_ACCY[6] - Kaccy*P[10][5]*SK_ACCY[5] - 2*Kaccy*P[10][22]*SK_ACCY[3] + Kaccy*P[10][23]*SK_ACCY[6]); -Kfusion[11] = SK_ACCY[0]*(Kaccy*P[11][0]*SK_ACCY[2] + Kaccy*P[11][2]*SK_ACCY[1] + Kaccy*P[11][3]*SK_ACCY[4] - Kaccy*P[11][4]*SK_ACCY[6] - Kaccy*P[11][5]*SK_ACCY[5] - 2*Kaccy*P[11][22]*SK_ACCY[3] + Kaccy*P[11][23]*SK_ACCY[6]); -Kfusion[12] = SK_ACCY[0]*(Kaccy*P[12][0]*SK_ACCY[2] + Kaccy*P[12][2]*SK_ACCY[1] + Kaccy*P[12][3]*SK_ACCY[4] - Kaccy*P[12][4]*SK_ACCY[6] - Kaccy*P[12][5]*SK_ACCY[5] - 2*Kaccy*P[12][22]*SK_ACCY[3] + Kaccy*P[12][23]*SK_ACCY[6]); -Kfusion[13] = SK_ACCY[0]*(Kaccy*P[13][0]*SK_ACCY[2] + Kaccy*P[13][2]*SK_ACCY[1] + Kaccy*P[13][3]*SK_ACCY[4] - Kaccy*P[13][4]*SK_ACCY[6] - Kaccy*P[13][5]*SK_ACCY[5] - 2*Kaccy*P[13][22]*SK_ACCY[3] + Kaccy*P[13][23]*SK_ACCY[6]); -Kfusion[14] = SK_ACCY[0]*(Kaccy*P[14][0]*SK_ACCY[2] + Kaccy*P[14][2]*SK_ACCY[1] + Kaccy*P[14][3]*SK_ACCY[4] - Kaccy*P[14][4]*SK_ACCY[6] - Kaccy*P[14][5]*SK_ACCY[5] - 2*Kaccy*P[14][22]*SK_ACCY[3] + Kaccy*P[14][23]*SK_ACCY[6]); -Kfusion[15] = SK_ACCY[0]*(Kaccy*P[15][0]*SK_ACCY[2] + Kaccy*P[15][2]*SK_ACCY[1] + Kaccy*P[15][3]*SK_ACCY[4] - Kaccy*P[15][4]*SK_ACCY[6] - Kaccy*P[15][5]*SK_ACCY[5] - 2*Kaccy*P[15][22]*SK_ACCY[3] + Kaccy*P[15][23]*SK_ACCY[6]); -Kfusion[16] = SK_ACCY[0]*(Kaccy*P[16][0]*SK_ACCY[2] + Kaccy*P[16][2]*SK_ACCY[1] + Kaccy*P[16][3]*SK_ACCY[4] - Kaccy*P[16][4]*SK_ACCY[6] - Kaccy*P[16][5]*SK_ACCY[5] - 2*Kaccy*P[16][22]*SK_ACCY[3] + Kaccy*P[16][23]*SK_ACCY[6]); -Kfusion[17] = SK_ACCY[0]*(Kaccy*P[17][0]*SK_ACCY[2] + Kaccy*P[17][2]*SK_ACCY[1] + Kaccy*P[17][3]*SK_ACCY[4] - Kaccy*P[17][4]*SK_ACCY[6] - Kaccy*P[17][5]*SK_ACCY[5] - 2*Kaccy*P[17][22]*SK_ACCY[3] + Kaccy*P[17][23]*SK_ACCY[6]); -Kfusion[18] = SK_ACCY[0]*(Kaccy*P[18][0]*SK_ACCY[2] + Kaccy*P[18][2]*SK_ACCY[1] + Kaccy*P[18][3]*SK_ACCY[4] - Kaccy*P[18][4]*SK_ACCY[6] - Kaccy*P[18][5]*SK_ACCY[5] - 2*Kaccy*P[18][22]*SK_ACCY[3] + Kaccy*P[18][23]*SK_ACCY[6]); -Kfusion[19] = SK_ACCY[0]*(Kaccy*P[19][0]*SK_ACCY[2] + Kaccy*P[19][2]*SK_ACCY[1] + Kaccy*P[19][3]*SK_ACCY[4] - Kaccy*P[19][4]*SK_ACCY[6] - Kaccy*P[19][5]*SK_ACCY[5] - 2*Kaccy*P[19][22]*SK_ACCY[3] + Kaccy*P[19][23]*SK_ACCY[6]); -Kfusion[20] = SK_ACCY[0]*(Kaccy*P[20][0]*SK_ACCY[2] + Kaccy*P[20][2]*SK_ACCY[1] + Kaccy*P[20][3]*SK_ACCY[4] - Kaccy*P[20][4]*SK_ACCY[6] - Kaccy*P[20][5]*SK_ACCY[5] - 2*Kaccy*P[20][22]*SK_ACCY[3] + Kaccy*P[20][23]*SK_ACCY[6]); -Kfusion[21] = SK_ACCY[0]*(Kaccy*P[21][0]*SK_ACCY[2] + Kaccy*P[21][2]*SK_ACCY[1] + Kaccy*P[21][3]*SK_ACCY[4] - Kaccy*P[21][4]*SK_ACCY[6] - Kaccy*P[21][5]*SK_ACCY[5] - 2*Kaccy*P[21][22]*SK_ACCY[3] + Kaccy*P[21][23]*SK_ACCY[6]); -Kfusion[22] = SK_ACCY[0]*(Kaccy*P[22][0]*SK_ACCY[2] + Kaccy*P[22][2]*SK_ACCY[1] + Kaccy*P[22][3]*SK_ACCY[4] - Kaccy*P[22][4]*SK_ACCY[6] - Kaccy*P[22][5]*SK_ACCY[5] - 2*Kaccy*P[22][22]*SK_ACCY[3] + Kaccy*P[22][23]*SK_ACCY[6]); -Kfusion[23] = SK_ACCY[0]*(Kaccy*P[23][0]*SK_ACCY[2] + Kaccy*P[23][2]*SK_ACCY[1] + Kaccy*P[23][3]*SK_ACCY[4] - Kaccy*P[23][4]*SK_ACCY[6] - Kaccy*P[23][5]*SK_ACCY[5] - 2*Kaccy*P[23][22]*SK_ACCY[3] + Kaccy*P[23][23]*SK_ACCY[6]); +Kfusion[0] = -SK_ACCY[0]*(Kaccy*P[0][0]*SK_ACCY[3] + Kaccy*P[0][1]*SK_ACCY[2] - Kaccy*P[0][3]*SK_ACCY[1] + Kaccy*P[0][2]*SK_ACCY[4] - Kaccy*P[0][4]*SK_ACCY[5] + Kaccy*P[0][5]*SK_ACCY[8] + Kaccy*P[0][6]*SK_ACCY[7] + 2*Kaccy*P[0][22]*SK_ACCY[6] - Kaccy*P[0][23]*SK_ACCY[8]); +Kfusion[1] = -SK_ACCY[0]*(Kaccy*P[1][0]*SK_ACCY[3] + Kaccy*P[1][1]*SK_ACCY[2] - Kaccy*P[1][3]*SK_ACCY[1] + Kaccy*P[1][2]*SK_ACCY[4] - Kaccy*P[1][4]*SK_ACCY[5] + Kaccy*P[1][5]*SK_ACCY[8] + Kaccy*P[1][6]*SK_ACCY[7] + 2*Kaccy*P[1][22]*SK_ACCY[6] - Kaccy*P[1][23]*SK_ACCY[8]); +Kfusion[2] = -SK_ACCY[0]*(Kaccy*P[2][0]*SK_ACCY[3] + Kaccy*P[2][1]*SK_ACCY[2] - Kaccy*P[2][3]*SK_ACCY[1] + Kaccy*P[2][2]*SK_ACCY[4] - Kaccy*P[2][4]*SK_ACCY[5] + Kaccy*P[2][5]*SK_ACCY[8] + Kaccy*P[2][6]*SK_ACCY[7] + 2*Kaccy*P[2][22]*SK_ACCY[6] - Kaccy*P[2][23]*SK_ACCY[8]); +Kfusion[3] = -SK_ACCY[0]*(Kaccy*P[3][0]*SK_ACCY[3] + Kaccy*P[3][1]*SK_ACCY[2] - Kaccy*P[3][3]*SK_ACCY[1] + Kaccy*P[3][2]*SK_ACCY[4] - Kaccy*P[3][4]*SK_ACCY[5] + Kaccy*P[3][5]*SK_ACCY[8] + Kaccy*P[3][6]*SK_ACCY[7] + 2*Kaccy*P[3][22]*SK_ACCY[6] - Kaccy*P[3][23]*SK_ACCY[8]); +Kfusion[4] = -SK_ACCY[0]*(Kaccy*P[4][0]*SK_ACCY[3] + Kaccy*P[4][1]*SK_ACCY[2] - Kaccy*P[4][3]*SK_ACCY[1] + Kaccy*P[4][2]*SK_ACCY[4] - Kaccy*P[4][4]*SK_ACCY[5] + Kaccy*P[4][5]*SK_ACCY[8] + Kaccy*P[4][6]*SK_ACCY[7] + 2*Kaccy*P[4][22]*SK_ACCY[6] - Kaccy*P[4][23]*SK_ACCY[8]); +Kfusion[5] = -SK_ACCY[0]*(Kaccy*P[5][0]*SK_ACCY[3] + Kaccy*P[5][1]*SK_ACCY[2] - Kaccy*P[5][3]*SK_ACCY[1] + Kaccy*P[5][2]*SK_ACCY[4] - Kaccy*P[5][4]*SK_ACCY[5] + Kaccy*P[5][5]*SK_ACCY[8] + Kaccy*P[5][6]*SK_ACCY[7] + 2*Kaccy*P[5][22]*SK_ACCY[6] - Kaccy*P[5][23]*SK_ACCY[8]); +Kfusion[6] = -SK_ACCY[0]*(Kaccy*P[6][0]*SK_ACCY[3] + Kaccy*P[6][1]*SK_ACCY[2] - Kaccy*P[6][3]*SK_ACCY[1] + Kaccy*P[6][2]*SK_ACCY[4] - Kaccy*P[6][4]*SK_ACCY[5] + Kaccy*P[6][5]*SK_ACCY[8] + Kaccy*P[6][6]*SK_ACCY[7] + 2*Kaccy*P[6][22]*SK_ACCY[6] - Kaccy*P[6][23]*SK_ACCY[8]); +Kfusion[7] = -SK_ACCY[0]*(Kaccy*P[7][0]*SK_ACCY[3] + Kaccy*P[7][1]*SK_ACCY[2] - Kaccy*P[7][3]*SK_ACCY[1] + Kaccy*P[7][2]*SK_ACCY[4] - Kaccy*P[7][4]*SK_ACCY[5] + Kaccy*P[7][5]*SK_ACCY[8] + Kaccy*P[7][6]*SK_ACCY[7] + 2*Kaccy*P[7][22]*SK_ACCY[6] - Kaccy*P[7][23]*SK_ACCY[8]); +Kfusion[8] = -SK_ACCY[0]*(Kaccy*P[8][0]*SK_ACCY[3] + Kaccy*P[8][1]*SK_ACCY[2] - Kaccy*P[8][3]*SK_ACCY[1] + Kaccy*P[8][2]*SK_ACCY[4] - Kaccy*P[8][4]*SK_ACCY[5] + Kaccy*P[8][5]*SK_ACCY[8] + Kaccy*P[8][6]*SK_ACCY[7] + 2*Kaccy*P[8][22]*SK_ACCY[6] - Kaccy*P[8][23]*SK_ACCY[8]); +Kfusion[9] = -SK_ACCY[0]*(Kaccy*P[9][0]*SK_ACCY[3] + Kaccy*P[9][1]*SK_ACCY[2] - Kaccy*P[9][3]*SK_ACCY[1] + Kaccy*P[9][2]*SK_ACCY[4] - Kaccy*P[9][4]*SK_ACCY[5] + Kaccy*P[9][5]*SK_ACCY[8] + Kaccy*P[9][6]*SK_ACCY[7] + 2*Kaccy*P[9][22]*SK_ACCY[6] - Kaccy*P[9][23]*SK_ACCY[8]); +Kfusion[10] = -SK_ACCY[0]*(Kaccy*P[10][0]*SK_ACCY[3] + Kaccy*P[10][1]*SK_ACCY[2] - Kaccy*P[10][3]*SK_ACCY[1] + Kaccy*P[10][2]*SK_ACCY[4] - Kaccy*P[10][4]*SK_ACCY[5] + Kaccy*P[10][5]*SK_ACCY[8] + Kaccy*P[10][6]*SK_ACCY[7] + 2*Kaccy*P[10][22]*SK_ACCY[6] - Kaccy*P[10][23]*SK_ACCY[8]); +Kfusion[11] = -SK_ACCY[0]*(Kaccy*P[11][0]*SK_ACCY[3] + Kaccy*P[11][1]*SK_ACCY[2] - Kaccy*P[11][3]*SK_ACCY[1] + Kaccy*P[11][2]*SK_ACCY[4] - Kaccy*P[11][4]*SK_ACCY[5] + Kaccy*P[11][5]*SK_ACCY[8] + Kaccy*P[11][6]*SK_ACCY[7] + 2*Kaccy*P[11][22]*SK_ACCY[6] - Kaccy*P[11][23]*SK_ACCY[8]); +Kfusion[12] = -SK_ACCY[0]*(Kaccy*P[12][0]*SK_ACCY[3] + Kaccy*P[12][1]*SK_ACCY[2] - Kaccy*P[12][3]*SK_ACCY[1] + Kaccy*P[12][2]*SK_ACCY[4] - Kaccy*P[12][4]*SK_ACCY[5] + Kaccy*P[12][5]*SK_ACCY[8] + Kaccy*P[12][6]*SK_ACCY[7] + 2*Kaccy*P[12][22]*SK_ACCY[6] - Kaccy*P[12][23]*SK_ACCY[8]); +Kfusion[13] = -SK_ACCY[0]*(Kaccy*P[13][0]*SK_ACCY[3] + Kaccy*P[13][1]*SK_ACCY[2] - Kaccy*P[13][3]*SK_ACCY[1] + Kaccy*P[13][2]*SK_ACCY[4] - Kaccy*P[13][4]*SK_ACCY[5] + Kaccy*P[13][5]*SK_ACCY[8] + Kaccy*P[13][6]*SK_ACCY[7] + 2*Kaccy*P[13][22]*SK_ACCY[6] - Kaccy*P[13][23]*SK_ACCY[8]); +Kfusion[14] = -SK_ACCY[0]*(Kaccy*P[14][0]*SK_ACCY[3] + Kaccy*P[14][1]*SK_ACCY[2] - Kaccy*P[14][3]*SK_ACCY[1] + Kaccy*P[14][2]*SK_ACCY[4] - Kaccy*P[14][4]*SK_ACCY[5] + Kaccy*P[14][5]*SK_ACCY[8] + Kaccy*P[14][6]*SK_ACCY[7] + 2*Kaccy*P[14][22]*SK_ACCY[6] - Kaccy*P[14][23]*SK_ACCY[8]); +Kfusion[15] = -SK_ACCY[0]*(Kaccy*P[15][0]*SK_ACCY[3] + Kaccy*P[15][1]*SK_ACCY[2] - Kaccy*P[15][3]*SK_ACCY[1] + Kaccy*P[15][2]*SK_ACCY[4] - Kaccy*P[15][4]*SK_ACCY[5] + Kaccy*P[15][5]*SK_ACCY[8] + Kaccy*P[15][6]*SK_ACCY[7] + 2*Kaccy*P[15][22]*SK_ACCY[6] - Kaccy*P[15][23]*SK_ACCY[8]); +Kfusion[16] = -SK_ACCY[0]*(Kaccy*P[16][0]*SK_ACCY[3] + Kaccy*P[16][1]*SK_ACCY[2] - Kaccy*P[16][3]*SK_ACCY[1] + Kaccy*P[16][2]*SK_ACCY[4] - Kaccy*P[16][4]*SK_ACCY[5] + Kaccy*P[16][5]*SK_ACCY[8] + Kaccy*P[16][6]*SK_ACCY[7] + 2*Kaccy*P[16][22]*SK_ACCY[6] - Kaccy*P[16][23]*SK_ACCY[8]); +Kfusion[17] = -SK_ACCY[0]*(Kaccy*P[17][0]*SK_ACCY[3] + Kaccy*P[17][1]*SK_ACCY[2] - Kaccy*P[17][3]*SK_ACCY[1] + Kaccy*P[17][2]*SK_ACCY[4] - Kaccy*P[17][4]*SK_ACCY[5] + Kaccy*P[17][5]*SK_ACCY[8] + Kaccy*P[17][6]*SK_ACCY[7] + 2*Kaccy*P[17][22]*SK_ACCY[6] - Kaccy*P[17][23]*SK_ACCY[8]); +Kfusion[18] = -SK_ACCY[0]*(Kaccy*P[18][0]*SK_ACCY[3] + Kaccy*P[18][1]*SK_ACCY[2] - Kaccy*P[18][3]*SK_ACCY[1] + Kaccy*P[18][2]*SK_ACCY[4] - Kaccy*P[18][4]*SK_ACCY[5] + Kaccy*P[18][5]*SK_ACCY[8] + Kaccy*P[18][6]*SK_ACCY[7] + 2*Kaccy*P[18][22]*SK_ACCY[6] - Kaccy*P[18][23]*SK_ACCY[8]); +Kfusion[19] = -SK_ACCY[0]*(Kaccy*P[19][0]*SK_ACCY[3] + Kaccy*P[19][1]*SK_ACCY[2] - Kaccy*P[19][3]*SK_ACCY[1] + Kaccy*P[19][2]*SK_ACCY[4] - Kaccy*P[19][4]*SK_ACCY[5] + Kaccy*P[19][5]*SK_ACCY[8] + Kaccy*P[19][6]*SK_ACCY[7] + 2*Kaccy*P[19][22]*SK_ACCY[6] - Kaccy*P[19][23]*SK_ACCY[8]); +Kfusion[20] = -SK_ACCY[0]*(Kaccy*P[20][0]*SK_ACCY[3] + Kaccy*P[20][1]*SK_ACCY[2] - Kaccy*P[20][3]*SK_ACCY[1] + Kaccy*P[20][2]*SK_ACCY[4] - Kaccy*P[20][4]*SK_ACCY[5] + Kaccy*P[20][5]*SK_ACCY[8] + Kaccy*P[20][6]*SK_ACCY[7] + 2*Kaccy*P[20][22]*SK_ACCY[6] - Kaccy*P[20][23]*SK_ACCY[8]); +Kfusion[21] = -SK_ACCY[0]*(Kaccy*P[21][0]*SK_ACCY[3] + Kaccy*P[21][1]*SK_ACCY[2] - Kaccy*P[21][3]*SK_ACCY[1] + Kaccy*P[21][2]*SK_ACCY[4] - Kaccy*P[21][4]*SK_ACCY[5] + Kaccy*P[21][5]*SK_ACCY[8] + Kaccy*P[21][6]*SK_ACCY[7] + 2*Kaccy*P[21][22]*SK_ACCY[6] - Kaccy*P[21][23]*SK_ACCY[8]); +Kfusion[22] = -SK_ACCY[0]*(Kaccy*P[22][0]*SK_ACCY[3] + Kaccy*P[22][1]*SK_ACCY[2] - Kaccy*P[22][3]*SK_ACCY[1] + Kaccy*P[22][2]*SK_ACCY[4] - Kaccy*P[22][4]*SK_ACCY[5] + Kaccy*P[22][5]*SK_ACCY[8] + Kaccy*P[22][6]*SK_ACCY[7] + 2*Kaccy*P[22][22]*SK_ACCY[6] - Kaccy*P[22][23]*SK_ACCY[8]); +Kfusion[23] = -SK_ACCY[0]*(Kaccy*P[23][0]*SK_ACCY[3] + Kaccy*P[23][1]*SK_ACCY[2] - Kaccy*P[23][3]*SK_ACCY[1] + Kaccy*P[23][2]*SK_ACCY[4] - Kaccy*P[23][4]*SK_ACCY[5] + Kaccy*P[23][5]*SK_ACCY[8] + Kaccy*P[23][6]*SK_ACCY[7] + 2*Kaccy*P[23][22]*SK_ACCY[6] - Kaccy*P[23][23]*SK_ACCY[8]); diff --git a/matlab/scripts/Inertial Nav EKF/Drag.mat b/matlab/scripts/Inertial Nav EKF/Drag.mat index 7b9b77124a..bf456947ab 100644 Binary files a/matlab/scripts/Inertial Nav EKF/Drag.mat and b/matlab/scripts/Inertial Nav EKF/Drag.mat differ diff --git a/matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m b/matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m index b579294b26..929113abbb 100644 --- a/matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m +++ b/matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m @@ -12,20 +12,12 @@ % Author: Paul Riseborough -% Based on use of a rotation vector for attitude estimation as described -% here: - -% Mark E. Pittelkau. "Rotation Vector in Attitude Estimation", -% Journal of Guidance, Control, and Dynamics, Vol. 26, No. 6 (2003), -% pp. 855-860. - % State vector: -% error rotation vector in body frame (X,Y,Z) +% attitude quaternion % Velocity - m/sec (North, East, Down) % Position - m (North, East, Down) % Delta Angle bias - rad (X,Y,Z) -% Delta Angle scale factor (X,Y,Z) -% Delta Velocity bias - m/s (Z) +% Delta Velocity bias - m/s (X,Y,Z) % Earth Magnetic Field Vector - (North, East, Down) % Body Magnetic Field Vector - (X,Y,Z) % Wind Vector - m/sec (North,East) @@ -51,11 +43,10 @@ syms q0 q1 q2 q3 real % quaternions defining attitude of body axes relative to l syms vn ve vd real % NED velocity - m/sec syms pn pe pd real % NED position - m syms dax_b day_b daz_b real % delta angle bias - rad -syms dax_s day_s daz_s real % delta angle scale factor -syms dvz_b dvy_b dvz_b real % delta velocity bias - m/sec +syms dvx_b dvy_b dvz_b real % delta velocity bias - m/sec syms dt real % IMU time step - sec syms gravity real % gravity - m/sec^2 -syms daxNoise dayNoise dazNoise dvxNoise dvyNoise dvzNoise real; % IMU delta angle and delta velocity measurement noise +syms daxVar dayVar dazVar dvxVar dvyVar dvzVar real; % IMU delta angle and delta velocity measurement variances syms vwn vwe real; % NE wind velocity - m/sec syms magX magY magZ real; % XYZ body fixed magnetic field measurements - milligauss syms magN magE magD real; % NED earth fixed magnetic field components - milligauss @@ -66,7 +57,6 @@ syms R_MAG real % variance for magnetic flux measurements - milligauss^2 syms R_BETA real % variance of sidelsip measurements rad^2 syms R_LOS real % variance of LOS angular rate mesurements (rad/sec)^2 syms ptd real % location of terrain in D axis -syms rotErrX rotErrY rotErrZ real; % error rotation vector in body frame syms decl real; % earth magnetic field declination from true north syms R_DECL R_YAW real; % variance of declination or yaw angle observation syms BCXinv BCYinv real % inverse of ballistic coefficient for wind relative movement along the x and y body axes @@ -82,28 +72,23 @@ dVelMeas = [dvx; dvy; dvz]; % define the IMU bias errors and scale factor dAngBias = [dax_b; day_b; daz_b]; -dAngScale = [dax_s; day_s; daz_s]; -dVelBias = [0;0;dvz_b]; +dVelBias = [dvx_b; dvy_b; dvz_b]; % define the quaternion rotation vector for the state estimate -estQuat = [q0;q1;q2;q3]; - -% define the attitude error rotation vector, where error = truth - estimate -errRotVec = [rotErrX;rotErrY;rotErrZ]; - -% define the attitude error quaternion using a first order linearisation -errQuat = [1;0.5*errRotVec]; - -% Define the truth quaternion as the estimate + error -truthQuat = QuatMult(estQuat, errQuat); +quat = [q0;q1;q2;q3]; % derive the truth body to nav direction cosine matrix -Tbn = Quat2Tbn(truthQuat); +Tbn = Quat2Tbn(quat); % define the truth delta angle % ignore coning compensation as these effects are negligible in terms of % covariance growth for our application and grade of sensor -dAngTruth = dAngMeas.*dAngScale - dAngBias - [daxNoise;dayNoise;dazNoise]; +dAngTruth = dAngMeas - dAngBias; + +% Define the truth delta velocity -ignore sculling and transport rate +% corrections as these negligible are in terms of covariance growth for our +% application and grade of sensor +dVelTruth = dVelMeas - dVelBias; % define the attitude update equations % use a first order expansion of rotation to calculate the quaternion increment @@ -113,16 +98,7 @@ deltaQuat = [1; 0.5*dAngTruth(2); 0.5*dAngTruth(3); ]; -truthQuatNew = QuatMult(truthQuat,deltaQuat); -% calculate the updated attitude error quaternion with respect to the previous estimate -errQuatNew = QuatDivide(truthQuatNew,estQuat); -% change to a rotaton vector - this is the error rotation vector updated state -errRotNew = 2 * [errQuatNew(2);errQuatNew(3);errQuatNew(4)]; - -% Define the truth delta velocity -ignore sculling and transport rate -% corrections as these negligible are in terms of covariance growth for our -% application and grade of sensor -dVelTruth = dVelMeas - dVelBias - [dvxNoise;dvyNoise;dvzNoise]; +quatNew = QuatMult(quat,deltaQuat); % define the velocity update equations % ignore coriolis terms for linearisation purposes @@ -132,9 +108,8 @@ vNew = [vn;ve;vd] + [0;0;gravity]*dt + Tbn*dVelTruth; pNew = [pn;pe;pd] + [vn;ve;vd]*dt; % define the IMU error update equations -dabNew = [dax_b; day_b; daz_b]; -dasNew = [dax_s; day_s; daz_s]; -dvbNew = dvz_b; +dAngBiasNew = dAngBias; +dVelBiasNew = dVelBias; % define the wind velocity update equations vwnNew = vwn; @@ -151,16 +126,15 @@ magYnew = magY; magZnew = magZ; % Define the state vector & number of states -stateVector = [errRotVec;vn;ve;vd;pn;pe;pd;dax_b;day_b;daz_b;dax_s;day_s;daz_s;dvz_b;magN;magE;magD;magX;magY;magZ;vwn;vwe]; +stateVector = [quat;vn;ve;vd;pn;pe;pd;dAngBias;dVelBias;magN;magE;magD;magX;magY;magZ;vwn;vwe]; nStates=numel(stateVector); % Define vector of process equations -newStateVector = [errRotNew;vNew;pNew;dabNew;dasNew;dvbNew;magNnew;magEnew;magDnew;magXnew;magYnew;magZnew;vwnNew;vweNew]; +newStateVector = [quatNew;vNew;pNew;dAngBiasNew;dVelBiasNew;magNnew;magEnew;magDnew;magXnew;magYnew;magZnew;vwnNew;vweNew]; % derive the state transition matrix F = jacobian(newStateVector, stateVector); % set the rotation error states to zero -F = subs(F, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0}); [F,SF]=OptimiseAlgebra(F,'SF'); % define a symbolic covariance matrix using strings to represent @@ -181,27 +155,19 @@ save 'StatePrediction.mat'; % This reduces the number of floating point operations by a factor of 6 or % more compared to using the standard matrix operations in code -% Define the control (disturbance) vector. Error growth in the inertial -% solution is assumed to be driven by 'noise' in the delta angles and -% velocities, after bias effects have been removed. This is OK becasue we -% have sensor bias accounted for in the state equations. -distVector = [daxNoise;dayNoise;dazNoise;dvxNoise;dvyNoise;dvzNoise]; +% Error growth in the inertial solution is assumed to be driven by 'noise' in the delta angles and +% velocities, after bias effects have been removed. -% derive the control(disturbance) influence matrix -G = jacobian(newStateVector, distVector); -G = subs(G, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0}); +% derive the control(disturbance) influence matrix from IMu noise to state +% noise +G = jacobian(newStateVector, [dAngMeas;dVelMeas]); [G,SG]=OptimiseAlgebra(G,'SG'); % derive the state error matrix -distMatrix = diag(distVector.^2); +distMatrix = diag([daxVar dayVar dazVar dvxVar dvyVar dvzVar]); Q = G*distMatrix*transpose(G); [Q,SQ]=OptimiseAlgebra(Q,'SQ'); -% remove the disturbance noise from the process equations as it is only -% needed when calculating the disturbance influence matrix -vNew = subs(vNew,{'daxNoise','dayNoise','dazNoise','dvxNoise','dvyNoise','dvzNoise'}, {0,0,0,0,0,0}); -errRotNew = subs(errRotNew,{'daxNoise','dayNoise','dazNoise','dvxNoise','dvyNoise','dvzNoise'}, {0,0,0,0,0,0}); - % Derive the predicted covariance matrix using the standard equation PP = F*P*transpose(F) + Q; @@ -216,7 +182,6 @@ reset(symengine); load('StatePrediction.mat'); VtasPred = sqrt((vn-vwn)^2 + (ve-vwe)^2 + vd^2); % predicted measurement H_TAS = jacobian(VtasPred,stateVector); % measurement Jacobian -H_TAS = subs(H_TAS, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0}); [H_TAS,SH_TAS]=OptimiseAlgebra(H_TAS,'SH_TAS'); % optimise processing K_TAS = (P*transpose(H_TAS))/(H_TAS*P*transpose(H_TAS) + R_TAS); [K_TAS,SK_TAS]=OptimiseAlgebra(K_TAS,'SK_TAS'); % Kalman gain vector @@ -230,11 +195,10 @@ reset(symengine); load('StatePrediction.mat'); % calculate wind relative velocities in nav frame and rotate into body frame -Vbw = Tbn'*[(vn-vwn);(ve-vwe);vd]; +Vbw = transpose(Tbn)*[(vn-vwn);(ve-vwe);vd]; % calculate predicted angle of sideslip using small angle assumption BetaPred = Vbw(2)/Vbw(1); H_BETA = jacobian(BetaPred,stateVector); % measurement Jacobian -H_BETA = subs(H_BETA, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0}); [H_BETA,SH_BETA]=OptimiseAlgebra(H_BETA,'SH_BETA'); % optimise processing K_BETA = (P*transpose(H_BETA))/(H_BETA*P*transpose(H_BETA) + R_BETA);[K_BETA,SK_BETA]=OptimiseAlgebra(K_BETA,'SK_BETA'); % Kalman gain vector @@ -248,7 +212,6 @@ load('StatePrediction.mat'); magMeas = transpose(Tbn)*[magN;magE;magD] + [magX;magY;magZ]; % predicted measurement H_MAG = jacobian(magMeas,stateVector); % measurement Jacobian -H_MAG = subs(H_MAG, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0}); [H_MAG,SH_MAG]=OptimiseAlgebra(H_MAG,'SH_MAG'); K_MX = (P*transpose(H_MAG(1,:)))/(H_MAG(1,:)*P*transpose(H_MAG(1,:)) + R_MAG); % Kalman gain vector @@ -281,7 +244,6 @@ save('temp1.mat','losRateX','losRateY'); % calculate the observation Jacobian for the X axis H_LOSX = jacobian(losRateX,stateVector); % measurement Jacobian -H_LOSX = subs(H_LOSX, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0}); H_LOSX = simplify(H_LOSX); save('temp2.mat','H_LOSX'); ccode(H_LOSX,'file','H_LOSX.c'); @@ -294,7 +256,6 @@ load('temp1.mat'); % calculate the observation Jacobian for the Y axis H_LOSY = jacobian(losRateY,stateVector); % measurement Jacobian -H_LOSY = subs(H_LOSY, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0}); H_LOSY = simplify(H_LOSY); save('temp3.mat','H_LOSY'); ccode(H_LOSY,'file','H_LOSY.c'); @@ -308,7 +269,6 @@ load('temp2.mat'); % calculate Kalman gain vector for the X axis K_LOSX = (P*transpose(H_LOSX))/(H_LOSX*P*transpose(H_LOSX) + R_LOS); % Kalman gain vector -K_LOSX = subs(K_LOSX, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0}); K_LOSX = simplify(K_LOSX); ccode(K_LOSX,'file','K_LOSX.c'); fix_c_code('K_LOSX.c'); @@ -321,7 +281,6 @@ load('temp3.mat'); % calculate Kalman gain vector for the Y axis K_LOSY = (P*transpose(H_LOSY))/(H_LOSY*P*transpose(H_LOSY) + R_LOS); % Kalman gain vector -K_LOSY = subs(K_LOSY, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0}); K_LOSY = simplify(K_LOSY); ccode(K_LOSY,'file','K_LOSY.c'); fix_c_code('K_LOSY.c'); @@ -336,7 +295,6 @@ load('StatePrediction.mat'); % Calculate the yaw (first rotation) angle from the 321 rotation sequence angMeas = atan(Tbn(2,1)/Tbn(1,1)); H_YAW321 = jacobian(angMeas,stateVector); % measurement Jacobian -H_YAW321 = subs(H_YAW321, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0}); H_YAW321 = simplify(H_YAW321); ccode(H_YAW321,'file','calcH_YAW321.c'); fix_c_code('calcH_YAW321.c'); @@ -351,7 +309,6 @@ load('StatePrediction.mat'); % Calculate the yaw (first rotation) angle from an Euler 312 sequence angMeas = atan(-Tbn(1,2)/Tbn(2,2)); H_YAW312 = jacobian(angMeas,stateVector); % measurement Jacobianclea -H_YAW312 = subs(H_YAW312, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0}); H_YAW312 = simplify(H_YAW312); ccode(H_YAW312,'file','calcH_YAW312.c'); fix_c_code('calcH_YAW312.c'); @@ -367,7 +324,6 @@ load('StatePrediction.mat'); % component of the measured field angMeas = atan(magE/magN); H_MAGD = jacobian(angMeas,stateVector); % measurement Jacobian -H_MAGD = subs(H_MAGD, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0}); H_MAGD = simplify(H_MAGD); K_MAGD = (P*transpose(H_MAGD))/(H_MAGD*P*transpose(H_MAGD) + R_DECL); K_MAGD = simplify(K_MAGD); @@ -404,7 +360,6 @@ accYpred = -Kaccy*vrel(2); % predicted acceleration measured along Y body axis % Derive observation Jacobian and Kalman gain matrix for X accel fusion H_ACCX = jacobian(accXpred,stateVector); % measurement Jacobian -H_ACCX = subs(H_ACCX, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0}); H_ACCX = simplify(H_ACCX); [H_ACCX,SH_ACCX]=OptimiseAlgebra(H_ACCX,'SH_ACCX'); % optimise processing K_ACCX = (P*transpose(H_ACCX))/(H_ACCX*P*transpose(H_ACCX) + R_ACC); @@ -412,7 +367,6 @@ K_ACCX = (P*transpose(H_ACCX))/(H_ACCX*P*transpose(H_ACCX) + R_ACC); % Derive observation Jacobian and Kalman gain matrix for Y accel fusion H_ACCY = jacobian(accYpred,stateVector); % measurement Jacobian -H_ACCY = subs(H_ACCY, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0}); H_ACCY = simplify(H_ACCY); [H_ACCY,SH_ACCY]=OptimiseAlgebra(H_ACCY,'SH_ACCY'); % optimise processing K_ACCY = (P*transpose(H_ACCY))/(H_ACCY*P*transpose(H_ACCY) + R_ACC); @@ -436,4 +390,4 @@ fileName = strcat('SymbolicOutput',int2str(nStates),'.mat'); save(fileName); SaveScriptCode(nStates); ConvertToM(nStates); -ConvertToC(nStates); \ No newline at end of file +ConvertToC(nStates); diff --git a/matlab/scripts/Inertial Nav EKF/H_LOSX.c b/matlab/scripts/Inertial Nav EKF/H_LOSX.c index c7ba63108e..e2223cc9e4 100644 --- a/matlab/scripts/Inertial Nav EKF/H_LOSX.c +++ b/matlab/scripts/Inertial Nav EKF/H_LOSX.c @@ -1,15 +1,8 @@ t2 = 1.0/range; -t3 = q0*q0; -t4 = q1*q1; -t5 = q2*q2; -t6 = q3*q3; -t7 = q0*q2*2.0; -t8 = q1*q3*2.0; -t9 = q0*q3*2.0; -t10 = q1*q2*2.0; -t11 = q0*q1*2.0; -A0[0][0] = t2*(vn*(t7+t8)+vd*(t3-t4-t5+t6)-ve*(t11-q2*q3*2.0)); -A0[0][2] = -t2*(ve*(t9+t10)-vd*(t7-t8)+vn*(t3+t4-t5-t6)); -A0[0][3] = -t2*(t9-t10); -A0[0][4] = t2*(t3-t4+t5-t6); -A0[0][5] = t2*(t11+q2*q3*2.0); +A0[0][0] = t2*(q1*vd*2.0+q0*ve*2.0-q3*vn*2.0); +A0[0][1] = t2*(q0*vd*2.0-q1*ve*2.0+q2*vn*2.0); +A0[0][2] = t2*(q3*vd*2.0+q2*ve*2.0+q1*vn*2.0); +A0[0][3] = -t2*(q2*vd*-2.0+q3*ve*2.0+q0*vn*2.0); +A0[0][4] = -t2*(q0*q3*2.0-q1*q2*2.0); +A0[0][5] = t2*(q0*q0-q1*q1+q2*q2-q3*q3); +A0[0][6] = t2*(q0*q1*2.0+q2*q3*2.0); diff --git a/matlab/scripts/Inertial Nav EKF/H_LOSY.c b/matlab/scripts/Inertial Nav EKF/H_LOSY.c index 1c7ee5be18..306eed204b 100644 --- a/matlab/scripts/Inertial Nav EKF/H_LOSY.c +++ b/matlab/scripts/Inertial Nav EKF/H_LOSY.c @@ -1,14 +1,8 @@ t2 = 1.0/range; -t3 = q0*q0; -t4 = q1*q1; -t5 = q2*q2; -t6 = q3*q3; -t7 = q0*q1*2.0; -t8 = q0*q3*2.0; -t9 = q0*q2*2.0; -t10 = q1*q3*2.0; -A0[0][1] = t2*(vn*(t9+t10)+vd*(t3-t4-t5+t6)-ve*(t7-q2*q3*2.0)); -A0[0][2] = -t2*(ve*(t3-t4+t5-t6)+vd*(t7+q2*q3*2.0)-vn*(t8-q1*q2*2.0)); -A0[0][3] = -t2*(t3+t4-t5-t6); -A0[0][4] = -t2*(t8+q1*q2*2.0); -A0[0][5] = t2*(t9-t10); +A0[0][0] = -t2*(q2*vd*-2.0+q3*ve*2.0+q0*vn*2.0); +A0[0][1] = -t2*(q3*vd*2.0+q2*ve*2.0+q1*vn*2.0); +A0[0][2] = t2*(q0*vd*2.0-q1*ve*2.0+q2*vn*2.0); +A0[0][3] = -t2*(q1*vd*2.0+q0*ve*2.0-q3*vn*2.0); +A0[0][4] = -t2*(q0*q0+q1*q1-q2*q2-q3*q3); +A0[0][5] = -t2*(q0*q3*2.0+q1*q2*2.0); +A0[0][6] = t2*(q0*q2*2.0-q1*q3*2.0); diff --git a/matlab/scripts/Inertial Nav EKF/K_LOSX.c b/matlab/scripts/Inertial Nav EKF/K_LOSX.c index fe44362016..5044d0cc6e 100644 --- a/matlab/scripts/Inertial Nav EKF/K_LOSX.c +++ b/matlab/scripts/Inertial Nav EKF/K_LOSX.c @@ -1,89 +1,117 @@ t2 = 1.0/range; -t3 = q0*q1*2.0; -t4 = q2*q3*2.0; -t5 = q0*q0; -t6 = q1*q1; -t7 = q2*q2; -t8 = q3*q3; -t9 = q0*q2*2.0; -t10 = q1*q3*2.0; -t11 = q0*q3*2.0; -t12 = q1*q2*2.0; -t13 = t11-t12; -t14 = t3+t4; -t15 = t5-t6-t7+t8; -t16 = t15*vd; -t17 = t3-t4; -t18 = t9+t10; -t19 = t18*vn; -t28 = t17*ve; -t20 = t16+t19-t28; -t21 = t5+t6-t7-t8; -t22 = t21*vn; -t23 = t9-t10; -t24 = t11+t12; -t25 = t24*ve; -t29 = t23*vd; -t26 = t22+t25-t29; -t27 = t5-t6+t7-t8; -t30 = P[0][0]*t2*t20; -t31 = P[5][3]*t2*t14; -t32 = P[0][3]*t2*t20; -t33 = P[4][3]*t2*t27; -t56 = P[3][3]*t2*t13; -t57 = P[2][3]*t2*t26; -t34 = t31+t32+t33-t56-t57; -t35 = P[5][5]*t2*t14; -t36 = P[0][5]*t2*t20; -t37 = P[4][5]*t2*t27; -t59 = P[3][5]*t2*t13; -t60 = P[2][5]*t2*t26; -t38 = t35+t36+t37-t59-t60; -t39 = t2*t14*t38; -t40 = P[5][0]*t2*t14; -t41 = P[4][0]*t2*t27; -t61 = P[3][0]*t2*t13; -t62 = P[2][0]*t2*t26; -t42 = t30+t40+t41-t61-t62; -t43 = t2*t20*t42; -t44 = P[5][2]*t2*t14; -t45 = P[0][2]*t2*t20; -t46 = P[4][2]*t2*t27; -t55 = P[2][2]*t2*t26; -t63 = P[3][2]*t2*t13; -t47 = t44+t45+t46-t55-t63; -t48 = P[5][4]*t2*t14; -t49 = P[0][4]*t2*t20; -t50 = P[4][4]*t2*t27; -t65 = P[3][4]*t2*t13; -t66 = P[2][4]*t2*t26; -t51 = t48+t49+t50-t65-t66; -t52 = t2*t27*t51; -t58 = t2*t13*t34; -t64 = t2*t26*t47; -t53 = R_LOS+t39+t43+t52-t58-t64; -t54 = 1.0/t53; -A0[0][0] = t54*(t30-P[0][3]*t2*(t11-q1*q2*2.0)+P[0][5]*t2*t14-P[0][2]*t2*t26+P[0][4]*t2*t27); -A0[1][0] = t54*(-P[1][3]*t2*t13+P[1][5]*t2*t14+P[1][0]*t2*t20-P[1][2]*t2*t26+P[1][4]*t2*t27); -A0[2][0] = t54*(-t55-P[2][3]*t2*t13+P[2][5]*t2*t14+P[2][0]*t2*t20+P[2][4]*t2*t27); -A0[3][0] = t54*(-t56+P[3][5]*t2*t14+P[3][0]*t2*t20-P[3][2]*t2*t26+P[3][4]*t2*t27); -A0[4][0] = t54*(t50-P[4][3]*t2*t13+P[4][5]*t2*t14+P[4][0]*t2*t20-P[4][2]*t2*t26); -A0[5][0] = t54*(t35-P[5][3]*t2*t13+P[5][0]*t2*t20-P[5][2]*t2*t26+P[5][4]*t2*t27); -A0[6][0] = t54*(-P[6][3]*t2*t13+P[6][5]*t2*t14+P[6][0]*t2*t20-P[6][2]*t2*t26+P[6][4]*t2*t27); -A0[7][0] = t54*(-P[7][3]*t2*t13+P[7][5]*t2*t14+P[7][0]*t2*t20-P[7][2]*t2*t26+P[7][4]*t2*t27); -A0[8][0] = t54*(-P[8][3]*t2*t13+P[8][5]*t2*t14+P[8][0]*t2*t20-P[8][2]*t2*t26+P[8][4]*t2*t27); -A0[9][0] = t54*(-P[9][3]*t2*t13+P[9][5]*t2*t14+P[9][0]*t2*t20-P[9][2]*t2*t26+P[9][4]*t2*t27); -A0[10][0] = t54*(-P[10][3]*t2*t13+P[10][5]*t2*t14+P[10][0]*t2*t20-P[10][2]*t2*t26+P[10][4]*t2*t27); -A0[11][0] = t54*(-P[11][3]*t2*t13+P[11][5]*t2*t14+P[11][0]*t2*t20-P[11][2]*t2*t26+P[11][4]*t2*t27); -A0[12][0] = t54*(-P[12][3]*t2*t13+P[12][5]*t2*t14+P[12][0]*t2*t20-P[12][2]*t2*t26+P[12][4]*t2*t27); -A0[13][0] = t54*(-P[13][3]*t2*t13+P[13][5]*t2*t14+P[13][0]*t2*t20-P[13][2]*t2*t26+P[13][4]*t2*t27); -A0[14][0] = t54*(-P[14][3]*t2*t13+P[14][5]*t2*t14+P[14][0]*t2*t20-P[14][2]*t2*t26+P[14][4]*t2*t27); -A0[15][0] = t54*(-P[15][3]*t2*t13+P[15][5]*t2*t14+P[15][0]*t2*t20-P[15][2]*t2*t26+P[15][4]*t2*t27); -A0[16][0] = t54*(-P[16][3]*t2*t13+P[16][5]*t2*t14+P[16][0]*t2*t20-P[16][2]*t2*t26+P[16][4]*t2*t27); -A0[17][0] = t54*(-P[17][3]*t2*t13+P[17][5]*t2*t14+P[17][0]*t2*t20-P[17][2]*t2*t26+P[17][4]*t2*t27); -A0[18][0] = t54*(-P[18][3]*t2*t13+P[18][5]*t2*t14+P[18][0]*t2*t20-P[18][2]*t2*t26+P[18][4]*t2*t27); -A0[19][0] = t54*(-P[19][3]*t2*t13+P[19][5]*t2*t14+P[19][0]*t2*t20-P[19][2]*t2*t26+P[19][4]*t2*t27); -A0[20][0] = t54*(-P[20][3]*t2*t13+P[20][5]*t2*t14+P[20][0]*t2*t20-P[20][2]*t2*t26+P[20][4]*t2*t27); -A0[21][0] = t54*(-P[21][3]*t2*t13+P[21][5]*t2*t14+P[21][0]*t2*t20-P[21][2]*t2*t26+P[21][4]*t2*t27); -A0[22][0] = t54*(-P[22][3]*t2*t13+P[22][5]*t2*t14+P[22][0]*t2*t20-P[22][2]*t2*t26+P[22][4]*t2*t27); -A0[23][0] = t54*(-P[23][3]*t2*t13+P[23][5]*t2*t14+P[23][0]*t2*t20-P[23][2]*t2*t26+P[23][4]*t2*t27); +t3 = q1*vd*2.0; +t4 = q0*ve*2.0; +t11 = q3*vn*2.0; +t5 = t3+t4-t11; +t6 = q0*q3*2.0; +t29 = q1*q2*2.0; +t7 = t6-t29; +t8 = q0*q1*2.0; +t9 = q2*q3*2.0; +t10 = t8+t9; +t12 = P[0][0]*t2*t5; +t13 = q0*vd*2.0; +t14 = q2*vn*2.0; +t28 = q1*ve*2.0; +t15 = t13+t14-t28; +t16 = q3*vd*2.0; +t17 = q2*ve*2.0; +t18 = q1*vn*2.0; +t19 = t16+t17+t18; +t20 = q3*ve*2.0; +t21 = q0*vn*2.0; +t30 = q2*vd*2.0; +t22 = t20+t21-t30; +t23 = q0*q0; +t24 = q1*q1; +t25 = q2*q2; +t26 = q3*q3; +t27 = t23-t24+t25-t26; +t31 = P[1][1]*t2*t15; +t32 = P[6][0]*t2*t10; +t33 = P[1][0]*t2*t15; +t34 = P[2][0]*t2*t19; +t35 = P[5][0]*t2*t27; +t79 = P[4][0]*t2*t7; +t80 = P[3][0]*t2*t22; +t36 = t12+t32+t33+t34+t35-t79-t80; +t37 = t2*t5*t36; +t38 = P[6][1]*t2*t10; +t39 = P[0][1]*t2*t5; +t40 = P[2][1]*t2*t19; +t41 = P[5][1]*t2*t27; +t81 = P[4][1]*t2*t7; +t82 = P[3][1]*t2*t22; +t42 = t31+t38+t39+t40+t41-t81-t82; +t43 = t2*t15*t42; +t44 = P[6][2]*t2*t10; +t45 = P[0][2]*t2*t5; +t46 = P[1][2]*t2*t15; +t47 = P[2][2]*t2*t19; +t48 = P[5][2]*t2*t27; +t83 = P[4][2]*t2*t7; +t84 = P[3][2]*t2*t22; +t49 = t44+t45+t46+t47+t48-t83-t84; +t50 = t2*t19*t49; +t51 = P[6][3]*t2*t10; +t52 = P[0][3]*t2*t5; +t53 = P[1][3]*t2*t15; +t54 = P[2][3]*t2*t19; +t55 = P[5][3]*t2*t27; +t85 = P[4][3]*t2*t7; +t86 = P[3][3]*t2*t22; +t56 = t51+t52+t53+t54+t55-t85-t86; +t57 = P[6][5]*t2*t10; +t58 = P[0][5]*t2*t5; +t59 = P[1][5]*t2*t15; +t60 = P[2][5]*t2*t19; +t61 = P[5][5]*t2*t27; +t88 = P[4][5]*t2*t7; +t89 = P[3][5]*t2*t22; +t62 = t57+t58+t59+t60+t61-t88-t89; +t63 = t2*t27*t62; +t64 = P[6][4]*t2*t10; +t65 = P[0][4]*t2*t5; +t66 = P[1][4]*t2*t15; +t67 = P[2][4]*t2*t19; +t68 = P[5][4]*t2*t27; +t90 = P[4][4]*t2*t7; +t91 = P[3][4]*t2*t22; +t69 = t64+t65+t66+t67+t68-t90-t91; +t70 = P[6][6]*t2*t10; +t71 = P[0][6]*t2*t5; +t72 = P[1][6]*t2*t15; +t73 = P[2][6]*t2*t19; +t74 = P[5][6]*t2*t27; +t93 = P[4][6]*t2*t7; +t94 = P[3][6]*t2*t22; +t75 = t70+t71+t72+t73+t74-t93-t94; +t76 = t2*t10*t75; +t87 = t2*t22*t56; +t92 = t2*t7*t69; +t77 = R_LOS+t37+t43+t50+t63+t76-t87-t92; +t78 = 1.0/t77; +A0[0][0] = t78*(t12-P[0][4]*t2*t7+P[0][1]*t2*t15+P[0][6]*t2*t10+P[0][2]*t2*t19-P[0][3]*t2*t22+P[0][5]*t2*t27); +A0[1][0] = t78*(t31+P[1][0]*t2*t5-P[1][4]*t2*t7+P[1][6]*t2*t10+P[1][2]*t2*t19-P[1][3]*t2*t22+P[1][5]*t2*t27); +A0[2][0] = t78*(t47+P[2][0]*t2*t5-P[2][4]*t2*t7+P[2][1]*t2*t15+P[2][6]*t2*t10-P[2][3]*t2*t22+P[2][5]*t2*t27); +A0[3][0] = t78*(-t86+P[3][0]*t2*t5-P[3][4]*t2*t7+P[3][1]*t2*t15+P[3][6]*t2*t10+P[3][2]*t2*t19+P[3][5]*t2*t27); +A0[4][0] = t78*(-t90+P[4][0]*t2*t5+P[4][1]*t2*t15+P[4][6]*t2*t10+P[4][2]*t2*t19-P[4][3]*t2*t22+P[4][5]*t2*t27); +A0[5][0] = t78*(t61+P[5][0]*t2*t5-P[5][4]*t2*t7+P[5][1]*t2*t15+P[5][6]*t2*t10+P[5][2]*t2*t19-P[5][3]*t2*t22); +A0[6][0] = t78*(t70+P[6][0]*t2*t5-P[6][4]*t2*t7+P[6][1]*t2*t15+P[6][2]*t2*t19-P[6][3]*t2*t22+P[6][5]*t2*t27); +A0[7][0] = t78*(P[7][0]*t2*t5-P[7][4]*t2*t7+P[7][1]*t2*t15+P[7][6]*t2*t10+P[7][2]*t2*t19-P[7][3]*t2*t22+P[7][5]*t2*t27); +A0[8][0] = t78*(P[8][0]*t2*t5-P[8][4]*t2*t7+P[8][1]*t2*t15+P[8][6]*t2*t10+P[8][2]*t2*t19-P[8][3]*t2*t22+P[8][5]*t2*t27); +A0[9][0] = t78*(P[9][0]*t2*t5-P[9][4]*t2*t7+P[9][1]*t2*t15+P[9][6]*t2*t10+P[9][2]*t2*t19-P[9][3]*t2*t22+P[9][5]*t2*t27); +A0[10][0] = t78*(P[10][0]*t2*t5-P[10][4]*t2*t7+P[10][1]*t2*t15+P[10][6]*t2*t10+P[10][2]*t2*t19-P[10][3]*t2*t22+P[10][5]*t2*t27); +A0[11][0] = t78*(P[11][0]*t2*t5-P[11][4]*t2*t7+P[11][1]*t2*t15+P[11][6]*t2*t10+P[11][2]*t2*t19-P[11][3]*t2*t22+P[11][5]*t2*t27); +A0[12][0] = t78*(P[12][0]*t2*t5-P[12][4]*t2*t7+P[12][1]*t2*t15+P[12][6]*t2*t10+P[12][2]*t2*t19-P[12][3]*t2*t22+P[12][5]*t2*t27); +A0[13][0] = t78*(P[13][0]*t2*t5-P[13][4]*t2*t7+P[13][1]*t2*t15+P[13][6]*t2*t10+P[13][2]*t2*t19-P[13][3]*t2*t22+P[13][5]*t2*t27); +A0[14][0] = t78*(P[14][0]*t2*t5-P[14][4]*t2*t7+P[14][1]*t2*t15+P[14][6]*t2*t10+P[14][2]*t2*t19-P[14][3]*t2*t22+P[14][5]*t2*t27); +A0[15][0] = t78*(P[15][0]*t2*t5-P[15][4]*t2*t7+P[15][1]*t2*t15+P[15][6]*t2*t10+P[15][2]*t2*t19-P[15][3]*t2*t22+P[15][5]*t2*t27); +A0[16][0] = t78*(P[16][0]*t2*t5-P[16][4]*t2*t7+P[16][1]*t2*t15+P[16][6]*t2*t10+P[16][2]*t2*t19-P[16][3]*t2*t22+P[16][5]*t2*t27); +A0[17][0] = t78*(P[17][0]*t2*t5-P[17][4]*t2*t7+P[17][1]*t2*t15+P[17][6]*t2*t10+P[17][2]*t2*t19-P[17][3]*t2*t22+P[17][5]*t2*t27); +A0[18][0] = t78*(P[18][0]*t2*t5-P[18][4]*t2*t7+P[18][1]*t2*t15+P[18][6]*t2*t10+P[18][2]*t2*t19-P[18][3]*t2*t22+P[18][5]*t2*t27); +A0[19][0] = t78*(P[19][0]*t2*t5-P[19][4]*t2*t7+P[19][1]*t2*t15+P[19][6]*t2*t10+P[19][2]*t2*t19-P[19][3]*t2*t22+P[19][5]*t2*t27); +A0[20][0] = t78*(P[20][0]*t2*t5-P[20][4]*t2*t7+P[20][1]*t2*t15+P[20][6]*t2*t10+P[20][2]*t2*t19-P[20][3]*t2*t22+P[20][5]*t2*t27); +A0[21][0] = t78*(P[21][0]*t2*t5-P[21][4]*t2*t7+P[21][1]*t2*t15+P[21][6]*t2*t10+P[21][2]*t2*t19-P[21][3]*t2*t22+P[21][5]*t2*t27); +A0[22][0] = t78*(P[22][0]*t2*t5-P[22][4]*t2*t7+P[22][1]*t2*t15+P[22][6]*t2*t10+P[22][2]*t2*t19-P[22][3]*t2*t22+P[22][5]*t2*t27); +A0[23][0] = t78*(P[23][0]*t2*t5-P[23][4]*t2*t7+P[23][1]*t2*t15+P[23][6]*t2*t10+P[23][2]*t2*t19-P[23][3]*t2*t22+P[23][5]*t2*t27); diff --git a/matlab/scripts/Inertial Nav EKF/K_LOSY.c b/matlab/scripts/Inertial Nav EKF/K_LOSY.c index abd0c8f983..8abde33643 100644 --- a/matlab/scripts/Inertial Nav EKF/K_LOSY.c +++ b/matlab/scripts/Inertial Nav EKF/K_LOSY.c @@ -1,89 +1,117 @@ t2 = 1.0/range; -t3 = q0*q2*2.0; -t4 = q0*q0; -t5 = q1*q1; -t6 = q2*q2; -t7 = q3*q3; -t8 = q0*q1*2.0; -t9 = q0*q3*2.0; -t10 = q1*q2*2.0; -t11 = t9+t10; -t12 = q1*q3*2.0; -t13 = t4-t5-t6+t7; -t14 = t13*vd; -t15 = q2*q3*2.0; -t16 = t3+t12; -t17 = t16*vn; -t18 = t4-t5+t6-t7; -t19 = t18*ve; -t20 = t8+t15; -t21 = t20*vd; -t22 = t9-t10; -t28 = t22*vn; -t23 = t19+t21-t28; -t24 = t4+t5-t6-t7; -t25 = t3-t12; -t26 = t8-t15; -t29 = t26*ve; -t27 = t14+t17-t29; -t30 = P[4][4]*t2*t11; -t31 = P[2][4]*t2*t23; -t32 = P[3][4]*t2*t24; -t56 = P[5][4]*t2*t25; -t57 = P[1][4]*t2*t27; -t33 = t30+t31+t32-t56-t57; -t34 = t2*t11*t33; -t35 = P[4][5]*t2*t11; -t36 = P[2][5]*t2*t23; -t37 = P[3][5]*t2*t24; -t58 = P[5][5]*t2*t25; -t59 = P[1][5]*t2*t27; -t38 = t35+t36+t37-t58-t59; -t39 = P[4][1]*t2*t11; -t40 = P[2][1]*t2*t23; -t41 = P[3][1]*t2*t24; -t55 = P[1][1]*t2*t27; -t61 = P[5][1]*t2*t25; -t42 = t39+t40+t41-t55-t61; -t43 = P[4][2]*t2*t11; -t44 = P[2][2]*t2*t23; -t45 = P[3][2]*t2*t24; -t63 = P[5][2]*t2*t25; -t64 = P[1][2]*t2*t27; -t46 = t43+t44+t45-t63-t64; -t47 = t2*t23*t46; -t48 = P[4][3]*t2*t11; -t49 = P[2][3]*t2*t23; -t50 = P[3][3]*t2*t24; -t65 = P[5][3]*t2*t25; -t66 = P[1][3]*t2*t27; -t51 = t48+t49+t50-t65-t66; -t52 = t2*t24*t51; -t60 = t2*t25*t38; -t62 = t2*t27*t42; -t53 = R_LOS+t34+t47+t52-t60-t62; -t54 = 1.0/t53; -A0[0][0] = -t54*(P[0][4]*t2*t11+P[0][2]*t2*t23+P[0][3]*t2*t24-P[0][1]*t2*t27-P[0][5]*t2*t25); -A0[1][0] = -t54*(-t55+P[1][4]*t2*t11+P[1][2]*t2*t23+P[1][3]*t2*t24-P[1][5]*t2*t25); -A0[2][0] = -t54*(t44+P[2][4]*t2*t11+P[2][3]*t2*t24-P[2][1]*t2*t27-P[2][5]*t2*t25); -A0[3][0] = -t54*(t50+P[3][4]*t2*t11+P[3][2]*t2*t23-P[3][1]*t2*t27-P[3][5]*t2*t25); -A0[4][0] = -t54*(t30+P[4][2]*t2*t23+P[4][3]*t2*t24-P[4][1]*t2*t27-P[4][5]*t2*t25); -A0[5][0] = -t54*(-t58+P[5][4]*t2*t11+P[5][2]*t2*t23+P[5][3]*t2*t24-P[5][1]*t2*t27); -A0[6][0] = -t54*(P[6][4]*t2*t11+P[6][2]*t2*t23+P[6][3]*t2*t24-P[6][1]*t2*t27-P[6][5]*t2*t25); -A0[7][0] = -t54*(P[7][4]*t2*t11+P[7][2]*t2*t23+P[7][3]*t2*t24-P[7][1]*t2*t27-P[7][5]*t2*t25); -A0[8][0] = -t54*(P[8][4]*t2*t11+P[8][2]*t2*t23+P[8][3]*t2*t24-P[8][1]*t2*t27-P[8][5]*t2*t25); -A0[9][0] = -t54*(P[9][4]*t2*t11+P[9][2]*t2*t23+P[9][3]*t2*t24-P[9][1]*t2*t27-P[9][5]*t2*t25); -A0[10][0] = -t54*(P[10][4]*t2*t11+P[10][2]*t2*t23+P[10][3]*t2*t24-P[10][1]*t2*t27-P[10][5]*t2*t25); -A0[11][0] = -t54*(P[11][4]*t2*t11+P[11][2]*t2*t23+P[11][3]*t2*t24-P[11][1]*t2*t27-P[11][5]*t2*t25); -A0[12][0] = -t54*(P[12][4]*t2*t11+P[12][2]*t2*t23+P[12][3]*t2*t24-P[12][1]*t2*t27-P[12][5]*t2*t25); -A0[13][0] = -t54*(P[13][4]*t2*t11+P[13][2]*t2*t23+P[13][3]*t2*t24-P[13][1]*t2*t27-P[13][5]*t2*t25); -A0[14][0] = -t54*(P[14][4]*t2*t11+P[14][2]*t2*t23+P[14][3]*t2*t24-P[14][1]*t2*t27-P[14][5]*t2*t25); -A0[15][0] = -t54*(P[15][4]*t2*t11+P[15][2]*t2*t23+P[15][3]*t2*t24-P[15][1]*t2*t27-P[15][5]*t2*t25); -A0[16][0] = -t54*(P[16][4]*t2*t11+P[16][2]*t2*t23+P[16][3]*t2*t24-P[16][1]*t2*t27-P[16][5]*t2*t25); -A0[17][0] = -t54*(P[17][4]*t2*t11+P[17][2]*t2*t23+P[17][3]*t2*t24-P[17][1]*t2*t27-P[17][5]*t2*t25); -A0[18][0] = -t54*(P[18][4]*t2*t11+P[18][2]*t2*t23+P[18][3]*t2*t24-P[18][1]*t2*t27-P[18][5]*t2*t25); -A0[19][0] = -t54*(P[19][4]*t2*t11+P[19][2]*t2*t23+P[19][3]*t2*t24-P[19][1]*t2*t27-P[19][5]*t2*t25); -A0[20][0] = -t54*(P[20][4]*t2*t11+P[20][2]*t2*t23+P[20][3]*t2*t24-P[20][1]*t2*t27-P[20][5]*t2*t25); -A0[21][0] = -t54*(P[21][4]*t2*t11+P[21][2]*t2*t23+P[21][3]*t2*t24-P[21][1]*t2*t27-P[21][5]*t2*t25); -A0[22][0] = -t54*(P[22][4]*t2*t11+P[22][2]*t2*t23+P[22][3]*t2*t24-P[22][1]*t2*t27-P[22][5]*t2*t25); -A0[23][0] = -t54*(P[23][4]*t2*t11+P[23][2]*t2*t23+P[23][3]*t2*t24-P[23][1]*t2*t27-P[23][5]*t2*t25); +t3 = q3*ve*2.0; +t4 = q0*vn*2.0; +t11 = q2*vd*2.0; +t5 = t3+t4-t11; +t6 = q0*q3*2.0; +t7 = q1*q2*2.0; +t8 = t6+t7; +t9 = q0*q2*2.0; +t28 = q1*q3*2.0; +t10 = t9-t28; +t12 = P[0][0]*t2*t5; +t13 = q3*vd*2.0; +t14 = q2*ve*2.0; +t15 = q1*vn*2.0; +t16 = t13+t14+t15; +t17 = q0*vd*2.0; +t18 = q2*vn*2.0; +t29 = q1*ve*2.0; +t19 = t17+t18-t29; +t20 = q1*vd*2.0; +t21 = q0*ve*2.0; +t30 = q3*vn*2.0; +t22 = t20+t21-t30; +t23 = q0*q0; +t24 = q1*q1; +t25 = q2*q2; +t26 = q3*q3; +t27 = t23+t24-t25-t26; +t31 = P[1][1]*t2*t16; +t32 = P[5][0]*t2*t8; +t33 = P[1][0]*t2*t16; +t34 = P[3][0]*t2*t22; +t35 = P[4][0]*t2*t27; +t80 = P[6][0]*t2*t10; +t81 = P[2][0]*t2*t19; +t36 = t12+t32+t33+t34+t35-t80-t81; +t37 = t2*t5*t36; +t38 = P[5][1]*t2*t8; +t39 = P[0][1]*t2*t5; +t40 = P[3][1]*t2*t22; +t41 = P[4][1]*t2*t27; +t82 = P[6][1]*t2*t10; +t83 = P[2][1]*t2*t19; +t42 = t31+t38+t39+t40+t41-t82-t83; +t43 = t2*t16*t42; +t44 = P[5][2]*t2*t8; +t45 = P[0][2]*t2*t5; +t46 = P[1][2]*t2*t16; +t47 = P[3][2]*t2*t22; +t48 = P[4][2]*t2*t27; +t79 = P[2][2]*t2*t19; +t84 = P[6][2]*t2*t10; +t49 = t44+t45+t46+t47+t48-t79-t84; +t50 = P[5][3]*t2*t8; +t51 = P[0][3]*t2*t5; +t52 = P[1][3]*t2*t16; +t53 = P[3][3]*t2*t22; +t54 = P[4][3]*t2*t27; +t86 = P[6][3]*t2*t10; +t87 = P[2][3]*t2*t19; +t55 = t50+t51+t52+t53+t54-t86-t87; +t56 = t2*t22*t55; +t57 = P[5][4]*t2*t8; +t58 = P[0][4]*t2*t5; +t59 = P[1][4]*t2*t16; +t60 = P[3][4]*t2*t22; +t61 = P[4][4]*t2*t27; +t88 = P[6][4]*t2*t10; +t89 = P[2][4]*t2*t19; +t62 = t57+t58+t59+t60+t61-t88-t89; +t63 = t2*t27*t62; +t64 = P[5][5]*t2*t8; +t65 = P[0][5]*t2*t5; +t66 = P[1][5]*t2*t16; +t67 = P[3][5]*t2*t22; +t68 = P[4][5]*t2*t27; +t90 = P[6][5]*t2*t10; +t91 = P[2][5]*t2*t19; +t69 = t64+t65+t66+t67+t68-t90-t91; +t70 = t2*t8*t69; +t71 = P[5][6]*t2*t8; +t72 = P[0][6]*t2*t5; +t73 = P[1][6]*t2*t16; +t74 = P[3][6]*t2*t22; +t75 = P[4][6]*t2*t27; +t92 = P[6][6]*t2*t10; +t93 = P[2][6]*t2*t19; +t76 = t71+t72+t73+t74+t75-t92-t93; +t85 = t2*t19*t49; +t94 = t2*t10*t76; +t77 = R_LOS+t37+t43+t56+t63+t70-t85-t94; +t78 = 1.0/t77; +A0[0][0] = -t78*(t12+P[0][5]*t2*t8-P[0][6]*t2*t10+P[0][1]*t2*t16-P[0][2]*t2*t19+P[0][3]*t2*t22+P[0][4]*t2*t27); +A0[1][0] = -t78*(t31+P[1][0]*t2*t5+P[1][5]*t2*t8-P[1][6]*t2*t10-P[1][2]*t2*t19+P[1][3]*t2*t22+P[1][4]*t2*t27); +A0[2][0] = -t78*(-t79+P[2][0]*t2*t5+P[2][5]*t2*t8-P[2][6]*t2*t10+P[2][1]*t2*t16+P[2][3]*t2*t22+P[2][4]*t2*t27); +A0[3][0] = -t78*(t53+P[3][0]*t2*t5+P[3][5]*t2*t8-P[3][6]*t2*t10+P[3][1]*t2*t16-P[3][2]*t2*t19+P[3][4]*t2*t27); +A0[4][0] = -t78*(t61+P[4][0]*t2*t5+P[4][5]*t2*t8-P[4][6]*t2*t10+P[4][1]*t2*t16-P[4][2]*t2*t19+P[4][3]*t2*t22); +A0[5][0] = -t78*(t64+P[5][0]*t2*t5-P[5][6]*t2*t10+P[5][1]*t2*t16-P[5][2]*t2*t19+P[5][3]*t2*t22+P[5][4]*t2*t27); +A0[6][0] = -t78*(-t92+P[6][0]*t2*t5+P[6][5]*t2*t8+P[6][1]*t2*t16-P[6][2]*t2*t19+P[6][3]*t2*t22+P[6][4]*t2*t27); +A0[7][0] = -t78*(P[7][0]*t2*t5+P[7][5]*t2*t8-P[7][6]*t2*t10+P[7][1]*t2*t16-P[7][2]*t2*t19+P[7][3]*t2*t22+P[7][4]*t2*t27); +A0[8][0] = -t78*(P[8][0]*t2*t5+P[8][5]*t2*t8-P[8][6]*t2*t10+P[8][1]*t2*t16-P[8][2]*t2*t19+P[8][3]*t2*t22+P[8][4]*t2*t27); +A0[9][0] = -t78*(P[9][0]*t2*t5+P[9][5]*t2*t8-P[9][6]*t2*t10+P[9][1]*t2*t16-P[9][2]*t2*t19+P[9][3]*t2*t22+P[9][4]*t2*t27); +A0[10][0] = -t78*(P[10][0]*t2*t5+P[10][5]*t2*t8-P[10][6]*t2*t10+P[10][1]*t2*t16-P[10][2]*t2*t19+P[10][3]*t2*t22+P[10][4]*t2*t27); +A0[11][0] = -t78*(P[11][0]*t2*t5+P[11][5]*t2*t8-P[11][6]*t2*t10+P[11][1]*t2*t16-P[11][2]*t2*t19+P[11][3]*t2*t22+P[11][4]*t2*t27); +A0[12][0] = -t78*(P[12][0]*t2*t5+P[12][5]*t2*t8-P[12][6]*t2*t10+P[12][1]*t2*t16-P[12][2]*t2*t19+P[12][3]*t2*t22+P[12][4]*t2*t27); +A0[13][0] = -t78*(P[13][0]*t2*t5+P[13][5]*t2*t8-P[13][6]*t2*t10+P[13][1]*t2*t16-P[13][2]*t2*t19+P[13][3]*t2*t22+P[13][4]*t2*t27); +A0[14][0] = -t78*(P[14][0]*t2*t5+P[14][5]*t2*t8-P[14][6]*t2*t10+P[14][1]*t2*t16-P[14][2]*t2*t19+P[14][3]*t2*t22+P[14][4]*t2*t27); +A0[15][0] = -t78*(P[15][0]*t2*t5+P[15][5]*t2*t8-P[15][6]*t2*t10+P[15][1]*t2*t16-P[15][2]*t2*t19+P[15][3]*t2*t22+P[15][4]*t2*t27); +A0[16][0] = -t78*(P[16][0]*t2*t5+P[16][5]*t2*t8-P[16][6]*t2*t10+P[16][1]*t2*t16-P[16][2]*t2*t19+P[16][3]*t2*t22+P[16][4]*t2*t27); +A0[17][0] = -t78*(P[17][0]*t2*t5+P[17][5]*t2*t8-P[17][6]*t2*t10+P[17][1]*t2*t16-P[17][2]*t2*t19+P[17][3]*t2*t22+P[17][4]*t2*t27); +A0[18][0] = -t78*(P[18][0]*t2*t5+P[18][5]*t2*t8-P[18][6]*t2*t10+P[18][1]*t2*t16-P[18][2]*t2*t19+P[18][3]*t2*t22+P[18][4]*t2*t27); +A0[19][0] = -t78*(P[19][0]*t2*t5+P[19][5]*t2*t8-P[19][6]*t2*t10+P[19][1]*t2*t16-P[19][2]*t2*t19+P[19][3]*t2*t22+P[19][4]*t2*t27); +A0[20][0] = -t78*(P[20][0]*t2*t5+P[20][5]*t2*t8-P[20][6]*t2*t10+P[20][1]*t2*t16-P[20][2]*t2*t19+P[20][3]*t2*t22+P[20][4]*t2*t27); +A0[21][0] = -t78*(P[21][0]*t2*t5+P[21][5]*t2*t8-P[21][6]*t2*t10+P[21][1]*t2*t16-P[21][2]*t2*t19+P[21][3]*t2*t22+P[21][4]*t2*t27); +A0[22][0] = -t78*(P[22][0]*t2*t5+P[22][5]*t2*t8-P[22][6]*t2*t10+P[22][1]*t2*t16-P[22][2]*t2*t19+P[22][3]*t2*t22+P[22][4]*t2*t27); +A0[23][0] = -t78*(P[23][0]*t2*t5+P[23][5]*t2*t8-P[23][6]*t2*t10+P[23][1]*t2*t16-P[23][2]*t2*t19+P[23][3]*t2*t22+P[23][4]*t2*t27); diff --git a/matlab/scripts/Inertial Nav EKF/M_code24.txt b/matlab/scripts/Inertial Nav EKF/M_code24.txt index 00527b0767..aa51c6ff7e 100644 --- a/matlab/scripts/Inertial Nav EKF/M_code24.txt +++ b/matlab/scripts/Inertial Nav EKF/M_code24.txt @@ -1,217 +1,205 @@ -SF = zeros(25,1); -SF(1) = daz_b/2 + dazNoise/2 - (daz*daz_s)/2; -SF(2) = day_b/2 + dayNoise/2 - (day*day_s)/2; -SF(3) = dax_b/2 + daxNoise/2 - (dax*dax_s)/2; -SF(4) = q3/2 - (q0*SF(1))/2 + (q1*SF(2))/2 - (q2*SF(3))/2; -SF(5) = q0/2 - (q1*SF(3))/2 - (q2*SF(2))/2 + (q3*SF(1))/2; -SF(6) = q1/2 + (q0*SF(3))/2 - (q2*SF(1))/2 - (q3*SF(2))/2; -SF(7) = q3/2 + (q0*SF(1))/2 - (q1*SF(2))/2 - (q2*SF(3))/2; -SF(8) = q0/2 - (q1*SF(3))/2 + (q2*SF(2))/2 - (q3*SF(1))/2; -SF(9) = q0/2 + (q1*SF(3))/2 - (q2*SF(2))/2 - (q3*SF(1))/2; -SF(10) = q2/2 + (q0*SF(2))/2 + (q1*SF(1))/2 + (q3*SF(3))/2; -SF(11) = q2/2 - (q0*SF(2))/2 - (q1*SF(1))/2 + (q3*SF(3))/2; -SF(12) = q2/2 + (q0*SF(2))/2 - (q1*SF(1))/2 - (q3*SF(3))/2; -SF(13) = q1/2 + (q0*SF(3))/2 + (q2*SF(1))/2 + (q3*SF(2))/2; -SF(14) = q1/2 - (q0*SF(3))/2 + (q2*SF(1))/2 - (q3*SF(2))/2; -SF(15) = q3/2 + (q0*SF(1))/2 + (q1*SF(2))/2 + (q2*SF(3))/2; -SF(16) = - q0^2 - q1^2 - q2^2 - q3^2; -SF(17) = dvz_b - dvz + dvzNoise; -SF(18) = dvx - dvxNoise; -SF(19) = dvy - dvyNoise; -SF(20) = q2^2; -SF(21) = SF(20) - q0^2 + q1^2 - q3^2; -SF(22) = SF(20) + q0^2 - q1^2 - q3^2; -SF(23) = 2*q0*q1 - 2*q2*q3; -SF(24) = SF(20) - q0^2 - q1^2 + q3^2; -SF(25) = 2*q1*q2; -SG = zeros(5,1); -SG(1) = - q0^2 - q1^2 - q2^2 - q3^2; +SF = zeros(21,1); +SF(1) = dvz - dvz_b; +SF(2) = dvy - dvy_b; +SF(3) = dvx - dvx_b; +SF(4) = 2*q1*SF(3) + 2*q2*SF(2) + 2*q3*SF(1); +SF(5) = 2*q0*SF(2) - 2*q1*SF(1) + 2*q3*SF(3); +SF(6) = 2*q0*SF(3) + 2*q2*SF(1) - 2*q3*SF(2); +SF(7) = day/2 - day_b/2; +SF(8) = daz/2 - daz_b/2; +SF(9) = dax/2 - dax_b/2; +SF(10) = dax_b/2 - dax/2; +SF(11) = daz_b/2 - daz/2; +SF(12) = day_b/2 - day/2; +SF(13) = 2*q1*SF(2); +SF(14) = 2*q0*SF(1); +SF(15) = q1/2; +SF(16) = q2/2; +SF(17) = q3/2; +SF(18) = q3^2; +SF(19) = q2^2; +SF(20) = q1^2; +SF(21) = q0^2; +SG = zeros(8,1); +SG(1) = q0/2; SG(2) = q3^2; SG(3) = q2^2; SG(4) = q1^2; SG(5) = q0^2; -SQ = zeros(10,1); -SQ(1) = - dvyNoise^2*(2*q0*q1 + 2*q2*q3)*(SG(2) - SG(3) + SG(4) - SG(5)) - dvzNoise^2*(2*q0*q1 - 2*q2*q3)*(SG(2) - SG(3) - SG(4) + SG(5)) - dvxNoise^2*(2*q0*q2 - 2*q1*q3)*(2*q0*q3 + 2*q1*q2); -SQ(2) = dvxNoise^2*(2*q0*q2 - 2*q1*q3)*(SG(2) + SG(3) - SG(4) - SG(5)) + dvzNoise^2*(2*q0*q2 + 2*q1*q3)*(SG(2) - SG(3) - SG(4) + SG(5)) - dvyNoise^2*(2*q0*q1 + 2*q2*q3)*(2*q0*q3 - 2*q1*q2); -SQ(3) = dvyNoise^2*(2*q0*q3 - 2*q1*q2)*(SG(2) - SG(3) + SG(4) - SG(5)) - dvxNoise^2*(2*q0*q3 + 2*q1*q2)*(SG(2) + SG(3) - SG(4) - SG(5)) - dvzNoise^2*(2*q0*q1 - 2*q2*q3)*(2*q0*q2 + 2*q1*q3); -SQ(4) = SG(1)^2; -SQ(5) = dvyNoise^2; -SQ(6) = dvzNoise^2; -SQ(7) = dvxNoise^2; -SQ(8) = 2*q2*q3; -SQ(9) = 2*q1*q3; -SQ(10) = 2*q1*q2; -SPP = zeros(23,1); -SPP(1) = SF(18)*(2*q0*q1 + 2*q2*q3) + SF(19)*(2*q0*q2 - 2*q1*q3); -SPP(2) = SF(19)*(2*q0*q2 + 2*q1*q3) + SF(17)*(SF(25) - 2*q0*q3); -SPP(3) = 2*q3*SF(9) + 2*q1*SF(12) - 2*q0*SF(15) - 2*q2*SF(14); -SPP(4) = 2*q1*SF(8) + 2*q2*SF(7) - 2*q0*SF(13) - 2*q3*SF(11); -SPP(5) = 2*q0*SF(7) - 2*q3*SF(8) - 2*q1*SF(11) + 2*q2*SF(13); -SPP(6) = 2*q0*SF(9) + 2*q2*SF(12) + 2*q1*SF(14) + 2*q3*SF(15); -SPP(7) = 2*q0*SF(8) + 2*q3*SF(7) + 2*q2*SF(11) + 2*q1*SF(13); -SPP(8) = 2*q1*SF(4) - 2*q2*SF(5) - 2*q3*SF(6) + 2*q0*SF(10); -SPP(9) = 2*q0*SF(6) - 2*q1*SF(5) - 2*q2*SF(4) + 2*q3*SF(10); -SPP(10) = SF(19)*SF(21) - SF(17)*(2*q0*q1 + 2*q2*q3); -SPP(11) = SF(18)*SF(21) + SF(17)*(2*q0*q2 - 2*q1*q3); -SPP(12) = SF(18)*SF(22) - SF(19)*(SF(25) + 2*q0*q3); -SPP(13) = SF(18)*SF(23) - SF(17)*(SF(25) + 2*q0*q3); -SPP(14) = 2*q0*SF(5) + 2*q1*SF(6) + 2*q3*SF(4) + 2*q2*SF(10); -SPP(15) = 2*q2*SF(9) - 2*q0*SF(12) - 2*q1*SF(15) + 2*q3*SF(14); -SPP(16) = SF(19)*SF(24) + SF(18)*(SF(25) - 2*q0*q3); -SPP(17) = daz*SF(20) + daz*q0^2 + daz*q1^2 + daz*q3^2; -SPP(18) = day*SF(20) + day*q0^2 + day*q1^2 + day*q3^2; -SPP(19) = dax*SF(20) + dax*q0^2 + dax*q1^2 + dax*q3^2; -SPP(20) = SF(17)*SF(24) - SF(18)*(2*q0*q2 + 2*q1*q3); -SPP(21) = SF(17)*SF(22) - SF(19)*SF(23); -SPP(22) = 2*q0*q2 + 2*q1*q3; -SPP(23) = SF(16); +SG(6) = 2*q2*q3; +SG(7) = 2*q1*q3; +SG(8) = 2*q1*q2; +SQ = zeros(11,1); +SQ(1) = dvzVar*(SG(6) - 2*q0*q1)*(SG(2) - SG(3) - SG(4) + SG(5)) - dvyVar*(SG(6) + 2*q0*q1)*(SG(2) - SG(3) + SG(4) - SG(5)) + dvxVar*(SG(7) - 2*q0*q2)*(SG(8) + 2*q0*q3); +SQ(2) = dvzVar*(SG(7) + 2*q0*q2)*(SG(2) - SG(3) - SG(4) + SG(5)) - dvxVar*(SG(7) - 2*q0*q2)*(SG(2) + SG(3) - SG(4) - SG(5)) + dvyVar*(SG(6) + 2*q0*q1)*(SG(8) - 2*q0*q3); +SQ(3) = dvzVar*(SG(6) - 2*q0*q1)*(SG(7) + 2*q0*q2) - dvyVar*(SG(8) - 2*q0*q3)*(SG(2) - SG(3) + SG(4) - SG(5)) - dvxVar*(SG(8) + 2*q0*q3)*(SG(2) + SG(3) - SG(4) - SG(5)); +SQ(4) = (dayVar*q1*SG(1))/2 - (dazVar*q1*SG(1))/2 - (daxVar*q2*q3)/4; +SQ(5) = (dazVar*q2*SG(1))/2 - (daxVar*q2*SG(1))/2 - (dayVar*q1*q3)/4; +SQ(6) = (daxVar*q3*SG(1))/2 - (dayVar*q3*SG(1))/2 - (dazVar*q1*q2)/4; +SQ(7) = (daxVar*q1*q2)/4 - (dazVar*q3*SG(1))/2 - (dayVar*q1*q2)/4; +SQ(8) = (dazVar*q1*q3)/4 - (daxVar*q1*q3)/4 - (dayVar*q2*SG(1))/2; +SQ(9) = (dayVar*q2*q3)/4 - (daxVar*q1*SG(1))/2 - (dazVar*q2*q3)/4; +SQ(10) = SG(1)^2; +SQ(11) = q1^2; +SPP = zeros(11,1); +SPP(1) = SF(13) + SF(14) - 2*q2*SF(3); +SPP(2) = SF(18) - SF(19) - SF(20) + SF(21); +SPP(3) = SF(18) - SF(19) + SF(20) - SF(21); +SPP(4) = SF(18) + SF(19) - SF(20) - SF(21); +SPP(5) = 2*q0*q2 - 2*q1*q3; +SPP(6) = 2*q0*q1 - 2*q2*q3; +SPP(7) = 2*q0*q3 - 2*q1*q2; +SPP(8) = 2*q0*q1 + 2*q2*q3; +SPP(9) = 2*q0*q3 + 2*q1*q2; +SPP(10) = 2*q0*q2 + 2*q1*q3; +SPP(11) = SF(17); nextP = zeros(24,24); -nextP(1,1) = SPP(6)*(OP(1,1)*SPP(6) - OP(2,1)*SPP(5) + OP(3,1)*SPP(8) + OP(10,1)*SPP(23) + OP(13,1)*SPP(19)) - SPP(5)*(OP(1,2)*SPP(6) - OP(2,2)*SPP(5) + OP(3,2)*SPP(8) + OP(10,2)*SPP(23) + OP(13,2)*SPP(19)) + SPP(8)*(OP(1,3)*SPP(6) - OP(2,3)*SPP(5) + OP(3,3)*SPP(8) + OP(10,3)*SPP(23) + OP(13,3)*SPP(19)) + SPP(23)*(OP(1,10)*SPP(6) - OP(2,10)*SPP(5) + OP(3,10)*SPP(8) + OP(10,10)*SPP(23) + OP(13,10)*SPP(19)) + SPP(19)*(OP(1,13)*SPP(6) - OP(2,13)*SPP(5) + OP(3,13)*SPP(8) + OP(10,13)*SPP(23) + OP(13,13)*SPP(19)) + daxNoise^2*SQ(4); -nextP(1,2) = SPP(7)*(OP(1,2)*SPP(6) - OP(2,2)*SPP(5) + OP(3,2)*SPP(8) + OP(10,2)*SPP(23) + OP(13,2)*SPP(19)) - SPP(3)*(OP(1,1)*SPP(6) - OP(2,1)*SPP(5) + OP(3,1)*SPP(8) + OP(10,1)*SPP(23) + OP(13,1)*SPP(19)) - SPP(9)*(OP(1,3)*SPP(6) - OP(2,3)*SPP(5) + OP(3,3)*SPP(8) + OP(10,3)*SPP(23) + OP(13,3)*SPP(19)) + SPP(23)*(OP(1,11)*SPP(6) - OP(2,11)*SPP(5) + OP(3,11)*SPP(8) + OP(10,11)*SPP(23) + OP(13,11)*SPP(19)) + SPP(18)*(OP(1,14)*SPP(6) - OP(2,14)*SPP(5) + OP(3,14)*SPP(8) + OP(10,14)*SPP(23) + OP(13,14)*SPP(19)); -nextP(2,2) = SPP(7)*(OP(2,2)*SPP(7) - OP(1,2)*SPP(3) - OP(3,2)*SPP(9) + OP(11,2)*SPP(23) + OP(14,2)*SPP(18)) - SPP(3)*(OP(2,1)*SPP(7) - OP(1,1)*SPP(3) - OP(3,1)*SPP(9) + OP(11,1)*SPP(23) + OP(14,1)*SPP(18)) - SPP(9)*(OP(2,3)*SPP(7) - OP(1,3)*SPP(3) - OP(3,3)*SPP(9) + OP(11,3)*SPP(23) + OP(14,3)*SPP(18)) + SPP(23)*(OP(2,11)*SPP(7) - OP(1,11)*SPP(3) - OP(3,11)*SPP(9) + OP(11,11)*SPP(23) + OP(14,11)*SPP(18)) + SPP(18)*(OP(2,14)*SPP(7) - OP(1,14)*SPP(3) - OP(3,14)*SPP(9) + OP(11,14)*SPP(23) + OP(14,14)*SPP(18)) + dayNoise^2*SQ(4); -nextP(1,3) = SPP(15)*(OP(1,1)*SPP(6) - OP(2,1)*SPP(5) + OP(3,1)*SPP(8) + OP(10,1)*SPP(23) + OP(13,1)*SPP(19)) - SPP(4)*(OP(1,2)*SPP(6) - OP(2,2)*SPP(5) + OP(3,2)*SPP(8) + OP(10,2)*SPP(23) + OP(13,2)*SPP(19)) + SPP(14)*(OP(1,3)*SPP(6) - OP(2,3)*SPP(5) + OP(3,3)*SPP(8) + OP(10,3)*SPP(23) + OP(13,3)*SPP(19)) + SPP(23)*(OP(1,12)*SPP(6) - OP(2,12)*SPP(5) + OP(3,12)*SPP(8) + OP(10,12)*SPP(23) + OP(13,12)*SPP(19)) + SPP(17)*(OP(1,15)*SPP(6) - OP(2,15)*SPP(5) + OP(3,15)*SPP(8) + OP(10,15)*SPP(23) + OP(13,15)*SPP(19)); -nextP(2,3) = SPP(15)*(OP(2,1)*SPP(7) - OP(1,1)*SPP(3) - OP(3,1)*SPP(9) + OP(11,1)*SPP(23) + OP(14,1)*SPP(18)) - SPP(4)*(OP(2,2)*SPP(7) - OP(1,2)*SPP(3) - OP(3,2)*SPP(9) + OP(11,2)*SPP(23) + OP(14,2)*SPP(18)) + SPP(14)*(OP(2,3)*SPP(7) - OP(1,3)*SPP(3) - OP(3,3)*SPP(9) + OP(11,3)*SPP(23) + OP(14,3)*SPP(18)) + SPP(23)*(OP(2,12)*SPP(7) - OP(1,12)*SPP(3) - OP(3,12)*SPP(9) + OP(11,12)*SPP(23) + OP(14,12)*SPP(18)) + SPP(17)*(OP(2,15)*SPP(7) - OP(1,15)*SPP(3) - OP(3,15)*SPP(9) + OP(11,15)*SPP(23) + OP(14,15)*SPP(18)); -nextP(3,3) = SPP(15)*(OP(1,1)*SPP(15) - OP(2,1)*SPP(4) + OP(3,1)*SPP(14) + OP(12,1)*SPP(23) + OP(15,1)*SPP(17)) - SPP(4)*(OP(1,2)*SPP(15) - OP(2,2)*SPP(4) + OP(3,2)*SPP(14) + OP(12,2)*SPP(23) + OP(15,2)*SPP(17)) + SPP(14)*(OP(1,3)*SPP(15) - OP(2,3)*SPP(4) + OP(3,3)*SPP(14) + OP(12,3)*SPP(23) + OP(15,3)*SPP(17)) + SPP(23)*(OP(1,12)*SPP(15) - OP(2,12)*SPP(4) + OP(3,12)*SPP(14) + OP(12,12)*SPP(23) + OP(15,12)*SPP(17)) + SPP(17)*(OP(1,15)*SPP(15) - OP(2,15)*SPP(4) + OP(3,15)*SPP(14) + OP(12,15)*SPP(23) + OP(15,15)*SPP(17)) + dazNoise^2*SQ(4); -nextP(1,4) = OP(1,4)*SPP(6) - OP(2,4)*SPP(5) + OP(3,4)*SPP(8) + OP(10,4)*SPP(23) + OP(13,4)*SPP(19) + SPP(2)*(OP(1,1)*SPP(6) - OP(2,1)*SPP(5) + OP(3,1)*SPP(8) + OP(10,1)*SPP(23) + OP(13,1)*SPP(19)) + SPP(20)*(OP(1,2)*SPP(6) - OP(2,2)*SPP(5) + OP(3,2)*SPP(8) + OP(10,2)*SPP(23) + OP(13,2)*SPP(19)) + SPP(16)*(OP(1,3)*SPP(6) - OP(2,3)*SPP(5) + OP(3,3)*SPP(8) + OP(10,3)*SPP(23) + OP(13,3)*SPP(19)) - SPP(22)*(OP(1,16)*SPP(6) - OP(2,16)*SPP(5) + OP(3,16)*SPP(8) + OP(10,16)*SPP(23) + OP(13,16)*SPP(19)); -nextP(2,4) = OP(2,4)*SPP(7) - OP(1,4)*SPP(3) - OP(3,4)*SPP(9) + OP(11,4)*SPP(23) + OP(14,4)*SPP(18) + SPP(2)*(OP(2,1)*SPP(7) - OP(1,1)*SPP(3) - OP(3,1)*SPP(9) + OP(11,1)*SPP(23) + OP(14,1)*SPP(18)) + SPP(20)*(OP(2,2)*SPP(7) - OP(1,2)*SPP(3) - OP(3,2)*SPP(9) + OP(11,2)*SPP(23) + OP(14,2)*SPP(18)) + SPP(16)*(OP(2,3)*SPP(7) - OP(1,3)*SPP(3) - OP(3,3)*SPP(9) + OP(11,3)*SPP(23) + OP(14,3)*SPP(18)) - SPP(22)*(OP(2,16)*SPP(7) - OP(1,16)*SPP(3) - OP(3,16)*SPP(9) + OP(11,16)*SPP(23) + OP(14,16)*SPP(18)); -nextP(3,4) = OP(1,4)*SPP(15) - OP(2,4)*SPP(4) + OP(3,4)*SPP(14) + OP(12,4)*SPP(23) + OP(15,4)*SPP(17) + SPP(2)*(OP(1,1)*SPP(15) - OP(2,1)*SPP(4) + OP(3,1)*SPP(14) + OP(12,1)*SPP(23) + OP(15,1)*SPP(17)) + SPP(20)*(OP(1,2)*SPP(15) - OP(2,2)*SPP(4) + OP(3,2)*SPP(14) + OP(12,2)*SPP(23) + OP(15,2)*SPP(17)) + SPP(16)*(OP(1,3)*SPP(15) - OP(2,3)*SPP(4) + OP(3,3)*SPP(14) + OP(12,3)*SPP(23) + OP(15,3)*SPP(17)) - SPP(22)*(OP(1,16)*SPP(15) - OP(2,16)*SPP(4) + OP(3,16)*SPP(14) + OP(12,16)*SPP(23) + OP(15,16)*SPP(17)); -nextP(4,4) = OP(4,4) + OP(1,4)*SPP(2) + OP(2,4)*SPP(20) + OP(3,4)*SPP(16) - OP(16,4)*SPP(22) + SQ(6)*(SQ(9) + 2*q0*q2)^2 + SQ(5)*(SQ(10) - 2*q0*q3)^2 + SPP(2)*(OP(4,1) + OP(1,1)*SPP(2) + OP(2,1)*SPP(20) + OP(3,1)*SPP(16) - OP(16,1)*SPP(22)) + SPP(20)*(OP(4,2) + OP(1,2)*SPP(2) + OP(2,2)*SPP(20) + OP(3,2)*SPP(16) - OP(16,2)*SPP(22)) + SPP(16)*(OP(4,3) + OP(1,3)*SPP(2) + OP(2,3)*SPP(20) + OP(3,3)*SPP(16) - OP(16,3)*SPP(22)) - SPP(22)*(OP(4,16) + OP(1,16)*SPP(2) + OP(2,16)*SPP(20) + OP(3,16)*SPP(16) - OP(16,16)*SPP(22)) + SQ(7)*(SG(2) + SG(3) - SG(4) - SG(5))^2; -nextP(1,5) = OP(1,5)*SPP(6) - OP(2,5)*SPP(5) + OP(3,5)*SPP(8) + OP(10,5)*SPP(23) + OP(13,5)*SPP(19) + SF(23)*(OP(1,16)*SPP(6) - OP(2,16)*SPP(5) + OP(3,16)*SPP(8) + OP(10,16)*SPP(23) + OP(13,16)*SPP(19)) + SPP(13)*(OP(1,2)*SPP(6) - OP(2,2)*SPP(5) + OP(3,2)*SPP(8) + OP(10,2)*SPP(23) + OP(13,2)*SPP(19)) + SPP(21)*(OP(1,1)*SPP(6) - OP(2,1)*SPP(5) + OP(3,1)*SPP(8) + OP(10,1)*SPP(23) + OP(13,1)*SPP(19)) + SPP(12)*(OP(1,3)*SPP(6) - OP(2,3)*SPP(5) + OP(3,3)*SPP(8) + OP(10,3)*SPP(23) + OP(13,3)*SPP(19)); -nextP(2,5) = OP(2,5)*SPP(7) - OP(1,5)*SPP(3) - OP(3,5)*SPP(9) + OP(11,5)*SPP(23) + OP(14,5)*SPP(18) + SF(23)*(OP(2,16)*SPP(7) - OP(1,16)*SPP(3) - OP(3,16)*SPP(9) + OP(11,16)*SPP(23) + OP(14,16)*SPP(18)) + SPP(13)*(OP(2,2)*SPP(7) - OP(1,2)*SPP(3) - OP(3,2)*SPP(9) + OP(11,2)*SPP(23) + OP(14,2)*SPP(18)) + SPP(21)*(OP(2,1)*SPP(7) - OP(1,1)*SPP(3) - OP(3,1)*SPP(9) + OP(11,1)*SPP(23) + OP(14,1)*SPP(18)) + SPP(12)*(OP(2,3)*SPP(7) - OP(1,3)*SPP(3) - OP(3,3)*SPP(9) + OP(11,3)*SPP(23) + OP(14,3)*SPP(18)); -nextP(3,5) = OP(1,5)*SPP(15) - OP(2,5)*SPP(4) + OP(3,5)*SPP(14) + OP(12,5)*SPP(23) + OP(15,5)*SPP(17) + SF(23)*(OP(1,16)*SPP(15) - OP(2,16)*SPP(4) + OP(3,16)*SPP(14) + OP(12,16)*SPP(23) + OP(15,16)*SPP(17)) + SPP(13)*(OP(1,2)*SPP(15) - OP(2,2)*SPP(4) + OP(3,2)*SPP(14) + OP(12,2)*SPP(23) + OP(15,2)*SPP(17)) + SPP(21)*(OP(1,1)*SPP(15) - OP(2,1)*SPP(4) + OP(3,1)*SPP(14) + OP(12,1)*SPP(23) + OP(15,1)*SPP(17)) + SPP(12)*(OP(1,3)*SPP(15) - OP(2,3)*SPP(4) + OP(3,3)*SPP(14) + OP(12,3)*SPP(23) + OP(15,3)*SPP(17)); -nextP(4,5) = OP(4,5) + SQ(3) + OP(1,5)*SPP(2) + OP(2,5)*SPP(20) + OP(3,5)*SPP(16) - OP(16,5)*SPP(22) + SF(23)*(OP(4,16) + OP(1,16)*SPP(2) + OP(2,16)*SPP(20) + OP(3,16)*SPP(16) - OP(16,16)*SPP(22)) + SPP(13)*(OP(4,2) + OP(1,2)*SPP(2) + OP(2,2)*SPP(20) + OP(3,2)*SPP(16) - OP(16,2)*SPP(22)) + SPP(21)*(OP(4,1) + OP(1,1)*SPP(2) + OP(2,1)*SPP(20) + OP(3,1)*SPP(16) - OP(16,1)*SPP(22)) + SPP(12)*(OP(4,3) + OP(1,3)*SPP(2) + OP(2,3)*SPP(20) + OP(3,3)*SPP(16) - OP(16,3)*SPP(22)); -nextP(5,5) = OP(5,5) + OP(16,5)*SF(23) + OP(1,5)*SPP(21) + OP(2,5)*SPP(13) + OP(3,5)*SPP(12) + SQ(6)*(SQ(8) - 2*q0*q1)^2 + SQ(7)*(SQ(10) + 2*q0*q3)^2 + SF(23)*(OP(5,16) + OP(16,16)*SF(23) + OP(1,16)*SPP(21) + OP(2,16)*SPP(13) + OP(3,16)*SPP(12)) + SPP(13)*(OP(5,2) + OP(16,2)*SF(23) + OP(1,2)*SPP(21) + OP(2,2)*SPP(13) + OP(3,2)*SPP(12)) + SPP(21)*(OP(5,1) + OP(16,1)*SF(23) + OP(1,1)*SPP(21) + OP(2,1)*SPP(13) + OP(3,1)*SPP(12)) + SPP(12)*(OP(5,3) + OP(16,3)*SF(23) + OP(1,3)*SPP(21) + OP(2,3)*SPP(13) + OP(3,3)*SPP(12)) + SQ(5)*(SG(2) - SG(3) + SG(4) - SG(5))^2; -nextP(1,6) = OP(1,6)*SPP(6) - OP(2,6)*SPP(5) + OP(3,6)*SPP(8) + OP(10,6)*SPP(23) + OP(13,6)*SPP(19) + SF(21)*(OP(1,16)*SPP(6) - OP(2,16)*SPP(5) + OP(3,16)*SPP(8) + OP(10,16)*SPP(23) + OP(13,16)*SPP(19)) - SPP(10)*(OP(1,1)*SPP(6) - OP(2,1)*SPP(5) + OP(3,1)*SPP(8) + OP(10,1)*SPP(23) + OP(13,1)*SPP(19)) + SPP(1)*(OP(1,3)*SPP(6) - OP(2,3)*SPP(5) + OP(3,3)*SPP(8) + OP(10,3)*SPP(23) + OP(13,3)*SPP(19)) + SPP(11)*(OP(1,2)*SPP(6) - OP(2,2)*SPP(5) + OP(3,2)*SPP(8) + OP(10,2)*SPP(23) + OP(13,2)*SPP(19)); -nextP(2,6) = OP(2,6)*SPP(7) - OP(1,6)*SPP(3) - OP(3,6)*SPP(9) + OP(11,6)*SPP(23) + OP(14,6)*SPP(18) + SF(21)*(OP(2,16)*SPP(7) - OP(1,16)*SPP(3) - OP(3,16)*SPP(9) + OP(11,16)*SPP(23) + OP(14,16)*SPP(18)) - SPP(10)*(OP(2,1)*SPP(7) - OP(1,1)*SPP(3) - OP(3,1)*SPP(9) + OP(11,1)*SPP(23) + OP(14,1)*SPP(18)) + SPP(1)*(OP(2,3)*SPP(7) - OP(1,3)*SPP(3) - OP(3,3)*SPP(9) + OP(11,3)*SPP(23) + OP(14,3)*SPP(18)) + SPP(11)*(OP(2,2)*SPP(7) - OP(1,2)*SPP(3) - OP(3,2)*SPP(9) + OP(11,2)*SPP(23) + OP(14,2)*SPP(18)); -nextP(3,6) = OP(1,6)*SPP(15) - OP(2,6)*SPP(4) + OP(3,6)*SPP(14) + OP(12,6)*SPP(23) + OP(15,6)*SPP(17) + SF(21)*(OP(1,16)*SPP(15) - OP(2,16)*SPP(4) + OP(3,16)*SPP(14) + OP(12,16)*SPP(23) + OP(15,16)*SPP(17)) - SPP(10)*(OP(1,1)*SPP(15) - OP(2,1)*SPP(4) + OP(3,1)*SPP(14) + OP(12,1)*SPP(23) + OP(15,1)*SPP(17)) + SPP(1)*(OP(1,3)*SPP(15) - OP(2,3)*SPP(4) + OP(3,3)*SPP(14) + OP(12,3)*SPP(23) + OP(15,3)*SPP(17)) + SPP(11)*(OP(1,2)*SPP(15) - OP(2,2)*SPP(4) + OP(3,2)*SPP(14) + OP(12,2)*SPP(23) + OP(15,2)*SPP(17)); -nextP(4,6) = OP(4,6) + SQ(2) + OP(1,6)*SPP(2) + OP(2,6)*SPP(20) + OP(3,6)*SPP(16) - OP(16,6)*SPP(22) + SF(21)*(OP(4,16) + OP(1,16)*SPP(2) + OP(2,16)*SPP(20) + OP(3,16)*SPP(16) - OP(16,16)*SPP(22)) - SPP(10)*(OP(4,1) + OP(1,1)*SPP(2) + OP(2,1)*SPP(20) + OP(3,1)*SPP(16) - OP(16,1)*SPP(22)) + SPP(1)*(OP(4,3) + OP(1,3)*SPP(2) + OP(2,3)*SPP(20) + OP(3,3)*SPP(16) - OP(16,3)*SPP(22)) + SPP(11)*(OP(4,2) + OP(1,2)*SPP(2) + OP(2,2)*SPP(20) + OP(3,2)*SPP(16) - OP(16,2)*SPP(22)); -nextP(5,6) = OP(5,6) + SQ(1) + OP(16,6)*SF(23) + OP(1,6)*SPP(21) + OP(2,6)*SPP(13) + OP(3,6)*SPP(12) + SF(21)*(OP(5,16) + OP(16,16)*SF(23) + OP(1,16)*SPP(21) + OP(2,16)*SPP(13) + OP(3,16)*SPP(12)) - SPP(10)*(OP(5,1) + OP(16,1)*SF(23) + OP(1,1)*SPP(21) + OP(2,1)*SPP(13) + OP(3,1)*SPP(12)) + SPP(1)*(OP(5,3) + OP(16,3)*SF(23) + OP(1,3)*SPP(21) + OP(2,3)*SPP(13) + OP(3,3)*SPP(12)) + SPP(11)*(OP(5,2) + OP(16,2)*SF(23) + OP(1,2)*SPP(21) + OP(2,2)*SPP(13) + OP(3,2)*SPP(12)); -nextP(6,6) = OP(6,6) + OP(16,6)*SF(21) - OP(1,6)*SPP(10) + OP(2,6)*SPP(11) + OP(3,6)*SPP(1) + SQ(5)*(SQ(8) + 2*q0*q1)^2 + SQ(7)*(SQ(9) - 2*q0*q2)^2 + SF(21)*(OP(6,16) + OP(16,16)*SF(21) - OP(1,16)*SPP(10) + OP(2,16)*SPP(11) + OP(3,16)*SPP(1)) - SPP(10)*(OP(6,1) + OP(16,1)*SF(21) - OP(1,1)*SPP(10) + OP(2,1)*SPP(11) + OP(3,1)*SPP(1)) + SPP(1)*(OP(6,3) + OP(16,3)*SF(21) - OP(1,3)*SPP(10) + OP(2,3)*SPP(11) + OP(3,3)*SPP(1)) + SPP(11)*(OP(6,2) + OP(16,2)*SF(21) - OP(1,2)*SPP(10) + OP(2,2)*SPP(11) + OP(3,2)*SPP(1)) + SQ(6)*(SG(2) - SG(3) - SG(4) + SG(5))^2; -nextP(1,7) = OP(1,7)*SPP(6) - OP(2,7)*SPP(5) + OP(3,7)*SPP(8) + OP(10,7)*SPP(23) + OP(13,7)*SPP(19) + dt*(OP(1,4)*SPP(6) - OP(2,4)*SPP(5) + OP(3,4)*SPP(8) + OP(10,4)*SPP(23) + OP(13,4)*SPP(19)); -nextP(2,7) = OP(2,7)*SPP(7) - OP(1,7)*SPP(3) - OP(3,7)*SPP(9) + OP(11,7)*SPP(23) + OP(14,7)*SPP(18) + dt*(OP(2,4)*SPP(7) - OP(1,4)*SPP(3) - OP(3,4)*SPP(9) + OP(11,4)*SPP(23) + OP(14,4)*SPP(18)); -nextP(3,7) = OP(1,7)*SPP(15) - OP(2,7)*SPP(4) + OP(3,7)*SPP(14) + OP(12,7)*SPP(23) + OP(15,7)*SPP(17) + dt*(OP(1,4)*SPP(15) - OP(2,4)*SPP(4) + OP(3,4)*SPP(14) + OP(12,4)*SPP(23) + OP(15,4)*SPP(17)); -nextP(4,7) = OP(4,7) + OP(1,7)*SPP(2) + OP(2,7)*SPP(20) + OP(3,7)*SPP(16) - OP(16,7)*SPP(22) + dt*(OP(4,4) + OP(1,4)*SPP(2) + OP(2,4)*SPP(20) + OP(3,4)*SPP(16) - OP(16,4)*SPP(22)); -nextP(5,7) = OP(5,7) + OP(16,7)*SF(23) + OP(1,7)*SPP(21) + OP(2,7)*SPP(13) + OP(3,7)*SPP(12) + dt*(OP(5,4) + OP(16,4)*SF(23) + OP(1,4)*SPP(21) + OP(2,4)*SPP(13) + OP(3,4)*SPP(12)); -nextP(6,7) = OP(6,7) + OP(16,7)*SF(21) - OP(1,7)*SPP(10) + OP(2,7)*SPP(11) + OP(3,7)*SPP(1) + dt*(OP(6,4) + OP(16,4)*SF(21) - OP(1,4)*SPP(10) + OP(2,4)*SPP(11) + OP(3,4)*SPP(1)); -nextP(7,7) = OP(7,7) + OP(4,7)*dt + dt*(OP(7,4) + OP(4,4)*dt); -nextP(1,8) = OP(1,8)*SPP(6) - OP(2,8)*SPP(5) + OP(3,8)*SPP(8) + OP(10,8)*SPP(23) + OP(13,8)*SPP(19) + dt*(OP(1,5)*SPP(6) - OP(2,5)*SPP(5) + OP(3,5)*SPP(8) + OP(10,5)*SPP(23) + OP(13,5)*SPP(19)); -nextP(2,8) = OP(2,8)*SPP(7) - OP(1,8)*SPP(3) - OP(3,8)*SPP(9) + OP(11,8)*SPP(23) + OP(14,8)*SPP(18) + dt*(OP(2,5)*SPP(7) - OP(1,5)*SPP(3) - OP(3,5)*SPP(9) + OP(11,5)*SPP(23) + OP(14,5)*SPP(18)); -nextP(3,8) = OP(1,8)*SPP(15) - OP(2,8)*SPP(4) + OP(3,8)*SPP(14) + OP(12,8)*SPP(23) + OP(15,8)*SPP(17) + dt*(OP(1,5)*SPP(15) - OP(2,5)*SPP(4) + OP(3,5)*SPP(14) + OP(12,5)*SPP(23) + OP(15,5)*SPP(17)); -nextP(4,8) = OP(4,8) + OP(1,8)*SPP(2) + OP(2,8)*SPP(20) + OP(3,8)*SPP(16) - OP(16,8)*SPP(22) + dt*(OP(4,5) + OP(1,5)*SPP(2) + OP(2,5)*SPP(20) + OP(3,5)*SPP(16) - OP(16,5)*SPP(22)); -nextP(5,8) = OP(5,8) + OP(16,8)*SF(23) + OP(1,8)*SPP(21) + OP(2,8)*SPP(13) + OP(3,8)*SPP(12) + dt*(OP(5,5) + OP(16,5)*SF(23) + OP(1,5)*SPP(21) + OP(2,5)*SPP(13) + OP(3,5)*SPP(12)); -nextP(6,8) = OP(6,8) + OP(16,8)*SF(21) - OP(1,8)*SPP(10) + OP(2,8)*SPP(11) + OP(3,8)*SPP(1) + dt*(OP(6,5) + OP(16,5)*SF(21) - OP(1,5)*SPP(10) + OP(2,5)*SPP(11) + OP(3,5)*SPP(1)); -nextP(7,8) = OP(7,8) + OP(4,8)*dt + dt*(OP(7,5) + OP(4,5)*dt); +nextP(1,1) = OP(1,1) + OP(2,1)*SF(10) + OP(3,1)*SF(12) + OP(4,1)*SF(11) + OP(11,1)*SF(15) + OP(12,1)*SF(16) + OP(13,1)*SPP(11) + (daxVar*SQ(11))/4 + SF(10)*(OP(1,2) + OP(2,2)*SF(10) + OP(3,2)*SF(12) + OP(4,2)*SF(11) + OP(11,2)*SF(15) + OP(12,2)*SF(16) + OP(13,2)*SPP(11)) + SF(12)*(OP(1,3) + OP(2,3)*SF(10) + OP(3,3)*SF(12) + OP(4,3)*SF(11) + OP(11,3)*SF(15) + OP(12,3)*SF(16) + OP(13,3)*SPP(11)) + SF(11)*(OP(1,4) + OP(2,4)*SF(10) + OP(3,4)*SF(12) + OP(4,4)*SF(11) + OP(11,4)*SF(15) + OP(12,4)*SF(16) + OP(13,4)*SPP(11)) + SF(15)*(OP(1,11) + OP(2,11)*SF(10) + OP(3,11)*SF(12) + OP(4,11)*SF(11) + OP(11,11)*SF(15) + OP(12,11)*SF(16) + OP(13,11)*SPP(11)) + SF(16)*(OP(1,12) + OP(2,12)*SF(10) + OP(3,12)*SF(12) + OP(4,12)*SF(11) + OP(11,12)*SF(15) + OP(12,12)*SF(16) + OP(13,12)*SPP(11)) + SPP(11)*(OP(1,13) + OP(2,13)*SF(10) + OP(3,13)*SF(12) + OP(4,13)*SF(11) + OP(11,13)*SF(15) + OP(12,13)*SF(16) + OP(13,13)*SPP(11)) + (dayVar*q2^2)/4 + (dazVar*q3^2)/4; +nextP(1,2) = OP(1,2) + SQ(9) + OP(2,2)*SF(10) + OP(3,2)*SF(12) + OP(4,2)*SF(11) + OP(11,2)*SF(15) + OP(12,2)*SF(16) + OP(13,2)*SPP(11) + SF(9)*(OP(1,1) + OP(2,1)*SF(10) + OP(3,1)*SF(12) + OP(4,1)*SF(11) + OP(11,1)*SF(15) + OP(12,1)*SF(16) + OP(13,1)*SPP(11)) + SF(8)*(OP(1,3) + OP(2,3)*SF(10) + OP(3,3)*SF(12) + OP(4,3)*SF(11) + OP(11,3)*SF(15) + OP(12,3)*SF(16) + OP(13,3)*SPP(11)) + SF(12)*(OP(1,4) + OP(2,4)*SF(10) + OP(3,4)*SF(12) + OP(4,4)*SF(11) + OP(11,4)*SF(15) + OP(12,4)*SF(16) + OP(13,4)*SPP(11)) - SF(16)*(OP(1,13) + OP(2,13)*SF(10) + OP(3,13)*SF(12) + OP(4,13)*SF(11) + OP(11,13)*SF(15) + OP(12,13)*SF(16) + OP(13,13)*SPP(11)) + SPP(11)*(OP(1,12) + OP(2,12)*SF(10) + OP(3,12)*SF(12) + OP(4,12)*SF(11) + OP(11,12)*SF(15) + OP(12,12)*SF(16) + OP(13,12)*SPP(11)) - (q0*(OP(1,11) + OP(2,11)*SF(10) + OP(3,11)*SF(12) + OP(4,11)*SF(11) + OP(11,11)*SF(15) + OP(12,11)*SF(16) + OP(13,11)*SPP(11)))/2; +nextP(2,2) = OP(2,2) + OP(1,2)*SF(9) + OP(3,2)*SF(8) + OP(4,2)*SF(12) - OP(13,2)*SF(16) + OP(12,2)*SPP(11) + daxVar*SQ(10) - (OP(11,2)*q0)/2 + SF(9)*(OP(2,1) + OP(1,1)*SF(9) + OP(3,1)*SF(8) + OP(4,1)*SF(12) - OP(13,1)*SF(16) + OP(12,1)*SPP(11) - (OP(11,1)*q0)/2) + SF(8)*(OP(2,3) + OP(1,3)*SF(9) + OP(3,3)*SF(8) + OP(4,3)*SF(12) - OP(13,3)*SF(16) + OP(12,3)*SPP(11) - (OP(11,3)*q0)/2) + SF(12)*(OP(2,4) + OP(1,4)*SF(9) + OP(3,4)*SF(8) + OP(4,4)*SF(12) - OP(13,4)*SF(16) + OP(12,4)*SPP(11) - (OP(11,4)*q0)/2) - SF(16)*(OP(2,13) + OP(1,13)*SF(9) + OP(3,13)*SF(8) + OP(4,13)*SF(12) - OP(13,13)*SF(16) + OP(12,13)*SPP(11) - (OP(11,13)*q0)/2) + SPP(11)*(OP(2,12) + OP(1,12)*SF(9) + OP(3,12)*SF(8) + OP(4,12)*SF(12) - OP(13,12)*SF(16) + OP(12,12)*SPP(11) - (OP(11,12)*q0)/2) + (dayVar*q3^2)/4 + (dazVar*q2^2)/4 - (q0*(OP(2,11) + OP(1,11)*SF(9) + OP(3,11)*SF(8) + OP(4,11)*SF(12) - OP(13,11)*SF(16) + OP(12,11)*SPP(11) - (OP(11,11)*q0)/2))/2; +nextP(1,3) = OP(1,3) + SQ(8) + OP(2,3)*SF(10) + OP(3,3)*SF(12) + OP(4,3)*SF(11) + OP(11,3)*SF(15) + OP(12,3)*SF(16) + OP(13,3)*SPP(11) + SF(7)*(OP(1,1) + OP(2,1)*SF(10) + OP(3,1)*SF(12) + OP(4,1)*SF(11) + OP(11,1)*SF(15) + OP(12,1)*SF(16) + OP(13,1)*SPP(11)) + SF(11)*(OP(1,2) + OP(2,2)*SF(10) + OP(3,2)*SF(12) + OP(4,2)*SF(11) + OP(11,2)*SF(15) + OP(12,2)*SF(16) + OP(13,2)*SPP(11)) + SF(9)*(OP(1,4) + OP(2,4)*SF(10) + OP(3,4)*SF(12) + OP(4,4)*SF(11) + OP(11,4)*SF(15) + OP(12,4)*SF(16) + OP(13,4)*SPP(11)) + SF(15)*(OP(1,13) + OP(2,13)*SF(10) + OP(3,13)*SF(12) + OP(4,13)*SF(11) + OP(11,13)*SF(15) + OP(12,13)*SF(16) + OP(13,13)*SPP(11)) - SPP(11)*(OP(1,11) + OP(2,11)*SF(10) + OP(3,11)*SF(12) + OP(4,11)*SF(11) + OP(11,11)*SF(15) + OP(12,11)*SF(16) + OP(13,11)*SPP(11)) - (q0*(OP(1,12) + OP(2,12)*SF(10) + OP(3,12)*SF(12) + OP(4,12)*SF(11) + OP(11,12)*SF(15) + OP(12,12)*SF(16) + OP(13,12)*SPP(11)))/2; +nextP(2,3) = OP(2,3) + SQ(6) + OP(1,3)*SF(9) + OP(3,3)*SF(8) + OP(4,3)*SF(12) - OP(13,3)*SF(16) + OP(12,3)*SPP(11) - (OP(11,3)*q0)/2 + SF(7)*(OP(2,1) + OP(1,1)*SF(9) + OP(3,1)*SF(8) + OP(4,1)*SF(12) - OP(13,1)*SF(16) + OP(12,1)*SPP(11) - (OP(11,1)*q0)/2) + SF(11)*(OP(2,2) + OP(1,2)*SF(9) + OP(3,2)*SF(8) + OP(4,2)*SF(12) - OP(13,2)*SF(16) + OP(12,2)*SPP(11) - (OP(11,2)*q0)/2) + SF(9)*(OP(2,4) + OP(1,4)*SF(9) + OP(3,4)*SF(8) + OP(4,4)*SF(12) - OP(13,4)*SF(16) + OP(12,4)*SPP(11) - (OP(11,4)*q0)/2) + SF(15)*(OP(2,13) + OP(1,13)*SF(9) + OP(3,13)*SF(8) + OP(4,13)*SF(12) - OP(13,13)*SF(16) + OP(12,13)*SPP(11) - (OP(11,13)*q0)/2) - SPP(11)*(OP(2,11) + OP(1,11)*SF(9) + OP(3,11)*SF(8) + OP(4,11)*SF(12) - OP(13,11)*SF(16) + OP(12,11)*SPP(11) - (OP(11,11)*q0)/2) - (q0*(OP(2,12) + OP(1,12)*SF(9) + OP(3,12)*SF(8) + OP(4,12)*SF(12) - OP(13,12)*SF(16) + OP(12,12)*SPP(11) - (OP(11,12)*q0)/2))/2; +nextP(3,3) = OP(3,3) + OP(1,3)*SF(7) + OP(2,3)*SF(11) + OP(4,3)*SF(9) + OP(13,3)*SF(15) - OP(11,3)*SPP(11) + dayVar*SQ(10) + (dazVar*SQ(11))/4 - (OP(12,3)*q0)/2 + SF(7)*(OP(3,1) + OP(1,1)*SF(7) + OP(2,1)*SF(11) + OP(4,1)*SF(9) + OP(13,1)*SF(15) - OP(11,1)*SPP(11) - (OP(12,1)*q0)/2) + SF(11)*(OP(3,2) + OP(1,2)*SF(7) + OP(2,2)*SF(11) + OP(4,2)*SF(9) + OP(13,2)*SF(15) - OP(11,2)*SPP(11) - (OP(12,2)*q0)/2) + SF(9)*(OP(3,4) + OP(1,4)*SF(7) + OP(2,4)*SF(11) + OP(4,4)*SF(9) + OP(13,4)*SF(15) - OP(11,4)*SPP(11) - (OP(12,4)*q0)/2) + SF(15)*(OP(3,13) + OP(1,13)*SF(7) + OP(2,13)*SF(11) + OP(4,13)*SF(9) + OP(13,13)*SF(15) - OP(11,13)*SPP(11) - (OP(12,13)*q0)/2) - SPP(11)*(OP(3,11) + OP(1,11)*SF(7) + OP(2,11)*SF(11) + OP(4,11)*SF(9) + OP(13,11)*SF(15) - OP(11,11)*SPP(11) - (OP(12,11)*q0)/2) + (daxVar*q3^2)/4 - (q0*(OP(3,12) + OP(1,12)*SF(7) + OP(2,12)*SF(11) + OP(4,12)*SF(9) + OP(13,12)*SF(15) - OP(11,12)*SPP(11) - (OP(12,12)*q0)/2))/2; +nextP(1,4) = OP(1,4) + SQ(7) + OP(2,4)*SF(10) + OP(3,4)*SF(12) + OP(4,4)*SF(11) + OP(11,4)*SF(15) + OP(12,4)*SF(16) + OP(13,4)*SPP(11) + SF(8)*(OP(1,1) + OP(2,1)*SF(10) + OP(3,1)*SF(12) + OP(4,1)*SF(11) + OP(11,1)*SF(15) + OP(12,1)*SF(16) + OP(13,1)*SPP(11)) + SF(7)*(OP(1,2) + OP(2,2)*SF(10) + OP(3,2)*SF(12) + OP(4,2)*SF(11) + OP(11,2)*SF(15) + OP(12,2)*SF(16) + OP(13,2)*SPP(11)) + SF(10)*(OP(1,3) + OP(2,3)*SF(10) + OP(3,3)*SF(12) + OP(4,3)*SF(11) + OP(11,3)*SF(15) + OP(12,3)*SF(16) + OP(13,3)*SPP(11)) + SF(16)*(OP(1,11) + OP(2,11)*SF(10) + OP(3,11)*SF(12) + OP(4,11)*SF(11) + OP(11,11)*SF(15) + OP(12,11)*SF(16) + OP(13,11)*SPP(11)) - SF(15)*(OP(1,12) + OP(2,12)*SF(10) + OP(3,12)*SF(12) + OP(4,12)*SF(11) + OP(11,12)*SF(15) + OP(12,12)*SF(16) + OP(13,12)*SPP(11)) - (q0*(OP(1,13) + OP(2,13)*SF(10) + OP(3,13)*SF(12) + OP(4,13)*SF(11) + OP(11,13)*SF(15) + OP(12,13)*SF(16) + OP(13,13)*SPP(11)))/2; +nextP(2,4) = OP(2,4) + SQ(5) + OP(1,4)*SF(9) + OP(3,4)*SF(8) + OP(4,4)*SF(12) - OP(13,4)*SF(16) + OP(12,4)*SPP(11) - (OP(11,4)*q0)/2 + SF(8)*(OP(2,1) + OP(1,1)*SF(9) + OP(3,1)*SF(8) + OP(4,1)*SF(12) - OP(13,1)*SF(16) + OP(12,1)*SPP(11) - (OP(11,1)*q0)/2) + SF(7)*(OP(2,2) + OP(1,2)*SF(9) + OP(3,2)*SF(8) + OP(4,2)*SF(12) - OP(13,2)*SF(16) + OP(12,2)*SPP(11) - (OP(11,2)*q0)/2) + SF(10)*(OP(2,3) + OP(1,3)*SF(9) + OP(3,3)*SF(8) + OP(4,3)*SF(12) - OP(13,3)*SF(16) + OP(12,3)*SPP(11) - (OP(11,3)*q0)/2) + SF(16)*(OP(2,11) + OP(1,11)*SF(9) + OP(3,11)*SF(8) + OP(4,11)*SF(12) - OP(13,11)*SF(16) + OP(12,11)*SPP(11) - (OP(11,11)*q0)/2) - SF(15)*(OP(2,12) + OP(1,12)*SF(9) + OP(3,12)*SF(8) + OP(4,12)*SF(12) - OP(13,12)*SF(16) + OP(12,12)*SPP(11) - (OP(11,12)*q0)/2) - (q0*(OP(2,13) + OP(1,13)*SF(9) + OP(3,13)*SF(8) + OP(4,13)*SF(12) - OP(13,13)*SF(16) + OP(12,13)*SPP(11) - (OP(11,13)*q0)/2))/2; +nextP(3,4) = OP(3,4) + SQ(4) + OP(1,4)*SF(7) + OP(2,4)*SF(11) + OP(4,4)*SF(9) + OP(13,4)*SF(15) - OP(11,4)*SPP(11) - (OP(12,4)*q0)/2 + SF(8)*(OP(3,1) + OP(1,1)*SF(7) + OP(2,1)*SF(11) + OP(4,1)*SF(9) + OP(13,1)*SF(15) - OP(11,1)*SPP(11) - (OP(12,1)*q0)/2) + SF(7)*(OP(3,2) + OP(1,2)*SF(7) + OP(2,2)*SF(11) + OP(4,2)*SF(9) + OP(13,2)*SF(15) - OP(11,2)*SPP(11) - (OP(12,2)*q0)/2) + SF(10)*(OP(3,3) + OP(1,3)*SF(7) + OP(2,3)*SF(11) + OP(4,3)*SF(9) + OP(13,3)*SF(15) - OP(11,3)*SPP(11) - (OP(12,3)*q0)/2) + SF(16)*(OP(3,11) + OP(1,11)*SF(7) + OP(2,11)*SF(11) + OP(4,11)*SF(9) + OP(13,11)*SF(15) - OP(11,11)*SPP(11) - (OP(12,11)*q0)/2) - SF(15)*(OP(3,12) + OP(1,12)*SF(7) + OP(2,12)*SF(11) + OP(4,12)*SF(9) + OP(13,12)*SF(15) - OP(11,12)*SPP(11) - (OP(12,12)*q0)/2) - (q0*(OP(3,13) + OP(1,13)*SF(7) + OP(2,13)*SF(11) + OP(4,13)*SF(9) + OP(13,13)*SF(15) - OP(11,13)*SPP(11) - (OP(12,13)*q0)/2))/2; +nextP(4,4) = OP(4,4) + OP(1,4)*SF(8) + OP(2,4)*SF(7) + OP(3,4)*SF(10) + OP(11,4)*SF(16) - OP(12,4)*SF(15) + (dayVar*SQ(11))/4 + dazVar*SQ(10) - (OP(13,4)*q0)/2 + SF(8)*(OP(4,1) + OP(1,1)*SF(8) + OP(2,1)*SF(7) + OP(3,1)*SF(10) + OP(11,1)*SF(16) - OP(12,1)*SF(15) - (OP(13,1)*q0)/2) + SF(7)*(OP(4,2) + OP(1,2)*SF(8) + OP(2,2)*SF(7) + OP(3,2)*SF(10) + OP(11,2)*SF(16) - OP(12,2)*SF(15) - (OP(13,2)*q0)/2) + SF(10)*(OP(4,3) + OP(1,3)*SF(8) + OP(2,3)*SF(7) + OP(3,3)*SF(10) + OP(11,3)*SF(16) - OP(12,3)*SF(15) - (OP(13,3)*q0)/2) + SF(16)*(OP(4,11) + OP(1,11)*SF(8) + OP(2,11)*SF(7) + OP(3,11)*SF(10) + OP(11,11)*SF(16) - OP(12,11)*SF(15) - (OP(13,11)*q0)/2) - SF(15)*(OP(4,12) + OP(1,12)*SF(8) + OP(2,12)*SF(7) + OP(3,12)*SF(10) + OP(11,12)*SF(16) - OP(12,12)*SF(15) - (OP(13,12)*q0)/2) + (daxVar*q2^2)/4 - (q0*(OP(4,13) + OP(1,13)*SF(8) + OP(2,13)*SF(7) + OP(3,13)*SF(10) + OP(11,13)*SF(16) - OP(12,13)*SF(15) - (OP(13,13)*q0)/2))/2; +nextP(1,5) = OP(1,5) + OP(2,5)*SF(10) + OP(3,5)*SF(12) + OP(4,5)*SF(11) + OP(11,5)*SF(15) + OP(12,5)*SF(16) + OP(13,5)*SPP(11) + SF(6)*(OP(1,1) + OP(2,1)*SF(10) + OP(3,1)*SF(12) + OP(4,1)*SF(11) + OP(11,1)*SF(15) + OP(12,1)*SF(16) + OP(13,1)*SPP(11)) + SF(4)*(OP(1,2) + OP(2,2)*SF(10) + OP(3,2)*SF(12) + OP(4,2)*SF(11) + OP(11,2)*SF(15) + OP(12,2)*SF(16) + OP(13,2)*SPP(11)) - SF(5)*(OP(1,4) + OP(2,4)*SF(10) + OP(3,4)*SF(12) + OP(4,4)*SF(11) + OP(11,4)*SF(15) + OP(12,4)*SF(16) + OP(13,4)*SPP(11)) + SPP(1)*(OP(1,3) + OP(2,3)*SF(10) + OP(3,3)*SF(12) + OP(4,3)*SF(11) + OP(11,3)*SF(15) + OP(12,3)*SF(16) + OP(13,3)*SPP(11)) + SPP(4)*(OP(1,14) + OP(2,14)*SF(10) + OP(3,14)*SF(12) + OP(4,14)*SF(11) + OP(11,14)*SF(15) + OP(12,14)*SF(16) + OP(13,14)*SPP(11)) + SPP(7)*(OP(1,15) + OP(2,15)*SF(10) + OP(3,15)*SF(12) + OP(4,15)*SF(11) + OP(11,15)*SF(15) + OP(12,15)*SF(16) + OP(13,15)*SPP(11)) - SPP(10)*(OP(1,16) + OP(2,16)*SF(10) + OP(3,16)*SF(12) + OP(4,16)*SF(11) + OP(11,16)*SF(15) + OP(12,16)*SF(16) + OP(13,16)*SPP(11)); +nextP(2,5) = OP(2,5) + OP(1,5)*SF(9) + OP(3,5)*SF(8) + OP(4,5)*SF(12) - OP(13,5)*SF(16) + OP(12,5)*SPP(11) - (OP(11,5)*q0)/2 + SF(6)*(OP(2,1) + OP(1,1)*SF(9) + OP(3,1)*SF(8) + OP(4,1)*SF(12) - OP(13,1)*SF(16) + OP(12,1)*SPP(11) - (OP(11,1)*q0)/2) + SF(4)*(OP(2,2) + OP(1,2)*SF(9) + OP(3,2)*SF(8) + OP(4,2)*SF(12) - OP(13,2)*SF(16) + OP(12,2)*SPP(11) - (OP(11,2)*q0)/2) - SF(5)*(OP(2,4) + OP(1,4)*SF(9) + OP(3,4)*SF(8) + OP(4,4)*SF(12) - OP(13,4)*SF(16) + OP(12,4)*SPP(11) - (OP(11,4)*q0)/2) + SPP(1)*(OP(2,3) + OP(1,3)*SF(9) + OP(3,3)*SF(8) + OP(4,3)*SF(12) - OP(13,3)*SF(16) + OP(12,3)*SPP(11) - (OP(11,3)*q0)/2) + SPP(4)*(OP(2,14) + OP(1,14)*SF(9) + OP(3,14)*SF(8) + OP(4,14)*SF(12) - OP(13,14)*SF(16) + OP(12,14)*SPP(11) - (OP(11,14)*q0)/2) + SPP(7)*(OP(2,15) + OP(1,15)*SF(9) + OP(3,15)*SF(8) + OP(4,15)*SF(12) - OP(13,15)*SF(16) + OP(12,15)*SPP(11) - (OP(11,15)*q0)/2) - SPP(10)*(OP(2,16) + OP(1,16)*SF(9) + OP(3,16)*SF(8) + OP(4,16)*SF(12) - OP(13,16)*SF(16) + OP(12,16)*SPP(11) - (OP(11,16)*q0)/2); +nextP(3,5) = OP(3,5) + OP(1,5)*SF(7) + OP(2,5)*SF(11) + OP(4,5)*SF(9) + OP(13,5)*SF(15) - OP(11,5)*SPP(11) - (OP(12,5)*q0)/2 + SF(6)*(OP(3,1) + OP(1,1)*SF(7) + OP(2,1)*SF(11) + OP(4,1)*SF(9) + OP(13,1)*SF(15) - OP(11,1)*SPP(11) - (OP(12,1)*q0)/2) + SF(4)*(OP(3,2) + OP(1,2)*SF(7) + OP(2,2)*SF(11) + OP(4,2)*SF(9) + OP(13,2)*SF(15) - OP(11,2)*SPP(11) - (OP(12,2)*q0)/2) - SF(5)*(OP(3,4) + OP(1,4)*SF(7) + OP(2,4)*SF(11) + OP(4,4)*SF(9) + OP(13,4)*SF(15) - OP(11,4)*SPP(11) - (OP(12,4)*q0)/2) + SPP(1)*(OP(3,3) + OP(1,3)*SF(7) + OP(2,3)*SF(11) + OP(4,3)*SF(9) + OP(13,3)*SF(15) - OP(11,3)*SPP(11) - (OP(12,3)*q0)/2) + SPP(4)*(OP(3,14) + OP(1,14)*SF(7) + OP(2,14)*SF(11) + OP(4,14)*SF(9) + OP(13,14)*SF(15) - OP(11,14)*SPP(11) - (OP(12,14)*q0)/2) + SPP(7)*(OP(3,15) + OP(1,15)*SF(7) + OP(2,15)*SF(11) + OP(4,15)*SF(9) + OP(13,15)*SF(15) - OP(11,15)*SPP(11) - (OP(12,15)*q0)/2) - SPP(10)*(OP(3,16) + OP(1,16)*SF(7) + OP(2,16)*SF(11) + OP(4,16)*SF(9) + OP(13,16)*SF(15) - OP(11,16)*SPP(11) - (OP(12,16)*q0)/2); +nextP(4,5) = OP(4,5) + OP(1,5)*SF(8) + OP(2,5)*SF(7) + OP(3,5)*SF(10) + OP(11,5)*SF(16) - OP(12,5)*SF(15) - (OP(13,5)*q0)/2 + SF(6)*(OP(4,1) + OP(1,1)*SF(8) + OP(2,1)*SF(7) + OP(3,1)*SF(10) + OP(11,1)*SF(16) - OP(12,1)*SF(15) - (OP(13,1)*q0)/2) + SF(4)*(OP(4,2) + OP(1,2)*SF(8) + OP(2,2)*SF(7) + OP(3,2)*SF(10) + OP(11,2)*SF(16) - OP(12,2)*SF(15) - (OP(13,2)*q0)/2) - SF(5)*(OP(4,4) + OP(1,4)*SF(8) + OP(2,4)*SF(7) + OP(3,4)*SF(10) + OP(11,4)*SF(16) - OP(12,4)*SF(15) - (OP(13,4)*q0)/2) + SPP(1)*(OP(4,3) + OP(1,3)*SF(8) + OP(2,3)*SF(7) + OP(3,3)*SF(10) + OP(11,3)*SF(16) - OP(12,3)*SF(15) - (OP(13,3)*q0)/2) + SPP(4)*(OP(4,14) + OP(1,14)*SF(8) + OP(2,14)*SF(7) + OP(3,14)*SF(10) + OP(11,14)*SF(16) - OP(12,14)*SF(15) - (OP(13,14)*q0)/2) + SPP(7)*(OP(4,15) + OP(1,15)*SF(8) + OP(2,15)*SF(7) + OP(3,15)*SF(10) + OP(11,15)*SF(16) - OP(12,15)*SF(15) - (OP(13,15)*q0)/2) - SPP(10)*(OP(4,16) + OP(1,16)*SF(8) + OP(2,16)*SF(7) + OP(3,16)*SF(10) + OP(11,16)*SF(16) - OP(12,16)*SF(15) - (OP(13,16)*q0)/2); +nextP(5,5) = OP(5,5) + OP(1,5)*SF(6) + OP(2,5)*SF(4) - OP(4,5)*SF(5) + OP(3,5)*SPP(1) + OP(14,5)*SPP(4) + OP(15,5)*SPP(7) - OP(16,5)*SPP(10) + dvyVar*(SG(8) - 2*q0*q3)^2 + dvzVar*(SG(7) + 2*q0*q2)^2 + SF(6)*(OP(5,1) + OP(1,1)*SF(6) + OP(2,1)*SF(4) - OP(4,1)*SF(5) + OP(3,1)*SPP(1) + OP(14,1)*SPP(4) + OP(15,1)*SPP(7) - OP(16,1)*SPP(10)) + SF(4)*(OP(5,2) + OP(1,2)*SF(6) + OP(2,2)*SF(4) - OP(4,2)*SF(5) + OP(3,2)*SPP(1) + OP(14,2)*SPP(4) + OP(15,2)*SPP(7) - OP(16,2)*SPP(10)) - SF(5)*(OP(5,4) + OP(1,4)*SF(6) + OP(2,4)*SF(4) - OP(4,4)*SF(5) + OP(3,4)*SPP(1) + OP(14,4)*SPP(4) + OP(15,4)*SPP(7) - OP(16,4)*SPP(10)) + SPP(1)*(OP(5,3) + OP(1,3)*SF(6) + OP(2,3)*SF(4) - OP(4,3)*SF(5) + OP(3,3)*SPP(1) + OP(14,3)*SPP(4) + OP(15,3)*SPP(7) - OP(16,3)*SPP(10)) + SPP(4)*(OP(5,14) + OP(1,14)*SF(6) + OP(2,14)*SF(4) - OP(4,14)*SF(5) + OP(3,14)*SPP(1) + OP(14,14)*SPP(4) + OP(15,14)*SPP(7) - OP(16,14)*SPP(10)) + SPP(7)*(OP(5,15) + OP(1,15)*SF(6) + OP(2,15)*SF(4) - OP(4,15)*SF(5) + OP(3,15)*SPP(1) + OP(14,15)*SPP(4) + OP(15,15)*SPP(7) - OP(16,15)*SPP(10)) - SPP(10)*(OP(5,16) + OP(1,16)*SF(6) + OP(2,16)*SF(4) - OP(4,16)*SF(5) + OP(3,16)*SPP(1) + OP(14,16)*SPP(4) + OP(15,16)*SPP(7) - OP(16,16)*SPP(10)) + dvxVar*(SG(2) + SG(3) - SG(4) - SG(5))^2; +nextP(1,6) = OP(1,6) + OP(2,6)*SF(10) + OP(3,6)*SF(12) + OP(4,6)*SF(11) + OP(11,6)*SF(15) + OP(12,6)*SF(16) + OP(13,6)*SPP(11) + SF(5)*(OP(1,1) + OP(2,1)*SF(10) + OP(3,1)*SF(12) + OP(4,1)*SF(11) + OP(11,1)*SF(15) + OP(12,1)*SF(16) + OP(13,1)*SPP(11)) + SF(4)*(OP(1,3) + OP(2,3)*SF(10) + OP(3,3)*SF(12) + OP(4,3)*SF(11) + OP(11,3)*SF(15) + OP(12,3)*SF(16) + OP(13,3)*SPP(11)) + SF(6)*(OP(1,4) + OP(2,4)*SF(10) + OP(3,4)*SF(12) + OP(4,4)*SF(11) + OP(11,4)*SF(15) + OP(12,4)*SF(16) + OP(13,4)*SPP(11)) - SPP(1)*(OP(1,2) + OP(2,2)*SF(10) + OP(3,2)*SF(12) + OP(4,2)*SF(11) + OP(11,2)*SF(15) + OP(12,2)*SF(16) + OP(13,2)*SPP(11)) - SPP(9)*(OP(1,14) + OP(2,14)*SF(10) + OP(3,14)*SF(12) + OP(4,14)*SF(11) + OP(11,14)*SF(15) + OP(12,14)*SF(16) + OP(13,14)*SPP(11)) + SPP(3)*(OP(1,15) + OP(2,15)*SF(10) + OP(3,15)*SF(12) + OP(4,15)*SF(11) + OP(11,15)*SF(15) + OP(12,15)*SF(16) + OP(13,15)*SPP(11)) + SPP(6)*(OP(1,16) + OP(2,16)*SF(10) + OP(3,16)*SF(12) + OP(4,16)*SF(11) + OP(11,16)*SF(15) + OP(12,16)*SF(16) + OP(13,16)*SPP(11)); +nextP(2,6) = OP(2,6) + OP(1,6)*SF(9) + OP(3,6)*SF(8) + OP(4,6)*SF(12) - OP(13,6)*SF(16) + OP(12,6)*SPP(11) - (OP(11,6)*q0)/2 + SF(5)*(OP(2,1) + OP(1,1)*SF(9) + OP(3,1)*SF(8) + OP(4,1)*SF(12) - OP(13,1)*SF(16) + OP(12,1)*SPP(11) - (OP(11,1)*q0)/2) + SF(4)*(OP(2,3) + OP(1,3)*SF(9) + OP(3,3)*SF(8) + OP(4,3)*SF(12) - OP(13,3)*SF(16) + OP(12,3)*SPP(11) - (OP(11,3)*q0)/2) + SF(6)*(OP(2,4) + OP(1,4)*SF(9) + OP(3,4)*SF(8) + OP(4,4)*SF(12) - OP(13,4)*SF(16) + OP(12,4)*SPP(11) - (OP(11,4)*q0)/2) - SPP(1)*(OP(2,2) + OP(1,2)*SF(9) + OP(3,2)*SF(8) + OP(4,2)*SF(12) - OP(13,2)*SF(16) + OP(12,2)*SPP(11) - (OP(11,2)*q0)/2) - SPP(9)*(OP(2,14) + OP(1,14)*SF(9) + OP(3,14)*SF(8) + OP(4,14)*SF(12) - OP(13,14)*SF(16) + OP(12,14)*SPP(11) - (OP(11,14)*q0)/2) + SPP(3)*(OP(2,15) + OP(1,15)*SF(9) + OP(3,15)*SF(8) + OP(4,15)*SF(12) - OP(13,15)*SF(16) + OP(12,15)*SPP(11) - (OP(11,15)*q0)/2) + SPP(6)*(OP(2,16) + OP(1,16)*SF(9) + OP(3,16)*SF(8) + OP(4,16)*SF(12) - OP(13,16)*SF(16) + OP(12,16)*SPP(11) - (OP(11,16)*q0)/2); +nextP(3,6) = OP(3,6) + OP(1,6)*SF(7) + OP(2,6)*SF(11) + OP(4,6)*SF(9) + OP(13,6)*SF(15) - OP(11,6)*SPP(11) - (OP(12,6)*q0)/2 + SF(5)*(OP(3,1) + OP(1,1)*SF(7) + OP(2,1)*SF(11) + OP(4,1)*SF(9) + OP(13,1)*SF(15) - OP(11,1)*SPP(11) - (OP(12,1)*q0)/2) + SF(4)*(OP(3,3) + OP(1,3)*SF(7) + OP(2,3)*SF(11) + OP(4,3)*SF(9) + OP(13,3)*SF(15) - OP(11,3)*SPP(11) - (OP(12,3)*q0)/2) + SF(6)*(OP(3,4) + OP(1,4)*SF(7) + OP(2,4)*SF(11) + OP(4,4)*SF(9) + OP(13,4)*SF(15) - OP(11,4)*SPP(11) - (OP(12,4)*q0)/2) - SPP(1)*(OP(3,2) + OP(1,2)*SF(7) + OP(2,2)*SF(11) + OP(4,2)*SF(9) + OP(13,2)*SF(15) - OP(11,2)*SPP(11) - (OP(12,2)*q0)/2) - SPP(9)*(OP(3,14) + OP(1,14)*SF(7) + OP(2,14)*SF(11) + OP(4,14)*SF(9) + OP(13,14)*SF(15) - OP(11,14)*SPP(11) - (OP(12,14)*q0)/2) + SPP(3)*(OP(3,15) + OP(1,15)*SF(7) + OP(2,15)*SF(11) + OP(4,15)*SF(9) + OP(13,15)*SF(15) - OP(11,15)*SPP(11) - (OP(12,15)*q0)/2) + SPP(6)*(OP(3,16) + OP(1,16)*SF(7) + OP(2,16)*SF(11) + OP(4,16)*SF(9) + OP(13,16)*SF(15) - OP(11,16)*SPP(11) - (OP(12,16)*q0)/2); +nextP(4,6) = OP(4,6) + OP(1,6)*SF(8) + OP(2,6)*SF(7) + OP(3,6)*SF(10) + OP(11,6)*SF(16) - OP(12,6)*SF(15) - (OP(13,6)*q0)/2 + SF(5)*(OP(4,1) + OP(1,1)*SF(8) + OP(2,1)*SF(7) + OP(3,1)*SF(10) + OP(11,1)*SF(16) - OP(12,1)*SF(15) - (OP(13,1)*q0)/2) + SF(4)*(OP(4,3) + OP(1,3)*SF(8) + OP(2,3)*SF(7) + OP(3,3)*SF(10) + OP(11,3)*SF(16) - OP(12,3)*SF(15) - (OP(13,3)*q0)/2) + SF(6)*(OP(4,4) + OP(1,4)*SF(8) + OP(2,4)*SF(7) + OP(3,4)*SF(10) + OP(11,4)*SF(16) - OP(12,4)*SF(15) - (OP(13,4)*q0)/2) - SPP(1)*(OP(4,2) + OP(1,2)*SF(8) + OP(2,2)*SF(7) + OP(3,2)*SF(10) + OP(11,2)*SF(16) - OP(12,2)*SF(15) - (OP(13,2)*q0)/2) - SPP(9)*(OP(4,14) + OP(1,14)*SF(8) + OP(2,14)*SF(7) + OP(3,14)*SF(10) + OP(11,14)*SF(16) - OP(12,14)*SF(15) - (OP(13,14)*q0)/2) + SPP(3)*(OP(4,15) + OP(1,15)*SF(8) + OP(2,15)*SF(7) + OP(3,15)*SF(10) + OP(11,15)*SF(16) - OP(12,15)*SF(15) - (OP(13,15)*q0)/2) + SPP(6)*(OP(4,16) + OP(1,16)*SF(8) + OP(2,16)*SF(7) + OP(3,16)*SF(10) + OP(11,16)*SF(16) - OP(12,16)*SF(15) - (OP(13,16)*q0)/2); +nextP(5,6) = OP(5,6) + SQ(3) + OP(1,6)*SF(6) + OP(2,6)*SF(4) - OP(4,6)*SF(5) + OP(3,6)*SPP(1) + OP(14,6)*SPP(4) + OP(15,6)*SPP(7) - OP(16,6)*SPP(10) + SF(5)*(OP(5,1) + OP(1,1)*SF(6) + OP(2,1)*SF(4) - OP(4,1)*SF(5) + OP(3,1)*SPP(1) + OP(14,1)*SPP(4) + OP(15,1)*SPP(7) - OP(16,1)*SPP(10)) + SF(4)*(OP(5,3) + OP(1,3)*SF(6) + OP(2,3)*SF(4) - OP(4,3)*SF(5) + OP(3,3)*SPP(1) + OP(14,3)*SPP(4) + OP(15,3)*SPP(7) - OP(16,3)*SPP(10)) + SF(6)*(OP(5,4) + OP(1,4)*SF(6) + OP(2,4)*SF(4) - OP(4,4)*SF(5) + OP(3,4)*SPP(1) + OP(14,4)*SPP(4) + OP(15,4)*SPP(7) - OP(16,4)*SPP(10)) - SPP(1)*(OP(5,2) + OP(1,2)*SF(6) + OP(2,2)*SF(4) - OP(4,2)*SF(5) + OP(3,2)*SPP(1) + OP(14,2)*SPP(4) + OP(15,2)*SPP(7) - OP(16,2)*SPP(10)) - SPP(9)*(OP(5,14) + OP(1,14)*SF(6) + OP(2,14)*SF(4) - OP(4,14)*SF(5) + OP(3,14)*SPP(1) + OP(14,14)*SPP(4) + OP(15,14)*SPP(7) - OP(16,14)*SPP(10)) + SPP(3)*(OP(5,15) + OP(1,15)*SF(6) + OP(2,15)*SF(4) - OP(4,15)*SF(5) + OP(3,15)*SPP(1) + OP(14,15)*SPP(4) + OP(15,15)*SPP(7) - OP(16,15)*SPP(10)) + SPP(6)*(OP(5,16) + OP(1,16)*SF(6) + OP(2,16)*SF(4) - OP(4,16)*SF(5) + OP(3,16)*SPP(1) + OP(14,16)*SPP(4) + OP(15,16)*SPP(7) - OP(16,16)*SPP(10)); +nextP(6,6) = OP(6,6) + OP(1,6)*SF(5) + OP(3,6)*SF(4) + OP(4,6)*SF(6) - OP(2,6)*SPP(1) - OP(14,6)*SPP(9) + OP(15,6)*SPP(3) + OP(16,6)*SPP(6) + dvxVar*(SG(8) + 2*q0*q3)^2 + dvzVar*(SG(6) - 2*q0*q1)^2 + SF(5)*(OP(6,1) + OP(1,1)*SF(5) + OP(3,1)*SF(4) + OP(4,1)*SF(6) - OP(2,1)*SPP(1) - OP(14,1)*SPP(9) + OP(15,1)*SPP(3) + OP(16,1)*SPP(6)) + SF(4)*(OP(6,3) + OP(1,3)*SF(5) + OP(3,3)*SF(4) + OP(4,3)*SF(6) - OP(2,3)*SPP(1) - OP(14,3)*SPP(9) + OP(15,3)*SPP(3) + OP(16,3)*SPP(6)) + SF(6)*(OP(6,4) + OP(1,4)*SF(5) + OP(3,4)*SF(4) + OP(4,4)*SF(6) - OP(2,4)*SPP(1) - OP(14,4)*SPP(9) + OP(15,4)*SPP(3) + OP(16,4)*SPP(6)) - SPP(1)*(OP(6,2) + OP(1,2)*SF(5) + OP(3,2)*SF(4) + OP(4,2)*SF(6) - OP(2,2)*SPP(1) - OP(14,2)*SPP(9) + OP(15,2)*SPP(3) + OP(16,2)*SPP(6)) - SPP(9)*(OP(6,14) + OP(1,14)*SF(5) + OP(3,14)*SF(4) + OP(4,14)*SF(6) - OP(2,14)*SPP(1) - OP(14,14)*SPP(9) + OP(15,14)*SPP(3) + OP(16,14)*SPP(6)) + SPP(3)*(OP(6,15) + OP(1,15)*SF(5) + OP(3,15)*SF(4) + OP(4,15)*SF(6) - OP(2,15)*SPP(1) - OP(14,15)*SPP(9) + OP(15,15)*SPP(3) + OP(16,15)*SPP(6)) + SPP(6)*(OP(6,16) + OP(1,16)*SF(5) + OP(3,16)*SF(4) + OP(4,16)*SF(6) - OP(2,16)*SPP(1) - OP(14,16)*SPP(9) + OP(15,16)*SPP(3) + OP(16,16)*SPP(6)) + dvyVar*(SG(2) - SG(3) + SG(4) - SG(5))^2; +nextP(1,7) = OP(1,7) + OP(2,7)*SF(10) + OP(3,7)*SF(12) + OP(4,7)*SF(11) + OP(11,7)*SF(15) + OP(12,7)*SF(16) + OP(13,7)*SPP(11) + SF(5)*(OP(1,2) + OP(2,2)*SF(10) + OP(3,2)*SF(12) + OP(4,2)*SF(11) + OP(11,2)*SF(15) + OP(12,2)*SF(16) + OP(13,2)*SPP(11)) - SF(6)*(OP(1,3) + OP(2,3)*SF(10) + OP(3,3)*SF(12) + OP(4,3)*SF(11) + OP(11,3)*SF(15) + OP(12,3)*SF(16) + OP(13,3)*SPP(11)) + SF(4)*(OP(1,4) + OP(2,4)*SF(10) + OP(3,4)*SF(12) + OP(4,4)*SF(11) + OP(11,4)*SF(15) + OP(12,4)*SF(16) + OP(13,4)*SPP(11)) + SPP(1)*(OP(1,1) + OP(2,1)*SF(10) + OP(3,1)*SF(12) + OP(4,1)*SF(11) + OP(11,1)*SF(15) + OP(12,1)*SF(16) + OP(13,1)*SPP(11)) + SPP(5)*(OP(1,14) + OP(2,14)*SF(10) + OP(3,14)*SF(12) + OP(4,14)*SF(11) + OP(11,14)*SF(15) + OP(12,14)*SF(16) + OP(13,14)*SPP(11)) - SPP(8)*(OP(1,15) + OP(2,15)*SF(10) + OP(3,15)*SF(12) + OP(4,15)*SF(11) + OP(11,15)*SF(15) + OP(12,15)*SF(16) + OP(13,15)*SPP(11)) - SPP(2)*(OP(1,16) + OP(2,16)*SF(10) + OP(3,16)*SF(12) + OP(4,16)*SF(11) + OP(11,16)*SF(15) + OP(12,16)*SF(16) + OP(13,16)*SPP(11)); +nextP(2,7) = OP(2,7) + OP(1,7)*SF(9) + OP(3,7)*SF(8) + OP(4,7)*SF(12) - OP(13,7)*SF(16) + OP(12,7)*SPP(11) - (OP(11,7)*q0)/2 + SF(5)*(OP(2,2) + OP(1,2)*SF(9) + OP(3,2)*SF(8) + OP(4,2)*SF(12) - OP(13,2)*SF(16) + OP(12,2)*SPP(11) - (OP(11,2)*q0)/2) - SF(6)*(OP(2,3) + OP(1,3)*SF(9) + OP(3,3)*SF(8) + OP(4,3)*SF(12) - OP(13,3)*SF(16) + OP(12,3)*SPP(11) - (OP(11,3)*q0)/2) + SF(4)*(OP(2,4) + OP(1,4)*SF(9) + OP(3,4)*SF(8) + OP(4,4)*SF(12) - OP(13,4)*SF(16) + OP(12,4)*SPP(11) - (OP(11,4)*q0)/2) + SPP(1)*(OP(2,1) + OP(1,1)*SF(9) + OP(3,1)*SF(8) + OP(4,1)*SF(12) - OP(13,1)*SF(16) + OP(12,1)*SPP(11) - (OP(11,1)*q0)/2) + SPP(5)*(OP(2,14) + OP(1,14)*SF(9) + OP(3,14)*SF(8) + OP(4,14)*SF(12) - OP(13,14)*SF(16) + OP(12,14)*SPP(11) - (OP(11,14)*q0)/2) - SPP(8)*(OP(2,15) + OP(1,15)*SF(9) + OP(3,15)*SF(8) + OP(4,15)*SF(12) - OP(13,15)*SF(16) + OP(12,15)*SPP(11) - (OP(11,15)*q0)/2) - SPP(2)*(OP(2,16) + OP(1,16)*SF(9) + OP(3,16)*SF(8) + OP(4,16)*SF(12) - OP(13,16)*SF(16) + OP(12,16)*SPP(11) - (OP(11,16)*q0)/2); +nextP(3,7) = OP(3,7) + OP(1,7)*SF(7) + OP(2,7)*SF(11) + OP(4,7)*SF(9) + OP(13,7)*SF(15) - OP(11,7)*SPP(11) - (OP(12,7)*q0)/2 + SF(5)*(OP(3,2) + OP(1,2)*SF(7) + OP(2,2)*SF(11) + OP(4,2)*SF(9) + OP(13,2)*SF(15) - OP(11,2)*SPP(11) - (OP(12,2)*q0)/2) - SF(6)*(OP(3,3) + OP(1,3)*SF(7) + OP(2,3)*SF(11) + OP(4,3)*SF(9) + OP(13,3)*SF(15) - OP(11,3)*SPP(11) - (OP(12,3)*q0)/2) + SF(4)*(OP(3,4) + OP(1,4)*SF(7) + OP(2,4)*SF(11) + OP(4,4)*SF(9) + OP(13,4)*SF(15) - OP(11,4)*SPP(11) - (OP(12,4)*q0)/2) + SPP(1)*(OP(3,1) + OP(1,1)*SF(7) + OP(2,1)*SF(11) + OP(4,1)*SF(9) + OP(13,1)*SF(15) - OP(11,1)*SPP(11) - (OP(12,1)*q0)/2) + SPP(5)*(OP(3,14) + OP(1,14)*SF(7) + OP(2,14)*SF(11) + OP(4,14)*SF(9) + OP(13,14)*SF(15) - OP(11,14)*SPP(11) - (OP(12,14)*q0)/2) - SPP(8)*(OP(3,15) + OP(1,15)*SF(7) + OP(2,15)*SF(11) + OP(4,15)*SF(9) + OP(13,15)*SF(15) - OP(11,15)*SPP(11) - (OP(12,15)*q0)/2) - SPP(2)*(OP(3,16) + OP(1,16)*SF(7) + OP(2,16)*SF(11) + OP(4,16)*SF(9) + OP(13,16)*SF(15) - OP(11,16)*SPP(11) - (OP(12,16)*q0)/2); +nextP(4,7) = OP(4,7) + OP(1,7)*SF(8) + OP(2,7)*SF(7) + OP(3,7)*SF(10) + OP(11,7)*SF(16) - OP(12,7)*SF(15) - (OP(13,7)*q0)/2 + SF(5)*(OP(4,2) + OP(1,2)*SF(8) + OP(2,2)*SF(7) + OP(3,2)*SF(10) + OP(11,2)*SF(16) - OP(12,2)*SF(15) - (OP(13,2)*q0)/2) - SF(6)*(OP(4,3) + OP(1,3)*SF(8) + OP(2,3)*SF(7) + OP(3,3)*SF(10) + OP(11,3)*SF(16) - OP(12,3)*SF(15) - (OP(13,3)*q0)/2) + SF(4)*(OP(4,4) + OP(1,4)*SF(8) + OP(2,4)*SF(7) + OP(3,4)*SF(10) + OP(11,4)*SF(16) - OP(12,4)*SF(15) - (OP(13,4)*q0)/2) + SPP(1)*(OP(4,1) + OP(1,1)*SF(8) + OP(2,1)*SF(7) + OP(3,1)*SF(10) + OP(11,1)*SF(16) - OP(12,1)*SF(15) - (OP(13,1)*q0)/2) + SPP(5)*(OP(4,14) + OP(1,14)*SF(8) + OP(2,14)*SF(7) + OP(3,14)*SF(10) + OP(11,14)*SF(16) - OP(12,14)*SF(15) - (OP(13,14)*q0)/2) - SPP(8)*(OP(4,15) + OP(1,15)*SF(8) + OP(2,15)*SF(7) + OP(3,15)*SF(10) + OP(11,15)*SF(16) - OP(12,15)*SF(15) - (OP(13,15)*q0)/2) - SPP(2)*(OP(4,16) + OP(1,16)*SF(8) + OP(2,16)*SF(7) + OP(3,16)*SF(10) + OP(11,16)*SF(16) - OP(12,16)*SF(15) - (OP(13,16)*q0)/2); +nextP(5,7) = OP(5,7) + SQ(2) + OP(1,7)*SF(6) + OP(2,7)*SF(4) - OP(4,7)*SF(5) + OP(3,7)*SPP(1) + OP(14,7)*SPP(4) + OP(15,7)*SPP(7) - OP(16,7)*SPP(10) + SF(5)*(OP(5,2) + OP(1,2)*SF(6) + OP(2,2)*SF(4) - OP(4,2)*SF(5) + OP(3,2)*SPP(1) + OP(14,2)*SPP(4) + OP(15,2)*SPP(7) - OP(16,2)*SPP(10)) - SF(6)*(OP(5,3) + OP(1,3)*SF(6) + OP(2,3)*SF(4) - OP(4,3)*SF(5) + OP(3,3)*SPP(1) + OP(14,3)*SPP(4) + OP(15,3)*SPP(7) - OP(16,3)*SPP(10)) + SF(4)*(OP(5,4) + OP(1,4)*SF(6) + OP(2,4)*SF(4) - OP(4,4)*SF(5) + OP(3,4)*SPP(1) + OP(14,4)*SPP(4) + OP(15,4)*SPP(7) - OP(16,4)*SPP(10)) + SPP(1)*(OP(5,1) + OP(1,1)*SF(6) + OP(2,1)*SF(4) - OP(4,1)*SF(5) + OP(3,1)*SPP(1) + OP(14,1)*SPP(4) + OP(15,1)*SPP(7) - OP(16,1)*SPP(10)) + SPP(5)*(OP(5,14) + OP(1,14)*SF(6) + OP(2,14)*SF(4) - OP(4,14)*SF(5) + OP(3,14)*SPP(1) + OP(14,14)*SPP(4) + OP(15,14)*SPP(7) - OP(16,14)*SPP(10)) - SPP(8)*(OP(5,15) + OP(1,15)*SF(6) + OP(2,15)*SF(4) - OP(4,15)*SF(5) + OP(3,15)*SPP(1) + OP(14,15)*SPP(4) + OP(15,15)*SPP(7) - OP(16,15)*SPP(10)) - SPP(2)*(OP(5,16) + OP(1,16)*SF(6) + OP(2,16)*SF(4) - OP(4,16)*SF(5) + OP(3,16)*SPP(1) + OP(14,16)*SPP(4) + OP(15,16)*SPP(7) - OP(16,16)*SPP(10)); +nextP(6,7) = OP(6,7) + SQ(1) + OP(1,7)*SF(5) + OP(3,7)*SF(4) + OP(4,7)*SF(6) - OP(2,7)*SPP(1) - OP(14,7)*SPP(9) + OP(15,7)*SPP(3) + OP(16,7)*SPP(6) + SF(5)*(OP(6,2) + OP(1,2)*SF(5) + OP(3,2)*SF(4) + OP(4,2)*SF(6) - OP(2,2)*SPP(1) - OP(14,2)*SPP(9) + OP(15,2)*SPP(3) + OP(16,2)*SPP(6)) - SF(6)*(OP(6,3) + OP(1,3)*SF(5) + OP(3,3)*SF(4) + OP(4,3)*SF(6) - OP(2,3)*SPP(1) - OP(14,3)*SPP(9) + OP(15,3)*SPP(3) + OP(16,3)*SPP(6)) + SF(4)*(OP(6,4) + OP(1,4)*SF(5) + OP(3,4)*SF(4) + OP(4,4)*SF(6) - OP(2,4)*SPP(1) - OP(14,4)*SPP(9) + OP(15,4)*SPP(3) + OP(16,4)*SPP(6)) + SPP(1)*(OP(6,1) + OP(1,1)*SF(5) + OP(3,1)*SF(4) + OP(4,1)*SF(6) - OP(2,1)*SPP(1) - OP(14,1)*SPP(9) + OP(15,1)*SPP(3) + OP(16,1)*SPP(6)) + SPP(5)*(OP(6,14) + OP(1,14)*SF(5) + OP(3,14)*SF(4) + OP(4,14)*SF(6) - OP(2,14)*SPP(1) - OP(14,14)*SPP(9) + OP(15,14)*SPP(3) + OP(16,14)*SPP(6)) - SPP(8)*(OP(6,15) + OP(1,15)*SF(5) + OP(3,15)*SF(4) + OP(4,15)*SF(6) - OP(2,15)*SPP(1) - OP(14,15)*SPP(9) + OP(15,15)*SPP(3) + OP(16,15)*SPP(6)) - SPP(2)*(OP(6,16) + OP(1,16)*SF(5) + OP(3,16)*SF(4) + OP(4,16)*SF(6) - OP(2,16)*SPP(1) - OP(14,16)*SPP(9) + OP(15,16)*SPP(3) + OP(16,16)*SPP(6)); +nextP(7,7) = OP(7,7) + OP(2,7)*SF(5) - OP(3,7)*SF(6) + OP(4,7)*SF(4) + OP(1,7)*SPP(1) + OP(14,7)*SPP(5) - OP(15,7)*SPP(8) - OP(16,7)*SPP(2) + dvxVar*(SG(7) - 2*q0*q2)^2 + dvyVar*(SG(6) + 2*q0*q1)^2 + SF(5)*(OP(7,2) + OP(2,2)*SF(5) - OP(3,2)*SF(6) + OP(4,2)*SF(4) + OP(1,2)*SPP(1) + OP(14,2)*SPP(5) - OP(15,2)*SPP(8) - OP(16,2)*SPP(2)) - SF(6)*(OP(7,3) + OP(2,3)*SF(5) - OP(3,3)*SF(6) + OP(4,3)*SF(4) + OP(1,3)*SPP(1) + OP(14,3)*SPP(5) - OP(15,3)*SPP(8) - OP(16,3)*SPP(2)) + SF(4)*(OP(7,4) + OP(2,4)*SF(5) - OP(3,4)*SF(6) + OP(4,4)*SF(4) + OP(1,4)*SPP(1) + OP(14,4)*SPP(5) - OP(15,4)*SPP(8) - OP(16,4)*SPP(2)) + SPP(1)*(OP(7,1) + OP(2,1)*SF(5) - OP(3,1)*SF(6) + OP(4,1)*SF(4) + OP(1,1)*SPP(1) + OP(14,1)*SPP(5) - OP(15,1)*SPP(8) - OP(16,1)*SPP(2)) + SPP(5)*(OP(7,14) + OP(2,14)*SF(5) - OP(3,14)*SF(6) + OP(4,14)*SF(4) + OP(1,14)*SPP(1) + OP(14,14)*SPP(5) - OP(15,14)*SPP(8) - OP(16,14)*SPP(2)) - SPP(8)*(OP(7,15) + OP(2,15)*SF(5) - OP(3,15)*SF(6) + OP(4,15)*SF(4) + OP(1,15)*SPP(1) + OP(14,15)*SPP(5) - OP(15,15)*SPP(8) - OP(16,15)*SPP(2)) - SPP(2)*(OP(7,16) + OP(2,16)*SF(5) - OP(3,16)*SF(6) + OP(4,16)*SF(4) + OP(1,16)*SPP(1) + OP(14,16)*SPP(5) - OP(15,16)*SPP(8) - OP(16,16)*SPP(2)) + dvzVar*(SG(2) - SG(3) - SG(4) + SG(5))^2; +nextP(1,8) = OP(1,8) + OP(2,8)*SF(10) + OP(3,8)*SF(12) + OP(4,8)*SF(11) + OP(11,8)*SF(15) + OP(12,8)*SF(16) + OP(13,8)*SPP(11) + dt*(OP(1,5) + OP(2,5)*SF(10) + OP(3,5)*SF(12) + OP(4,5)*SF(11) + OP(11,5)*SF(15) + OP(12,5)*SF(16) + OP(13,5)*SPP(11)); +nextP(2,8) = OP(2,8) + OP(1,8)*SF(9) + OP(3,8)*SF(8) + OP(4,8)*SF(12) - OP(13,8)*SF(16) + OP(12,8)*SPP(11) - (OP(11,8)*q0)/2 + dt*(OP(2,5) + OP(1,5)*SF(9) + OP(3,5)*SF(8) + OP(4,5)*SF(12) - OP(13,5)*SF(16) + OP(12,5)*SPP(11) - (OP(11,5)*q0)/2); +nextP(3,8) = OP(3,8) + OP(1,8)*SF(7) + OP(2,8)*SF(11) + OP(4,8)*SF(9) + OP(13,8)*SF(15) - OP(11,8)*SPP(11) - (OP(12,8)*q0)/2 + dt*(OP(3,5) + OP(1,5)*SF(7) + OP(2,5)*SF(11) + OP(4,5)*SF(9) + OP(13,5)*SF(15) - OP(11,5)*SPP(11) - (OP(12,5)*q0)/2); +nextP(4,8) = OP(4,8) + OP(1,8)*SF(8) + OP(2,8)*SF(7) + OP(3,8)*SF(10) + OP(11,8)*SF(16) - OP(12,8)*SF(15) - (OP(13,8)*q0)/2 + dt*(OP(4,5) + OP(1,5)*SF(8) + OP(2,5)*SF(7) + OP(3,5)*SF(10) + OP(11,5)*SF(16) - OP(12,5)*SF(15) - (OP(13,5)*q0)/2); +nextP(5,8) = OP(5,8) + OP(1,8)*SF(6) + OP(2,8)*SF(4) - OP(4,8)*SF(5) + OP(3,8)*SPP(1) + OP(14,8)*SPP(4) + OP(15,8)*SPP(7) - OP(16,8)*SPP(10) + dt*(OP(5,5) + OP(1,5)*SF(6) + OP(2,5)*SF(4) - OP(4,5)*SF(5) + OP(3,5)*SPP(1) + OP(14,5)*SPP(4) + OP(15,5)*SPP(7) - OP(16,5)*SPP(10)); +nextP(6,8) = OP(6,8) + OP(1,8)*SF(5) + OP(3,8)*SF(4) + OP(4,8)*SF(6) - OP(2,8)*SPP(1) - OP(14,8)*SPP(9) + OP(15,8)*SPP(3) + OP(16,8)*SPP(6) + dt*(OP(6,5) + OP(1,5)*SF(5) + OP(3,5)*SF(4) + OP(4,5)*SF(6) - OP(2,5)*SPP(1) - OP(14,5)*SPP(9) + OP(15,5)*SPP(3) + OP(16,5)*SPP(6)); +nextP(7,8) = OP(7,8) + OP(2,8)*SF(5) - OP(3,8)*SF(6) + OP(4,8)*SF(4) + OP(1,8)*SPP(1) + OP(14,8)*SPP(5) - OP(15,8)*SPP(8) - OP(16,8)*SPP(2) + dt*(OP(7,5) + OP(2,5)*SF(5) - OP(3,5)*SF(6) + OP(4,5)*SF(4) + OP(1,5)*SPP(1) + OP(14,5)*SPP(5) - OP(15,5)*SPP(8) - OP(16,5)*SPP(2)); nextP(8,8) = OP(8,8) + OP(5,8)*dt + dt*(OP(8,5) + OP(5,5)*dt); -nextP(1,9) = OP(1,9)*SPP(6) - OP(2,9)*SPP(5) + OP(3,9)*SPP(8) + OP(10,9)*SPP(23) + OP(13,9)*SPP(19) + dt*(OP(1,6)*SPP(6) - OP(2,6)*SPP(5) + OP(3,6)*SPP(8) + OP(10,6)*SPP(23) + OP(13,6)*SPP(19)); -nextP(2,9) = OP(2,9)*SPP(7) - OP(1,9)*SPP(3) - OP(3,9)*SPP(9) + OP(11,9)*SPP(23) + OP(14,9)*SPP(18) + dt*(OP(2,6)*SPP(7) - OP(1,6)*SPP(3) - OP(3,6)*SPP(9) + OP(11,6)*SPP(23) + OP(14,6)*SPP(18)); -nextP(3,9) = OP(1,9)*SPP(15) - OP(2,9)*SPP(4) + OP(3,9)*SPP(14) + OP(12,9)*SPP(23) + OP(15,9)*SPP(17) + dt*(OP(1,6)*SPP(15) - OP(2,6)*SPP(4) + OP(3,6)*SPP(14) + OP(12,6)*SPP(23) + OP(15,6)*SPP(17)); -nextP(4,9) = OP(4,9) + OP(1,9)*SPP(2) + OP(2,9)*SPP(20) + OP(3,9)*SPP(16) - OP(16,9)*SPP(22) + dt*(OP(4,6) + OP(1,6)*SPP(2) + OP(2,6)*SPP(20) + OP(3,6)*SPP(16) - OP(16,6)*SPP(22)); -nextP(5,9) = OP(5,9) + OP(16,9)*SF(23) + OP(1,9)*SPP(21) + OP(2,9)*SPP(13) + OP(3,9)*SPP(12) + dt*(OP(5,6) + OP(16,6)*SF(23) + OP(1,6)*SPP(21) + OP(2,6)*SPP(13) + OP(3,6)*SPP(12)); -nextP(6,9) = OP(6,9) + OP(16,9)*SF(21) - OP(1,9)*SPP(10) + OP(2,9)*SPP(11) + OP(3,9)*SPP(1) + dt*(OP(6,6) + OP(16,6)*SF(21) - OP(1,6)*SPP(10) + OP(2,6)*SPP(11) + OP(3,6)*SPP(1)); -nextP(7,9) = OP(7,9) + OP(4,9)*dt + dt*(OP(7,6) + OP(4,6)*dt); +nextP(1,9) = OP(1,9) + OP(2,9)*SF(10) + OP(3,9)*SF(12) + OP(4,9)*SF(11) + OP(11,9)*SF(15) + OP(12,9)*SF(16) + OP(13,9)*SPP(11) + dt*(OP(1,6) + OP(2,6)*SF(10) + OP(3,6)*SF(12) + OP(4,6)*SF(11) + OP(11,6)*SF(15) + OP(12,6)*SF(16) + OP(13,6)*SPP(11)); +nextP(2,9) = OP(2,9) + OP(1,9)*SF(9) + OP(3,9)*SF(8) + OP(4,9)*SF(12) - OP(13,9)*SF(16) + OP(12,9)*SPP(11) - (OP(11,9)*q0)/2 + dt*(OP(2,6) + OP(1,6)*SF(9) + OP(3,6)*SF(8) + OP(4,6)*SF(12) - OP(13,6)*SF(16) + OP(12,6)*SPP(11) - (OP(11,6)*q0)/2); +nextP(3,9) = OP(3,9) + OP(1,9)*SF(7) + OP(2,9)*SF(11) + OP(4,9)*SF(9) + OP(13,9)*SF(15) - OP(11,9)*SPP(11) - (OP(12,9)*q0)/2 + dt*(OP(3,6) + OP(1,6)*SF(7) + OP(2,6)*SF(11) + OP(4,6)*SF(9) + OP(13,6)*SF(15) - OP(11,6)*SPP(11) - (OP(12,6)*q0)/2); +nextP(4,9) = OP(4,9) + OP(1,9)*SF(8) + OP(2,9)*SF(7) + OP(3,9)*SF(10) + OP(11,9)*SF(16) - OP(12,9)*SF(15) - (OP(13,9)*q0)/2 + dt*(OP(4,6) + OP(1,6)*SF(8) + OP(2,6)*SF(7) + OP(3,6)*SF(10) + OP(11,6)*SF(16) - OP(12,6)*SF(15) - (OP(13,6)*q0)/2); +nextP(5,9) = OP(5,9) + OP(1,9)*SF(6) + OP(2,9)*SF(4) - OP(4,9)*SF(5) + OP(3,9)*SPP(1) + OP(14,9)*SPP(4) + OP(15,9)*SPP(7) - OP(16,9)*SPP(10) + dt*(OP(5,6) + OP(1,6)*SF(6) + OP(2,6)*SF(4) - OP(4,6)*SF(5) + OP(3,6)*SPP(1) + OP(14,6)*SPP(4) + OP(15,6)*SPP(7) - OP(16,6)*SPP(10)); +nextP(6,9) = OP(6,9) + OP(1,9)*SF(5) + OP(3,9)*SF(4) + OP(4,9)*SF(6) - OP(2,9)*SPP(1) - OP(14,9)*SPP(9) + OP(15,9)*SPP(3) + OP(16,9)*SPP(6) + dt*(OP(6,6) + OP(1,6)*SF(5) + OP(3,6)*SF(4) + OP(4,6)*SF(6) - OP(2,6)*SPP(1) - OP(14,6)*SPP(9) + OP(15,6)*SPP(3) + OP(16,6)*SPP(6)); +nextP(7,9) = OP(7,9) + OP(2,9)*SF(5) - OP(3,9)*SF(6) + OP(4,9)*SF(4) + OP(1,9)*SPP(1) + OP(14,9)*SPP(5) - OP(15,9)*SPP(8) - OP(16,9)*SPP(2) + dt*(OP(7,6) + OP(2,6)*SF(5) - OP(3,6)*SF(6) + OP(4,6)*SF(4) + OP(1,6)*SPP(1) + OP(14,6)*SPP(5) - OP(15,6)*SPP(8) - OP(16,6)*SPP(2)); nextP(8,9) = OP(8,9) + OP(5,9)*dt + dt*(OP(8,6) + OP(5,6)*dt); nextP(9,9) = OP(9,9) + OP(6,9)*dt + dt*(OP(9,6) + OP(6,6)*dt); -nextP(1,10) = OP(1,10)*SPP(6) - OP(2,10)*SPP(5) + OP(3,10)*SPP(8) + OP(10,10)*SPP(23) + OP(13,10)*SPP(19); -nextP(2,10) = OP(2,10)*SPP(7) - OP(1,10)*SPP(3) - OP(3,10)*SPP(9) + OP(11,10)*SPP(23) + OP(14,10)*SPP(18); -nextP(3,10) = OP(1,10)*SPP(15) - OP(2,10)*SPP(4) + OP(3,10)*SPP(14) + OP(12,10)*SPP(23) + OP(15,10)*SPP(17); -nextP(4,10) = OP(4,10) + OP(1,10)*SPP(2) + OP(2,10)*SPP(20) + OP(3,10)*SPP(16) - OP(16,10)*SPP(22); -nextP(5,10) = OP(5,10) + OP(16,10)*SF(23) + OP(1,10)*SPP(21) + OP(2,10)*SPP(13) + OP(3,10)*SPP(12); -nextP(6,10) = OP(6,10) + OP(16,10)*SF(21) - OP(1,10)*SPP(10) + OP(2,10)*SPP(11) + OP(3,10)*SPP(1); -nextP(7,10) = OP(7,10) + OP(4,10)*dt; -nextP(8,10) = OP(8,10) + OP(5,10)*dt; -nextP(9,10) = OP(9,10) + OP(6,10)*dt; -nextP(10,10) = OP(10,10); -nextP(1,11) = OP(1,11)*SPP(6) - OP(2,11)*SPP(5) + OP(3,11)*SPP(8) + OP(10,11)*SPP(23) + OP(13,11)*SPP(19); -nextP(2,11) = OP(2,11)*SPP(7) - OP(1,11)*SPP(3) - OP(3,11)*SPP(9) + OP(11,11)*SPP(23) + OP(14,11)*SPP(18); -nextP(3,11) = OP(1,11)*SPP(15) - OP(2,11)*SPP(4) + OP(3,11)*SPP(14) + OP(12,11)*SPP(23) + OP(15,11)*SPP(17); -nextP(4,11) = OP(4,11) + OP(1,11)*SPP(2) + OP(2,11)*SPP(20) + OP(3,11)*SPP(16) - OP(16,11)*SPP(22); -nextP(5,11) = OP(5,11) + OP(16,11)*SF(23) + OP(1,11)*SPP(21) + OP(2,11)*SPP(13) + OP(3,11)*SPP(12); -nextP(6,11) = OP(6,11) + OP(16,11)*SF(21) - OP(1,11)*SPP(10) + OP(2,11)*SPP(11) + OP(3,11)*SPP(1); -nextP(7,11) = OP(7,11) + OP(4,11)*dt; +nextP(1,10) = OP(1,10) + OP(2,10)*SF(10) + OP(3,10)*SF(12) + OP(4,10)*SF(11) + OP(11,10)*SF(15) + OP(12,10)*SF(16) + OP(13,10)*SPP(11) + dt*(OP(1,7) + OP(2,7)*SF(10) + OP(3,7)*SF(12) + OP(4,7)*SF(11) + OP(11,7)*SF(15) + OP(12,7)*SF(16) + OP(13,7)*SPP(11)); +nextP(2,10) = OP(2,10) + OP(1,10)*SF(9) + OP(3,10)*SF(8) + OP(4,10)*SF(12) - OP(13,10)*SF(16) + OP(12,10)*SPP(11) - (OP(11,10)*q0)/2 + dt*(OP(2,7) + OP(1,7)*SF(9) + OP(3,7)*SF(8) + OP(4,7)*SF(12) - OP(13,7)*SF(16) + OP(12,7)*SPP(11) - (OP(11,7)*q0)/2); +nextP(3,10) = OP(3,10) + OP(1,10)*SF(7) + OP(2,10)*SF(11) + OP(4,10)*SF(9) + OP(13,10)*SF(15) - OP(11,10)*SPP(11) - (OP(12,10)*q0)/2 + dt*(OP(3,7) + OP(1,7)*SF(7) + OP(2,7)*SF(11) + OP(4,7)*SF(9) + OP(13,7)*SF(15) - OP(11,7)*SPP(11) - (OP(12,7)*q0)/2); +nextP(4,10) = OP(4,10) + OP(1,10)*SF(8) + OP(2,10)*SF(7) + OP(3,10)*SF(10) + OP(11,10)*SF(16) - OP(12,10)*SF(15) - (OP(13,10)*q0)/2 + dt*(OP(4,7) + OP(1,7)*SF(8) + OP(2,7)*SF(7) + OP(3,7)*SF(10) + OP(11,7)*SF(16) - OP(12,7)*SF(15) - (OP(13,7)*q0)/2); +nextP(5,10) = OP(5,10) + OP(1,10)*SF(6) + OP(2,10)*SF(4) - OP(4,10)*SF(5) + OP(3,10)*SPP(1) + OP(14,10)*SPP(4) + OP(15,10)*SPP(7) - OP(16,10)*SPP(10) + dt*(OP(5,7) + OP(1,7)*SF(6) + OP(2,7)*SF(4) - OP(4,7)*SF(5) + OP(3,7)*SPP(1) + OP(14,7)*SPP(4) + OP(15,7)*SPP(7) - OP(16,7)*SPP(10)); +nextP(6,10) = OP(6,10) + OP(1,10)*SF(5) + OP(3,10)*SF(4) + OP(4,10)*SF(6) - OP(2,10)*SPP(1) - OP(14,10)*SPP(9) + OP(15,10)*SPP(3) + OP(16,10)*SPP(6) + dt*(OP(6,7) + OP(1,7)*SF(5) + OP(3,7)*SF(4) + OP(4,7)*SF(6) - OP(2,7)*SPP(1) - OP(14,7)*SPP(9) + OP(15,7)*SPP(3) + OP(16,7)*SPP(6)); +nextP(7,10) = OP(7,10) + OP(2,10)*SF(5) - OP(3,10)*SF(6) + OP(4,10)*SF(4) + OP(1,10)*SPP(1) + OP(14,10)*SPP(5) - OP(15,10)*SPP(8) - OP(16,10)*SPP(2) + dt*(OP(7,7) + OP(2,7)*SF(5) - OP(3,7)*SF(6) + OP(4,7)*SF(4) + OP(1,7)*SPP(1) + OP(14,7)*SPP(5) - OP(15,7)*SPP(8) - OP(16,7)*SPP(2)); +nextP(8,10) = OP(8,10) + OP(5,10)*dt + dt*(OP(8,7) + OP(5,7)*dt); +nextP(9,10) = OP(9,10) + OP(6,10)*dt + dt*(OP(9,7) + OP(6,7)*dt); +nextP(10,10) = OP(10,10) + OP(7,10)*dt + dt*(OP(10,7) + OP(7,7)*dt); +nextP(1,11) = OP(1,11) + OP(2,11)*SF(10) + OP(3,11)*SF(12) + OP(4,11)*SF(11) + OP(11,11)*SF(15) + OP(12,11)*SF(16) + OP(13,11)*SPP(11); +nextP(2,11) = OP(2,11) + OP(1,11)*SF(9) + OP(3,11)*SF(8) + OP(4,11)*SF(12) - OP(13,11)*SF(16) + OP(12,11)*SPP(11) - (OP(11,11)*q0)/2; +nextP(3,11) = OP(3,11) + OP(1,11)*SF(7) + OP(2,11)*SF(11) + OP(4,11)*SF(9) + OP(13,11)*SF(15) - OP(11,11)*SPP(11) - (OP(12,11)*q0)/2; +nextP(4,11) = OP(4,11) + OP(1,11)*SF(8) + OP(2,11)*SF(7) + OP(3,11)*SF(10) + OP(11,11)*SF(16) - OP(12,11)*SF(15) - (OP(13,11)*q0)/2; +nextP(5,11) = OP(5,11) + OP(1,11)*SF(6) + OP(2,11)*SF(4) - OP(4,11)*SF(5) + OP(3,11)*SPP(1) + OP(14,11)*SPP(4) + OP(15,11)*SPP(7) - OP(16,11)*SPP(10); +nextP(6,11) = OP(6,11) + OP(1,11)*SF(5) + OP(3,11)*SF(4) + OP(4,11)*SF(6) - OP(2,11)*SPP(1) - OP(14,11)*SPP(9) + OP(15,11)*SPP(3) + OP(16,11)*SPP(6); +nextP(7,11) = OP(7,11) + OP(2,11)*SF(5) - OP(3,11)*SF(6) + OP(4,11)*SF(4) + OP(1,11)*SPP(1) + OP(14,11)*SPP(5) - OP(15,11)*SPP(8) - OP(16,11)*SPP(2); nextP(8,11) = OP(8,11) + OP(5,11)*dt; nextP(9,11) = OP(9,11) + OP(6,11)*dt; -nextP(10,11) = OP(10,11); +nextP(10,11) = OP(10,11) + OP(7,11)*dt; nextP(11,11) = OP(11,11); -nextP(1,12) = OP(1,12)*SPP(6) - OP(2,12)*SPP(5) + OP(3,12)*SPP(8) + OP(10,12)*SPP(23) + OP(13,12)*SPP(19); -nextP(2,12) = OP(2,12)*SPP(7) - OP(1,12)*SPP(3) - OP(3,12)*SPP(9) + OP(11,12)*SPP(23) + OP(14,12)*SPP(18); -nextP(3,12) = OP(1,12)*SPP(15) - OP(2,12)*SPP(4) + OP(3,12)*SPP(14) + OP(12,12)*SPP(23) + OP(15,12)*SPP(17); -nextP(4,12) = OP(4,12) + OP(1,12)*SPP(2) + OP(2,12)*SPP(20) + OP(3,12)*SPP(16) - OP(16,12)*SPP(22); -nextP(5,12) = OP(5,12) + OP(16,12)*SF(23) + OP(1,12)*SPP(21) + OP(2,12)*SPP(13) + OP(3,12)*SPP(12); -nextP(6,12) = OP(6,12) + OP(16,12)*SF(21) - OP(1,12)*SPP(10) + OP(2,12)*SPP(11) + OP(3,12)*SPP(1); -nextP(7,12) = OP(7,12) + OP(4,12)*dt; +nextP(1,12) = OP(1,12) + OP(2,12)*SF(10) + OP(3,12)*SF(12) + OP(4,12)*SF(11) + OP(11,12)*SF(15) + OP(12,12)*SF(16) + OP(13,12)*SPP(11); +nextP(2,12) = OP(2,12) + OP(1,12)*SF(9) + OP(3,12)*SF(8) + OP(4,12)*SF(12) - OP(13,12)*SF(16) + OP(12,12)*SPP(11) - (OP(11,12)*q0)/2; +nextP(3,12) = OP(3,12) + OP(1,12)*SF(7) + OP(2,12)*SF(11) + OP(4,12)*SF(9) + OP(13,12)*SF(15) - OP(11,12)*SPP(11) - (OP(12,12)*q0)/2; +nextP(4,12) = OP(4,12) + OP(1,12)*SF(8) + OP(2,12)*SF(7) + OP(3,12)*SF(10) + OP(11,12)*SF(16) - OP(12,12)*SF(15) - (OP(13,12)*q0)/2; +nextP(5,12) = OP(5,12) + OP(1,12)*SF(6) + OP(2,12)*SF(4) - OP(4,12)*SF(5) + OP(3,12)*SPP(1) + OP(14,12)*SPP(4) + OP(15,12)*SPP(7) - OP(16,12)*SPP(10); +nextP(6,12) = OP(6,12) + OP(1,12)*SF(5) + OP(3,12)*SF(4) + OP(4,12)*SF(6) - OP(2,12)*SPP(1) - OP(14,12)*SPP(9) + OP(15,12)*SPP(3) + OP(16,12)*SPP(6); +nextP(7,12) = OP(7,12) + OP(2,12)*SF(5) - OP(3,12)*SF(6) + OP(4,12)*SF(4) + OP(1,12)*SPP(1) + OP(14,12)*SPP(5) - OP(15,12)*SPP(8) - OP(16,12)*SPP(2); nextP(8,12) = OP(8,12) + OP(5,12)*dt; nextP(9,12) = OP(9,12) + OP(6,12)*dt; -nextP(10,12) = OP(10,12); +nextP(10,12) = OP(10,12) + OP(7,12)*dt; nextP(11,12) = OP(11,12); nextP(12,12) = OP(12,12); -nextP(1,13) = OP(1,13)*SPP(6) - OP(2,13)*SPP(5) + OP(3,13)*SPP(8) + OP(10,13)*SPP(23) + OP(13,13)*SPP(19); -nextP(2,13) = OP(2,13)*SPP(7) - OP(1,13)*SPP(3) - OP(3,13)*SPP(9) + OP(11,13)*SPP(23) + OP(14,13)*SPP(18); -nextP(3,13) = OP(1,13)*SPP(15) - OP(2,13)*SPP(4) + OP(3,13)*SPP(14) + OP(12,13)*SPP(23) + OP(15,13)*SPP(17); -nextP(4,13) = OP(4,13) + OP(1,13)*SPP(2) + OP(2,13)*SPP(20) + OP(3,13)*SPP(16) - OP(16,13)*SPP(22); -nextP(5,13) = OP(5,13) + OP(16,13)*SF(23) + OP(1,13)*SPP(21) + OP(2,13)*SPP(13) + OP(3,13)*SPP(12); -nextP(6,13) = OP(6,13) + OP(16,13)*SF(21) - OP(1,13)*SPP(10) + OP(2,13)*SPP(11) + OP(3,13)*SPP(1); -nextP(7,13) = OP(7,13) + OP(4,13)*dt; +nextP(1,13) = OP(1,13) + OP(2,13)*SF(10) + OP(3,13)*SF(12) + OP(4,13)*SF(11) + OP(11,13)*SF(15) + OP(12,13)*SF(16) + OP(13,13)*SPP(11); +nextP(2,13) = OP(2,13) + OP(1,13)*SF(9) + OP(3,13)*SF(8) + OP(4,13)*SF(12) - OP(13,13)*SF(16) + OP(12,13)*SPP(11) - (OP(11,13)*q0)/2; +nextP(3,13) = OP(3,13) + OP(1,13)*SF(7) + OP(2,13)*SF(11) + OP(4,13)*SF(9) + OP(13,13)*SF(15) - OP(11,13)*SPP(11) - (OP(12,13)*q0)/2; +nextP(4,13) = OP(4,13) + OP(1,13)*SF(8) + OP(2,13)*SF(7) + OP(3,13)*SF(10) + OP(11,13)*SF(16) - OP(12,13)*SF(15) - (OP(13,13)*q0)/2; +nextP(5,13) = OP(5,13) + OP(1,13)*SF(6) + OP(2,13)*SF(4) - OP(4,13)*SF(5) + OP(3,13)*SPP(1) + OP(14,13)*SPP(4) + OP(15,13)*SPP(7) - OP(16,13)*SPP(10); +nextP(6,13) = OP(6,13) + OP(1,13)*SF(5) + OP(3,13)*SF(4) + OP(4,13)*SF(6) - OP(2,13)*SPP(1) - OP(14,13)*SPP(9) + OP(15,13)*SPP(3) + OP(16,13)*SPP(6); +nextP(7,13) = OP(7,13) + OP(2,13)*SF(5) - OP(3,13)*SF(6) + OP(4,13)*SF(4) + OP(1,13)*SPP(1) + OP(14,13)*SPP(5) - OP(15,13)*SPP(8) - OP(16,13)*SPP(2); nextP(8,13) = OP(8,13) + OP(5,13)*dt; nextP(9,13) = OP(9,13) + OP(6,13)*dt; -nextP(10,13) = OP(10,13); +nextP(10,13) = OP(10,13) + OP(7,13)*dt; nextP(11,13) = OP(11,13); nextP(12,13) = OP(12,13); nextP(13,13) = OP(13,13); -nextP(1,14) = OP(1,14)*SPP(6) - OP(2,14)*SPP(5) + OP(3,14)*SPP(8) + OP(10,14)*SPP(23) + OP(13,14)*SPP(19); -nextP(2,14) = OP(2,14)*SPP(7) - OP(1,14)*SPP(3) - OP(3,14)*SPP(9) + OP(11,14)*SPP(23) + OP(14,14)*SPP(18); -nextP(3,14) = OP(1,14)*SPP(15) - OP(2,14)*SPP(4) + OP(3,14)*SPP(14) + OP(12,14)*SPP(23) + OP(15,14)*SPP(17); -nextP(4,14) = OP(4,14) + OP(1,14)*SPP(2) + OP(2,14)*SPP(20) + OP(3,14)*SPP(16) - OP(16,14)*SPP(22); -nextP(5,14) = OP(5,14) + OP(16,14)*SF(23) + OP(1,14)*SPP(21) + OP(2,14)*SPP(13) + OP(3,14)*SPP(12); -nextP(6,14) = OP(6,14) + OP(16,14)*SF(21) - OP(1,14)*SPP(10) + OP(2,14)*SPP(11) + OP(3,14)*SPP(1); -nextP(7,14) = OP(7,14) + OP(4,14)*dt; +nextP(1,14) = OP(1,14) + OP(2,14)*SF(10) + OP(3,14)*SF(12) + OP(4,14)*SF(11) + OP(11,14)*SF(15) + OP(12,14)*SF(16) + OP(13,14)*SPP(11); +nextP(2,14) = OP(2,14) + OP(1,14)*SF(9) + OP(3,14)*SF(8) + OP(4,14)*SF(12) - OP(13,14)*SF(16) + OP(12,14)*SPP(11) - (OP(11,14)*q0)/2; +nextP(3,14) = OP(3,14) + OP(1,14)*SF(7) + OP(2,14)*SF(11) + OP(4,14)*SF(9) + OP(13,14)*SF(15) - OP(11,14)*SPP(11) - (OP(12,14)*q0)/2; +nextP(4,14) = OP(4,14) + OP(1,14)*SF(8) + OP(2,14)*SF(7) + OP(3,14)*SF(10) + OP(11,14)*SF(16) - OP(12,14)*SF(15) - (OP(13,14)*q0)/2; +nextP(5,14) = OP(5,14) + OP(1,14)*SF(6) + OP(2,14)*SF(4) - OP(4,14)*SF(5) + OP(3,14)*SPP(1) + OP(14,14)*SPP(4) + OP(15,14)*SPP(7) - OP(16,14)*SPP(10); +nextP(6,14) = OP(6,14) + OP(1,14)*SF(5) + OP(3,14)*SF(4) + OP(4,14)*SF(6) - OP(2,14)*SPP(1) - OP(14,14)*SPP(9) + OP(15,14)*SPP(3) + OP(16,14)*SPP(6); +nextP(7,14) = OP(7,14) + OP(2,14)*SF(5) - OP(3,14)*SF(6) + OP(4,14)*SF(4) + OP(1,14)*SPP(1) + OP(14,14)*SPP(5) - OP(15,14)*SPP(8) - OP(16,14)*SPP(2); nextP(8,14) = OP(8,14) + OP(5,14)*dt; nextP(9,14) = OP(9,14) + OP(6,14)*dt; -nextP(10,14) = OP(10,14); +nextP(10,14) = OP(10,14) + OP(7,14)*dt; nextP(11,14) = OP(11,14); nextP(12,14) = OP(12,14); nextP(13,14) = OP(13,14); nextP(14,14) = OP(14,14); -nextP(1,15) = OP(1,15)*SPP(6) - OP(2,15)*SPP(5) + OP(3,15)*SPP(8) + OP(10,15)*SPP(23) + OP(13,15)*SPP(19); -nextP(2,15) = OP(2,15)*SPP(7) - OP(1,15)*SPP(3) - OP(3,15)*SPP(9) + OP(11,15)*SPP(23) + OP(14,15)*SPP(18); -nextP(3,15) = OP(1,15)*SPP(15) - OP(2,15)*SPP(4) + OP(3,15)*SPP(14) + OP(12,15)*SPP(23) + OP(15,15)*SPP(17); -nextP(4,15) = OP(4,15) + OP(1,15)*SPP(2) + OP(2,15)*SPP(20) + OP(3,15)*SPP(16) - OP(16,15)*SPP(22); -nextP(5,15) = OP(5,15) + OP(16,15)*SF(23) + OP(1,15)*SPP(21) + OP(2,15)*SPP(13) + OP(3,15)*SPP(12); -nextP(6,15) = OP(6,15) + OP(16,15)*SF(21) - OP(1,15)*SPP(10) + OP(2,15)*SPP(11) + OP(3,15)*SPP(1); -nextP(7,15) = OP(7,15) + OP(4,15)*dt; +nextP(1,15) = OP(1,15) + OP(2,15)*SF(10) + OP(3,15)*SF(12) + OP(4,15)*SF(11) + OP(11,15)*SF(15) + OP(12,15)*SF(16) + OP(13,15)*SPP(11); +nextP(2,15) = OP(2,15) + OP(1,15)*SF(9) + OP(3,15)*SF(8) + OP(4,15)*SF(12) - OP(13,15)*SF(16) + OP(12,15)*SPP(11) - (OP(11,15)*q0)/2; +nextP(3,15) = OP(3,15) + OP(1,15)*SF(7) + OP(2,15)*SF(11) + OP(4,15)*SF(9) + OP(13,15)*SF(15) - OP(11,15)*SPP(11) - (OP(12,15)*q0)/2; +nextP(4,15) = OP(4,15) + OP(1,15)*SF(8) + OP(2,15)*SF(7) + OP(3,15)*SF(10) + OP(11,15)*SF(16) - OP(12,15)*SF(15) - (OP(13,15)*q0)/2; +nextP(5,15) = OP(5,15) + OP(1,15)*SF(6) + OP(2,15)*SF(4) - OP(4,15)*SF(5) + OP(3,15)*SPP(1) + OP(14,15)*SPP(4) + OP(15,15)*SPP(7) - OP(16,15)*SPP(10); +nextP(6,15) = OP(6,15) + OP(1,15)*SF(5) + OP(3,15)*SF(4) + OP(4,15)*SF(6) - OP(2,15)*SPP(1) - OP(14,15)*SPP(9) + OP(15,15)*SPP(3) + OP(16,15)*SPP(6); +nextP(7,15) = OP(7,15) + OP(2,15)*SF(5) - OP(3,15)*SF(6) + OP(4,15)*SF(4) + OP(1,15)*SPP(1) + OP(14,15)*SPP(5) - OP(15,15)*SPP(8) - OP(16,15)*SPP(2); nextP(8,15) = OP(8,15) + OP(5,15)*dt; nextP(9,15) = OP(9,15) + OP(6,15)*dt; -nextP(10,15) = OP(10,15); +nextP(10,15) = OP(10,15) + OP(7,15)*dt; nextP(11,15) = OP(11,15); nextP(12,15) = OP(12,15); nextP(13,15) = OP(13,15); nextP(14,15) = OP(14,15); nextP(15,15) = OP(15,15); -nextP(1,16) = OP(1,16)*SPP(6) - OP(2,16)*SPP(5) + OP(3,16)*SPP(8) + OP(10,16)*SPP(23) + OP(13,16)*SPP(19); -nextP(2,16) = OP(2,16)*SPP(7) - OP(1,16)*SPP(3) - OP(3,16)*SPP(9) + OP(11,16)*SPP(23) + OP(14,16)*SPP(18); -nextP(3,16) = OP(1,16)*SPP(15) - OP(2,16)*SPP(4) + OP(3,16)*SPP(14) + OP(12,16)*SPP(23) + OP(15,16)*SPP(17); -nextP(4,16) = OP(4,16) + OP(1,16)*SPP(2) + OP(2,16)*SPP(20) + OP(3,16)*SPP(16) - OP(16,16)*SPP(22); -nextP(5,16) = OP(5,16) + OP(16,16)*SF(23) + OP(1,16)*SPP(21) + OP(2,16)*SPP(13) + OP(3,16)*SPP(12); -nextP(6,16) = OP(6,16) + OP(16,16)*SF(21) - OP(1,16)*SPP(10) + OP(2,16)*SPP(11) + OP(3,16)*SPP(1); -nextP(7,16) = OP(7,16) + OP(4,16)*dt; +nextP(1,16) = OP(1,16) + OP(2,16)*SF(10) + OP(3,16)*SF(12) + OP(4,16)*SF(11) + OP(11,16)*SF(15) + OP(12,16)*SF(16) + OP(13,16)*SPP(11); +nextP(2,16) = OP(2,16) + OP(1,16)*SF(9) + OP(3,16)*SF(8) + OP(4,16)*SF(12) - OP(13,16)*SF(16) + OP(12,16)*SPP(11) - (OP(11,16)*q0)/2; +nextP(3,16) = OP(3,16) + OP(1,16)*SF(7) + OP(2,16)*SF(11) + OP(4,16)*SF(9) + OP(13,16)*SF(15) - OP(11,16)*SPP(11) - (OP(12,16)*q0)/2; +nextP(4,16) = OP(4,16) + OP(1,16)*SF(8) + OP(2,16)*SF(7) + OP(3,16)*SF(10) + OP(11,16)*SF(16) - OP(12,16)*SF(15) - (OP(13,16)*q0)/2; +nextP(5,16) = OP(5,16) + OP(1,16)*SF(6) + OP(2,16)*SF(4) - OP(4,16)*SF(5) + OP(3,16)*SPP(1) + OP(14,16)*SPP(4) + OP(15,16)*SPP(7) - OP(16,16)*SPP(10); +nextP(6,16) = OP(6,16) + OP(1,16)*SF(5) + OP(3,16)*SF(4) + OP(4,16)*SF(6) - OP(2,16)*SPP(1) - OP(14,16)*SPP(9) + OP(15,16)*SPP(3) + OP(16,16)*SPP(6); +nextP(7,16) = OP(7,16) + OP(2,16)*SF(5) - OP(3,16)*SF(6) + OP(4,16)*SF(4) + OP(1,16)*SPP(1) + OP(14,16)*SPP(5) - OP(15,16)*SPP(8) - OP(16,16)*SPP(2); nextP(8,16) = OP(8,16) + OP(5,16)*dt; nextP(9,16) = OP(9,16) + OP(6,16)*dt; -nextP(10,16) = OP(10,16); +nextP(10,16) = OP(10,16) + OP(7,16)*dt; nextP(11,16) = OP(11,16); nextP(12,16) = OP(12,16); nextP(13,16) = OP(13,16); nextP(14,16) = OP(14,16); nextP(15,16) = OP(15,16); nextP(16,16) = OP(16,16); -nextP(1,17) = OP(1,17)*SPP(6) - OP(2,17)*SPP(5) + OP(3,17)*SPP(8) + OP(10,17)*SPP(23) + OP(13,17)*SPP(19); -nextP(2,17) = OP(2,17)*SPP(7) - OP(1,17)*SPP(3) - OP(3,17)*SPP(9) + OP(11,17)*SPP(23) + OP(14,17)*SPP(18); -nextP(3,17) = OP(1,17)*SPP(15) - OP(2,17)*SPP(4) + OP(3,17)*SPP(14) + OP(12,17)*SPP(23) + OP(15,17)*SPP(17); -nextP(4,17) = OP(4,17) + OP(1,17)*SPP(2) + OP(2,17)*SPP(20) + OP(3,17)*SPP(16) - OP(16,17)*SPP(22); -nextP(5,17) = OP(5,17) + OP(16,17)*SF(23) + OP(1,17)*SPP(21) + OP(2,17)*SPP(13) + OP(3,17)*SPP(12); -nextP(6,17) = OP(6,17) + OP(16,17)*SF(21) - OP(1,17)*SPP(10) + OP(2,17)*SPP(11) + OP(3,17)*SPP(1); -nextP(7,17) = OP(7,17) + OP(4,17)*dt; +nextP(1,17) = OP(1,17) + OP(2,17)*SF(10) + OP(3,17)*SF(12) + OP(4,17)*SF(11) + OP(11,17)*SF(15) + OP(12,17)*SF(16) + OP(13,17)*SPP(11); +nextP(2,17) = OP(2,17) + OP(1,17)*SF(9) + OP(3,17)*SF(8) + OP(4,17)*SF(12) - OP(13,17)*SF(16) + OP(12,17)*SPP(11) - (OP(11,17)*q0)/2; +nextP(3,17) = OP(3,17) + OP(1,17)*SF(7) + OP(2,17)*SF(11) + OP(4,17)*SF(9) + OP(13,17)*SF(15) - OP(11,17)*SPP(11) - (OP(12,17)*q0)/2; +nextP(4,17) = OP(4,17) + OP(1,17)*SF(8) + OP(2,17)*SF(7) + OP(3,17)*SF(10) + OP(11,17)*SF(16) - OP(12,17)*SF(15) - (OP(13,17)*q0)/2; +nextP(5,17) = OP(5,17) + OP(1,17)*SF(6) + OP(2,17)*SF(4) - OP(4,17)*SF(5) + OP(3,17)*SPP(1) + OP(14,17)*SPP(4) + OP(15,17)*SPP(7) - OP(16,17)*SPP(10); +nextP(6,17) = OP(6,17) + OP(1,17)*SF(5) + OP(3,17)*SF(4) + OP(4,17)*SF(6) - OP(2,17)*SPP(1) - OP(14,17)*SPP(9) + OP(15,17)*SPP(3) + OP(16,17)*SPP(6); +nextP(7,17) = OP(7,17) + OP(2,17)*SF(5) - OP(3,17)*SF(6) + OP(4,17)*SF(4) + OP(1,17)*SPP(1) + OP(14,17)*SPP(5) - OP(15,17)*SPP(8) - OP(16,17)*SPP(2); nextP(8,17) = OP(8,17) + OP(5,17)*dt; nextP(9,17) = OP(9,17) + OP(6,17)*dt; -nextP(10,17) = OP(10,17); +nextP(10,17) = OP(10,17) + OP(7,17)*dt; nextP(11,17) = OP(11,17); nextP(12,17) = OP(12,17); nextP(13,17) = OP(13,17); @@ -219,16 +207,16 @@ nextP(14,17) = OP(14,17); nextP(15,17) = OP(15,17); nextP(16,17) = OP(16,17); nextP(17,17) = OP(17,17); -nextP(1,18) = OP(1,18)*SPP(6) - OP(2,18)*SPP(5) + OP(3,18)*SPP(8) + OP(10,18)*SPP(23) + OP(13,18)*SPP(19); -nextP(2,18) = OP(2,18)*SPP(7) - OP(1,18)*SPP(3) - OP(3,18)*SPP(9) + OP(11,18)*SPP(23) + OP(14,18)*SPP(18); -nextP(3,18) = OP(1,18)*SPP(15) - OP(2,18)*SPP(4) + OP(3,18)*SPP(14) + OP(12,18)*SPP(23) + OP(15,18)*SPP(17); -nextP(4,18) = OP(4,18) + OP(1,18)*SPP(2) + OP(2,18)*SPP(20) + OP(3,18)*SPP(16) - OP(16,18)*SPP(22); -nextP(5,18) = OP(5,18) + OP(16,18)*SF(23) + OP(1,18)*SPP(21) + OP(2,18)*SPP(13) + OP(3,18)*SPP(12); -nextP(6,18) = OP(6,18) + OP(16,18)*SF(21) - OP(1,18)*SPP(10) + OP(2,18)*SPP(11) + OP(3,18)*SPP(1); -nextP(7,18) = OP(7,18) + OP(4,18)*dt; +nextP(1,18) = OP(1,18) + OP(2,18)*SF(10) + OP(3,18)*SF(12) + OP(4,18)*SF(11) + OP(11,18)*SF(15) + OP(12,18)*SF(16) + OP(13,18)*SPP(11); +nextP(2,18) = OP(2,18) + OP(1,18)*SF(9) + OP(3,18)*SF(8) + OP(4,18)*SF(12) - OP(13,18)*SF(16) + OP(12,18)*SPP(11) - (OP(11,18)*q0)/2; +nextP(3,18) = OP(3,18) + OP(1,18)*SF(7) + OP(2,18)*SF(11) + OP(4,18)*SF(9) + OP(13,18)*SF(15) - OP(11,18)*SPP(11) - (OP(12,18)*q0)/2; +nextP(4,18) = OP(4,18) + OP(1,18)*SF(8) + OP(2,18)*SF(7) + OP(3,18)*SF(10) + OP(11,18)*SF(16) - OP(12,18)*SF(15) - (OP(13,18)*q0)/2; +nextP(5,18) = OP(5,18) + OP(1,18)*SF(6) + OP(2,18)*SF(4) - OP(4,18)*SF(5) + OP(3,18)*SPP(1) + OP(14,18)*SPP(4) + OP(15,18)*SPP(7) - OP(16,18)*SPP(10); +nextP(6,18) = OP(6,18) + OP(1,18)*SF(5) + OP(3,18)*SF(4) + OP(4,18)*SF(6) - OP(2,18)*SPP(1) - OP(14,18)*SPP(9) + OP(15,18)*SPP(3) + OP(16,18)*SPP(6); +nextP(7,18) = OP(7,18) + OP(2,18)*SF(5) - OP(3,18)*SF(6) + OP(4,18)*SF(4) + OP(1,18)*SPP(1) + OP(14,18)*SPP(5) - OP(15,18)*SPP(8) - OP(16,18)*SPP(2); nextP(8,18) = OP(8,18) + OP(5,18)*dt; nextP(9,18) = OP(9,18) + OP(6,18)*dt; -nextP(10,18) = OP(10,18); +nextP(10,18) = OP(10,18) + OP(7,18)*dt; nextP(11,18) = OP(11,18); nextP(12,18) = OP(12,18); nextP(13,18) = OP(13,18); @@ -237,16 +225,16 @@ nextP(15,18) = OP(15,18); nextP(16,18) = OP(16,18); nextP(17,18) = OP(17,18); nextP(18,18) = OP(18,18); -nextP(1,19) = OP(1,19)*SPP(6) - OP(2,19)*SPP(5) + OP(3,19)*SPP(8) + OP(10,19)*SPP(23) + OP(13,19)*SPP(19); -nextP(2,19) = OP(2,19)*SPP(7) - OP(1,19)*SPP(3) - OP(3,19)*SPP(9) + OP(11,19)*SPP(23) + OP(14,19)*SPP(18); -nextP(3,19) = OP(1,19)*SPP(15) - OP(2,19)*SPP(4) + OP(3,19)*SPP(14) + OP(12,19)*SPP(23) + OP(15,19)*SPP(17); -nextP(4,19) = OP(4,19) + OP(1,19)*SPP(2) + OP(2,19)*SPP(20) + OP(3,19)*SPP(16) - OP(16,19)*SPP(22); -nextP(5,19) = OP(5,19) + OP(16,19)*SF(23) + OP(1,19)*SPP(21) + OP(2,19)*SPP(13) + OP(3,19)*SPP(12); -nextP(6,19) = OP(6,19) + OP(16,19)*SF(21) - OP(1,19)*SPP(10) + OP(2,19)*SPP(11) + OP(3,19)*SPP(1); -nextP(7,19) = OP(7,19) + OP(4,19)*dt; +nextP(1,19) = OP(1,19) + OP(2,19)*SF(10) + OP(3,19)*SF(12) + OP(4,19)*SF(11) + OP(11,19)*SF(15) + OP(12,19)*SF(16) + OP(13,19)*SPP(11); +nextP(2,19) = OP(2,19) + OP(1,19)*SF(9) + OP(3,19)*SF(8) + OP(4,19)*SF(12) - OP(13,19)*SF(16) + OP(12,19)*SPP(11) - (OP(11,19)*q0)/2; +nextP(3,19) = OP(3,19) + OP(1,19)*SF(7) + OP(2,19)*SF(11) + OP(4,19)*SF(9) + OP(13,19)*SF(15) - OP(11,19)*SPP(11) - (OP(12,19)*q0)/2; +nextP(4,19) = OP(4,19) + OP(1,19)*SF(8) + OP(2,19)*SF(7) + OP(3,19)*SF(10) + OP(11,19)*SF(16) - OP(12,19)*SF(15) - (OP(13,19)*q0)/2; +nextP(5,19) = OP(5,19) + OP(1,19)*SF(6) + OP(2,19)*SF(4) - OP(4,19)*SF(5) + OP(3,19)*SPP(1) + OP(14,19)*SPP(4) + OP(15,19)*SPP(7) - OP(16,19)*SPP(10); +nextP(6,19) = OP(6,19) + OP(1,19)*SF(5) + OP(3,19)*SF(4) + OP(4,19)*SF(6) - OP(2,19)*SPP(1) - OP(14,19)*SPP(9) + OP(15,19)*SPP(3) + OP(16,19)*SPP(6); +nextP(7,19) = OP(7,19) + OP(2,19)*SF(5) - OP(3,19)*SF(6) + OP(4,19)*SF(4) + OP(1,19)*SPP(1) + OP(14,19)*SPP(5) - OP(15,19)*SPP(8) - OP(16,19)*SPP(2); nextP(8,19) = OP(8,19) + OP(5,19)*dt; nextP(9,19) = OP(9,19) + OP(6,19)*dt; -nextP(10,19) = OP(10,19); +nextP(10,19) = OP(10,19) + OP(7,19)*dt; nextP(11,19) = OP(11,19); nextP(12,19) = OP(12,19); nextP(13,19) = OP(13,19); @@ -256,16 +244,16 @@ nextP(16,19) = OP(16,19); nextP(17,19) = OP(17,19); nextP(18,19) = OP(18,19); nextP(19,19) = OP(19,19); -nextP(1,20) = OP(1,20)*SPP(6) - OP(2,20)*SPP(5) + OP(3,20)*SPP(8) + OP(10,20)*SPP(23) + OP(13,20)*SPP(19); -nextP(2,20) = OP(2,20)*SPP(7) - OP(1,20)*SPP(3) - OP(3,20)*SPP(9) + OP(11,20)*SPP(23) + OP(14,20)*SPP(18); -nextP(3,20) = OP(1,20)*SPP(15) - OP(2,20)*SPP(4) + OP(3,20)*SPP(14) + OP(12,20)*SPP(23) + OP(15,20)*SPP(17); -nextP(4,20) = OP(4,20) + OP(1,20)*SPP(2) + OP(2,20)*SPP(20) + OP(3,20)*SPP(16) - OP(16,20)*SPP(22); -nextP(5,20) = OP(5,20) + OP(16,20)*SF(23) + OP(1,20)*SPP(21) + OP(2,20)*SPP(13) + OP(3,20)*SPP(12); -nextP(6,20) = OP(6,20) + OP(16,20)*SF(21) - OP(1,20)*SPP(10) + OP(2,20)*SPP(11) + OP(3,20)*SPP(1); -nextP(7,20) = OP(7,20) + OP(4,20)*dt; +nextP(1,20) = OP(1,20) + OP(2,20)*SF(10) + OP(3,20)*SF(12) + OP(4,20)*SF(11) + OP(11,20)*SF(15) + OP(12,20)*SF(16) + OP(13,20)*SPP(11); +nextP(2,20) = OP(2,20) + OP(1,20)*SF(9) + OP(3,20)*SF(8) + OP(4,20)*SF(12) - OP(13,20)*SF(16) + OP(12,20)*SPP(11) - (OP(11,20)*q0)/2; +nextP(3,20) = OP(3,20) + OP(1,20)*SF(7) + OP(2,20)*SF(11) + OP(4,20)*SF(9) + OP(13,20)*SF(15) - OP(11,20)*SPP(11) - (OP(12,20)*q0)/2; +nextP(4,20) = OP(4,20) + OP(1,20)*SF(8) + OP(2,20)*SF(7) + OP(3,20)*SF(10) + OP(11,20)*SF(16) - OP(12,20)*SF(15) - (OP(13,20)*q0)/2; +nextP(5,20) = OP(5,20) + OP(1,20)*SF(6) + OP(2,20)*SF(4) - OP(4,20)*SF(5) + OP(3,20)*SPP(1) + OP(14,20)*SPP(4) + OP(15,20)*SPP(7) - OP(16,20)*SPP(10); +nextP(6,20) = OP(6,20) + OP(1,20)*SF(5) + OP(3,20)*SF(4) + OP(4,20)*SF(6) - OP(2,20)*SPP(1) - OP(14,20)*SPP(9) + OP(15,20)*SPP(3) + OP(16,20)*SPP(6); +nextP(7,20) = OP(7,20) + OP(2,20)*SF(5) - OP(3,20)*SF(6) + OP(4,20)*SF(4) + OP(1,20)*SPP(1) + OP(14,20)*SPP(5) - OP(15,20)*SPP(8) - OP(16,20)*SPP(2); nextP(8,20) = OP(8,20) + OP(5,20)*dt; nextP(9,20) = OP(9,20) + OP(6,20)*dt; -nextP(10,20) = OP(10,20); +nextP(10,20) = OP(10,20) + OP(7,20)*dt; nextP(11,20) = OP(11,20); nextP(12,20) = OP(12,20); nextP(13,20) = OP(13,20); @@ -276,16 +264,16 @@ nextP(17,20) = OP(17,20); nextP(18,20) = OP(18,20); nextP(19,20) = OP(19,20); nextP(20,20) = OP(20,20); -nextP(1,21) = OP(1,21)*SPP(6) - OP(2,21)*SPP(5) + OP(3,21)*SPP(8) + OP(10,21)*SPP(23) + OP(13,21)*SPP(19); -nextP(2,21) = OP(2,21)*SPP(7) - OP(1,21)*SPP(3) - OP(3,21)*SPP(9) + OP(11,21)*SPP(23) + OP(14,21)*SPP(18); -nextP(3,21) = OP(1,21)*SPP(15) - OP(2,21)*SPP(4) + OP(3,21)*SPP(14) + OP(12,21)*SPP(23) + OP(15,21)*SPP(17); -nextP(4,21) = OP(4,21) + OP(1,21)*SPP(2) + OP(2,21)*SPP(20) + OP(3,21)*SPP(16) - OP(16,21)*SPP(22); -nextP(5,21) = OP(5,21) + OP(16,21)*SF(23) + OP(1,21)*SPP(21) + OP(2,21)*SPP(13) + OP(3,21)*SPP(12); -nextP(6,21) = OP(6,21) + OP(16,21)*SF(21) - OP(1,21)*SPP(10) + OP(2,21)*SPP(11) + OP(3,21)*SPP(1); -nextP(7,21) = OP(7,21) + OP(4,21)*dt; +nextP(1,21) = OP(1,21) + OP(2,21)*SF(10) + OP(3,21)*SF(12) + OP(4,21)*SF(11) + OP(11,21)*SF(15) + OP(12,21)*SF(16) + OP(13,21)*SPP(11); +nextP(2,21) = OP(2,21) + OP(1,21)*SF(9) + OP(3,21)*SF(8) + OP(4,21)*SF(12) - OP(13,21)*SF(16) + OP(12,21)*SPP(11) - (OP(11,21)*q0)/2; +nextP(3,21) = OP(3,21) + OP(1,21)*SF(7) + OP(2,21)*SF(11) + OP(4,21)*SF(9) + OP(13,21)*SF(15) - OP(11,21)*SPP(11) - (OP(12,21)*q0)/2; +nextP(4,21) = OP(4,21) + OP(1,21)*SF(8) + OP(2,21)*SF(7) + OP(3,21)*SF(10) + OP(11,21)*SF(16) - OP(12,21)*SF(15) - (OP(13,21)*q0)/2; +nextP(5,21) = OP(5,21) + OP(1,21)*SF(6) + OP(2,21)*SF(4) - OP(4,21)*SF(5) + OP(3,21)*SPP(1) + OP(14,21)*SPP(4) + OP(15,21)*SPP(7) - OP(16,21)*SPP(10); +nextP(6,21) = OP(6,21) + OP(1,21)*SF(5) + OP(3,21)*SF(4) + OP(4,21)*SF(6) - OP(2,21)*SPP(1) - OP(14,21)*SPP(9) + OP(15,21)*SPP(3) + OP(16,21)*SPP(6); +nextP(7,21) = OP(7,21) + OP(2,21)*SF(5) - OP(3,21)*SF(6) + OP(4,21)*SF(4) + OP(1,21)*SPP(1) + OP(14,21)*SPP(5) - OP(15,21)*SPP(8) - OP(16,21)*SPP(2); nextP(8,21) = OP(8,21) + OP(5,21)*dt; nextP(9,21) = OP(9,21) + OP(6,21)*dt; -nextP(10,21) = OP(10,21); +nextP(10,21) = OP(10,21) + OP(7,21)*dt; nextP(11,21) = OP(11,21); nextP(12,21) = OP(12,21); nextP(13,21) = OP(13,21); @@ -297,16 +285,16 @@ nextP(18,21) = OP(18,21); nextP(19,21) = OP(19,21); nextP(20,21) = OP(20,21); nextP(21,21) = OP(21,21); -nextP(1,22) = OP(1,22)*SPP(6) - OP(2,22)*SPP(5) + OP(3,22)*SPP(8) + OP(10,22)*SPP(23) + OP(13,22)*SPP(19); -nextP(2,22) = OP(2,22)*SPP(7) - OP(1,22)*SPP(3) - OP(3,22)*SPP(9) + OP(11,22)*SPP(23) + OP(14,22)*SPP(18); -nextP(3,22) = OP(1,22)*SPP(15) - OP(2,22)*SPP(4) + OP(3,22)*SPP(14) + OP(12,22)*SPP(23) + OP(15,22)*SPP(17); -nextP(4,22) = OP(4,22) + OP(1,22)*SPP(2) + OP(2,22)*SPP(20) + OP(3,22)*SPP(16) - OP(16,22)*SPP(22); -nextP(5,22) = OP(5,22) + OP(16,22)*SF(23) + OP(1,22)*SPP(21) + OP(2,22)*SPP(13) + OP(3,22)*SPP(12); -nextP(6,22) = OP(6,22) + OP(16,22)*SF(21) - OP(1,22)*SPP(10) + OP(2,22)*SPP(11) + OP(3,22)*SPP(1); -nextP(7,22) = OP(7,22) + OP(4,22)*dt; +nextP(1,22) = OP(1,22) + OP(2,22)*SF(10) + OP(3,22)*SF(12) + OP(4,22)*SF(11) + OP(11,22)*SF(15) + OP(12,22)*SF(16) + OP(13,22)*SPP(11); +nextP(2,22) = OP(2,22) + OP(1,22)*SF(9) + OP(3,22)*SF(8) + OP(4,22)*SF(12) - OP(13,22)*SF(16) + OP(12,22)*SPP(11) - (OP(11,22)*q0)/2; +nextP(3,22) = OP(3,22) + OP(1,22)*SF(7) + OP(2,22)*SF(11) + OP(4,22)*SF(9) + OP(13,22)*SF(15) - OP(11,22)*SPP(11) - (OP(12,22)*q0)/2; +nextP(4,22) = OP(4,22) + OP(1,22)*SF(8) + OP(2,22)*SF(7) + OP(3,22)*SF(10) + OP(11,22)*SF(16) - OP(12,22)*SF(15) - (OP(13,22)*q0)/2; +nextP(5,22) = OP(5,22) + OP(1,22)*SF(6) + OP(2,22)*SF(4) - OP(4,22)*SF(5) + OP(3,22)*SPP(1) + OP(14,22)*SPP(4) + OP(15,22)*SPP(7) - OP(16,22)*SPP(10); +nextP(6,22) = OP(6,22) + OP(1,22)*SF(5) + OP(3,22)*SF(4) + OP(4,22)*SF(6) - OP(2,22)*SPP(1) - OP(14,22)*SPP(9) + OP(15,22)*SPP(3) + OP(16,22)*SPP(6); +nextP(7,22) = OP(7,22) + OP(2,22)*SF(5) - OP(3,22)*SF(6) + OP(4,22)*SF(4) + OP(1,22)*SPP(1) + OP(14,22)*SPP(5) - OP(15,22)*SPP(8) - OP(16,22)*SPP(2); nextP(8,22) = OP(8,22) + OP(5,22)*dt; nextP(9,22) = OP(9,22) + OP(6,22)*dt; -nextP(10,22) = OP(10,22); +nextP(10,22) = OP(10,22) + OP(7,22)*dt; nextP(11,22) = OP(11,22); nextP(12,22) = OP(12,22); nextP(13,22) = OP(13,22); @@ -319,16 +307,16 @@ nextP(19,22) = OP(19,22); nextP(20,22) = OP(20,22); nextP(21,22) = OP(21,22); nextP(22,22) = OP(22,22); -nextP(1,23) = OP(1,23)*SPP(6) - OP(2,23)*SPP(5) + OP(3,23)*SPP(8) + OP(10,23)*SPP(23) + OP(13,23)*SPP(19); -nextP(2,23) = OP(2,23)*SPP(7) - OP(1,23)*SPP(3) - OP(3,23)*SPP(9) + OP(11,23)*SPP(23) + OP(14,23)*SPP(18); -nextP(3,23) = OP(1,23)*SPP(15) - OP(2,23)*SPP(4) + OP(3,23)*SPP(14) + OP(12,23)*SPP(23) + OP(15,23)*SPP(17); -nextP(4,23) = OP(4,23) + OP(1,23)*SPP(2) + OP(2,23)*SPP(20) + OP(3,23)*SPP(16) - OP(16,23)*SPP(22); -nextP(5,23) = OP(5,23) + OP(16,23)*SF(23) + OP(1,23)*SPP(21) + OP(2,23)*SPP(13) + OP(3,23)*SPP(12); -nextP(6,23) = OP(6,23) + OP(16,23)*SF(21) - OP(1,23)*SPP(10) + OP(2,23)*SPP(11) + OP(3,23)*SPP(1); -nextP(7,23) = OP(7,23) + OP(4,23)*dt; +nextP(1,23) = OP(1,23) + OP(2,23)*SF(10) + OP(3,23)*SF(12) + OP(4,23)*SF(11) + OP(11,23)*SF(15) + OP(12,23)*SF(16) + OP(13,23)*SPP(11); +nextP(2,23) = OP(2,23) + OP(1,23)*SF(9) + OP(3,23)*SF(8) + OP(4,23)*SF(12) - OP(13,23)*SF(16) + OP(12,23)*SPP(11) - (OP(11,23)*q0)/2; +nextP(3,23) = OP(3,23) + OP(1,23)*SF(7) + OP(2,23)*SF(11) + OP(4,23)*SF(9) + OP(13,23)*SF(15) - OP(11,23)*SPP(11) - (OP(12,23)*q0)/2; +nextP(4,23) = OP(4,23) + OP(1,23)*SF(8) + OP(2,23)*SF(7) + OP(3,23)*SF(10) + OP(11,23)*SF(16) - OP(12,23)*SF(15) - (OP(13,23)*q0)/2; +nextP(5,23) = OP(5,23) + OP(1,23)*SF(6) + OP(2,23)*SF(4) - OP(4,23)*SF(5) + OP(3,23)*SPP(1) + OP(14,23)*SPP(4) + OP(15,23)*SPP(7) - OP(16,23)*SPP(10); +nextP(6,23) = OP(6,23) + OP(1,23)*SF(5) + OP(3,23)*SF(4) + OP(4,23)*SF(6) - OP(2,23)*SPP(1) - OP(14,23)*SPP(9) + OP(15,23)*SPP(3) + OP(16,23)*SPP(6); +nextP(7,23) = OP(7,23) + OP(2,23)*SF(5) - OP(3,23)*SF(6) + OP(4,23)*SF(4) + OP(1,23)*SPP(1) + OP(14,23)*SPP(5) - OP(15,23)*SPP(8) - OP(16,23)*SPP(2); nextP(8,23) = OP(8,23) + OP(5,23)*dt; nextP(9,23) = OP(9,23) + OP(6,23)*dt; -nextP(10,23) = OP(10,23); +nextP(10,23) = OP(10,23) + OP(7,23)*dt; nextP(11,23) = OP(11,23); nextP(12,23) = OP(12,23); nextP(13,23) = OP(13,23); @@ -342,16 +330,16 @@ nextP(20,23) = OP(20,23); nextP(21,23) = OP(21,23); nextP(22,23) = OP(22,23); nextP(23,23) = OP(23,23); -nextP(1,24) = OP(1,24)*SPP(6) - OP(2,24)*SPP(5) + OP(3,24)*SPP(8) + OP(10,24)*SPP(23) + OP(13,24)*SPP(19); -nextP(2,24) = OP(2,24)*SPP(7) - OP(1,24)*SPP(3) - OP(3,24)*SPP(9) + OP(11,24)*SPP(23) + OP(14,24)*SPP(18); -nextP(3,24) = OP(1,24)*SPP(15) - OP(2,24)*SPP(4) + OP(3,24)*SPP(14) + OP(12,24)*SPP(23) + OP(15,24)*SPP(17); -nextP(4,24) = OP(4,24) + OP(1,24)*SPP(2) + OP(2,24)*SPP(20) + OP(3,24)*SPP(16) - OP(16,24)*SPP(22); -nextP(5,24) = OP(5,24) + OP(16,24)*SF(23) + OP(1,24)*SPP(21) + OP(2,24)*SPP(13) + OP(3,24)*SPP(12); -nextP(6,24) = OP(6,24) + OP(16,24)*SF(21) - OP(1,24)*SPP(10) + OP(2,24)*SPP(11) + OP(3,24)*SPP(1); -nextP(7,24) = OP(7,24) + OP(4,24)*dt; +nextP(1,24) = OP(1,24) + OP(2,24)*SF(10) + OP(3,24)*SF(12) + OP(4,24)*SF(11) + OP(11,24)*SF(15) + OP(12,24)*SF(16) + OP(13,24)*SPP(11); +nextP(2,24) = OP(2,24) + OP(1,24)*SF(9) + OP(3,24)*SF(8) + OP(4,24)*SF(12) - OP(13,24)*SF(16) + OP(12,24)*SPP(11) - (OP(11,24)*q0)/2; +nextP(3,24) = OP(3,24) + OP(1,24)*SF(7) + OP(2,24)*SF(11) + OP(4,24)*SF(9) + OP(13,24)*SF(15) - OP(11,24)*SPP(11) - (OP(12,24)*q0)/2; +nextP(4,24) = OP(4,24) + OP(1,24)*SF(8) + OP(2,24)*SF(7) + OP(3,24)*SF(10) + OP(11,24)*SF(16) - OP(12,24)*SF(15) - (OP(13,24)*q0)/2; +nextP(5,24) = OP(5,24) + OP(1,24)*SF(6) + OP(2,24)*SF(4) - OP(4,24)*SF(5) + OP(3,24)*SPP(1) + OP(14,24)*SPP(4) + OP(15,24)*SPP(7) - OP(16,24)*SPP(10); +nextP(6,24) = OP(6,24) + OP(1,24)*SF(5) + OP(3,24)*SF(4) + OP(4,24)*SF(6) - OP(2,24)*SPP(1) - OP(14,24)*SPP(9) + OP(15,24)*SPP(3) + OP(16,24)*SPP(6); +nextP(7,24) = OP(7,24) + OP(2,24)*SF(5) - OP(3,24)*SF(6) + OP(4,24)*SF(4) + OP(1,24)*SPP(1) + OP(14,24)*SPP(5) - OP(15,24)*SPP(8) - OP(16,24)*SPP(2); nextP(8,24) = OP(8,24) + OP(5,24)*dt; nextP(9,24) = OP(9,24) + OP(6,24)*dt; -nextP(10,24) = OP(10,24); +nextP(10,24) = OP(10,24) + OP(7,24)*dt; nextP(11,24) = OP(11,24); nextP(12,24) = OP(12,24); nextP(13,24) = OP(13,24); @@ -371,311 +359,328 @@ SH_TAS(1) = 1/((ve - vwe)^2 + (vn - vwn)^2 + vd^2)^(1/2); SH_TAS(2) = (SH_TAS(1)*(2*ve - 2*vwe))/2; SH_TAS(3) = (SH_TAS(1)*(2*vn - 2*vwn))/2; H_TAS = zeros(1,24); -H_TAS(1,4) = SH_TAS(3); -H_TAS(1,5) = SH_TAS(2); -H_TAS(1,6) = vd*SH_TAS(1); +H_TAS(1,5) = SH_TAS(3); +H_TAS(1,6) = SH_TAS(2); +H_TAS(1,7) = vd*SH_TAS(1); H_TAS(1,23) = -SH_TAS(3); H_TAS(1,24) = -SH_TAS(2); SK_TAS = zeros(2,1); -SK_TAS(1) = 1/(R_TAS + SH_TAS(3)*(OP(4,4)*SH_TAS(3) + OP(5,4)*SH_TAS(2) - OP(23,4)*SH_TAS(3) - OP(24,4)*SH_TAS(2) + OP(6,4)*vd*SH_TAS(1)) + SH_TAS(2)*(OP(4,5)*SH_TAS(3) + OP(5,5)*SH_TAS(2) - OP(23,5)*SH_TAS(3) - OP(24,5)*SH_TAS(2) + OP(6,5)*vd*SH_TAS(1)) - SH_TAS(3)*(OP(4,23)*SH_TAS(3) + OP(5,23)*SH_TAS(2) - OP(23,23)*SH_TAS(3) - OP(24,23)*SH_TAS(2) + OP(6,23)*vd*SH_TAS(1)) - SH_TAS(2)*(OP(4,24)*SH_TAS(3) + OP(5,24)*SH_TAS(2) - OP(23,24)*SH_TAS(3) - OP(24,24)*SH_TAS(2) + OP(6,24)*vd*SH_TAS(1)) + vd*SH_TAS(1)*(OP(4,6)*SH_TAS(3) + OP(5,6)*SH_TAS(2) - OP(23,6)*SH_TAS(3) - OP(24,6)*SH_TAS(2) + OP(6,6)*vd*SH_TAS(1))); +SK_TAS(1) = 1/(R_TAS + SH_TAS(3)*(OP(5,5)*SH_TAS(3) + OP(6,5)*SH_TAS(2) - OP(23,5)*SH_TAS(3) - OP(24,5)*SH_TAS(2) + OP(7,5)*vd*SH_TAS(1)) + SH_TAS(2)*(OP(5,6)*SH_TAS(3) + OP(6,6)*SH_TAS(2) - OP(23,6)*SH_TAS(3) - OP(24,6)*SH_TAS(2) + OP(7,6)*vd*SH_TAS(1)) - SH_TAS(3)*(OP(5,23)*SH_TAS(3) + OP(6,23)*SH_TAS(2) - OP(23,23)*SH_TAS(3) - OP(24,23)*SH_TAS(2) + OP(7,23)*vd*SH_TAS(1)) - SH_TAS(2)*(OP(5,24)*SH_TAS(3) + OP(6,24)*SH_TAS(2) - OP(23,24)*SH_TAS(3) - OP(24,24)*SH_TAS(2) + OP(7,24)*vd*SH_TAS(1)) + vd*SH_TAS(1)*(OP(5,7)*SH_TAS(3) + OP(6,7)*SH_TAS(2) - OP(23,7)*SH_TAS(3) - OP(24,7)*SH_TAS(2) + OP(7,7)*vd*SH_TAS(1))); SK_TAS(2) = SH_TAS(2); Kfusion = zeros(24,1); Kfusion = zeros(1,1); -Kfusion(1) = SK_TAS(1)*(OP(1,4)*SH_TAS(3) - OP(1,23)*SH_TAS(3) + OP(1,5)*SK_TAS(2) - OP(1,24)*SK_TAS(2) + OP(1,6)*vd*SH_TAS(1)); -Kfusion(2) = SK_TAS(1)*(OP(2,4)*SH_TAS(3) - OP(2,23)*SH_TAS(3) + OP(2,5)*SK_TAS(2) - OP(2,24)*SK_TAS(2) + OP(2,6)*vd*SH_TAS(1)); -Kfusion(3) = SK_TAS(1)*(OP(3,4)*SH_TAS(3) - OP(3,23)*SH_TAS(3) + OP(3,5)*SK_TAS(2) - OP(3,24)*SK_TAS(2) + OP(3,6)*vd*SH_TAS(1)); -Kfusion(4) = SK_TAS(1)*(OP(4,4)*SH_TAS(3) - OP(4,23)*SH_TAS(3) + OP(4,5)*SK_TAS(2) - OP(4,24)*SK_TAS(2) + OP(4,6)*vd*SH_TAS(1)); -Kfusion(5) = SK_TAS(1)*(OP(5,4)*SH_TAS(3) - OP(5,23)*SH_TAS(3) + OP(5,5)*SK_TAS(2) - OP(5,24)*SK_TAS(2) + OP(5,6)*vd*SH_TAS(1)); -Kfusion(6) = SK_TAS(1)*(OP(6,4)*SH_TAS(3) - OP(6,23)*SH_TAS(3) + OP(6,5)*SK_TAS(2) - OP(6,24)*SK_TAS(2) + OP(6,6)*vd*SH_TAS(1)); -Kfusion(7) = SK_TAS(1)*(OP(7,4)*SH_TAS(3) - OP(7,23)*SH_TAS(3) + OP(7,5)*SK_TAS(2) - OP(7,24)*SK_TAS(2) + OP(7,6)*vd*SH_TAS(1)); -Kfusion(8) = SK_TAS(1)*(OP(8,4)*SH_TAS(3) - OP(8,23)*SH_TAS(3) + OP(8,5)*SK_TAS(2) - OP(8,24)*SK_TAS(2) + OP(8,6)*vd*SH_TAS(1)); -Kfusion(9) = SK_TAS(1)*(OP(9,4)*SH_TAS(3) - OP(9,23)*SH_TAS(3) + OP(9,5)*SK_TAS(2) - OP(9,24)*SK_TAS(2) + OP(9,6)*vd*SH_TAS(1)); -Kfusion(10) = SK_TAS(1)*(OP(10,4)*SH_TAS(3) - OP(10,23)*SH_TAS(3) + OP(10,5)*SK_TAS(2) - OP(10,24)*SK_TAS(2) + OP(10,6)*vd*SH_TAS(1)); -Kfusion(11) = SK_TAS(1)*(OP(11,4)*SH_TAS(3) - OP(11,23)*SH_TAS(3) + OP(11,5)*SK_TAS(2) - OP(11,24)*SK_TAS(2) + OP(11,6)*vd*SH_TAS(1)); -Kfusion(12) = SK_TAS(1)*(OP(12,4)*SH_TAS(3) - OP(12,23)*SH_TAS(3) + OP(12,5)*SK_TAS(2) - OP(12,24)*SK_TAS(2) + OP(12,6)*vd*SH_TAS(1)); -Kfusion(13) = SK_TAS(1)*(OP(13,4)*SH_TAS(3) - OP(13,23)*SH_TAS(3) + OP(13,5)*SK_TAS(2) - OP(13,24)*SK_TAS(2) + OP(13,6)*vd*SH_TAS(1)); -Kfusion(14) = SK_TAS(1)*(OP(14,4)*SH_TAS(3) - OP(14,23)*SH_TAS(3) + OP(14,5)*SK_TAS(2) - OP(14,24)*SK_TAS(2) + OP(14,6)*vd*SH_TAS(1)); -Kfusion(15) = SK_TAS(1)*(OP(15,4)*SH_TAS(3) - OP(15,23)*SH_TAS(3) + OP(15,5)*SK_TAS(2) - OP(15,24)*SK_TAS(2) + OP(15,6)*vd*SH_TAS(1)); -Kfusion(16) = SK_TAS(1)*(OP(16,4)*SH_TAS(3) - OP(16,23)*SH_TAS(3) + OP(16,5)*SK_TAS(2) - OP(16,24)*SK_TAS(2) + OP(16,6)*vd*SH_TAS(1)); -Kfusion(17) = SK_TAS(1)*(OP(17,4)*SH_TAS(3) - OP(17,23)*SH_TAS(3) + OP(17,5)*SK_TAS(2) - OP(17,24)*SK_TAS(2) + OP(17,6)*vd*SH_TAS(1)); -Kfusion(18) = SK_TAS(1)*(OP(18,4)*SH_TAS(3) - OP(18,23)*SH_TAS(3) + OP(18,5)*SK_TAS(2) - OP(18,24)*SK_TAS(2) + OP(18,6)*vd*SH_TAS(1)); -Kfusion(19) = SK_TAS(1)*(OP(19,4)*SH_TAS(3) - OP(19,23)*SH_TAS(3) + OP(19,5)*SK_TAS(2) - OP(19,24)*SK_TAS(2) + OP(19,6)*vd*SH_TAS(1)); -Kfusion(20) = SK_TAS(1)*(OP(20,4)*SH_TAS(3) - OP(20,23)*SH_TAS(3) + OP(20,5)*SK_TAS(2) - OP(20,24)*SK_TAS(2) + OP(20,6)*vd*SH_TAS(1)); -Kfusion(21) = SK_TAS(1)*(OP(21,4)*SH_TAS(3) - OP(21,23)*SH_TAS(3) + OP(21,5)*SK_TAS(2) - OP(21,24)*SK_TAS(2) + OP(21,6)*vd*SH_TAS(1)); -Kfusion(22) = SK_TAS(1)*(OP(22,4)*SH_TAS(3) - OP(22,23)*SH_TAS(3) + OP(22,5)*SK_TAS(2) - OP(22,24)*SK_TAS(2) + OP(22,6)*vd*SH_TAS(1)); -Kfusion(23) = SK_TAS(1)*(OP(23,4)*SH_TAS(3) - OP(23,23)*SH_TAS(3) + OP(23,5)*SK_TAS(2) - OP(23,24)*SK_TAS(2) + OP(23,6)*vd*SH_TAS(1)); -Kfusion(24) = SK_TAS(1)*(OP(24,4)*SH_TAS(3) - OP(24,23)*SH_TAS(3) + OP(24,5)*SK_TAS(2) - OP(24,24)*SK_TAS(2) + OP(24,6)*vd*SH_TAS(1)); -SH_BETA = zeros(10,1); -SH_BETA(1) = (2*conj(q0)*conj(q3) + 2*conj(q1)*conj(q2))*(ve - vwe) - vd*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3)) + (vn - vwn)*(conj(q0)^2 + conj(q1)^2 - conj(q2)^2 - conj(q3)^2); -SH_BETA(2) = vd*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) - (2*conj(q0)*conj(q3) - 2*conj(q1)*conj(q2))*(vn - vwn) + (ve - vwe)*(conj(q0)^2 - conj(q1)^2 + conj(q2)^2 - conj(q3)^2); -SH_BETA(3) = (2*conj(q0)*conj(q2) + 2*conj(q1)*conj(q3))*(vn - vwn) - (2*conj(q0)*conj(q1) - 2*conj(q2)*conj(q3))*(ve - vwe) + vd*(conj(q0)^2 - conj(q1)^2 - conj(q2)^2 + conj(q3)^2); -SH_BETA(4) = (conj(q0)^2 - conj(q1)^2 + conj(q2)^2 - conj(q3)^2)/SH_BETA(1); +Kfusion(1) = SK_TAS(1)*(OP(1,5)*SH_TAS(3) - OP(1,23)*SH_TAS(3) + OP(1,6)*SK_TAS(2) - OP(1,24)*SK_TAS(2) + OP(1,7)*vd*SH_TAS(1)); +Kfusion(2) = SK_TAS(1)*(OP(2,5)*SH_TAS(3) - OP(2,23)*SH_TAS(3) + OP(2,6)*SK_TAS(2) - OP(2,24)*SK_TAS(2) + OP(2,7)*vd*SH_TAS(1)); +Kfusion(3) = SK_TAS(1)*(OP(3,5)*SH_TAS(3) - OP(3,23)*SH_TAS(3) + OP(3,6)*SK_TAS(2) - OP(3,24)*SK_TAS(2) + OP(3,7)*vd*SH_TAS(1)); +Kfusion(4) = SK_TAS(1)*(OP(4,5)*SH_TAS(3) - OP(4,23)*SH_TAS(3) + OP(4,6)*SK_TAS(2) - OP(4,24)*SK_TAS(2) + OP(4,7)*vd*SH_TAS(1)); +Kfusion(5) = SK_TAS(1)*(OP(5,5)*SH_TAS(3) - OP(5,23)*SH_TAS(3) + OP(5,6)*SK_TAS(2) - OP(5,24)*SK_TAS(2) + OP(5,7)*vd*SH_TAS(1)); +Kfusion(6) = SK_TAS(1)*(OP(6,5)*SH_TAS(3) - OP(6,23)*SH_TAS(3) + OP(6,6)*SK_TAS(2) - OP(6,24)*SK_TAS(2) + OP(6,7)*vd*SH_TAS(1)); +Kfusion(7) = SK_TAS(1)*(OP(7,5)*SH_TAS(3) - OP(7,23)*SH_TAS(3) + OP(7,6)*SK_TAS(2) - OP(7,24)*SK_TAS(2) + OP(7,7)*vd*SH_TAS(1)); +Kfusion(8) = SK_TAS(1)*(OP(8,5)*SH_TAS(3) - OP(8,23)*SH_TAS(3) + OP(8,6)*SK_TAS(2) - OP(8,24)*SK_TAS(2) + OP(8,7)*vd*SH_TAS(1)); +Kfusion(9) = SK_TAS(1)*(OP(9,5)*SH_TAS(3) - OP(9,23)*SH_TAS(3) + OP(9,6)*SK_TAS(2) - OP(9,24)*SK_TAS(2) + OP(9,7)*vd*SH_TAS(1)); +Kfusion(10) = SK_TAS(1)*(OP(10,5)*SH_TAS(3) - OP(10,23)*SH_TAS(3) + OP(10,6)*SK_TAS(2) - OP(10,24)*SK_TAS(2) + OP(10,7)*vd*SH_TAS(1)); +Kfusion(11) = SK_TAS(1)*(OP(11,5)*SH_TAS(3) - OP(11,23)*SH_TAS(3) + OP(11,6)*SK_TAS(2) - OP(11,24)*SK_TAS(2) + OP(11,7)*vd*SH_TAS(1)); +Kfusion(12) = SK_TAS(1)*(OP(12,5)*SH_TAS(3) - OP(12,23)*SH_TAS(3) + OP(12,6)*SK_TAS(2) - OP(12,24)*SK_TAS(2) + OP(12,7)*vd*SH_TAS(1)); +Kfusion(13) = SK_TAS(1)*(OP(13,5)*SH_TAS(3) - OP(13,23)*SH_TAS(3) + OP(13,6)*SK_TAS(2) - OP(13,24)*SK_TAS(2) + OP(13,7)*vd*SH_TAS(1)); +Kfusion(14) = SK_TAS(1)*(OP(14,5)*SH_TAS(3) - OP(14,23)*SH_TAS(3) + OP(14,6)*SK_TAS(2) - OP(14,24)*SK_TAS(2) + OP(14,7)*vd*SH_TAS(1)); +Kfusion(15) = SK_TAS(1)*(OP(15,5)*SH_TAS(3) - OP(15,23)*SH_TAS(3) + OP(15,6)*SK_TAS(2) - OP(15,24)*SK_TAS(2) + OP(15,7)*vd*SH_TAS(1)); +Kfusion(16) = SK_TAS(1)*(OP(16,5)*SH_TAS(3) - OP(16,23)*SH_TAS(3) + OP(16,6)*SK_TAS(2) - OP(16,24)*SK_TAS(2) + OP(16,7)*vd*SH_TAS(1)); +Kfusion(17) = SK_TAS(1)*(OP(17,5)*SH_TAS(3) - OP(17,23)*SH_TAS(3) + OP(17,6)*SK_TAS(2) - OP(17,24)*SK_TAS(2) + OP(17,7)*vd*SH_TAS(1)); +Kfusion(18) = SK_TAS(1)*(OP(18,5)*SH_TAS(3) - OP(18,23)*SH_TAS(3) + OP(18,6)*SK_TAS(2) - OP(18,24)*SK_TAS(2) + OP(18,7)*vd*SH_TAS(1)); +Kfusion(19) = SK_TAS(1)*(OP(19,5)*SH_TAS(3) - OP(19,23)*SH_TAS(3) + OP(19,6)*SK_TAS(2) - OP(19,24)*SK_TAS(2) + OP(19,7)*vd*SH_TAS(1)); +Kfusion(20) = SK_TAS(1)*(OP(20,5)*SH_TAS(3) - OP(20,23)*SH_TAS(3) + OP(20,6)*SK_TAS(2) - OP(20,24)*SK_TAS(2) + OP(20,7)*vd*SH_TAS(1)); +Kfusion(21) = SK_TAS(1)*(OP(21,5)*SH_TAS(3) - OP(21,23)*SH_TAS(3) + OP(21,6)*SK_TAS(2) - OP(21,24)*SK_TAS(2) + OP(21,7)*vd*SH_TAS(1)); +Kfusion(22) = SK_TAS(1)*(OP(22,5)*SH_TAS(3) - OP(22,23)*SH_TAS(3) + OP(22,6)*SK_TAS(2) - OP(22,24)*SK_TAS(2) + OP(22,7)*vd*SH_TAS(1)); +Kfusion(23) = SK_TAS(1)*(OP(23,5)*SH_TAS(3) - OP(23,23)*SH_TAS(3) + OP(23,6)*SK_TAS(2) - OP(23,24)*SK_TAS(2) + OP(23,7)*vd*SH_TAS(1)); +Kfusion(24) = SK_TAS(1)*(OP(24,5)*SH_TAS(3) - OP(24,23)*SH_TAS(3) + OP(24,6)*SK_TAS(2) - OP(24,24)*SK_TAS(2) + OP(24,7)*vd*SH_TAS(1)); +SH_BETA = zeros(13,1); +SH_BETA(1) = (vn - vwn)*(q0^2 + q1^2 - q2^2 - q3^2) - vd*(2*q0*q2 - 2*q1*q3) + (ve - vwe)*(2*q0*q3 + 2*q1*q2); +SH_BETA(2) = (ve - vwe)*(q0^2 - q1^2 + q2^2 - q3^2) + vd*(2*q0*q1 + 2*q2*q3) - (vn - vwn)*(2*q0*q3 - 2*q1*q2); +SH_BETA(3) = vn - vwn; +SH_BETA(4) = ve - vwe; SH_BETA(5) = 1/SH_BETA(1)^2; -SH_BETA(6) = conj(q0)^2 + conj(q1)^2 - conj(q2)^2 - conj(q3)^2; -SH_BETA(7) = 2*conj(q0)*conj(q3); -SH_BETA(8) = 1/SH_BETA(1); -SH_BETA(9) = SH_BETA(7) + 2*conj(q1)*conj(q2); -SH_BETA(10) = SH_BETA(7) - 2*conj(q1)*conj(q2); +SH_BETA(6) = 1/SH_BETA(1); +SH_BETA(7) = SH_BETA(6)*(q0^2 - q1^2 + q2^2 - q3^2); +SH_BETA(8) = q0^2 + q1^2 - q2^2 - q3^2; +SH_BETA(9) = 2*q0*SH_BETA(4) - 2*q3*SH_BETA(3) + 2*q1*vd; +SH_BETA(10) = 2*q0*SH_BETA(3) + 2*q3*SH_BETA(4) - 2*q2*vd; +SH_BETA(11) = 2*q2*SH_BETA(3) - 2*q1*SH_BETA(4) + 2*q0*vd; +SH_BETA(12) = 2*q1*SH_BETA(3) + 2*q2*SH_BETA(4) + 2*q3*vd; +SH_BETA(13) = 2*q0*q3; H_BETA = zeros(1,24); -H_BETA(1,1) = SH_BETA(3)*SH_BETA(8); -H_BETA(1,2) = SH_BETA(2)*SH_BETA(3)*SH_BETA(5); -H_BETA(1,3) = - SH_BETA(2)^2*SH_BETA(5) - 1; -H_BETA(1,4) = - SH_BETA(8)*SH_BETA(10) - SH_BETA(2)*SH_BETA(5)*SH_BETA(6); -H_BETA(1,5) = SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9); -H_BETA(1,6) = SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3)); -H_BETA(1,23) = SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6); -H_BETA(1,24) = SH_BETA(2)*SH_BETA(5)*SH_BETA(9) - SH_BETA(4); -SK_BETA = zeros(6,1); -SK_BETA(1) = 1/(R_BETA + (SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3)))*(OP(23,6)*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP(4,6)*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP(3,6)*(SH_BETA(2)^2*SH_BETA(5) + 1) + OP(6,6)*(SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + OP(5,6)*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) - OP(24,6)*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP(1,6)*SH_BETA(3)*SH_BETA(8) + OP(2,6)*SH_BETA(2)*SH_BETA(3)*SH_BETA(5)) + (SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9))*(OP(23,5)*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP(4,5)*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP(3,5)*(SH_BETA(2)^2*SH_BETA(5) + 1) + OP(6,5)*(SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + OP(5,5)*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) - OP(24,5)*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP(1,5)*SH_BETA(3)*SH_BETA(8) + OP(2,5)*SH_BETA(2)*SH_BETA(3)*SH_BETA(5)) - (SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9))*(OP(23,24)*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP(4,24)*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP(3,24)*(SH_BETA(2)^2*SH_BETA(5) + 1) + OP(6,24)*(SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + OP(5,24)*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) - OP(24,24)*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP(1,24)*SH_BETA(3)*SH_BETA(8) + OP(2,24)*SH_BETA(2)*SH_BETA(3)*SH_BETA(5)) - (SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6))*(OP(23,4)*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP(4,4)*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP(3,4)*(SH_BETA(2)^2*SH_BETA(5) + 1) + OP(6,4)*(SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + OP(5,4)*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) - OP(24,4)*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP(1,4)*SH_BETA(3)*SH_BETA(8) + OP(2,4)*SH_BETA(2)*SH_BETA(3)*SH_BETA(5)) + (SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6))*(OP(23,23)*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP(4,23)*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP(3,23)*(SH_BETA(2)^2*SH_BETA(5) + 1) + OP(6,23)*(SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + OP(5,23)*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) - OP(24,23)*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP(1,23)*SH_BETA(3)*SH_BETA(8) + OP(2,23)*SH_BETA(2)*SH_BETA(3)*SH_BETA(5)) - (SH_BETA(2)^2*SH_BETA(5) + 1)*(OP(23,3)*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP(4,3)*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP(3,3)*(SH_BETA(2)^2*SH_BETA(5) + 1) + OP(6,3)*(SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + OP(5,3)*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) - OP(24,3)*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP(1,3)*SH_BETA(3)*SH_BETA(8) + OP(2,3)*SH_BETA(2)*SH_BETA(3)*SH_BETA(5)) + SH_BETA(3)*SH_BETA(8)*(OP(23,1)*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP(4,1)*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP(3,1)*(SH_BETA(2)^2*SH_BETA(5) + 1) + OP(6,1)*(SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + OP(5,1)*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) - OP(24,1)*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP(1,1)*SH_BETA(3)*SH_BETA(8) + OP(2,1)*SH_BETA(2)*SH_BETA(3)*SH_BETA(5)) + SH_BETA(2)*SH_BETA(3)*SH_BETA(5)*(OP(23,2)*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP(4,2)*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP(3,2)*(SH_BETA(2)^2*SH_BETA(5) + 1) + OP(6,2)*(SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + OP(5,2)*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) - OP(24,2)*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP(1,2)*SH_BETA(3)*SH_BETA(8) + OP(2,2)*SH_BETA(2)*SH_BETA(3)*SH_BETA(5))); -SK_BETA(2) = SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3)); -SK_BETA(3) = SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6); -SK_BETA(4) = SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9); -SK_BETA(5) = SH_BETA(2)^2*SH_BETA(5) + 1; -SK_BETA(6) = SH_BETA(3); +H_BETA(1,1) = SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10); +H_BETA(1,2) = SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12); +H_BETA(1,3) = SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11); +H_BETA(1,4) = - SH_BETA(6)*SH_BETA(10) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9); +H_BETA(1,5) = - SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) - SH_BETA(2)*SH_BETA(5)*SH_BETA(8); +H_BETA(1,6) = SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2); +H_BETA(1,7) = SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3); +H_BETA(1,23) = SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8); +H_BETA(1,24) = SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2) - SH_BETA(7); +SK_BETA = zeros(8,1); +SK_BETA(1) = 1/(R_BETA - (SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8))*(OP(23,5)*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) - OP(5,5)*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) + OP(6,5)*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) - OP(24,5)*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) + OP(1,5)*(SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10)) + OP(2,5)*(SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12)) + OP(3,5)*(SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11)) - OP(4,5)*(SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP(7,5)*(SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3))) + (SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8))*(OP(23,23)*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) - OP(5,23)*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) + OP(6,23)*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) - OP(24,23)*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) + OP(1,23)*(SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10)) + OP(2,23)*(SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12)) + OP(3,23)*(SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11)) - OP(4,23)*(SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP(7,23)*(SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3))) + (SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2))*(OP(23,6)*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) - OP(5,6)*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) + OP(6,6)*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) - OP(24,6)*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) + OP(1,6)*(SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10)) + OP(2,6)*(SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12)) + OP(3,6)*(SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11)) - OP(4,6)*(SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP(7,6)*(SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3))) - (SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2))*(OP(23,24)*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) - OP(5,24)*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) + OP(6,24)*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) - OP(24,24)*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) + OP(1,24)*(SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10)) + OP(2,24)*(SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12)) + OP(3,24)*(SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11)) - OP(4,24)*(SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP(7,24)*(SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3))) + (SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10))*(OP(23,1)*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) - OP(5,1)*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) + OP(6,1)*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) - OP(24,1)*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) + OP(1,1)*(SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10)) + OP(2,1)*(SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12)) + OP(3,1)*(SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11)) - OP(4,1)*(SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP(7,1)*(SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3))) + (SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12))*(OP(23,2)*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) - OP(5,2)*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) + OP(6,2)*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) - OP(24,2)*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) + OP(1,2)*(SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10)) + OP(2,2)*(SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12)) + OP(3,2)*(SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11)) - OP(4,2)*(SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP(7,2)*(SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3))) + (SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11))*(OP(23,3)*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) - OP(5,3)*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) + OP(6,3)*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) - OP(24,3)*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) + OP(1,3)*(SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10)) + OP(2,3)*(SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12)) + OP(3,3)*(SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11)) - OP(4,3)*(SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP(7,3)*(SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3))) - (SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9))*(OP(23,4)*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) - OP(5,4)*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) + OP(6,4)*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) - OP(24,4)*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) + OP(1,4)*(SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10)) + OP(2,4)*(SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12)) + OP(3,4)*(SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11)) - OP(4,4)*(SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP(7,4)*(SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3))) + (SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3))*(OP(23,7)*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) - OP(5,7)*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) + OP(6,7)*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) - OP(24,7)*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) + OP(1,7)*(SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10)) + OP(2,7)*(SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12)) + OP(3,7)*(SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11)) - OP(4,7)*(SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP(7,7)*(SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3)))); +SK_BETA(2) = SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8); +SK_BETA(3) = SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2); +SK_BETA(4) = SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3); +SK_BETA(5) = SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12); +SK_BETA(6) = SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10); +SK_BETA(7) = SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11); +SK_BETA(8) = SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9); Kfusion = zeros(24,1); Kfusion = zeros(1,1); -Kfusion(1) = SK_BETA(1)*(OP(1,6)*SK_BETA(2) - OP(1,3)*SK_BETA(5) - OP(1,4)*SK_BETA(3) + OP(1,5)*SK_BETA(4) + OP(1,23)*SK_BETA(3) - OP(1,24)*SK_BETA(4) + OP(1,1)*SH_BETA(8)*SK_BETA(6) + OP(1,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(2) = SK_BETA(1)*(OP(2,6)*SK_BETA(2) - OP(2,3)*SK_BETA(5) - OP(2,4)*SK_BETA(3) + OP(2,5)*SK_BETA(4) + OP(2,23)*SK_BETA(3) - OP(2,24)*SK_BETA(4) + OP(2,1)*SH_BETA(8)*SK_BETA(6) + OP(2,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(3) = SK_BETA(1)*(OP(3,6)*SK_BETA(2) - OP(3,3)*SK_BETA(5) - OP(3,4)*SK_BETA(3) + OP(3,5)*SK_BETA(4) + OP(3,23)*SK_BETA(3) - OP(3,24)*SK_BETA(4) + OP(3,1)*SH_BETA(8)*SK_BETA(6) + OP(3,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(4) = SK_BETA(1)*(OP(4,6)*SK_BETA(2) - OP(4,3)*SK_BETA(5) - OP(4,4)*SK_BETA(3) + OP(4,5)*SK_BETA(4) + OP(4,23)*SK_BETA(3) - OP(4,24)*SK_BETA(4) + OP(4,1)*SH_BETA(8)*SK_BETA(6) + OP(4,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(5) = SK_BETA(1)*(OP(5,6)*SK_BETA(2) - OP(5,3)*SK_BETA(5) - OP(5,4)*SK_BETA(3) + OP(5,5)*SK_BETA(4) + OP(5,23)*SK_BETA(3) - OP(5,24)*SK_BETA(4) + OP(5,1)*SH_BETA(8)*SK_BETA(6) + OP(5,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(6) = SK_BETA(1)*(OP(6,6)*SK_BETA(2) - OP(6,3)*SK_BETA(5) - OP(6,4)*SK_BETA(3) + OP(6,5)*SK_BETA(4) + OP(6,23)*SK_BETA(3) - OP(6,24)*SK_BETA(4) + OP(6,1)*SH_BETA(8)*SK_BETA(6) + OP(6,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(7) = SK_BETA(1)*(OP(7,6)*SK_BETA(2) - OP(7,3)*SK_BETA(5) - OP(7,4)*SK_BETA(3) + OP(7,5)*SK_BETA(4) + OP(7,23)*SK_BETA(3) - OP(7,24)*SK_BETA(4) + OP(7,1)*SH_BETA(8)*SK_BETA(6) + OP(7,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(8) = SK_BETA(1)*(OP(8,6)*SK_BETA(2) - OP(8,3)*SK_BETA(5) - OP(8,4)*SK_BETA(3) + OP(8,5)*SK_BETA(4) + OP(8,23)*SK_BETA(3) - OP(8,24)*SK_BETA(4) + OP(8,1)*SH_BETA(8)*SK_BETA(6) + OP(8,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(9) = SK_BETA(1)*(OP(9,6)*SK_BETA(2) - OP(9,3)*SK_BETA(5) - OP(9,4)*SK_BETA(3) + OP(9,5)*SK_BETA(4) + OP(9,23)*SK_BETA(3) - OP(9,24)*SK_BETA(4) + OP(9,1)*SH_BETA(8)*SK_BETA(6) + OP(9,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(10) = SK_BETA(1)*(OP(10,6)*SK_BETA(2) - OP(10,3)*SK_BETA(5) - OP(10,4)*SK_BETA(3) + OP(10,5)*SK_BETA(4) + OP(10,23)*SK_BETA(3) - OP(10,24)*SK_BETA(4) + OP(10,1)*SH_BETA(8)*SK_BETA(6) + OP(10,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(11) = SK_BETA(1)*(OP(11,6)*SK_BETA(2) - OP(11,3)*SK_BETA(5) - OP(11,4)*SK_BETA(3) + OP(11,5)*SK_BETA(4) + OP(11,23)*SK_BETA(3) - OP(11,24)*SK_BETA(4) + OP(11,1)*SH_BETA(8)*SK_BETA(6) + OP(11,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(12) = SK_BETA(1)*(OP(12,6)*SK_BETA(2) - OP(12,3)*SK_BETA(5) - OP(12,4)*SK_BETA(3) + OP(12,5)*SK_BETA(4) + OP(12,23)*SK_BETA(3) - OP(12,24)*SK_BETA(4) + OP(12,1)*SH_BETA(8)*SK_BETA(6) + OP(12,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(13) = SK_BETA(1)*(OP(13,6)*SK_BETA(2) - OP(13,3)*SK_BETA(5) - OP(13,4)*SK_BETA(3) + OP(13,5)*SK_BETA(4) + OP(13,23)*SK_BETA(3) - OP(13,24)*SK_BETA(4) + OP(13,1)*SH_BETA(8)*SK_BETA(6) + OP(13,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(14) = SK_BETA(1)*(OP(14,6)*SK_BETA(2) - OP(14,3)*SK_BETA(5) - OP(14,4)*SK_BETA(3) + OP(14,5)*SK_BETA(4) + OP(14,23)*SK_BETA(3) - OP(14,24)*SK_BETA(4) + OP(14,1)*SH_BETA(8)*SK_BETA(6) + OP(14,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(15) = SK_BETA(1)*(OP(15,6)*SK_BETA(2) - OP(15,3)*SK_BETA(5) - OP(15,4)*SK_BETA(3) + OP(15,5)*SK_BETA(4) + OP(15,23)*SK_BETA(3) - OP(15,24)*SK_BETA(4) + OP(15,1)*SH_BETA(8)*SK_BETA(6) + OP(15,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(16) = SK_BETA(1)*(OP(16,6)*SK_BETA(2) - OP(16,3)*SK_BETA(5) - OP(16,4)*SK_BETA(3) + OP(16,5)*SK_BETA(4) + OP(16,23)*SK_BETA(3) - OP(16,24)*SK_BETA(4) + OP(16,1)*SH_BETA(8)*SK_BETA(6) + OP(16,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(17) = SK_BETA(1)*(OP(17,6)*SK_BETA(2) - OP(17,3)*SK_BETA(5) - OP(17,4)*SK_BETA(3) + OP(17,5)*SK_BETA(4) + OP(17,23)*SK_BETA(3) - OP(17,24)*SK_BETA(4) + OP(17,1)*SH_BETA(8)*SK_BETA(6) + OP(17,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(18) = SK_BETA(1)*(OP(18,6)*SK_BETA(2) - OP(18,3)*SK_BETA(5) - OP(18,4)*SK_BETA(3) + OP(18,5)*SK_BETA(4) + OP(18,23)*SK_BETA(3) - OP(18,24)*SK_BETA(4) + OP(18,1)*SH_BETA(8)*SK_BETA(6) + OP(18,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(19) = SK_BETA(1)*(OP(19,6)*SK_BETA(2) - OP(19,3)*SK_BETA(5) - OP(19,4)*SK_BETA(3) + OP(19,5)*SK_BETA(4) + OP(19,23)*SK_BETA(3) - OP(19,24)*SK_BETA(4) + OP(19,1)*SH_BETA(8)*SK_BETA(6) + OP(19,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(20) = SK_BETA(1)*(OP(20,6)*SK_BETA(2) - OP(20,3)*SK_BETA(5) - OP(20,4)*SK_BETA(3) + OP(20,5)*SK_BETA(4) + OP(20,23)*SK_BETA(3) - OP(20,24)*SK_BETA(4) + OP(20,1)*SH_BETA(8)*SK_BETA(6) + OP(20,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(21) = SK_BETA(1)*(OP(21,6)*SK_BETA(2) - OP(21,3)*SK_BETA(5) - OP(21,4)*SK_BETA(3) + OP(21,5)*SK_BETA(4) + OP(21,23)*SK_BETA(3) - OP(21,24)*SK_BETA(4) + OP(21,1)*SH_BETA(8)*SK_BETA(6) + OP(21,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(22) = SK_BETA(1)*(OP(22,6)*SK_BETA(2) - OP(22,3)*SK_BETA(5) - OP(22,4)*SK_BETA(3) + OP(22,5)*SK_BETA(4) + OP(22,23)*SK_BETA(3) - OP(22,24)*SK_BETA(4) + OP(22,1)*SH_BETA(8)*SK_BETA(6) + OP(22,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(23) = SK_BETA(1)*(OP(23,6)*SK_BETA(2) - OP(23,3)*SK_BETA(5) - OP(23,4)*SK_BETA(3) + OP(23,5)*SK_BETA(4) + OP(23,23)*SK_BETA(3) - OP(23,24)*SK_BETA(4) + OP(23,1)*SH_BETA(8)*SK_BETA(6) + OP(23,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(24) = SK_BETA(1)*(OP(24,6)*SK_BETA(2) - OP(24,3)*SK_BETA(5) - OP(24,4)*SK_BETA(3) + OP(24,5)*SK_BETA(4) + OP(24,23)*SK_BETA(3) - OP(24,24)*SK_BETA(4) + OP(24,1)*SH_BETA(8)*SK_BETA(6) + OP(24,2)*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); +Kfusion(1) = SK_BETA(1)*(OP(1,1)*SK_BETA(6) + OP(1,2)*SK_BETA(5) - OP(1,5)*SK_BETA(2) + OP(1,6)*SK_BETA(3) + OP(1,3)*SK_BETA(7) + OP(1,7)*SK_BETA(4) - OP(1,4)*SK_BETA(8) + OP(1,23)*SK_BETA(2) - OP(1,24)*SK_BETA(3)); +Kfusion(2) = SK_BETA(1)*(OP(2,1)*SK_BETA(6) + OP(2,2)*SK_BETA(5) - OP(2,5)*SK_BETA(2) + OP(2,6)*SK_BETA(3) + OP(2,3)*SK_BETA(7) + OP(2,7)*SK_BETA(4) - OP(2,4)*SK_BETA(8) + OP(2,23)*SK_BETA(2) - OP(2,24)*SK_BETA(3)); +Kfusion(3) = SK_BETA(1)*(OP(3,1)*SK_BETA(6) + OP(3,2)*SK_BETA(5) - OP(3,5)*SK_BETA(2) + OP(3,6)*SK_BETA(3) + OP(3,3)*SK_BETA(7) + OP(3,7)*SK_BETA(4) - OP(3,4)*SK_BETA(8) + OP(3,23)*SK_BETA(2) - OP(3,24)*SK_BETA(3)); +Kfusion(4) = SK_BETA(1)*(OP(4,1)*SK_BETA(6) + OP(4,2)*SK_BETA(5) - OP(4,5)*SK_BETA(2) + OP(4,6)*SK_BETA(3) + OP(4,3)*SK_BETA(7) + OP(4,7)*SK_BETA(4) - OP(4,4)*SK_BETA(8) + OP(4,23)*SK_BETA(2) - OP(4,24)*SK_BETA(3)); +Kfusion(5) = SK_BETA(1)*(OP(5,1)*SK_BETA(6) + OP(5,2)*SK_BETA(5) - OP(5,5)*SK_BETA(2) + OP(5,6)*SK_BETA(3) + OP(5,3)*SK_BETA(7) + OP(5,7)*SK_BETA(4) - OP(5,4)*SK_BETA(8) + OP(5,23)*SK_BETA(2) - OP(5,24)*SK_BETA(3)); +Kfusion(6) = SK_BETA(1)*(OP(6,1)*SK_BETA(6) + OP(6,2)*SK_BETA(5) - OP(6,5)*SK_BETA(2) + OP(6,6)*SK_BETA(3) + OP(6,3)*SK_BETA(7) + OP(6,7)*SK_BETA(4) - OP(6,4)*SK_BETA(8) + OP(6,23)*SK_BETA(2) - OP(6,24)*SK_BETA(3)); +Kfusion(7) = SK_BETA(1)*(OP(7,1)*SK_BETA(6) + OP(7,2)*SK_BETA(5) - OP(7,5)*SK_BETA(2) + OP(7,6)*SK_BETA(3) + OP(7,3)*SK_BETA(7) + OP(7,7)*SK_BETA(4) - OP(7,4)*SK_BETA(8) + OP(7,23)*SK_BETA(2) - OP(7,24)*SK_BETA(3)); +Kfusion(8) = SK_BETA(1)*(OP(8,1)*SK_BETA(6) + OP(8,2)*SK_BETA(5) - OP(8,5)*SK_BETA(2) + OP(8,6)*SK_BETA(3) + OP(8,3)*SK_BETA(7) + OP(8,7)*SK_BETA(4) - OP(8,4)*SK_BETA(8) + OP(8,23)*SK_BETA(2) - OP(8,24)*SK_BETA(3)); +Kfusion(9) = SK_BETA(1)*(OP(9,1)*SK_BETA(6) + OP(9,2)*SK_BETA(5) - OP(9,5)*SK_BETA(2) + OP(9,6)*SK_BETA(3) + OP(9,3)*SK_BETA(7) + OP(9,7)*SK_BETA(4) - OP(9,4)*SK_BETA(8) + OP(9,23)*SK_BETA(2) - OP(9,24)*SK_BETA(3)); +Kfusion(10) = SK_BETA(1)*(OP(10,1)*SK_BETA(6) + OP(10,2)*SK_BETA(5) - OP(10,5)*SK_BETA(2) + OP(10,6)*SK_BETA(3) + OP(10,3)*SK_BETA(7) + OP(10,7)*SK_BETA(4) - OP(10,4)*SK_BETA(8) + OP(10,23)*SK_BETA(2) - OP(10,24)*SK_BETA(3)); +Kfusion(11) = SK_BETA(1)*(OP(11,1)*SK_BETA(6) + OP(11,2)*SK_BETA(5) - OP(11,5)*SK_BETA(2) + OP(11,6)*SK_BETA(3) + OP(11,3)*SK_BETA(7) + OP(11,7)*SK_BETA(4) - OP(11,4)*SK_BETA(8) + OP(11,23)*SK_BETA(2) - OP(11,24)*SK_BETA(3)); +Kfusion(12) = SK_BETA(1)*(OP(12,1)*SK_BETA(6) + OP(12,2)*SK_BETA(5) - OP(12,5)*SK_BETA(2) + OP(12,6)*SK_BETA(3) + OP(12,3)*SK_BETA(7) + OP(12,7)*SK_BETA(4) - OP(12,4)*SK_BETA(8) + OP(12,23)*SK_BETA(2) - OP(12,24)*SK_BETA(3)); +Kfusion(13) = SK_BETA(1)*(OP(13,1)*SK_BETA(6) + OP(13,2)*SK_BETA(5) - OP(13,5)*SK_BETA(2) + OP(13,6)*SK_BETA(3) + OP(13,3)*SK_BETA(7) + OP(13,7)*SK_BETA(4) - OP(13,4)*SK_BETA(8) + OP(13,23)*SK_BETA(2) - OP(13,24)*SK_BETA(3)); +Kfusion(14) = SK_BETA(1)*(OP(14,1)*SK_BETA(6) + OP(14,2)*SK_BETA(5) - OP(14,5)*SK_BETA(2) + OP(14,6)*SK_BETA(3) + OP(14,3)*SK_BETA(7) + OP(14,7)*SK_BETA(4) - OP(14,4)*SK_BETA(8) + OP(14,23)*SK_BETA(2) - OP(14,24)*SK_BETA(3)); +Kfusion(15) = SK_BETA(1)*(OP(15,1)*SK_BETA(6) + OP(15,2)*SK_BETA(5) - OP(15,5)*SK_BETA(2) + OP(15,6)*SK_BETA(3) + OP(15,3)*SK_BETA(7) + OP(15,7)*SK_BETA(4) - OP(15,4)*SK_BETA(8) + OP(15,23)*SK_BETA(2) - OP(15,24)*SK_BETA(3)); +Kfusion(16) = SK_BETA(1)*(OP(16,1)*SK_BETA(6) + OP(16,2)*SK_BETA(5) - OP(16,5)*SK_BETA(2) + OP(16,6)*SK_BETA(3) + OP(16,3)*SK_BETA(7) + OP(16,7)*SK_BETA(4) - OP(16,4)*SK_BETA(8) + OP(16,23)*SK_BETA(2) - OP(16,24)*SK_BETA(3)); +Kfusion(17) = SK_BETA(1)*(OP(17,1)*SK_BETA(6) + OP(17,2)*SK_BETA(5) - OP(17,5)*SK_BETA(2) + OP(17,6)*SK_BETA(3) + OP(17,3)*SK_BETA(7) + OP(17,7)*SK_BETA(4) - OP(17,4)*SK_BETA(8) + OP(17,23)*SK_BETA(2) - OP(17,24)*SK_BETA(3)); +Kfusion(18) = SK_BETA(1)*(OP(18,1)*SK_BETA(6) + OP(18,2)*SK_BETA(5) - OP(18,5)*SK_BETA(2) + OP(18,6)*SK_BETA(3) + OP(18,3)*SK_BETA(7) + OP(18,7)*SK_BETA(4) - OP(18,4)*SK_BETA(8) + OP(18,23)*SK_BETA(2) - OP(18,24)*SK_BETA(3)); +Kfusion(19) = SK_BETA(1)*(OP(19,1)*SK_BETA(6) + OP(19,2)*SK_BETA(5) - OP(19,5)*SK_BETA(2) + OP(19,6)*SK_BETA(3) + OP(19,3)*SK_BETA(7) + OP(19,7)*SK_BETA(4) - OP(19,4)*SK_BETA(8) + OP(19,23)*SK_BETA(2) - OP(19,24)*SK_BETA(3)); +Kfusion(20) = SK_BETA(1)*(OP(20,1)*SK_BETA(6) + OP(20,2)*SK_BETA(5) - OP(20,5)*SK_BETA(2) + OP(20,6)*SK_BETA(3) + OP(20,3)*SK_BETA(7) + OP(20,7)*SK_BETA(4) - OP(20,4)*SK_BETA(8) + OP(20,23)*SK_BETA(2) - OP(20,24)*SK_BETA(3)); +Kfusion(21) = SK_BETA(1)*(OP(21,1)*SK_BETA(6) + OP(21,2)*SK_BETA(5) - OP(21,5)*SK_BETA(2) + OP(21,6)*SK_BETA(3) + OP(21,3)*SK_BETA(7) + OP(21,7)*SK_BETA(4) - OP(21,4)*SK_BETA(8) + OP(21,23)*SK_BETA(2) - OP(21,24)*SK_BETA(3)); +Kfusion(22) = SK_BETA(1)*(OP(22,1)*SK_BETA(6) + OP(22,2)*SK_BETA(5) - OP(22,5)*SK_BETA(2) + OP(22,6)*SK_BETA(3) + OP(22,3)*SK_BETA(7) + OP(22,7)*SK_BETA(4) - OP(22,4)*SK_BETA(8) + OP(22,23)*SK_BETA(2) - OP(22,24)*SK_BETA(3)); +Kfusion(23) = SK_BETA(1)*(OP(23,1)*SK_BETA(6) + OP(23,2)*SK_BETA(5) - OP(23,5)*SK_BETA(2) + OP(23,6)*SK_BETA(3) + OP(23,3)*SK_BETA(7) + OP(23,7)*SK_BETA(4) - OP(23,4)*SK_BETA(8) + OP(23,23)*SK_BETA(2) - OP(23,24)*SK_BETA(3)); +Kfusion(24) = SK_BETA(1)*(OP(24,1)*SK_BETA(6) + OP(24,2)*SK_BETA(5) - OP(24,5)*SK_BETA(2) + OP(24,6)*SK_BETA(3) + OP(24,3)*SK_BETA(7) + OP(24,7)*SK_BETA(4) - OP(24,4)*SK_BETA(8) + OP(24,23)*SK_BETA(2) - OP(24,24)*SK_BETA(3)); SH_MAG = zeros(9,1); -SH_MAG(1) = q0^2 - q1^2 + q2^2 - q3^2; -SH_MAG(2) = q0^2 + q1^2 - q2^2 - q3^2; -SH_MAG(3) = q0^2 - q1^2 - q2^2 + q3^2; -SH_MAG(4) = 2*q0*q1 + 2*q2*q3; -SH_MAG(5) = 2*q0*q3 + 2*q1*q2; -SH_MAG(6) = 2*q0*q2 + 2*q1*q3; -SH_MAG(7) = magE*(2*q0*q1 - 2*q2*q3); -SH_MAG(8) = 2*q1*q3 - 2*q0*q2; -SH_MAG(9) = 2*q0*q3; +SH_MAG(1) = 2*magD*q3 + 2*magE*q2 + 2*magN*q1; +SH_MAG(2) = 2*magD*q0 - 2*magE*q1 + 2*magN*q2; +SH_MAG(3) = 2*magD*q1 + 2*magE*q0 - 2*magN*q3; +SH_MAG(4) = q3^2; +SH_MAG(5) = q2^2; +SH_MAG(6) = q1^2; +SH_MAG(7) = q0^2; +SH_MAG(8) = 2*magN*q0; +SH_MAG(9) = 2*magE*q3; H_MAG = zeros(1,24); -H_MAG(2) = SH_MAG(7) - magD*SH_MAG(3) - magN*SH_MAG(6); -H_MAG(3) = magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2); -H_MAG(17) = SH_MAG(2); -H_MAG(18) = SH_MAG(5); -H_MAG(19) = SH_MAG(8); +H_MAG(1) = SH_MAG(8) + SH_MAG(9) - 2*magD*q2; +H_MAG(2) = SH_MAG(1); +H_MAG(3) = -SH_MAG(2); +H_MAG(4) = SH_MAG(3); +H_MAG(17) = SH_MAG(6) - SH_MAG(5) - SH_MAG(4) + SH_MAG(7); +H_MAG(18) = 2*q0*q3 + 2*q1*q2; +H_MAG(19) = 2*q1*q3 - 2*q0*q2; H_MAG(20) = 1; -SK_MX = zeros(4,1); -SK_MX(1) = 1/(OP(20,20) + R_MAG - OP(2,20)*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP(17,20)*SH_MAG(2) + OP(18,20)*SH_MAG(5) + OP(19,20)*SH_MAG(8) + OP(3,20)*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2)) - (magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6))*(OP(20,2) - OP(2,2)*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP(17,2)*SH_MAG(2) + OP(18,2)*SH_MAG(5) + OP(19,2)*SH_MAG(8) + OP(3,2)*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2))) + SH_MAG(2)*(OP(20,17) - OP(2,17)*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP(17,17)*SH_MAG(2) + OP(18,17)*SH_MAG(5) + OP(19,17)*SH_MAG(8) + OP(3,17)*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2))) + SH_MAG(5)*(OP(20,18) - OP(2,18)*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP(17,18)*SH_MAG(2) + OP(18,18)*SH_MAG(5) + OP(19,18)*SH_MAG(8) + OP(3,18)*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2))) + SH_MAG(8)*(OP(20,19) - OP(2,19)*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP(17,19)*SH_MAG(2) + OP(18,19)*SH_MAG(5) + OP(19,19)*SH_MAG(8) + OP(3,19)*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2))) + (magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2))*(OP(20,3) - OP(2,3)*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP(17,3)*SH_MAG(2) + OP(18,3)*SH_MAG(5) + OP(19,3)*SH_MAG(8) + OP(3,3)*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2)))); -SK_MX(2) = magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2); -SK_MX(3) = magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6); -SK_MX(4) = SH_MAG(8); +SK_MX = zeros(5,1); +SK_MX(1) = 1/(OP(20,20) + R_MAG + OP(2,20)*SH_MAG(1) - OP(3,20)*SH_MAG(2) + OP(4,20)*SH_MAG(3) - OP(17,20)*(SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7)) + (2*q0*q3 + 2*q1*q2)*(OP(20,18) + OP(2,18)*SH_MAG(1) - OP(3,18)*SH_MAG(2) + OP(4,18)*SH_MAG(3) - OP(17,18)*(SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7)) + OP(18,18)*(2*q0*q3 + 2*q1*q2) - OP(19,18)*(2*q0*q2 - 2*q1*q3) + OP(1,18)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) - (2*q0*q2 - 2*q1*q3)*(OP(20,19) + OP(2,19)*SH_MAG(1) - OP(3,19)*SH_MAG(2) + OP(4,19)*SH_MAG(3) - OP(17,19)*(SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7)) + OP(18,19)*(2*q0*q3 + 2*q1*q2) - OP(19,19)*(2*q0*q2 - 2*q1*q3) + OP(1,19)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + (SH_MAG(8) + SH_MAG(9) - 2*magD*q2)*(OP(20,1) + OP(2,1)*SH_MAG(1) - OP(3,1)*SH_MAG(2) + OP(4,1)*SH_MAG(3) - OP(17,1)*(SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7)) + OP(18,1)*(2*q0*q3 + 2*q1*q2) - OP(19,1)*(2*q0*q2 - 2*q1*q3) + OP(1,1)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + OP(18,20)*(2*q0*q3 + 2*q1*q2) - OP(19,20)*(2*q0*q2 - 2*q1*q3) + SH_MAG(1)*(OP(20,2) + OP(2,2)*SH_MAG(1) - OP(3,2)*SH_MAG(2) + OP(4,2)*SH_MAG(3) - OP(17,2)*(SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7)) + OP(18,2)*(2*q0*q3 + 2*q1*q2) - OP(19,2)*(2*q0*q2 - 2*q1*q3) + OP(1,2)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) - SH_MAG(2)*(OP(20,3) + OP(2,3)*SH_MAG(1) - OP(3,3)*SH_MAG(2) + OP(4,3)*SH_MAG(3) - OP(17,3)*(SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7)) + OP(18,3)*(2*q0*q3 + 2*q1*q2) - OP(19,3)*(2*q0*q2 - 2*q1*q3) + OP(1,3)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + SH_MAG(3)*(OP(20,4) + OP(2,4)*SH_MAG(1) - OP(3,4)*SH_MAG(2) + OP(4,4)*SH_MAG(3) - OP(17,4)*(SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7)) + OP(18,4)*(2*q0*q3 + 2*q1*q2) - OP(19,4)*(2*q0*q2 - 2*q1*q3) + OP(1,4)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) - (SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7))*(OP(20,17) + OP(2,17)*SH_MAG(1) - OP(3,17)*SH_MAG(2) + OP(4,17)*SH_MAG(3) - OP(17,17)*(SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7)) + OP(18,17)*(2*q0*q3 + 2*q1*q2) - OP(19,17)*(2*q0*q2 - 2*q1*q3) + OP(1,17)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + OP(1,20)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)); +SK_MX(2) = SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7); +SK_MX(3) = SH_MAG(8) + SH_MAG(9) - 2*magD*q2; +SK_MX(4) = 2*q0*q2 - 2*q1*q3; +SK_MX(5) = 2*q0*q3 + 2*q1*q2; Kfusion = zeros(24,1); Kfusion = zeros(1,1); -Kfusion(1) = SK_MX(1)*(OP(1,20) + OP(1,17)*SH_MAG(2) + OP(1,18)*SH_MAG(5) - OP(1,2)*SK_MX(3) + OP(1,3)*SK_MX(2) + OP(1,19)*SK_MX(4)); -Kfusion(2) = SK_MX(1)*(OP(2,20) + OP(2,17)*SH_MAG(2) + OP(2,18)*SH_MAG(5) - OP(2,2)*SK_MX(3) + OP(2,3)*SK_MX(2) + OP(2,19)*SK_MX(4)); -Kfusion(3) = SK_MX(1)*(OP(3,20) + OP(3,17)*SH_MAG(2) + OP(3,18)*SH_MAG(5) - OP(3,2)*SK_MX(3) + OP(3,3)*SK_MX(2) + OP(3,19)*SK_MX(4)); -Kfusion(4) = SK_MX(1)*(OP(4,20) + OP(4,17)*SH_MAG(2) + OP(4,18)*SH_MAG(5) - OP(4,2)*SK_MX(3) + OP(4,3)*SK_MX(2) + OP(4,19)*SK_MX(4)); -Kfusion(5) = SK_MX(1)*(OP(5,20) + OP(5,17)*SH_MAG(2) + OP(5,18)*SH_MAG(5) - OP(5,2)*SK_MX(3) + OP(5,3)*SK_MX(2) + OP(5,19)*SK_MX(4)); -Kfusion(6) = SK_MX(1)*(OP(6,20) + OP(6,17)*SH_MAG(2) + OP(6,18)*SH_MAG(5) - OP(6,2)*SK_MX(3) + OP(6,3)*SK_MX(2) + OP(6,19)*SK_MX(4)); -Kfusion(7) = SK_MX(1)*(OP(7,20) + OP(7,17)*SH_MAG(2) + OP(7,18)*SH_MAG(5) - OP(7,2)*SK_MX(3) + OP(7,3)*SK_MX(2) + OP(7,19)*SK_MX(4)); -Kfusion(8) = SK_MX(1)*(OP(8,20) + OP(8,17)*SH_MAG(2) + OP(8,18)*SH_MAG(5) - OP(8,2)*SK_MX(3) + OP(8,3)*SK_MX(2) + OP(8,19)*SK_MX(4)); -Kfusion(9) = SK_MX(1)*(OP(9,20) + OP(9,17)*SH_MAG(2) + OP(9,18)*SH_MAG(5) - OP(9,2)*SK_MX(3) + OP(9,3)*SK_MX(2) + OP(9,19)*SK_MX(4)); -Kfusion(10) = SK_MX(1)*(OP(10,20) + OP(10,17)*SH_MAG(2) + OP(10,18)*SH_MAG(5) - OP(10,2)*SK_MX(3) + OP(10,3)*SK_MX(2) + OP(10,19)*SK_MX(4)); -Kfusion(11) = SK_MX(1)*(OP(11,20) + OP(11,17)*SH_MAG(2) + OP(11,18)*SH_MAG(5) - OP(11,2)*SK_MX(3) + OP(11,3)*SK_MX(2) + OP(11,19)*SK_MX(4)); -Kfusion(12) = SK_MX(1)*(OP(12,20) + OP(12,17)*SH_MAG(2) + OP(12,18)*SH_MAG(5) - OP(12,2)*SK_MX(3) + OP(12,3)*SK_MX(2) + OP(12,19)*SK_MX(4)); -Kfusion(13) = SK_MX(1)*(OP(13,20) + OP(13,17)*SH_MAG(2) + OP(13,18)*SH_MAG(5) - OP(13,2)*SK_MX(3) + OP(13,3)*SK_MX(2) + OP(13,19)*SK_MX(4)); -Kfusion(14) = SK_MX(1)*(OP(14,20) + OP(14,17)*SH_MAG(2) + OP(14,18)*SH_MAG(5) - OP(14,2)*SK_MX(3) + OP(14,3)*SK_MX(2) + OP(14,19)*SK_MX(4)); -Kfusion(15) = SK_MX(1)*(OP(15,20) + OP(15,17)*SH_MAG(2) + OP(15,18)*SH_MAG(5) - OP(15,2)*SK_MX(3) + OP(15,3)*SK_MX(2) + OP(15,19)*SK_MX(4)); -Kfusion(16) = SK_MX(1)*(OP(16,20) + OP(16,17)*SH_MAG(2) + OP(16,18)*SH_MAG(5) - OP(16,2)*SK_MX(3) + OP(16,3)*SK_MX(2) + OP(16,19)*SK_MX(4)); -Kfusion(17) = SK_MX(1)*(OP(17,20) + OP(17,17)*SH_MAG(2) + OP(17,18)*SH_MAG(5) - OP(17,2)*SK_MX(3) + OP(17,3)*SK_MX(2) + OP(17,19)*SK_MX(4)); -Kfusion(18) = SK_MX(1)*(OP(18,20) + OP(18,17)*SH_MAG(2) + OP(18,18)*SH_MAG(5) - OP(18,2)*SK_MX(3) + OP(18,3)*SK_MX(2) + OP(18,19)*SK_MX(4)); -Kfusion(19) = SK_MX(1)*(OP(19,20) + OP(19,17)*SH_MAG(2) + OP(19,18)*SH_MAG(5) - OP(19,2)*SK_MX(3) + OP(19,3)*SK_MX(2) + OP(19,19)*SK_MX(4)); -Kfusion(20) = SK_MX(1)*(OP(20,20) + OP(20,17)*SH_MAG(2) + OP(20,18)*SH_MAG(5) - OP(20,2)*SK_MX(3) + OP(20,3)*SK_MX(2) + OP(20,19)*SK_MX(4)); -Kfusion(21) = SK_MX(1)*(OP(21,20) + OP(21,17)*SH_MAG(2) + OP(21,18)*SH_MAG(5) - OP(21,2)*SK_MX(3) + OP(21,3)*SK_MX(2) + OP(21,19)*SK_MX(4)); -Kfusion(22) = SK_MX(1)*(OP(22,20) + OP(22,17)*SH_MAG(2) + OP(22,18)*SH_MAG(5) - OP(22,2)*SK_MX(3) + OP(22,3)*SK_MX(2) + OP(22,19)*SK_MX(4)); -Kfusion(23) = SK_MX(1)*(OP(23,20) + OP(23,17)*SH_MAG(2) + OP(23,18)*SH_MAG(5) - OP(23,2)*SK_MX(3) + OP(23,3)*SK_MX(2) + OP(23,19)*SK_MX(4)); -Kfusion(24) = SK_MX(1)*(OP(24,20) + OP(24,17)*SH_MAG(2) + OP(24,18)*SH_MAG(5) - OP(24,2)*SK_MX(3) + OP(24,3)*SK_MX(2) + OP(24,19)*SK_MX(4)); +Kfusion(1) = SK_MX(1)*(OP(1,20) + OP(1,2)*SH_MAG(1) - OP(1,3)*SH_MAG(2) + OP(1,4)*SH_MAG(3) + OP(1,1)*SK_MX(3) - OP(1,17)*SK_MX(2) + OP(1,18)*SK_MX(5) - OP(1,19)*SK_MX(4)); +Kfusion(2) = SK_MX(1)*(OP(2,20) + OP(2,2)*SH_MAG(1) - OP(2,3)*SH_MAG(2) + OP(2,4)*SH_MAG(3) + OP(2,1)*SK_MX(3) - OP(2,17)*SK_MX(2) + OP(2,18)*SK_MX(5) - OP(2,19)*SK_MX(4)); +Kfusion(3) = SK_MX(1)*(OP(3,20) + OP(3,2)*SH_MAG(1) - OP(3,3)*SH_MAG(2) + OP(3,4)*SH_MAG(3) + OP(3,1)*SK_MX(3) - OP(3,17)*SK_MX(2) + OP(3,18)*SK_MX(5) - OP(3,19)*SK_MX(4)); +Kfusion(4) = SK_MX(1)*(OP(4,20) + OP(4,2)*SH_MAG(1) - OP(4,3)*SH_MAG(2) + OP(4,4)*SH_MAG(3) + OP(4,1)*SK_MX(3) - OP(4,17)*SK_MX(2) + OP(4,18)*SK_MX(5) - OP(4,19)*SK_MX(4)); +Kfusion(5) = SK_MX(1)*(OP(5,20) + OP(5,2)*SH_MAG(1) - OP(5,3)*SH_MAG(2) + OP(5,4)*SH_MAG(3) + OP(5,1)*SK_MX(3) - OP(5,17)*SK_MX(2) + OP(5,18)*SK_MX(5) - OP(5,19)*SK_MX(4)); +Kfusion(6) = SK_MX(1)*(OP(6,20) + OP(6,2)*SH_MAG(1) - OP(6,3)*SH_MAG(2) + OP(6,4)*SH_MAG(3) + OP(6,1)*SK_MX(3) - OP(6,17)*SK_MX(2) + OP(6,18)*SK_MX(5) - OP(6,19)*SK_MX(4)); +Kfusion(7) = SK_MX(1)*(OP(7,20) + OP(7,2)*SH_MAG(1) - OP(7,3)*SH_MAG(2) + OP(7,4)*SH_MAG(3) + OP(7,1)*SK_MX(3) - OP(7,17)*SK_MX(2) + OP(7,18)*SK_MX(5) - OP(7,19)*SK_MX(4)); +Kfusion(8) = SK_MX(1)*(OP(8,20) + OP(8,2)*SH_MAG(1) - OP(8,3)*SH_MAG(2) + OP(8,4)*SH_MAG(3) + OP(8,1)*SK_MX(3) - OP(8,17)*SK_MX(2) + OP(8,18)*SK_MX(5) - OP(8,19)*SK_MX(4)); +Kfusion(9) = SK_MX(1)*(OP(9,20) + OP(9,2)*SH_MAG(1) - OP(9,3)*SH_MAG(2) + OP(9,4)*SH_MAG(3) + OP(9,1)*SK_MX(3) - OP(9,17)*SK_MX(2) + OP(9,18)*SK_MX(5) - OP(9,19)*SK_MX(4)); +Kfusion(10) = SK_MX(1)*(OP(10,20) + OP(10,2)*SH_MAG(1) - OP(10,3)*SH_MAG(2) + OP(10,4)*SH_MAG(3) + OP(10,1)*SK_MX(3) - OP(10,17)*SK_MX(2) + OP(10,18)*SK_MX(5) - OP(10,19)*SK_MX(4)); +Kfusion(11) = SK_MX(1)*(OP(11,20) + OP(11,2)*SH_MAG(1) - OP(11,3)*SH_MAG(2) + OP(11,4)*SH_MAG(3) + OP(11,1)*SK_MX(3) - OP(11,17)*SK_MX(2) + OP(11,18)*SK_MX(5) - OP(11,19)*SK_MX(4)); +Kfusion(12) = SK_MX(1)*(OP(12,20) + OP(12,2)*SH_MAG(1) - OP(12,3)*SH_MAG(2) + OP(12,4)*SH_MAG(3) + OP(12,1)*SK_MX(3) - OP(12,17)*SK_MX(2) + OP(12,18)*SK_MX(5) - OP(12,19)*SK_MX(4)); +Kfusion(13) = SK_MX(1)*(OP(13,20) + OP(13,2)*SH_MAG(1) - OP(13,3)*SH_MAG(2) + OP(13,4)*SH_MAG(3) + OP(13,1)*SK_MX(3) - OP(13,17)*SK_MX(2) + OP(13,18)*SK_MX(5) - OP(13,19)*SK_MX(4)); +Kfusion(14) = SK_MX(1)*(OP(14,20) + OP(14,2)*SH_MAG(1) - OP(14,3)*SH_MAG(2) + OP(14,4)*SH_MAG(3) + OP(14,1)*SK_MX(3) - OP(14,17)*SK_MX(2) + OP(14,18)*SK_MX(5) - OP(14,19)*SK_MX(4)); +Kfusion(15) = SK_MX(1)*(OP(15,20) + OP(15,2)*SH_MAG(1) - OP(15,3)*SH_MAG(2) + OP(15,4)*SH_MAG(3) + OP(15,1)*SK_MX(3) - OP(15,17)*SK_MX(2) + OP(15,18)*SK_MX(5) - OP(15,19)*SK_MX(4)); +Kfusion(16) = SK_MX(1)*(OP(16,20) + OP(16,2)*SH_MAG(1) - OP(16,3)*SH_MAG(2) + OP(16,4)*SH_MAG(3) + OP(16,1)*SK_MX(3) - OP(16,17)*SK_MX(2) + OP(16,18)*SK_MX(5) - OP(16,19)*SK_MX(4)); +Kfusion(17) = SK_MX(1)*(OP(17,20) + OP(17,2)*SH_MAG(1) - OP(17,3)*SH_MAG(2) + OP(17,4)*SH_MAG(3) + OP(17,1)*SK_MX(3) - OP(17,17)*SK_MX(2) + OP(17,18)*SK_MX(5) - OP(17,19)*SK_MX(4)); +Kfusion(18) = SK_MX(1)*(OP(18,20) + OP(18,2)*SH_MAG(1) - OP(18,3)*SH_MAG(2) + OP(18,4)*SH_MAG(3) + OP(18,1)*SK_MX(3) - OP(18,17)*SK_MX(2) + OP(18,18)*SK_MX(5) - OP(18,19)*SK_MX(4)); +Kfusion(19) = SK_MX(1)*(OP(19,20) + OP(19,2)*SH_MAG(1) - OP(19,3)*SH_MAG(2) + OP(19,4)*SH_MAG(3) + OP(19,1)*SK_MX(3) - OP(19,17)*SK_MX(2) + OP(19,18)*SK_MX(5) - OP(19,19)*SK_MX(4)); +Kfusion(20) = SK_MX(1)*(OP(20,20) + OP(20,2)*SH_MAG(1) - OP(20,3)*SH_MAG(2) + OP(20,4)*SH_MAG(3) + OP(20,1)*SK_MX(3) - OP(20,17)*SK_MX(2) + OP(20,18)*SK_MX(5) - OP(20,19)*SK_MX(4)); +Kfusion(21) = SK_MX(1)*(OP(21,20) + OP(21,2)*SH_MAG(1) - OP(21,3)*SH_MAG(2) + OP(21,4)*SH_MAG(3) + OP(21,1)*SK_MX(3) - OP(21,17)*SK_MX(2) + OP(21,18)*SK_MX(5) - OP(21,19)*SK_MX(4)); +Kfusion(22) = SK_MX(1)*(OP(22,20) + OP(22,2)*SH_MAG(1) - OP(22,3)*SH_MAG(2) + OP(22,4)*SH_MAG(3) + OP(22,1)*SK_MX(3) - OP(22,17)*SK_MX(2) + OP(22,18)*SK_MX(5) - OP(22,19)*SK_MX(4)); +Kfusion(23) = SK_MX(1)*(OP(23,20) + OP(23,2)*SH_MAG(1) - OP(23,3)*SH_MAG(2) + OP(23,4)*SH_MAG(3) + OP(23,1)*SK_MX(3) - OP(23,17)*SK_MX(2) + OP(23,18)*SK_MX(5) - OP(23,19)*SK_MX(4)); +Kfusion(24) = SK_MX(1)*(OP(24,20) + OP(24,2)*SH_MAG(1) - OP(24,3)*SH_MAG(2) + OP(24,4)*SH_MAG(3) + OP(24,1)*SK_MX(3) - OP(24,17)*SK_MX(2) + OP(24,18)*SK_MX(5) - OP(24,19)*SK_MX(4)); H_MAG = zeros(1,24); -H_MAG(1) = magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6); -H_MAG(3) = - magE*SH_MAG(5) - magD*SH_MAG(8) - magN*SH_MAG(2); -H_MAG(17) = 2*q1*q2 - SH_MAG(9); -H_MAG(18) = SH_MAG(1); -H_MAG(19) = SH_MAG(4); +H_MAG(1) = SH_MAG(3); +H_MAG(2) = SH_MAG(2); +H_MAG(3) = SH_MAG(1); +H_MAG(4) = 2*magD*q2 - SH_MAG(9) - SH_MAG(8); +H_MAG(17) = 2*q1*q2 - 2*q0*q3; +H_MAG(18) = SH_MAG(5) - SH_MAG(4) - SH_MAG(6) + SH_MAG(7); +H_MAG(19) = 2*q0*q1 + 2*q2*q3; H_MAG(21) = 1; -SK_MY = zeros(4,1); -SK_MY(1) = 1/(OP(21,21) + R_MAG + OP(1,21)*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP(18,21)*SH_MAG(1) + OP(19,21)*SH_MAG(4) - (SH_MAG(9) - 2*q1*q2)*(OP(21,17) + OP(1,17)*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP(18,17)*SH_MAG(1) + OP(19,17)*SH_MAG(4) - OP(3,17)*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) - OP(17,17)*(SH_MAG(9) - 2*q1*q2)) - OP(3,21)*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) + (magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6))*(OP(21,1) + OP(1,1)*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP(18,1)*SH_MAG(1) + OP(19,1)*SH_MAG(4) - OP(3,1)*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) - OP(17,1)*(SH_MAG(9) - 2*q1*q2)) + SH_MAG(1)*(OP(21,18) + OP(1,18)*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP(18,18)*SH_MAG(1) + OP(19,18)*SH_MAG(4) - OP(3,18)*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) - OP(17,18)*(SH_MAG(9) - 2*q1*q2)) + SH_MAG(4)*(OP(21,19) + OP(1,19)*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP(18,19)*SH_MAG(1) + OP(19,19)*SH_MAG(4) - OP(3,19)*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) - OP(17,19)*(SH_MAG(9) - 2*q1*q2)) - OP(17,21)*(SH_MAG(9) - 2*q1*q2) - (magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2))*(OP(21,3) + OP(1,3)*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP(18,3)*SH_MAG(1) + OP(19,3)*SH_MAG(4) - OP(3,3)*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) - OP(17,3)*(SH_MAG(9) - 2*q1*q2))); -SK_MY(2) = magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2); -SK_MY(3) = magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6); -SK_MY(4) = SH_MAG(9) - 2*q1*q2; +SK_MY = zeros(5,1); +SK_MY(1) = 1/(OP(21,21) + R_MAG + OP(1,21)*SH_MAG(3) + OP(2,21)*SH_MAG(2) + OP(3,21)*SH_MAG(1) - OP(18,21)*(SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7)) - (2*q0*q3 - 2*q1*q2)*(OP(21,17) + OP(1,17)*SH_MAG(3) + OP(2,17)*SH_MAG(2) + OP(3,17)*SH_MAG(1) - OP(18,17)*(SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7)) - OP(17,17)*(2*q0*q3 - 2*q1*q2) + OP(19,17)*(2*q0*q1 + 2*q2*q3) - OP(4,17)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + (2*q0*q1 + 2*q2*q3)*(OP(21,19) + OP(1,19)*SH_MAG(3) + OP(2,19)*SH_MAG(2) + OP(3,19)*SH_MAG(1) - OP(18,19)*(SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7)) - OP(17,19)*(2*q0*q3 - 2*q1*q2) + OP(19,19)*(2*q0*q1 + 2*q2*q3) - OP(4,19)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) - (SH_MAG(8) + SH_MAG(9) - 2*magD*q2)*(OP(21,4) + OP(1,4)*SH_MAG(3) + OP(2,4)*SH_MAG(2) + OP(3,4)*SH_MAG(1) - OP(18,4)*(SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7)) - OP(17,4)*(2*q0*q3 - 2*q1*q2) + OP(19,4)*(2*q0*q1 + 2*q2*q3) - OP(4,4)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) - OP(17,21)*(2*q0*q3 - 2*q1*q2) + OP(19,21)*(2*q0*q1 + 2*q2*q3) + SH_MAG(3)*(OP(21,1) + OP(1,1)*SH_MAG(3) + OP(2,1)*SH_MAG(2) + OP(3,1)*SH_MAG(1) - OP(18,1)*(SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7)) - OP(17,1)*(2*q0*q3 - 2*q1*q2) + OP(19,1)*(2*q0*q1 + 2*q2*q3) - OP(4,1)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + SH_MAG(2)*(OP(21,2) + OP(1,2)*SH_MAG(3) + OP(2,2)*SH_MAG(2) + OP(3,2)*SH_MAG(1) - OP(18,2)*(SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7)) - OP(17,2)*(2*q0*q3 - 2*q1*q2) + OP(19,2)*(2*q0*q1 + 2*q2*q3) - OP(4,2)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + SH_MAG(1)*(OP(21,3) + OP(1,3)*SH_MAG(3) + OP(2,3)*SH_MAG(2) + OP(3,3)*SH_MAG(1) - OP(18,3)*(SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7)) - OP(17,3)*(2*q0*q3 - 2*q1*q2) + OP(19,3)*(2*q0*q1 + 2*q2*q3) - OP(4,3)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) - (SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7))*(OP(21,18) + OP(1,18)*SH_MAG(3) + OP(2,18)*SH_MAG(2) + OP(3,18)*SH_MAG(1) - OP(18,18)*(SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7)) - OP(17,18)*(2*q0*q3 - 2*q1*q2) + OP(19,18)*(2*q0*q1 + 2*q2*q3) - OP(4,18)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) - OP(4,21)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)); +SK_MY(2) = SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7); +SK_MY(3) = SH_MAG(8) + SH_MAG(9) - 2*magD*q2; +SK_MY(4) = 2*q0*q3 - 2*q1*q2; +SK_MY(5) = 2*q0*q1 + 2*q2*q3; Kfusion = zeros(24,1); Kfusion = zeros(1,1); -Kfusion(1) = SK_MY(1)*(OP(1,21) + OP(1,18)*SH_MAG(1) + OP(1,19)*SH_MAG(4) + OP(1,1)*SK_MY(3) - OP(1,3)*SK_MY(2) - OP(1,17)*SK_MY(4)); -Kfusion(2) = SK_MY(1)*(OP(2,21) + OP(2,18)*SH_MAG(1) + OP(2,19)*SH_MAG(4) + OP(2,1)*SK_MY(3) - OP(2,3)*SK_MY(2) - OP(2,17)*SK_MY(4)); -Kfusion(3) = SK_MY(1)*(OP(3,21) + OP(3,18)*SH_MAG(1) + OP(3,19)*SH_MAG(4) + OP(3,1)*SK_MY(3) - OP(3,3)*SK_MY(2) - OP(3,17)*SK_MY(4)); -Kfusion(4) = SK_MY(1)*(OP(4,21) + OP(4,18)*SH_MAG(1) + OP(4,19)*SH_MAG(4) + OP(4,1)*SK_MY(3) - OP(4,3)*SK_MY(2) - OP(4,17)*SK_MY(4)); -Kfusion(5) = SK_MY(1)*(OP(5,21) + OP(5,18)*SH_MAG(1) + OP(5,19)*SH_MAG(4) + OP(5,1)*SK_MY(3) - OP(5,3)*SK_MY(2) - OP(5,17)*SK_MY(4)); -Kfusion(6) = SK_MY(1)*(OP(6,21) + OP(6,18)*SH_MAG(1) + OP(6,19)*SH_MAG(4) + OP(6,1)*SK_MY(3) - OP(6,3)*SK_MY(2) - OP(6,17)*SK_MY(4)); -Kfusion(7) = SK_MY(1)*(OP(7,21) + OP(7,18)*SH_MAG(1) + OP(7,19)*SH_MAG(4) + OP(7,1)*SK_MY(3) - OP(7,3)*SK_MY(2) - OP(7,17)*SK_MY(4)); -Kfusion(8) = SK_MY(1)*(OP(8,21) + OP(8,18)*SH_MAG(1) + OP(8,19)*SH_MAG(4) + OP(8,1)*SK_MY(3) - OP(8,3)*SK_MY(2) - OP(8,17)*SK_MY(4)); -Kfusion(9) = SK_MY(1)*(OP(9,21) + OP(9,18)*SH_MAG(1) + OP(9,19)*SH_MAG(4) + OP(9,1)*SK_MY(3) - OP(9,3)*SK_MY(2) - OP(9,17)*SK_MY(4)); -Kfusion(10) = SK_MY(1)*(OP(10,21) + OP(10,18)*SH_MAG(1) + OP(10,19)*SH_MAG(4) + OP(10,1)*SK_MY(3) - OP(10,3)*SK_MY(2) - OP(10,17)*SK_MY(4)); -Kfusion(11) = SK_MY(1)*(OP(11,21) + OP(11,18)*SH_MAG(1) + OP(11,19)*SH_MAG(4) + OP(11,1)*SK_MY(3) - OP(11,3)*SK_MY(2) - OP(11,17)*SK_MY(4)); -Kfusion(12) = SK_MY(1)*(OP(12,21) + OP(12,18)*SH_MAG(1) + OP(12,19)*SH_MAG(4) + OP(12,1)*SK_MY(3) - OP(12,3)*SK_MY(2) - OP(12,17)*SK_MY(4)); -Kfusion(13) = SK_MY(1)*(OP(13,21) + OP(13,18)*SH_MAG(1) + OP(13,19)*SH_MAG(4) + OP(13,1)*SK_MY(3) - OP(13,3)*SK_MY(2) - OP(13,17)*SK_MY(4)); -Kfusion(14) = SK_MY(1)*(OP(14,21) + OP(14,18)*SH_MAG(1) + OP(14,19)*SH_MAG(4) + OP(14,1)*SK_MY(3) - OP(14,3)*SK_MY(2) - OP(14,17)*SK_MY(4)); -Kfusion(15) = SK_MY(1)*(OP(15,21) + OP(15,18)*SH_MAG(1) + OP(15,19)*SH_MAG(4) + OP(15,1)*SK_MY(3) - OP(15,3)*SK_MY(2) - OP(15,17)*SK_MY(4)); -Kfusion(16) = SK_MY(1)*(OP(16,21) + OP(16,18)*SH_MAG(1) + OP(16,19)*SH_MAG(4) + OP(16,1)*SK_MY(3) - OP(16,3)*SK_MY(2) - OP(16,17)*SK_MY(4)); -Kfusion(17) = SK_MY(1)*(OP(17,21) + OP(17,18)*SH_MAG(1) + OP(17,19)*SH_MAG(4) + OP(17,1)*SK_MY(3) - OP(17,3)*SK_MY(2) - OP(17,17)*SK_MY(4)); -Kfusion(18) = SK_MY(1)*(OP(18,21) + OP(18,18)*SH_MAG(1) + OP(18,19)*SH_MAG(4) + OP(18,1)*SK_MY(3) - OP(18,3)*SK_MY(2) - OP(18,17)*SK_MY(4)); -Kfusion(19) = SK_MY(1)*(OP(19,21) + OP(19,18)*SH_MAG(1) + OP(19,19)*SH_MAG(4) + OP(19,1)*SK_MY(3) - OP(19,3)*SK_MY(2) - OP(19,17)*SK_MY(4)); -Kfusion(20) = SK_MY(1)*(OP(20,21) + OP(20,18)*SH_MAG(1) + OP(20,19)*SH_MAG(4) + OP(20,1)*SK_MY(3) - OP(20,3)*SK_MY(2) - OP(20,17)*SK_MY(4)); -Kfusion(21) = SK_MY(1)*(OP(21,21) + OP(21,18)*SH_MAG(1) + OP(21,19)*SH_MAG(4) + OP(21,1)*SK_MY(3) - OP(21,3)*SK_MY(2) - OP(21,17)*SK_MY(4)); -Kfusion(22) = SK_MY(1)*(OP(22,21) + OP(22,18)*SH_MAG(1) + OP(22,19)*SH_MAG(4) + OP(22,1)*SK_MY(3) - OP(22,3)*SK_MY(2) - OP(22,17)*SK_MY(4)); -Kfusion(23) = SK_MY(1)*(OP(23,21) + OP(23,18)*SH_MAG(1) + OP(23,19)*SH_MAG(4) + OP(23,1)*SK_MY(3) - OP(23,3)*SK_MY(2) - OP(23,17)*SK_MY(4)); -Kfusion(24) = SK_MY(1)*(OP(24,21) + OP(24,18)*SH_MAG(1) + OP(24,19)*SH_MAG(4) + OP(24,1)*SK_MY(3) - OP(24,3)*SK_MY(2) - OP(24,17)*SK_MY(4)); +Kfusion(1) = SK_MY(1)*(OP(1,21) + OP(1,1)*SH_MAG(3) + OP(1,2)*SH_MAG(2) + OP(1,3)*SH_MAG(1) - OP(1,4)*SK_MY(3) - OP(1,18)*SK_MY(2) - OP(1,17)*SK_MY(4) + OP(1,19)*SK_MY(5)); +Kfusion(2) = SK_MY(1)*(OP(2,21) + OP(2,1)*SH_MAG(3) + OP(2,2)*SH_MAG(2) + OP(2,3)*SH_MAG(1) - OP(2,4)*SK_MY(3) - OP(2,18)*SK_MY(2) - OP(2,17)*SK_MY(4) + OP(2,19)*SK_MY(5)); +Kfusion(3) = SK_MY(1)*(OP(3,21) + OP(3,1)*SH_MAG(3) + OP(3,2)*SH_MAG(2) + OP(3,3)*SH_MAG(1) - OP(3,4)*SK_MY(3) - OP(3,18)*SK_MY(2) - OP(3,17)*SK_MY(4) + OP(3,19)*SK_MY(5)); +Kfusion(4) = SK_MY(1)*(OP(4,21) + OP(4,1)*SH_MAG(3) + OP(4,2)*SH_MAG(2) + OP(4,3)*SH_MAG(1) - OP(4,4)*SK_MY(3) - OP(4,18)*SK_MY(2) - OP(4,17)*SK_MY(4) + OP(4,19)*SK_MY(5)); +Kfusion(5) = SK_MY(1)*(OP(5,21) + OP(5,1)*SH_MAG(3) + OP(5,2)*SH_MAG(2) + OP(5,3)*SH_MAG(1) - OP(5,4)*SK_MY(3) - OP(5,18)*SK_MY(2) - OP(5,17)*SK_MY(4) + OP(5,19)*SK_MY(5)); +Kfusion(6) = SK_MY(1)*(OP(6,21) + OP(6,1)*SH_MAG(3) + OP(6,2)*SH_MAG(2) + OP(6,3)*SH_MAG(1) - OP(6,4)*SK_MY(3) - OP(6,18)*SK_MY(2) - OP(6,17)*SK_MY(4) + OP(6,19)*SK_MY(5)); +Kfusion(7) = SK_MY(1)*(OP(7,21) + OP(7,1)*SH_MAG(3) + OP(7,2)*SH_MAG(2) + OP(7,3)*SH_MAG(1) - OP(7,4)*SK_MY(3) - OP(7,18)*SK_MY(2) - OP(7,17)*SK_MY(4) + OP(7,19)*SK_MY(5)); +Kfusion(8) = SK_MY(1)*(OP(8,21) + OP(8,1)*SH_MAG(3) + OP(8,2)*SH_MAG(2) + OP(8,3)*SH_MAG(1) - OP(8,4)*SK_MY(3) - OP(8,18)*SK_MY(2) - OP(8,17)*SK_MY(4) + OP(8,19)*SK_MY(5)); +Kfusion(9) = SK_MY(1)*(OP(9,21) + OP(9,1)*SH_MAG(3) + OP(9,2)*SH_MAG(2) + OP(9,3)*SH_MAG(1) - OP(9,4)*SK_MY(3) - OP(9,18)*SK_MY(2) - OP(9,17)*SK_MY(4) + OP(9,19)*SK_MY(5)); +Kfusion(10) = SK_MY(1)*(OP(10,21) + OP(10,1)*SH_MAG(3) + OP(10,2)*SH_MAG(2) + OP(10,3)*SH_MAG(1) - OP(10,4)*SK_MY(3) - OP(10,18)*SK_MY(2) - OP(10,17)*SK_MY(4) + OP(10,19)*SK_MY(5)); +Kfusion(11) = SK_MY(1)*(OP(11,21) + OP(11,1)*SH_MAG(3) + OP(11,2)*SH_MAG(2) + OP(11,3)*SH_MAG(1) - OP(11,4)*SK_MY(3) - OP(11,18)*SK_MY(2) - OP(11,17)*SK_MY(4) + OP(11,19)*SK_MY(5)); +Kfusion(12) = SK_MY(1)*(OP(12,21) + OP(12,1)*SH_MAG(3) + OP(12,2)*SH_MAG(2) + OP(12,3)*SH_MAG(1) - OP(12,4)*SK_MY(3) - OP(12,18)*SK_MY(2) - OP(12,17)*SK_MY(4) + OP(12,19)*SK_MY(5)); +Kfusion(13) = SK_MY(1)*(OP(13,21) + OP(13,1)*SH_MAG(3) + OP(13,2)*SH_MAG(2) + OP(13,3)*SH_MAG(1) - OP(13,4)*SK_MY(3) - OP(13,18)*SK_MY(2) - OP(13,17)*SK_MY(4) + OP(13,19)*SK_MY(5)); +Kfusion(14) = SK_MY(1)*(OP(14,21) + OP(14,1)*SH_MAG(3) + OP(14,2)*SH_MAG(2) + OP(14,3)*SH_MAG(1) - OP(14,4)*SK_MY(3) - OP(14,18)*SK_MY(2) - OP(14,17)*SK_MY(4) + OP(14,19)*SK_MY(5)); +Kfusion(15) = SK_MY(1)*(OP(15,21) + OP(15,1)*SH_MAG(3) + OP(15,2)*SH_MAG(2) + OP(15,3)*SH_MAG(1) - OP(15,4)*SK_MY(3) - OP(15,18)*SK_MY(2) - OP(15,17)*SK_MY(4) + OP(15,19)*SK_MY(5)); +Kfusion(16) = SK_MY(1)*(OP(16,21) + OP(16,1)*SH_MAG(3) + OP(16,2)*SH_MAG(2) + OP(16,3)*SH_MAG(1) - OP(16,4)*SK_MY(3) - OP(16,18)*SK_MY(2) - OP(16,17)*SK_MY(4) + OP(16,19)*SK_MY(5)); +Kfusion(17) = SK_MY(1)*(OP(17,21) + OP(17,1)*SH_MAG(3) + OP(17,2)*SH_MAG(2) + OP(17,3)*SH_MAG(1) - OP(17,4)*SK_MY(3) - OP(17,18)*SK_MY(2) - OP(17,17)*SK_MY(4) + OP(17,19)*SK_MY(5)); +Kfusion(18) = SK_MY(1)*(OP(18,21) + OP(18,1)*SH_MAG(3) + OP(18,2)*SH_MAG(2) + OP(18,3)*SH_MAG(1) - OP(18,4)*SK_MY(3) - OP(18,18)*SK_MY(2) - OP(18,17)*SK_MY(4) + OP(18,19)*SK_MY(5)); +Kfusion(19) = SK_MY(1)*(OP(19,21) + OP(19,1)*SH_MAG(3) + OP(19,2)*SH_MAG(2) + OP(19,3)*SH_MAG(1) - OP(19,4)*SK_MY(3) - OP(19,18)*SK_MY(2) - OP(19,17)*SK_MY(4) + OP(19,19)*SK_MY(5)); +Kfusion(20) = SK_MY(1)*(OP(20,21) + OP(20,1)*SH_MAG(3) + OP(20,2)*SH_MAG(2) + OP(20,3)*SH_MAG(1) - OP(20,4)*SK_MY(3) - OP(20,18)*SK_MY(2) - OP(20,17)*SK_MY(4) + OP(20,19)*SK_MY(5)); +Kfusion(21) = SK_MY(1)*(OP(21,21) + OP(21,1)*SH_MAG(3) + OP(21,2)*SH_MAG(2) + OP(21,3)*SH_MAG(1) - OP(21,4)*SK_MY(3) - OP(21,18)*SK_MY(2) - OP(21,17)*SK_MY(4) + OP(21,19)*SK_MY(5)); +Kfusion(22) = SK_MY(1)*(OP(22,21) + OP(22,1)*SH_MAG(3) + OP(22,2)*SH_MAG(2) + OP(22,3)*SH_MAG(1) - OP(22,4)*SK_MY(3) - OP(22,18)*SK_MY(2) - OP(22,17)*SK_MY(4) + OP(22,19)*SK_MY(5)); +Kfusion(23) = SK_MY(1)*(OP(23,21) + OP(23,1)*SH_MAG(3) + OP(23,2)*SH_MAG(2) + OP(23,3)*SH_MAG(1) - OP(23,4)*SK_MY(3) - OP(23,18)*SK_MY(2) - OP(23,17)*SK_MY(4) + OP(23,19)*SK_MY(5)); +Kfusion(24) = SK_MY(1)*(OP(24,21) + OP(24,1)*SH_MAG(3) + OP(24,2)*SH_MAG(2) + OP(24,3)*SH_MAG(1) - OP(24,4)*SK_MY(3) - OP(24,18)*SK_MY(2) - OP(24,17)*SK_MY(4) + OP(24,19)*SK_MY(5)); H_MAG = zeros(1,24); -H_MAG(1) = magN*(SH_MAG(9) - 2*q1*q2) - magD*SH_MAG(4) - magE*SH_MAG(1); -H_MAG(2) = magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2); -H_MAG(17) = SH_MAG(6); +H_MAG(1) = SH_MAG(2); +H_MAG(2) = -SH_MAG(3); +H_MAG(3) = SH_MAG(8) + SH_MAG(9) - 2*magD*q2; +H_MAG(4) = SH_MAG(1); +H_MAG(17) = 2*q0*q2 + 2*q1*q3; H_MAG(18) = 2*q2*q3 - 2*q0*q1; -H_MAG(19) = SH_MAG(3); +H_MAG(19) = SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7); H_MAG(22) = 1; -SK_MZ = zeros(4,1); -SK_MZ(1) = 1/(OP(22,22) + R_MAG + OP(17,22)*SH_MAG(6) + OP(19,22)*SH_MAG(3) - (2*q0*q1 - 2*q2*q3)*(OP(22,18) + OP(17,18)*SH_MAG(6) + OP(19,18)*SH_MAG(3) - OP(1,18)*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2)) + OP(2,18)*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) - OP(18,18)*(2*q0*q1 - 2*q2*q3)) - OP(1,22)*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2)) + OP(2,22)*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) + SH_MAG(6)*(OP(22,17) + OP(17,17)*SH_MAG(6) + OP(19,17)*SH_MAG(3) - OP(1,17)*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2)) + OP(2,17)*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) - OP(18,17)*(2*q0*q1 - 2*q2*q3)) + SH_MAG(3)*(OP(22,19) + OP(17,19)*SH_MAG(6) + OP(19,19)*SH_MAG(3) - OP(1,19)*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2)) + OP(2,19)*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) - OP(18,19)*(2*q0*q1 - 2*q2*q3)) - (magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2))*(OP(22,1) + OP(17,1)*SH_MAG(6) + OP(19,1)*SH_MAG(3) - OP(1,1)*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2)) + OP(2,1)*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) - OP(18,1)*(2*q0*q1 - 2*q2*q3)) - OP(18,22)*(2*q0*q1 - 2*q2*q3) + (magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2))*(OP(22,2) + OP(17,2)*SH_MAG(6) + OP(19,2)*SH_MAG(3) - OP(1,2)*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2)) + OP(2,2)*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) - OP(18,2)*(2*q0*q1 - 2*q2*q3))); -SK_MZ(2) = magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2); -SK_MZ(3) = magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2); +SK_MZ = zeros(5,1); +SK_MZ(1) = 1/(OP(22,22) + R_MAG + OP(1,22)*SH_MAG(2) - OP(2,22)*SH_MAG(3) + OP(4,22)*SH_MAG(1) + OP(19,22)*(SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7)) + (2*q0*q2 + 2*q1*q3)*(OP(22,17) + OP(1,17)*SH_MAG(2) - OP(2,17)*SH_MAG(3) + OP(4,17)*SH_MAG(1) + OP(19,17)*(SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7)) + OP(17,17)*(2*q0*q2 + 2*q1*q3) - OP(18,17)*(2*q0*q1 - 2*q2*q3) + OP(3,17)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) - (2*q0*q1 - 2*q2*q3)*(OP(22,18) + OP(1,18)*SH_MAG(2) - OP(2,18)*SH_MAG(3) + OP(4,18)*SH_MAG(1) + OP(19,18)*(SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7)) + OP(17,18)*(2*q0*q2 + 2*q1*q3) - OP(18,18)*(2*q0*q1 - 2*q2*q3) + OP(3,18)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + (SH_MAG(8) + SH_MAG(9) - 2*magD*q2)*(OP(22,3) + OP(1,3)*SH_MAG(2) - OP(2,3)*SH_MAG(3) + OP(4,3)*SH_MAG(1) + OP(19,3)*(SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7)) + OP(17,3)*(2*q0*q2 + 2*q1*q3) - OP(18,3)*(2*q0*q1 - 2*q2*q3) + OP(3,3)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + OP(17,22)*(2*q0*q2 + 2*q1*q3) - OP(18,22)*(2*q0*q1 - 2*q2*q3) + SH_MAG(2)*(OP(22,1) + OP(1,1)*SH_MAG(2) - OP(2,1)*SH_MAG(3) + OP(4,1)*SH_MAG(1) + OP(19,1)*(SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7)) + OP(17,1)*(2*q0*q2 + 2*q1*q3) - OP(18,1)*(2*q0*q1 - 2*q2*q3) + OP(3,1)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) - SH_MAG(3)*(OP(22,2) + OP(1,2)*SH_MAG(2) - OP(2,2)*SH_MAG(3) + OP(4,2)*SH_MAG(1) + OP(19,2)*(SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7)) + OP(17,2)*(2*q0*q2 + 2*q1*q3) - OP(18,2)*(2*q0*q1 - 2*q2*q3) + OP(3,2)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + SH_MAG(1)*(OP(22,4) + OP(1,4)*SH_MAG(2) - OP(2,4)*SH_MAG(3) + OP(4,4)*SH_MAG(1) + OP(19,4)*(SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7)) + OP(17,4)*(2*q0*q2 + 2*q1*q3) - OP(18,4)*(2*q0*q1 - 2*q2*q3) + OP(3,4)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + (SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7))*(OP(22,19) + OP(1,19)*SH_MAG(2) - OP(2,19)*SH_MAG(3) + OP(4,19)*SH_MAG(1) + OP(19,19)*(SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7)) + OP(17,19)*(2*q0*q2 + 2*q1*q3) - OP(18,19)*(2*q0*q1 - 2*q2*q3) + OP(3,19)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + OP(3,22)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)); +SK_MZ(2) = SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7); +SK_MZ(3) = SH_MAG(8) + SH_MAG(9) - 2*magD*q2; SK_MZ(4) = 2*q0*q1 - 2*q2*q3; +SK_MZ(5) = 2*q0*q2 + 2*q1*q3; Kfusion = zeros(24,1); Kfusion = zeros(1,1); -Kfusion(1) = SK_MZ(1)*(OP(1,22) + OP(1,19)*SH_MAG(3) + OP(1,17)*SH_MAG(6) - OP(1,1)*SK_MZ(2) + OP(1,2)*SK_MZ(3) - OP(1,18)*SK_MZ(4)); -Kfusion(2) = SK_MZ(1)*(OP(2,22) + OP(2,19)*SH_MAG(3) + OP(2,17)*SH_MAG(6) - OP(2,1)*SK_MZ(2) + OP(2,2)*SK_MZ(3) - OP(2,18)*SK_MZ(4)); -Kfusion(3) = SK_MZ(1)*(OP(3,22) + OP(3,19)*SH_MAG(3) + OP(3,17)*SH_MAG(6) - OP(3,1)*SK_MZ(2) + OP(3,2)*SK_MZ(3) - OP(3,18)*SK_MZ(4)); -Kfusion(4) = SK_MZ(1)*(OP(4,22) + OP(4,19)*SH_MAG(3) + OP(4,17)*SH_MAG(6) - OP(4,1)*SK_MZ(2) + OP(4,2)*SK_MZ(3) - OP(4,18)*SK_MZ(4)); -Kfusion(5) = SK_MZ(1)*(OP(5,22) + OP(5,19)*SH_MAG(3) + OP(5,17)*SH_MAG(6) - OP(5,1)*SK_MZ(2) + OP(5,2)*SK_MZ(3) - OP(5,18)*SK_MZ(4)); -Kfusion(6) = SK_MZ(1)*(OP(6,22) + OP(6,19)*SH_MAG(3) + OP(6,17)*SH_MAG(6) - OP(6,1)*SK_MZ(2) + OP(6,2)*SK_MZ(3) - OP(6,18)*SK_MZ(4)); -Kfusion(7) = SK_MZ(1)*(OP(7,22) + OP(7,19)*SH_MAG(3) + OP(7,17)*SH_MAG(6) - OP(7,1)*SK_MZ(2) + OP(7,2)*SK_MZ(3) - OP(7,18)*SK_MZ(4)); -Kfusion(8) = SK_MZ(1)*(OP(8,22) + OP(8,19)*SH_MAG(3) + OP(8,17)*SH_MAG(6) - OP(8,1)*SK_MZ(2) + OP(8,2)*SK_MZ(3) - OP(8,18)*SK_MZ(4)); -Kfusion(9) = SK_MZ(1)*(OP(9,22) + OP(9,19)*SH_MAG(3) + OP(9,17)*SH_MAG(6) - OP(9,1)*SK_MZ(2) + OP(9,2)*SK_MZ(3) - OP(9,18)*SK_MZ(4)); -Kfusion(10) = SK_MZ(1)*(OP(10,22) + OP(10,19)*SH_MAG(3) + OP(10,17)*SH_MAG(6) - OP(10,1)*SK_MZ(2) + OP(10,2)*SK_MZ(3) - OP(10,18)*SK_MZ(4)); -Kfusion(11) = SK_MZ(1)*(OP(11,22) + OP(11,19)*SH_MAG(3) + OP(11,17)*SH_MAG(6) - OP(11,1)*SK_MZ(2) + OP(11,2)*SK_MZ(3) - OP(11,18)*SK_MZ(4)); -Kfusion(12) = SK_MZ(1)*(OP(12,22) + OP(12,19)*SH_MAG(3) + OP(12,17)*SH_MAG(6) - OP(12,1)*SK_MZ(2) + OP(12,2)*SK_MZ(3) - OP(12,18)*SK_MZ(4)); -Kfusion(13) = SK_MZ(1)*(OP(13,22) + OP(13,19)*SH_MAG(3) + OP(13,17)*SH_MAG(6) - OP(13,1)*SK_MZ(2) + OP(13,2)*SK_MZ(3) - OP(13,18)*SK_MZ(4)); -Kfusion(14) = SK_MZ(1)*(OP(14,22) + OP(14,19)*SH_MAG(3) + OP(14,17)*SH_MAG(6) - OP(14,1)*SK_MZ(2) + OP(14,2)*SK_MZ(3) - OP(14,18)*SK_MZ(4)); -Kfusion(15) = SK_MZ(1)*(OP(15,22) + OP(15,19)*SH_MAG(3) + OP(15,17)*SH_MAG(6) - OP(15,1)*SK_MZ(2) + OP(15,2)*SK_MZ(3) - OP(15,18)*SK_MZ(4)); -Kfusion(16) = SK_MZ(1)*(OP(16,22) + OP(16,19)*SH_MAG(3) + OP(16,17)*SH_MAG(6) - OP(16,1)*SK_MZ(2) + OP(16,2)*SK_MZ(3) - OP(16,18)*SK_MZ(4)); -Kfusion(17) = SK_MZ(1)*(OP(17,22) + OP(17,19)*SH_MAG(3) + OP(17,17)*SH_MAG(6) - OP(17,1)*SK_MZ(2) + OP(17,2)*SK_MZ(3) - OP(17,18)*SK_MZ(4)); -Kfusion(18) = SK_MZ(1)*(OP(18,22) + OP(18,19)*SH_MAG(3) + OP(18,17)*SH_MAG(6) - OP(18,1)*SK_MZ(2) + OP(18,2)*SK_MZ(3) - OP(18,18)*SK_MZ(4)); -Kfusion(19) = SK_MZ(1)*(OP(19,22) + OP(19,19)*SH_MAG(3) + OP(19,17)*SH_MAG(6) - OP(19,1)*SK_MZ(2) + OP(19,2)*SK_MZ(3) - OP(19,18)*SK_MZ(4)); -Kfusion(20) = SK_MZ(1)*(OP(20,22) + OP(20,19)*SH_MAG(3) + OP(20,17)*SH_MAG(6) - OP(20,1)*SK_MZ(2) + OP(20,2)*SK_MZ(3) - OP(20,18)*SK_MZ(4)); -Kfusion(21) = SK_MZ(1)*(OP(21,22) + OP(21,19)*SH_MAG(3) + OP(21,17)*SH_MAG(6) - OP(21,1)*SK_MZ(2) + OP(21,2)*SK_MZ(3) - OP(21,18)*SK_MZ(4)); -Kfusion(22) = SK_MZ(1)*(OP(22,22) + OP(22,19)*SH_MAG(3) + OP(22,17)*SH_MAG(6) - OP(22,1)*SK_MZ(2) + OP(22,2)*SK_MZ(3) - OP(22,18)*SK_MZ(4)); -Kfusion(23) = SK_MZ(1)*(OP(23,22) + OP(23,19)*SH_MAG(3) + OP(23,17)*SH_MAG(6) - OP(23,1)*SK_MZ(2) + OP(23,2)*SK_MZ(3) - OP(23,18)*SK_MZ(4)); -Kfusion(24) = SK_MZ(1)*(OP(24,22) + OP(24,19)*SH_MAG(3) + OP(24,17)*SH_MAG(6) - OP(24,1)*SK_MZ(2) + OP(24,2)*SK_MZ(3) - OP(24,18)*SK_MZ(4)); -SH_ACCX = zeros(7,1); +Kfusion(1) = SK_MZ(1)*(OP(1,22) + OP(1,1)*SH_MAG(2) - OP(1,2)*SH_MAG(3) + OP(1,4)*SH_MAG(1) + OP(1,3)*SK_MZ(3) + OP(1,19)*SK_MZ(2) + OP(1,17)*SK_MZ(5) - OP(1,18)*SK_MZ(4)); +Kfusion(2) = SK_MZ(1)*(OP(2,22) + OP(2,1)*SH_MAG(2) - OP(2,2)*SH_MAG(3) + OP(2,4)*SH_MAG(1) + OP(2,3)*SK_MZ(3) + OP(2,19)*SK_MZ(2) + OP(2,17)*SK_MZ(5) - OP(2,18)*SK_MZ(4)); +Kfusion(3) = SK_MZ(1)*(OP(3,22) + OP(3,1)*SH_MAG(2) - OP(3,2)*SH_MAG(3) + OP(3,4)*SH_MAG(1) + OP(3,3)*SK_MZ(3) + OP(3,19)*SK_MZ(2) + OP(3,17)*SK_MZ(5) - OP(3,18)*SK_MZ(4)); +Kfusion(4) = SK_MZ(1)*(OP(4,22) + OP(4,1)*SH_MAG(2) - OP(4,2)*SH_MAG(3) + OP(4,4)*SH_MAG(1) + OP(4,3)*SK_MZ(3) + OP(4,19)*SK_MZ(2) + OP(4,17)*SK_MZ(5) - OP(4,18)*SK_MZ(4)); +Kfusion(5) = SK_MZ(1)*(OP(5,22) + OP(5,1)*SH_MAG(2) - OP(5,2)*SH_MAG(3) + OP(5,4)*SH_MAG(1) + OP(5,3)*SK_MZ(3) + OP(5,19)*SK_MZ(2) + OP(5,17)*SK_MZ(5) - OP(5,18)*SK_MZ(4)); +Kfusion(6) = SK_MZ(1)*(OP(6,22) + OP(6,1)*SH_MAG(2) - OP(6,2)*SH_MAG(3) + OP(6,4)*SH_MAG(1) + OP(6,3)*SK_MZ(3) + OP(6,19)*SK_MZ(2) + OP(6,17)*SK_MZ(5) - OP(6,18)*SK_MZ(4)); +Kfusion(7) = SK_MZ(1)*(OP(7,22) + OP(7,1)*SH_MAG(2) - OP(7,2)*SH_MAG(3) + OP(7,4)*SH_MAG(1) + OP(7,3)*SK_MZ(3) + OP(7,19)*SK_MZ(2) + OP(7,17)*SK_MZ(5) - OP(7,18)*SK_MZ(4)); +Kfusion(8) = SK_MZ(1)*(OP(8,22) + OP(8,1)*SH_MAG(2) - OP(8,2)*SH_MAG(3) + OP(8,4)*SH_MAG(1) + OP(8,3)*SK_MZ(3) + OP(8,19)*SK_MZ(2) + OP(8,17)*SK_MZ(5) - OP(8,18)*SK_MZ(4)); +Kfusion(9) = SK_MZ(1)*(OP(9,22) + OP(9,1)*SH_MAG(2) - OP(9,2)*SH_MAG(3) + OP(9,4)*SH_MAG(1) + OP(9,3)*SK_MZ(3) + OP(9,19)*SK_MZ(2) + OP(9,17)*SK_MZ(5) - OP(9,18)*SK_MZ(4)); +Kfusion(10) = SK_MZ(1)*(OP(10,22) + OP(10,1)*SH_MAG(2) - OP(10,2)*SH_MAG(3) + OP(10,4)*SH_MAG(1) + OP(10,3)*SK_MZ(3) + OP(10,19)*SK_MZ(2) + OP(10,17)*SK_MZ(5) - OP(10,18)*SK_MZ(4)); +Kfusion(11) = SK_MZ(1)*(OP(11,22) + OP(11,1)*SH_MAG(2) - OP(11,2)*SH_MAG(3) + OP(11,4)*SH_MAG(1) + OP(11,3)*SK_MZ(3) + OP(11,19)*SK_MZ(2) + OP(11,17)*SK_MZ(5) - OP(11,18)*SK_MZ(4)); +Kfusion(12) = SK_MZ(1)*(OP(12,22) + OP(12,1)*SH_MAG(2) - OP(12,2)*SH_MAG(3) + OP(12,4)*SH_MAG(1) + OP(12,3)*SK_MZ(3) + OP(12,19)*SK_MZ(2) + OP(12,17)*SK_MZ(5) - OP(12,18)*SK_MZ(4)); +Kfusion(13) = SK_MZ(1)*(OP(13,22) + OP(13,1)*SH_MAG(2) - OP(13,2)*SH_MAG(3) + OP(13,4)*SH_MAG(1) + OP(13,3)*SK_MZ(3) + OP(13,19)*SK_MZ(2) + OP(13,17)*SK_MZ(5) - OP(13,18)*SK_MZ(4)); +Kfusion(14) = SK_MZ(1)*(OP(14,22) + OP(14,1)*SH_MAG(2) - OP(14,2)*SH_MAG(3) + OP(14,4)*SH_MAG(1) + OP(14,3)*SK_MZ(3) + OP(14,19)*SK_MZ(2) + OP(14,17)*SK_MZ(5) - OP(14,18)*SK_MZ(4)); +Kfusion(15) = SK_MZ(1)*(OP(15,22) + OP(15,1)*SH_MAG(2) - OP(15,2)*SH_MAG(3) + OP(15,4)*SH_MAG(1) + OP(15,3)*SK_MZ(3) + OP(15,19)*SK_MZ(2) + OP(15,17)*SK_MZ(5) - OP(15,18)*SK_MZ(4)); +Kfusion(16) = SK_MZ(1)*(OP(16,22) + OP(16,1)*SH_MAG(2) - OP(16,2)*SH_MAG(3) + OP(16,4)*SH_MAG(1) + OP(16,3)*SK_MZ(3) + OP(16,19)*SK_MZ(2) + OP(16,17)*SK_MZ(5) - OP(16,18)*SK_MZ(4)); +Kfusion(17) = SK_MZ(1)*(OP(17,22) + OP(17,1)*SH_MAG(2) - OP(17,2)*SH_MAG(3) + OP(17,4)*SH_MAG(1) + OP(17,3)*SK_MZ(3) + OP(17,19)*SK_MZ(2) + OP(17,17)*SK_MZ(5) - OP(17,18)*SK_MZ(4)); +Kfusion(18) = SK_MZ(1)*(OP(18,22) + OP(18,1)*SH_MAG(2) - OP(18,2)*SH_MAG(3) + OP(18,4)*SH_MAG(1) + OP(18,3)*SK_MZ(3) + OP(18,19)*SK_MZ(2) + OP(18,17)*SK_MZ(5) - OP(18,18)*SK_MZ(4)); +Kfusion(19) = SK_MZ(1)*(OP(19,22) + OP(19,1)*SH_MAG(2) - OP(19,2)*SH_MAG(3) + OP(19,4)*SH_MAG(1) + OP(19,3)*SK_MZ(3) + OP(19,19)*SK_MZ(2) + OP(19,17)*SK_MZ(5) - OP(19,18)*SK_MZ(4)); +Kfusion(20) = SK_MZ(1)*(OP(20,22) + OP(20,1)*SH_MAG(2) - OP(20,2)*SH_MAG(3) + OP(20,4)*SH_MAG(1) + OP(20,3)*SK_MZ(3) + OP(20,19)*SK_MZ(2) + OP(20,17)*SK_MZ(5) - OP(20,18)*SK_MZ(4)); +Kfusion(21) = SK_MZ(1)*(OP(21,22) + OP(21,1)*SH_MAG(2) - OP(21,2)*SH_MAG(3) + OP(21,4)*SH_MAG(1) + OP(21,3)*SK_MZ(3) + OP(21,19)*SK_MZ(2) + OP(21,17)*SK_MZ(5) - OP(21,18)*SK_MZ(4)); +Kfusion(22) = SK_MZ(1)*(OP(22,22) + OP(22,1)*SH_MAG(2) - OP(22,2)*SH_MAG(3) + OP(22,4)*SH_MAG(1) + OP(22,3)*SK_MZ(3) + OP(22,19)*SK_MZ(2) + OP(22,17)*SK_MZ(5) - OP(22,18)*SK_MZ(4)); +Kfusion(23) = SK_MZ(1)*(OP(23,22) + OP(23,1)*SH_MAG(2) - OP(23,2)*SH_MAG(3) + OP(23,4)*SH_MAG(1) + OP(23,3)*SK_MZ(3) + OP(23,19)*SK_MZ(2) + OP(23,17)*SK_MZ(5) - OP(23,18)*SK_MZ(4)); +Kfusion(24) = SK_MZ(1)*(OP(24,22) + OP(24,1)*SH_MAG(2) - OP(24,2)*SH_MAG(3) + OP(24,4)*SH_MAG(1) + OP(24,3)*SK_MZ(3) + OP(24,19)*SK_MZ(2) + OP(24,17)*SK_MZ(5) - OP(24,18)*SK_MZ(4)); +SH_ACCX = zeros(4,1); SH_ACCX(1) = q0^2 + q1^2 - q2^2 - q3^2; -SH_ACCX(2) = 2*q0*q3 + 2*q1*q2; -SH_ACCX(3) = vn - vwn; -SH_ACCX(4) = ve - vwe; -SH_ACCX(5) = q3^2; -SH_ACCX(6) = 2*q0*q2; -SH_ACCX(7) = 2*q0*q1; +SH_ACCX(2) = vn - vwn; +SH_ACCX(3) = ve - vwe; +SH_ACCX(4) = 2*q0*q3 + 2*q1*q2; H_ACCX = zeros(1,24); -H_ACCX(1,2) = Kaccx*(SH_ACCX(3)*(SH_ACCX(6) + 2*q1*q3) - SH_ACCX(4)*(SH_ACCX(7) - 2*q2*q3) + vd*(SH_ACCX(5) + q0^2 - q1^2 - q2^2)); -H_ACCX(1,3) = Kaccx*(SH_ACCX(3)*(2*q0*q3 - 2*q1*q2) + SH_ACCX(4)*(SH_ACCX(5) - q0^2 + q1^2 - q2^2) - vd*(SH_ACCX(7) + 2*q2*q3)); -H_ACCX(1,4) = -Kaccx*SH_ACCX(1); -H_ACCX(1,5) = -Kaccx*SH_ACCX(2); -H_ACCX(1,6) = Kaccx*(SH_ACCX(6) - 2*q1*q3); +H_ACCX(1,1) = -Kaccx*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd); +H_ACCX(1,2) = -Kaccx*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd); +H_ACCX(1,3) = Kaccx*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd); +H_ACCX(1,4) = -Kaccx*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd); +H_ACCX(1,5) = -Kaccx*SH_ACCX(1); +H_ACCX(1,6) = -Kaccx*SH_ACCX(4); +H_ACCX(1,7) = Kaccx*(2*q0*q2 - 2*q1*q3); H_ACCX(1,23) = Kaccx*SH_ACCX(1); -H_ACCX(1,24) = Kaccx*SH_ACCX(2); -SK_ACCX = zeros(5,1); -SK_ACCX(1) = 1/(R_ACC - Kaccx*SH_ACCX(1)*(Kaccx*OP(23,4)*SH_ACCX(1) - Kaccx*OP(5,4)*SH_ACCX(2) - Kaccx*OP(4,4)*SH_ACCX(1) + Kaccx*OP(24,4)*SH_ACCX(2) + Kaccx*OP(3,4)*(SH_ACCX(3)*(2*q0*q3 - 2*q1*q2) + SH_ACCX(4)*(SH_ACCX(5) - q0^2 + q1^2 - q2^2) - vd*(SH_ACCX(7) + 2*q2*q3)) + Kaccx*OP(2,4)*(SH_ACCX(3)*(SH_ACCX(6) + 2*q1*q3) - SH_ACCX(4)*(SH_ACCX(7) - 2*q2*q3) + vd*(SH_ACCX(5) + q0^2 - q1^2 - q2^2)) + Kaccx*OP(6,4)*(SH_ACCX(6) - 2*q1*q3)) - Kaccx*SH_ACCX(2)*(Kaccx*OP(23,5)*SH_ACCX(1) - Kaccx*OP(5,5)*SH_ACCX(2) - Kaccx*OP(4,5)*SH_ACCX(1) + Kaccx*OP(24,5)*SH_ACCX(2) + Kaccx*OP(3,5)*(SH_ACCX(3)*(2*q0*q3 - 2*q1*q2) + SH_ACCX(4)*(SH_ACCX(5) - q0^2 + q1^2 - q2^2) - vd*(SH_ACCX(7) + 2*q2*q3)) + Kaccx*OP(2,5)*(SH_ACCX(3)*(SH_ACCX(6) + 2*q1*q3) - SH_ACCX(4)*(SH_ACCX(7) - 2*q2*q3) + vd*(SH_ACCX(5) + q0^2 - q1^2 - q2^2)) + Kaccx*OP(6,5)*(SH_ACCX(6) - 2*q1*q3)) + Kaccx*SH_ACCX(1)*(Kaccx*OP(23,23)*SH_ACCX(1) - Kaccx*OP(5,23)*SH_ACCX(2) - Kaccx*OP(4,23)*SH_ACCX(1) + Kaccx*OP(24,23)*SH_ACCX(2) + Kaccx*OP(3,23)*(SH_ACCX(3)*(2*q0*q3 - 2*q1*q2) + SH_ACCX(4)*(SH_ACCX(5) - q0^2 + q1^2 - q2^2) - vd*(SH_ACCX(7) + 2*q2*q3)) + Kaccx*OP(2,23)*(SH_ACCX(3)*(SH_ACCX(6) + 2*q1*q3) - SH_ACCX(4)*(SH_ACCX(7) - 2*q2*q3) + vd*(SH_ACCX(5) + q0^2 - q1^2 - q2^2)) + Kaccx*OP(6,23)*(SH_ACCX(6) - 2*q1*q3)) + Kaccx*SH_ACCX(2)*(Kaccx*OP(23,24)*SH_ACCX(1) - Kaccx*OP(5,24)*SH_ACCX(2) - Kaccx*OP(4,24)*SH_ACCX(1) + Kaccx*OP(24,24)*SH_ACCX(2) + Kaccx*OP(3,24)*(SH_ACCX(3)*(2*q0*q3 - 2*q1*q2) + SH_ACCX(4)*(SH_ACCX(5) - q0^2 + q1^2 - q2^2) - vd*(SH_ACCX(7) + 2*q2*q3)) + Kaccx*OP(2,24)*(SH_ACCX(3)*(SH_ACCX(6) + 2*q1*q3) - SH_ACCX(4)*(SH_ACCX(7) - 2*q2*q3) + vd*(SH_ACCX(5) + q0^2 - q1^2 - q2^2)) + Kaccx*OP(6,24)*(SH_ACCX(6) - 2*q1*q3)) + Kaccx*(SH_ACCX(3)*(2*q0*q3 - 2*q1*q2) + SH_ACCX(4)*(SH_ACCX(5) - q0^2 + q1^2 - q2^2) - vd*(SH_ACCX(7) + 2*q2*q3))*(Kaccx*OP(23,3)*SH_ACCX(1) - Kaccx*OP(5,3)*SH_ACCX(2) - Kaccx*OP(4,3)*SH_ACCX(1) + Kaccx*OP(24,3)*SH_ACCX(2) + Kaccx*OP(3,3)*(SH_ACCX(3)*(2*q0*q3 - 2*q1*q2) + SH_ACCX(4)*(SH_ACCX(5) - q0^2 + q1^2 - q2^2) - vd*(SH_ACCX(7) + 2*q2*q3)) + Kaccx*OP(2,3)*(SH_ACCX(3)*(SH_ACCX(6) + 2*q1*q3) - SH_ACCX(4)*(SH_ACCX(7) - 2*q2*q3) + vd*(SH_ACCX(5) + q0^2 - q1^2 - q2^2)) + Kaccx*OP(6,3)*(SH_ACCX(6) - 2*q1*q3)) + Kaccx*(SH_ACCX(3)*(SH_ACCX(6) + 2*q1*q3) - SH_ACCX(4)*(SH_ACCX(7) - 2*q2*q3) + vd*(SH_ACCX(5) + q0^2 - q1^2 - q2^2))*(Kaccx*OP(23,2)*SH_ACCX(1) - Kaccx*OP(5,2)*SH_ACCX(2) - Kaccx*OP(4,2)*SH_ACCX(1) + Kaccx*OP(24,2)*SH_ACCX(2) + Kaccx*OP(3,2)*(SH_ACCX(3)*(2*q0*q3 - 2*q1*q2) + SH_ACCX(4)*(SH_ACCX(5) - q0^2 + q1^2 - q2^2) - vd*(SH_ACCX(7) + 2*q2*q3)) + Kaccx*OP(2,2)*(SH_ACCX(3)*(SH_ACCX(6) + 2*q1*q3) - SH_ACCX(4)*(SH_ACCX(7) - 2*q2*q3) + vd*(SH_ACCX(5) + q0^2 - q1^2 - q2^2)) + Kaccx*OP(6,2)*(SH_ACCX(6) - 2*q1*q3)) + Kaccx*(SH_ACCX(6) - 2*q1*q3)*(Kaccx*OP(23,6)*SH_ACCX(1) - Kaccx*OP(5,6)*SH_ACCX(2) - Kaccx*OP(4,6)*SH_ACCX(1) + Kaccx*OP(24,6)*SH_ACCX(2) + Kaccx*OP(3,6)*(SH_ACCX(3)*(2*q0*q3 - 2*q1*q2) + SH_ACCX(4)*(SH_ACCX(5) - q0^2 + q1^2 - q2^2) - vd*(SH_ACCX(7) + 2*q2*q3)) + Kaccx*OP(2,6)*(SH_ACCX(3)*(SH_ACCX(6) + 2*q1*q3) - SH_ACCX(4)*(SH_ACCX(7) - 2*q2*q3) + vd*(SH_ACCX(5) + q0^2 - q1^2 - q2^2)) + Kaccx*OP(6,6)*(SH_ACCX(6) - 2*q1*q3))); -SK_ACCX(2) = SH_ACCX(3)*(2*q0*q3 - 2*q1*q2) + SH_ACCX(4)*(SH_ACCX(5) - q0^2 + q1^2 - q2^2) - vd*(SH_ACCX(7) + 2*q2*q3); -SK_ACCX(3) = SH_ACCX(3)*(SH_ACCX(6) + 2*q1*q3) - SH_ACCX(4)*(SH_ACCX(7) - 2*q2*q3) + vd*(SH_ACCX(5) + q0^2 - q1^2 - q2^2); -SK_ACCX(4) = SH_ACCX(6) - 2*q1*q3; -SK_ACCX(5) = SH_ACCX(2); +H_ACCX(1,24) = Kaccx*SH_ACCX(4); +SK_ACCX = zeros(7,1); +SK_ACCX(1) = 1/(R_ACC + Kaccx*SH_ACCX(1)*(Kaccx*OP(5,5)*SH_ACCX(1) + Kaccx*OP(6,5)*SH_ACCX(4) - Kaccx*OP(23,5)*SH_ACCX(1) - Kaccx*OP(24,5)*SH_ACCX(4) - Kaccx*OP(7,5)*(2*q0*q2 - 2*q1*q3) + Kaccx*OP(1,5)*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd) + Kaccx*OP(2,5)*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd) - Kaccx*OP(3,5)*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd) + Kaccx*OP(4,5)*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd)) + Kaccx*SH_ACCX(4)*(Kaccx*OP(5,6)*SH_ACCX(1) + Kaccx*OP(6,6)*SH_ACCX(4) - Kaccx*OP(23,6)*SH_ACCX(1) - Kaccx*OP(24,6)*SH_ACCX(4) - Kaccx*OP(7,6)*(2*q0*q2 - 2*q1*q3) + Kaccx*OP(1,6)*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd) + Kaccx*OP(2,6)*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd) - Kaccx*OP(3,6)*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd) + Kaccx*OP(4,6)*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd)) - Kaccx*SH_ACCX(1)*(Kaccx*OP(5,23)*SH_ACCX(1) + Kaccx*OP(6,23)*SH_ACCX(4) - Kaccx*OP(23,23)*SH_ACCX(1) - Kaccx*OP(24,23)*SH_ACCX(4) - Kaccx*OP(7,23)*(2*q0*q2 - 2*q1*q3) + Kaccx*OP(1,23)*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd) + Kaccx*OP(2,23)*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd) - Kaccx*OP(3,23)*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd) + Kaccx*OP(4,23)*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd)) - Kaccx*SH_ACCX(4)*(Kaccx*OP(5,24)*SH_ACCX(1) + Kaccx*OP(6,24)*SH_ACCX(4) - Kaccx*OP(23,24)*SH_ACCX(1) - Kaccx*OP(24,24)*SH_ACCX(4) - Kaccx*OP(7,24)*(2*q0*q2 - 2*q1*q3) + Kaccx*OP(1,24)*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd) + Kaccx*OP(2,24)*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd) - Kaccx*OP(3,24)*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd) + Kaccx*OP(4,24)*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd)) - Kaccx*(2*q0*q2 - 2*q1*q3)*(Kaccx*OP(5,7)*SH_ACCX(1) + Kaccx*OP(6,7)*SH_ACCX(4) - Kaccx*OP(23,7)*SH_ACCX(1) - Kaccx*OP(24,7)*SH_ACCX(4) - Kaccx*OP(7,7)*(2*q0*q2 - 2*q1*q3) + Kaccx*OP(1,7)*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd) + Kaccx*OP(2,7)*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd) - Kaccx*OP(3,7)*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd) + Kaccx*OP(4,7)*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd)) + Kaccx*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd)*(Kaccx*OP(5,1)*SH_ACCX(1) + Kaccx*OP(6,1)*SH_ACCX(4) - Kaccx*OP(23,1)*SH_ACCX(1) - Kaccx*OP(24,1)*SH_ACCX(4) - Kaccx*OP(7,1)*(2*q0*q2 - 2*q1*q3) + Kaccx*OP(1,1)*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd) + Kaccx*OP(2,1)*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd) - Kaccx*OP(3,1)*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd) + Kaccx*OP(4,1)*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd)) + Kaccx*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd)*(Kaccx*OP(5,2)*SH_ACCX(1) + Kaccx*OP(6,2)*SH_ACCX(4) - Kaccx*OP(23,2)*SH_ACCX(1) - Kaccx*OP(24,2)*SH_ACCX(4) - Kaccx*OP(7,2)*(2*q0*q2 - 2*q1*q3) + Kaccx*OP(1,2)*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd) + Kaccx*OP(2,2)*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd) - Kaccx*OP(3,2)*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd) + Kaccx*OP(4,2)*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd)) - Kaccx*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd)*(Kaccx*OP(5,3)*SH_ACCX(1) + Kaccx*OP(6,3)*SH_ACCX(4) - Kaccx*OP(23,3)*SH_ACCX(1) - Kaccx*OP(24,3)*SH_ACCX(4) - Kaccx*OP(7,3)*(2*q0*q2 - 2*q1*q3) + Kaccx*OP(1,3)*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd) + Kaccx*OP(2,3)*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd) - Kaccx*OP(3,3)*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd) + Kaccx*OP(4,3)*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd)) + Kaccx*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd)*(Kaccx*OP(5,4)*SH_ACCX(1) + Kaccx*OP(6,4)*SH_ACCX(4) - Kaccx*OP(23,4)*SH_ACCX(1) - Kaccx*OP(24,4)*SH_ACCX(4) - Kaccx*OP(7,4)*(2*q0*q2 - 2*q1*q3) + Kaccx*OP(1,4)*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd) + Kaccx*OP(2,4)*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd) - Kaccx*OP(3,4)*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd) + Kaccx*OP(4,4)*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd))); +SK_ACCX(2) = 2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd; +SK_ACCX(3) = 2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd; +SK_ACCX(4) = 2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd; +SK_ACCX(5) = 2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd; +SK_ACCX(6) = 2*q0*q2 - 2*q1*q3; +SK_ACCX(7) = SH_ACCX(4); Kfusion = zeros(24,1); Kfusion = zeros(1,1); -Kfusion(1) = SK_ACCX(1)*(Kaccx*OP(1,23)*SH_ACCX(1) - Kaccx*OP(1,4)*SH_ACCX(1) + Kaccx*OP(1,2)*SK_ACCX(3) + Kaccx*OP(1,3)*SK_ACCX(2) - Kaccx*OP(1,5)*SK_ACCX(5) + Kaccx*OP(1,6)*SK_ACCX(4) + Kaccx*OP(1,24)*SK_ACCX(5)); -Kfusion(2) = SK_ACCX(1)*(Kaccx*OP(2,23)*SH_ACCX(1) - Kaccx*OP(2,4)*SH_ACCX(1) + Kaccx*OP(2,2)*SK_ACCX(3) + Kaccx*OP(2,3)*SK_ACCX(2) - Kaccx*OP(2,5)*SK_ACCX(5) + Kaccx*OP(2,6)*SK_ACCX(4) + Kaccx*OP(2,24)*SK_ACCX(5)); -Kfusion(3) = SK_ACCX(1)*(Kaccx*OP(3,23)*SH_ACCX(1) - Kaccx*OP(3,4)*SH_ACCX(1) + Kaccx*OP(3,2)*SK_ACCX(3) + Kaccx*OP(3,3)*SK_ACCX(2) - Kaccx*OP(3,5)*SK_ACCX(5) + Kaccx*OP(3,6)*SK_ACCX(4) + Kaccx*OP(3,24)*SK_ACCX(5)); -Kfusion(4) = SK_ACCX(1)*(Kaccx*OP(4,23)*SH_ACCX(1) - Kaccx*OP(4,4)*SH_ACCX(1) + Kaccx*OP(4,2)*SK_ACCX(3) + Kaccx*OP(4,3)*SK_ACCX(2) - Kaccx*OP(4,5)*SK_ACCX(5) + Kaccx*OP(4,6)*SK_ACCX(4) + Kaccx*OP(4,24)*SK_ACCX(5)); -Kfusion(5) = SK_ACCX(1)*(Kaccx*OP(5,23)*SH_ACCX(1) - Kaccx*OP(5,4)*SH_ACCX(1) + Kaccx*OP(5,2)*SK_ACCX(3) + Kaccx*OP(5,3)*SK_ACCX(2) - Kaccx*OP(5,5)*SK_ACCX(5) + Kaccx*OP(5,6)*SK_ACCX(4) + Kaccx*OP(5,24)*SK_ACCX(5)); -Kfusion(6) = SK_ACCX(1)*(Kaccx*OP(6,23)*SH_ACCX(1) - Kaccx*OP(6,4)*SH_ACCX(1) + Kaccx*OP(6,2)*SK_ACCX(3) + Kaccx*OP(6,3)*SK_ACCX(2) - Kaccx*OP(6,5)*SK_ACCX(5) + Kaccx*OP(6,6)*SK_ACCX(4) + Kaccx*OP(6,24)*SK_ACCX(5)); -Kfusion(7) = SK_ACCX(1)*(Kaccx*OP(7,23)*SH_ACCX(1) - Kaccx*OP(7,4)*SH_ACCX(1) + Kaccx*OP(7,2)*SK_ACCX(3) + Kaccx*OP(7,3)*SK_ACCX(2) - Kaccx*OP(7,5)*SK_ACCX(5) + Kaccx*OP(7,6)*SK_ACCX(4) + Kaccx*OP(7,24)*SK_ACCX(5)); -Kfusion(8) = SK_ACCX(1)*(Kaccx*OP(8,23)*SH_ACCX(1) - Kaccx*OP(8,4)*SH_ACCX(1) + Kaccx*OP(8,2)*SK_ACCX(3) + Kaccx*OP(8,3)*SK_ACCX(2) - Kaccx*OP(8,5)*SK_ACCX(5) + Kaccx*OP(8,6)*SK_ACCX(4) + Kaccx*OP(8,24)*SK_ACCX(5)); -Kfusion(9) = SK_ACCX(1)*(Kaccx*OP(9,23)*SH_ACCX(1) - Kaccx*OP(9,4)*SH_ACCX(1) + Kaccx*OP(9,2)*SK_ACCX(3) + Kaccx*OP(9,3)*SK_ACCX(2) - Kaccx*OP(9,5)*SK_ACCX(5) + Kaccx*OP(9,6)*SK_ACCX(4) + Kaccx*OP(9,24)*SK_ACCX(5)); -Kfusion(10) = SK_ACCX(1)*(Kaccx*OP(10,23)*SH_ACCX(1) - Kaccx*OP(10,4)*SH_ACCX(1) + Kaccx*OP(10,2)*SK_ACCX(3) + Kaccx*OP(10,3)*SK_ACCX(2) - Kaccx*OP(10,5)*SK_ACCX(5) + Kaccx*OP(10,6)*SK_ACCX(4) + Kaccx*OP(10,24)*SK_ACCX(5)); -Kfusion(11) = SK_ACCX(1)*(Kaccx*OP(11,23)*SH_ACCX(1) - Kaccx*OP(11,4)*SH_ACCX(1) + Kaccx*OP(11,2)*SK_ACCX(3) + Kaccx*OP(11,3)*SK_ACCX(2) - Kaccx*OP(11,5)*SK_ACCX(5) + Kaccx*OP(11,6)*SK_ACCX(4) + Kaccx*OP(11,24)*SK_ACCX(5)); -Kfusion(12) = SK_ACCX(1)*(Kaccx*OP(12,23)*SH_ACCX(1) - Kaccx*OP(12,4)*SH_ACCX(1) + Kaccx*OP(12,2)*SK_ACCX(3) + Kaccx*OP(12,3)*SK_ACCX(2) - Kaccx*OP(12,5)*SK_ACCX(5) + Kaccx*OP(12,6)*SK_ACCX(4) + Kaccx*OP(12,24)*SK_ACCX(5)); -Kfusion(13) = SK_ACCX(1)*(Kaccx*OP(13,23)*SH_ACCX(1) - Kaccx*OP(13,4)*SH_ACCX(1) + Kaccx*OP(13,2)*SK_ACCX(3) + Kaccx*OP(13,3)*SK_ACCX(2) - Kaccx*OP(13,5)*SK_ACCX(5) + Kaccx*OP(13,6)*SK_ACCX(4) + Kaccx*OP(13,24)*SK_ACCX(5)); -Kfusion(14) = SK_ACCX(1)*(Kaccx*OP(14,23)*SH_ACCX(1) - Kaccx*OP(14,4)*SH_ACCX(1) + Kaccx*OP(14,2)*SK_ACCX(3) + Kaccx*OP(14,3)*SK_ACCX(2) - Kaccx*OP(14,5)*SK_ACCX(5) + Kaccx*OP(14,6)*SK_ACCX(4) + Kaccx*OP(14,24)*SK_ACCX(5)); -Kfusion(15) = SK_ACCX(1)*(Kaccx*OP(15,23)*SH_ACCX(1) - Kaccx*OP(15,4)*SH_ACCX(1) + Kaccx*OP(15,2)*SK_ACCX(3) + Kaccx*OP(15,3)*SK_ACCX(2) - Kaccx*OP(15,5)*SK_ACCX(5) + Kaccx*OP(15,6)*SK_ACCX(4) + Kaccx*OP(15,24)*SK_ACCX(5)); -Kfusion(16) = SK_ACCX(1)*(Kaccx*OP(16,23)*SH_ACCX(1) - Kaccx*OP(16,4)*SH_ACCX(1) + Kaccx*OP(16,2)*SK_ACCX(3) + Kaccx*OP(16,3)*SK_ACCX(2) - Kaccx*OP(16,5)*SK_ACCX(5) + Kaccx*OP(16,6)*SK_ACCX(4) + Kaccx*OP(16,24)*SK_ACCX(5)); -Kfusion(17) = SK_ACCX(1)*(Kaccx*OP(17,23)*SH_ACCX(1) - Kaccx*OP(17,4)*SH_ACCX(1) + Kaccx*OP(17,2)*SK_ACCX(3) + Kaccx*OP(17,3)*SK_ACCX(2) - Kaccx*OP(17,5)*SK_ACCX(5) + Kaccx*OP(17,6)*SK_ACCX(4) + Kaccx*OP(17,24)*SK_ACCX(5)); -Kfusion(18) = SK_ACCX(1)*(Kaccx*OP(18,23)*SH_ACCX(1) - Kaccx*OP(18,4)*SH_ACCX(1) + Kaccx*OP(18,2)*SK_ACCX(3) + Kaccx*OP(18,3)*SK_ACCX(2) - Kaccx*OP(18,5)*SK_ACCX(5) + Kaccx*OP(18,6)*SK_ACCX(4) + Kaccx*OP(18,24)*SK_ACCX(5)); -Kfusion(19) = SK_ACCX(1)*(Kaccx*OP(19,23)*SH_ACCX(1) - Kaccx*OP(19,4)*SH_ACCX(1) + Kaccx*OP(19,2)*SK_ACCX(3) + Kaccx*OP(19,3)*SK_ACCX(2) - Kaccx*OP(19,5)*SK_ACCX(5) + Kaccx*OP(19,6)*SK_ACCX(4) + Kaccx*OP(19,24)*SK_ACCX(5)); -Kfusion(20) = SK_ACCX(1)*(Kaccx*OP(20,23)*SH_ACCX(1) - Kaccx*OP(20,4)*SH_ACCX(1) + Kaccx*OP(20,2)*SK_ACCX(3) + Kaccx*OP(20,3)*SK_ACCX(2) - Kaccx*OP(20,5)*SK_ACCX(5) + Kaccx*OP(20,6)*SK_ACCX(4) + Kaccx*OP(20,24)*SK_ACCX(5)); -Kfusion(21) = SK_ACCX(1)*(Kaccx*OP(21,23)*SH_ACCX(1) - Kaccx*OP(21,4)*SH_ACCX(1) + Kaccx*OP(21,2)*SK_ACCX(3) + Kaccx*OP(21,3)*SK_ACCX(2) - Kaccx*OP(21,5)*SK_ACCX(5) + Kaccx*OP(21,6)*SK_ACCX(4) + Kaccx*OP(21,24)*SK_ACCX(5)); -Kfusion(22) = SK_ACCX(1)*(Kaccx*OP(22,23)*SH_ACCX(1) - Kaccx*OP(22,4)*SH_ACCX(1) + Kaccx*OP(22,2)*SK_ACCX(3) + Kaccx*OP(22,3)*SK_ACCX(2) - Kaccx*OP(22,5)*SK_ACCX(5) + Kaccx*OP(22,6)*SK_ACCX(4) + Kaccx*OP(22,24)*SK_ACCX(5)); -Kfusion(23) = SK_ACCX(1)*(Kaccx*OP(23,23)*SH_ACCX(1) - Kaccx*OP(23,4)*SH_ACCX(1) + Kaccx*OP(23,2)*SK_ACCX(3) + Kaccx*OP(23,3)*SK_ACCX(2) - Kaccx*OP(23,5)*SK_ACCX(5) + Kaccx*OP(23,6)*SK_ACCX(4) + Kaccx*OP(23,24)*SK_ACCX(5)); -Kfusion(24) = SK_ACCX(1)*(Kaccx*OP(24,23)*SH_ACCX(1) - Kaccx*OP(24,4)*SH_ACCX(1) + Kaccx*OP(24,2)*SK_ACCX(3) + Kaccx*OP(24,3)*SK_ACCX(2) - Kaccx*OP(24,5)*SK_ACCX(5) + Kaccx*OP(24,6)*SK_ACCX(4) + Kaccx*OP(24,24)*SK_ACCX(5)); -SH_ACCY = zeros(6,1); +Kfusion(1) = -SK_ACCX(1)*(Kaccx*OP(1,5)*SH_ACCX(1) - Kaccx*OP(1,23)*SH_ACCX(1) + Kaccx*OP(1,1)*SK_ACCX(4) - Kaccx*OP(1,3)*SK_ACCX(3) + Kaccx*OP(1,4)*SK_ACCX(2) + Kaccx*OP(1,2)*SK_ACCX(5) + Kaccx*OP(1,6)*SK_ACCX(7) - Kaccx*OP(1,7)*SK_ACCX(6) - Kaccx*OP(1,24)*SK_ACCX(7)); +Kfusion(2) = -SK_ACCX(1)*(Kaccx*OP(2,5)*SH_ACCX(1) - Kaccx*OP(2,23)*SH_ACCX(1) + Kaccx*OP(2,1)*SK_ACCX(4) - Kaccx*OP(2,3)*SK_ACCX(3) + Kaccx*OP(2,4)*SK_ACCX(2) + Kaccx*OP(2,2)*SK_ACCX(5) + Kaccx*OP(2,6)*SK_ACCX(7) - Kaccx*OP(2,7)*SK_ACCX(6) - Kaccx*OP(2,24)*SK_ACCX(7)); +Kfusion(3) = -SK_ACCX(1)*(Kaccx*OP(3,5)*SH_ACCX(1) - Kaccx*OP(3,23)*SH_ACCX(1) + Kaccx*OP(3,1)*SK_ACCX(4) - Kaccx*OP(3,3)*SK_ACCX(3) + Kaccx*OP(3,4)*SK_ACCX(2) + Kaccx*OP(3,2)*SK_ACCX(5) + Kaccx*OP(3,6)*SK_ACCX(7) - Kaccx*OP(3,7)*SK_ACCX(6) - Kaccx*OP(3,24)*SK_ACCX(7)); +Kfusion(4) = -SK_ACCX(1)*(Kaccx*OP(4,5)*SH_ACCX(1) - Kaccx*OP(4,23)*SH_ACCX(1) + Kaccx*OP(4,1)*SK_ACCX(4) - Kaccx*OP(4,3)*SK_ACCX(3) + Kaccx*OP(4,4)*SK_ACCX(2) + Kaccx*OP(4,2)*SK_ACCX(5) + Kaccx*OP(4,6)*SK_ACCX(7) - Kaccx*OP(4,7)*SK_ACCX(6) - Kaccx*OP(4,24)*SK_ACCX(7)); +Kfusion(5) = -SK_ACCX(1)*(Kaccx*OP(5,5)*SH_ACCX(1) - Kaccx*OP(5,23)*SH_ACCX(1) + Kaccx*OP(5,1)*SK_ACCX(4) - Kaccx*OP(5,3)*SK_ACCX(3) + Kaccx*OP(5,4)*SK_ACCX(2) + Kaccx*OP(5,2)*SK_ACCX(5) + Kaccx*OP(5,6)*SK_ACCX(7) - Kaccx*OP(5,7)*SK_ACCX(6) - Kaccx*OP(5,24)*SK_ACCX(7)); +Kfusion(6) = -SK_ACCX(1)*(Kaccx*OP(6,5)*SH_ACCX(1) - Kaccx*OP(6,23)*SH_ACCX(1) + Kaccx*OP(6,1)*SK_ACCX(4) - Kaccx*OP(6,3)*SK_ACCX(3) + Kaccx*OP(6,4)*SK_ACCX(2) + Kaccx*OP(6,2)*SK_ACCX(5) + Kaccx*OP(6,6)*SK_ACCX(7) - Kaccx*OP(6,7)*SK_ACCX(6) - Kaccx*OP(6,24)*SK_ACCX(7)); +Kfusion(7) = -SK_ACCX(1)*(Kaccx*OP(7,5)*SH_ACCX(1) - Kaccx*OP(7,23)*SH_ACCX(1) + Kaccx*OP(7,1)*SK_ACCX(4) - Kaccx*OP(7,3)*SK_ACCX(3) + Kaccx*OP(7,4)*SK_ACCX(2) + Kaccx*OP(7,2)*SK_ACCX(5) + Kaccx*OP(7,6)*SK_ACCX(7) - Kaccx*OP(7,7)*SK_ACCX(6) - Kaccx*OP(7,24)*SK_ACCX(7)); +Kfusion(8) = -SK_ACCX(1)*(Kaccx*OP(8,5)*SH_ACCX(1) - Kaccx*OP(8,23)*SH_ACCX(1) + Kaccx*OP(8,1)*SK_ACCX(4) - Kaccx*OP(8,3)*SK_ACCX(3) + Kaccx*OP(8,4)*SK_ACCX(2) + Kaccx*OP(8,2)*SK_ACCX(5) + Kaccx*OP(8,6)*SK_ACCX(7) - Kaccx*OP(8,7)*SK_ACCX(6) - Kaccx*OP(8,24)*SK_ACCX(7)); +Kfusion(9) = -SK_ACCX(1)*(Kaccx*OP(9,5)*SH_ACCX(1) - Kaccx*OP(9,23)*SH_ACCX(1) + Kaccx*OP(9,1)*SK_ACCX(4) - Kaccx*OP(9,3)*SK_ACCX(3) + Kaccx*OP(9,4)*SK_ACCX(2) + Kaccx*OP(9,2)*SK_ACCX(5) + Kaccx*OP(9,6)*SK_ACCX(7) - Kaccx*OP(9,7)*SK_ACCX(6) - Kaccx*OP(9,24)*SK_ACCX(7)); +Kfusion(10) = -SK_ACCX(1)*(Kaccx*OP(10,5)*SH_ACCX(1) - Kaccx*OP(10,23)*SH_ACCX(1) + Kaccx*OP(10,1)*SK_ACCX(4) - Kaccx*OP(10,3)*SK_ACCX(3) + Kaccx*OP(10,4)*SK_ACCX(2) + Kaccx*OP(10,2)*SK_ACCX(5) + Kaccx*OP(10,6)*SK_ACCX(7) - Kaccx*OP(10,7)*SK_ACCX(6) - Kaccx*OP(10,24)*SK_ACCX(7)); +Kfusion(11) = -SK_ACCX(1)*(Kaccx*OP(11,5)*SH_ACCX(1) - Kaccx*OP(11,23)*SH_ACCX(1) + Kaccx*OP(11,1)*SK_ACCX(4) - Kaccx*OP(11,3)*SK_ACCX(3) + Kaccx*OP(11,4)*SK_ACCX(2) + Kaccx*OP(11,2)*SK_ACCX(5) + Kaccx*OP(11,6)*SK_ACCX(7) - Kaccx*OP(11,7)*SK_ACCX(6) - Kaccx*OP(11,24)*SK_ACCX(7)); +Kfusion(12) = -SK_ACCX(1)*(Kaccx*OP(12,5)*SH_ACCX(1) - Kaccx*OP(12,23)*SH_ACCX(1) + Kaccx*OP(12,1)*SK_ACCX(4) - Kaccx*OP(12,3)*SK_ACCX(3) + Kaccx*OP(12,4)*SK_ACCX(2) + Kaccx*OP(12,2)*SK_ACCX(5) + Kaccx*OP(12,6)*SK_ACCX(7) - Kaccx*OP(12,7)*SK_ACCX(6) - Kaccx*OP(12,24)*SK_ACCX(7)); +Kfusion(13) = -SK_ACCX(1)*(Kaccx*OP(13,5)*SH_ACCX(1) - Kaccx*OP(13,23)*SH_ACCX(1) + Kaccx*OP(13,1)*SK_ACCX(4) - Kaccx*OP(13,3)*SK_ACCX(3) + Kaccx*OP(13,4)*SK_ACCX(2) + Kaccx*OP(13,2)*SK_ACCX(5) + Kaccx*OP(13,6)*SK_ACCX(7) - Kaccx*OP(13,7)*SK_ACCX(6) - Kaccx*OP(13,24)*SK_ACCX(7)); +Kfusion(14) = -SK_ACCX(1)*(Kaccx*OP(14,5)*SH_ACCX(1) - Kaccx*OP(14,23)*SH_ACCX(1) + Kaccx*OP(14,1)*SK_ACCX(4) - Kaccx*OP(14,3)*SK_ACCX(3) + Kaccx*OP(14,4)*SK_ACCX(2) + Kaccx*OP(14,2)*SK_ACCX(5) + Kaccx*OP(14,6)*SK_ACCX(7) - Kaccx*OP(14,7)*SK_ACCX(6) - Kaccx*OP(14,24)*SK_ACCX(7)); +Kfusion(15) = -SK_ACCX(1)*(Kaccx*OP(15,5)*SH_ACCX(1) - Kaccx*OP(15,23)*SH_ACCX(1) + Kaccx*OP(15,1)*SK_ACCX(4) - Kaccx*OP(15,3)*SK_ACCX(3) + Kaccx*OP(15,4)*SK_ACCX(2) + Kaccx*OP(15,2)*SK_ACCX(5) + Kaccx*OP(15,6)*SK_ACCX(7) - Kaccx*OP(15,7)*SK_ACCX(6) - Kaccx*OP(15,24)*SK_ACCX(7)); +Kfusion(16) = -SK_ACCX(1)*(Kaccx*OP(16,5)*SH_ACCX(1) - Kaccx*OP(16,23)*SH_ACCX(1) + Kaccx*OP(16,1)*SK_ACCX(4) - Kaccx*OP(16,3)*SK_ACCX(3) + Kaccx*OP(16,4)*SK_ACCX(2) + Kaccx*OP(16,2)*SK_ACCX(5) + Kaccx*OP(16,6)*SK_ACCX(7) - Kaccx*OP(16,7)*SK_ACCX(6) - Kaccx*OP(16,24)*SK_ACCX(7)); +Kfusion(17) = -SK_ACCX(1)*(Kaccx*OP(17,5)*SH_ACCX(1) - Kaccx*OP(17,23)*SH_ACCX(1) + Kaccx*OP(17,1)*SK_ACCX(4) - Kaccx*OP(17,3)*SK_ACCX(3) + Kaccx*OP(17,4)*SK_ACCX(2) + Kaccx*OP(17,2)*SK_ACCX(5) + Kaccx*OP(17,6)*SK_ACCX(7) - Kaccx*OP(17,7)*SK_ACCX(6) - Kaccx*OP(17,24)*SK_ACCX(7)); +Kfusion(18) = -SK_ACCX(1)*(Kaccx*OP(18,5)*SH_ACCX(1) - Kaccx*OP(18,23)*SH_ACCX(1) + Kaccx*OP(18,1)*SK_ACCX(4) - Kaccx*OP(18,3)*SK_ACCX(3) + Kaccx*OP(18,4)*SK_ACCX(2) + Kaccx*OP(18,2)*SK_ACCX(5) + Kaccx*OP(18,6)*SK_ACCX(7) - Kaccx*OP(18,7)*SK_ACCX(6) - Kaccx*OP(18,24)*SK_ACCX(7)); +Kfusion(19) = -SK_ACCX(1)*(Kaccx*OP(19,5)*SH_ACCX(1) - Kaccx*OP(19,23)*SH_ACCX(1) + Kaccx*OP(19,1)*SK_ACCX(4) - Kaccx*OP(19,3)*SK_ACCX(3) + Kaccx*OP(19,4)*SK_ACCX(2) + Kaccx*OP(19,2)*SK_ACCX(5) + Kaccx*OP(19,6)*SK_ACCX(7) - Kaccx*OP(19,7)*SK_ACCX(6) - Kaccx*OP(19,24)*SK_ACCX(7)); +Kfusion(20) = -SK_ACCX(1)*(Kaccx*OP(20,5)*SH_ACCX(1) - Kaccx*OP(20,23)*SH_ACCX(1) + Kaccx*OP(20,1)*SK_ACCX(4) - Kaccx*OP(20,3)*SK_ACCX(3) + Kaccx*OP(20,4)*SK_ACCX(2) + Kaccx*OP(20,2)*SK_ACCX(5) + Kaccx*OP(20,6)*SK_ACCX(7) - Kaccx*OP(20,7)*SK_ACCX(6) - Kaccx*OP(20,24)*SK_ACCX(7)); +Kfusion(21) = -SK_ACCX(1)*(Kaccx*OP(21,5)*SH_ACCX(1) - Kaccx*OP(21,23)*SH_ACCX(1) + Kaccx*OP(21,1)*SK_ACCX(4) - Kaccx*OP(21,3)*SK_ACCX(3) + Kaccx*OP(21,4)*SK_ACCX(2) + Kaccx*OP(21,2)*SK_ACCX(5) + Kaccx*OP(21,6)*SK_ACCX(7) - Kaccx*OP(21,7)*SK_ACCX(6) - Kaccx*OP(21,24)*SK_ACCX(7)); +Kfusion(22) = -SK_ACCX(1)*(Kaccx*OP(22,5)*SH_ACCX(1) - Kaccx*OP(22,23)*SH_ACCX(1) + Kaccx*OP(22,1)*SK_ACCX(4) - Kaccx*OP(22,3)*SK_ACCX(3) + Kaccx*OP(22,4)*SK_ACCX(2) + Kaccx*OP(22,2)*SK_ACCX(5) + Kaccx*OP(22,6)*SK_ACCX(7) - Kaccx*OP(22,7)*SK_ACCX(6) - Kaccx*OP(22,24)*SK_ACCX(7)); +Kfusion(23) = -SK_ACCX(1)*(Kaccx*OP(23,5)*SH_ACCX(1) - Kaccx*OP(23,23)*SH_ACCX(1) + Kaccx*OP(23,1)*SK_ACCX(4) - Kaccx*OP(23,3)*SK_ACCX(3) + Kaccx*OP(23,4)*SK_ACCX(2) + Kaccx*OP(23,2)*SK_ACCX(5) + Kaccx*OP(23,6)*SK_ACCX(7) - Kaccx*OP(23,7)*SK_ACCX(6) - Kaccx*OP(23,24)*SK_ACCX(7)); +Kfusion(24) = -SK_ACCX(1)*(Kaccx*OP(24,5)*SH_ACCX(1) - Kaccx*OP(24,23)*SH_ACCX(1) + Kaccx*OP(24,1)*SK_ACCX(4) - Kaccx*OP(24,3)*SK_ACCX(3) + Kaccx*OP(24,4)*SK_ACCX(2) + Kaccx*OP(24,2)*SK_ACCX(5) + Kaccx*OP(24,6)*SK_ACCX(7) - Kaccx*OP(24,7)*SK_ACCX(6) - Kaccx*OP(24,24)*SK_ACCX(7)); +SH_ACCY = zeros(3,1); SH_ACCY(1) = q0^2 - q1^2 + q2^2 - q3^2; -SH_ACCY(2) = ve - vwe; -SH_ACCY(3) = vn - vwn; -SH_ACCY(4) = q1^2; -SH_ACCY(5) = 2*q0*q1; -SH_ACCY(6) = 2*q0*q3; +SH_ACCY(2) = vn - vwn; +SH_ACCY(3) = ve - vwe; H_ACCY = zeros(1,24); -H_ACCY(1,1) = Kaccy*(SH_ACCY(2)*(SH_ACCY(5) - 2*q2*q3) - SH_ACCY(3)*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY(4) - q0^2 + q2^2 - q3^2)); -H_ACCY(1,3) = Kaccy*(SH_ACCY(3)*(SH_ACCY(4) + q0^2 - q2^2 - q3^2) + SH_ACCY(2)*(SH_ACCY(6) + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)); -H_ACCY(1,4) = Kaccy*(SH_ACCY(6) - 2*q1*q2); -H_ACCY(1,5) = -Kaccy*SH_ACCY(1); -H_ACCY(1,6) = -Kaccy*(SH_ACCY(5) + 2*q2*q3); +H_ACCY(1,1) = -Kaccy*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd); +H_ACCY(1,2) = -Kaccy*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd); +H_ACCY(1,3) = -Kaccy*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd); +H_ACCY(1,4) = Kaccy*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd); +H_ACCY(1,5) = Kaccy*(2*q0*q3 - 2*q1*q2); +H_ACCY(1,6) = -Kaccy*SH_ACCY(1); +H_ACCY(1,7) = -Kaccy*(2*q0*q1 + 2*q2*q3); H_ACCY(1,23) = -2*Kaccy*(q0*q3 - q1*q2); H_ACCY(1,24) = Kaccy*SH_ACCY(1); -SK_ACCY = zeros(7,1); -SK_ACCY(1) = 1/(R_ACC - Kaccy*SH_ACCY(1)*(Kaccy*OP(24,5)*SH_ACCY(1) - Kaccy*OP(5,5)*SH_ACCY(1) + Kaccy*OP(1,5)*(SH_ACCY(2)*(SH_ACCY(5) - 2*q2*q3) - SH_ACCY(3)*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY(4) - q0^2 + q2^2 - q3^2)) + Kaccy*OP(3,5)*(SH_ACCY(3)*(SH_ACCY(4) + q0^2 - q2^2 - q3^2) + SH_ACCY(2)*(SH_ACCY(6) + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)) - 2*Kaccy*OP(23,5)*(q0*q3 - q1*q2) + Kaccy*OP(4,5)*(SH_ACCY(6) - 2*q1*q2) - Kaccy*OP(6,5)*(SH_ACCY(5) + 2*q2*q3)) + Kaccy*SH_ACCY(1)*(Kaccy*OP(24,24)*SH_ACCY(1) - Kaccy*OP(5,24)*SH_ACCY(1) + Kaccy*OP(1,24)*(SH_ACCY(2)*(SH_ACCY(5) - 2*q2*q3) - SH_ACCY(3)*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY(4) - q0^2 + q2^2 - q3^2)) + Kaccy*OP(3,24)*(SH_ACCY(3)*(SH_ACCY(4) + q0^2 - q2^2 - q3^2) + SH_ACCY(2)*(SH_ACCY(6) + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)) - 2*Kaccy*OP(23,24)*(q0*q3 - q1*q2) + Kaccy*OP(4,24)*(SH_ACCY(6) - 2*q1*q2) - Kaccy*OP(6,24)*(SH_ACCY(5) + 2*q2*q3)) + Kaccy*(SH_ACCY(2)*(SH_ACCY(5) - 2*q2*q3) - SH_ACCY(3)*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY(4) - q0^2 + q2^2 - q3^2))*(Kaccy*OP(24,1)*SH_ACCY(1) - Kaccy*OP(5,1)*SH_ACCY(1) + Kaccy*OP(1,1)*(SH_ACCY(2)*(SH_ACCY(5) - 2*q2*q3) - SH_ACCY(3)*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY(4) - q0^2 + q2^2 - q3^2)) + Kaccy*OP(3,1)*(SH_ACCY(3)*(SH_ACCY(4) + q0^2 - q2^2 - q3^2) + SH_ACCY(2)*(SH_ACCY(6) + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)) - 2*Kaccy*OP(23,1)*(q0*q3 - q1*q2) + Kaccy*OP(4,1)*(SH_ACCY(6) - 2*q1*q2) - Kaccy*OP(6,1)*(SH_ACCY(5) + 2*q2*q3)) + Kaccy*(SH_ACCY(3)*(SH_ACCY(4) + q0^2 - q2^2 - q3^2) + SH_ACCY(2)*(SH_ACCY(6) + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3))*(Kaccy*OP(24,3)*SH_ACCY(1) - Kaccy*OP(5,3)*SH_ACCY(1) + Kaccy*OP(1,3)*(SH_ACCY(2)*(SH_ACCY(5) - 2*q2*q3) - SH_ACCY(3)*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY(4) - q0^2 + q2^2 - q3^2)) + Kaccy*OP(3,3)*(SH_ACCY(3)*(SH_ACCY(4) + q0^2 - q2^2 - q3^2) + SH_ACCY(2)*(SH_ACCY(6) + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)) - 2*Kaccy*OP(23,3)*(q0*q3 - q1*q2) + Kaccy*OP(4,3)*(SH_ACCY(6) - 2*q1*q2) - Kaccy*OP(6,3)*(SH_ACCY(5) + 2*q2*q3)) - 2*Kaccy*(q0*q3 - q1*q2)*(Kaccy*OP(24,23)*SH_ACCY(1) - Kaccy*OP(5,23)*SH_ACCY(1) + Kaccy*OP(1,23)*(SH_ACCY(2)*(SH_ACCY(5) - 2*q2*q3) - SH_ACCY(3)*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY(4) - q0^2 + q2^2 - q3^2)) + Kaccy*OP(3,23)*(SH_ACCY(3)*(SH_ACCY(4) + q0^2 - q2^2 - q3^2) + SH_ACCY(2)*(SH_ACCY(6) + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)) - 2*Kaccy*OP(23,23)*(q0*q3 - q1*q2) + Kaccy*OP(4,23)*(SH_ACCY(6) - 2*q1*q2) - Kaccy*OP(6,23)*(SH_ACCY(5) + 2*q2*q3)) + Kaccy*(SH_ACCY(6) - 2*q1*q2)*(Kaccy*OP(24,4)*SH_ACCY(1) - Kaccy*OP(5,4)*SH_ACCY(1) + Kaccy*OP(1,4)*(SH_ACCY(2)*(SH_ACCY(5) - 2*q2*q3) - SH_ACCY(3)*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY(4) - q0^2 + q2^2 - q3^2)) + Kaccy*OP(3,4)*(SH_ACCY(3)*(SH_ACCY(4) + q0^2 - q2^2 - q3^2) + SH_ACCY(2)*(SH_ACCY(6) + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)) - 2*Kaccy*OP(23,4)*(q0*q3 - q1*q2) + Kaccy*OP(4,4)*(SH_ACCY(6) - 2*q1*q2) - Kaccy*OP(6,4)*(SH_ACCY(5) + 2*q2*q3)) - Kaccy*(SH_ACCY(5) + 2*q2*q3)*(Kaccy*OP(24,6)*SH_ACCY(1) - Kaccy*OP(5,6)*SH_ACCY(1) + Kaccy*OP(1,6)*(SH_ACCY(2)*(SH_ACCY(5) - 2*q2*q3) - SH_ACCY(3)*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY(4) - q0^2 + q2^2 - q3^2)) + Kaccy*OP(3,6)*(SH_ACCY(3)*(SH_ACCY(4) + q0^2 - q2^2 - q3^2) + SH_ACCY(2)*(SH_ACCY(6) + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)) - 2*Kaccy*OP(23,6)*(q0*q3 - q1*q2) + Kaccy*OP(4,6)*(SH_ACCY(6) - 2*q1*q2) - Kaccy*OP(6,6)*(SH_ACCY(5) + 2*q2*q3))); -SK_ACCY(2) = SH_ACCY(3)*(SH_ACCY(4) + q0^2 - q2^2 - q3^2) + SH_ACCY(2)*(SH_ACCY(6) + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3); -SK_ACCY(3) = SH_ACCY(2)*(SH_ACCY(5) - 2*q2*q3) - SH_ACCY(3)*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY(4) - q0^2 + q2^2 - q3^2); -SK_ACCY(4) = q0*q3 - q1*q2; -SK_ACCY(5) = SH_ACCY(6) - 2*q1*q2; -SK_ACCY(6) = SH_ACCY(5) + 2*q2*q3; -SK_ACCY(7) = SH_ACCY(1); +SK_ACCY = zeros(9,1); +SK_ACCY(1) = 1/(R_ACC + Kaccy*SH_ACCY(1)*(Kaccy*OP(6,6)*SH_ACCY(1) - Kaccy*OP(24,6)*SH_ACCY(1) - Kaccy*OP(5,6)*(2*q0*q3 - 2*q1*q2) + Kaccy*OP(7,6)*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*OP(23,6)*(q0*q3 - q1*q2) + Kaccy*OP(1,6)*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd) + Kaccy*OP(2,6)*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd) + Kaccy*OP(3,6)*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd) - Kaccy*OP(4,6)*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd)) - Kaccy*SH_ACCY(1)*(Kaccy*OP(6,24)*SH_ACCY(1) - Kaccy*OP(24,24)*SH_ACCY(1) - Kaccy*OP(5,24)*(2*q0*q3 - 2*q1*q2) + Kaccy*OP(7,24)*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*OP(23,24)*(q0*q3 - q1*q2) + Kaccy*OP(1,24)*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd) + Kaccy*OP(2,24)*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd) + Kaccy*OP(3,24)*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd) - Kaccy*OP(4,24)*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd)) - Kaccy*(2*q0*q3 - 2*q1*q2)*(Kaccy*OP(6,5)*SH_ACCY(1) - Kaccy*OP(24,5)*SH_ACCY(1) - Kaccy*OP(5,5)*(2*q0*q3 - 2*q1*q2) + Kaccy*OP(7,5)*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*OP(23,5)*(q0*q3 - q1*q2) + Kaccy*OP(1,5)*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd) + Kaccy*OP(2,5)*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd) + Kaccy*OP(3,5)*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd) - Kaccy*OP(4,5)*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd)) + Kaccy*(2*q0*q1 + 2*q2*q3)*(Kaccy*OP(6,7)*SH_ACCY(1) - Kaccy*OP(24,7)*SH_ACCY(1) - Kaccy*OP(5,7)*(2*q0*q3 - 2*q1*q2) + Kaccy*OP(7,7)*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*OP(23,7)*(q0*q3 - q1*q2) + Kaccy*OP(1,7)*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd) + Kaccy*OP(2,7)*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd) + Kaccy*OP(3,7)*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd) - Kaccy*OP(4,7)*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd)) + 2*Kaccy*(q0*q3 - q1*q2)*(Kaccy*OP(6,23)*SH_ACCY(1) - Kaccy*OP(24,23)*SH_ACCY(1) - Kaccy*OP(5,23)*(2*q0*q3 - 2*q1*q2) + Kaccy*OP(7,23)*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*OP(23,23)*(q0*q3 - q1*q2) + Kaccy*OP(1,23)*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd) + Kaccy*OP(2,23)*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd) + Kaccy*OP(3,23)*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd) - Kaccy*OP(4,23)*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd)) + Kaccy*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd)*(Kaccy*OP(6,1)*SH_ACCY(1) - Kaccy*OP(24,1)*SH_ACCY(1) - Kaccy*OP(5,1)*(2*q0*q3 - 2*q1*q2) + Kaccy*OP(7,1)*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*OP(23,1)*(q0*q3 - q1*q2) + Kaccy*OP(1,1)*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd) + Kaccy*OP(2,1)*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd) + Kaccy*OP(3,1)*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd) - Kaccy*OP(4,1)*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd)) + Kaccy*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd)*(Kaccy*OP(6,2)*SH_ACCY(1) - Kaccy*OP(24,2)*SH_ACCY(1) - Kaccy*OP(5,2)*(2*q0*q3 - 2*q1*q2) + Kaccy*OP(7,2)*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*OP(23,2)*(q0*q3 - q1*q2) + Kaccy*OP(1,2)*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd) + Kaccy*OP(2,2)*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd) + Kaccy*OP(3,2)*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd) - Kaccy*OP(4,2)*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd)) + Kaccy*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd)*(Kaccy*OP(6,3)*SH_ACCY(1) - Kaccy*OP(24,3)*SH_ACCY(1) - Kaccy*OP(5,3)*(2*q0*q3 - 2*q1*q2) + Kaccy*OP(7,3)*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*OP(23,3)*(q0*q3 - q1*q2) + Kaccy*OP(1,3)*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd) + Kaccy*OP(2,3)*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd) + Kaccy*OP(3,3)*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd) - Kaccy*OP(4,3)*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd)) - Kaccy*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd)*(Kaccy*OP(6,4)*SH_ACCY(1) - Kaccy*OP(24,4)*SH_ACCY(1) - Kaccy*OP(5,4)*(2*q0*q3 - 2*q1*q2) + Kaccy*OP(7,4)*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*OP(23,4)*(q0*q3 - q1*q2) + Kaccy*OP(1,4)*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd) + Kaccy*OP(2,4)*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd) + Kaccy*OP(3,4)*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd) - Kaccy*OP(4,4)*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd))); +SK_ACCY(2) = 2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd; +SK_ACCY(3) = 2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd; +SK_ACCY(4) = 2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd; +SK_ACCY(5) = 2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd; +SK_ACCY(6) = 2*q0*q3 - 2*q1*q2; +SK_ACCY(7) = q0*q3 - q1*q2; +SK_ACCY(8) = 2*q0*q1 + 2*q2*q3; +SK_ACCY(9) = SH_ACCY(1); Kfusion = zeros(24,1); Kfusion = zeros(1,1); -Kfusion(1) = SK_ACCY(1)*(Kaccy*OP(1,1)*SK_ACCY(3) + Kaccy*OP(1,3)*SK_ACCY(2) + Kaccy*OP(1,4)*SK_ACCY(5) - Kaccy*OP(1,5)*SK_ACCY(7) - Kaccy*OP(1,6)*SK_ACCY(6) - 2*Kaccy*OP(1,23)*SK_ACCY(4) + Kaccy*OP(1,24)*SK_ACCY(7)); -Kfusion(2) = SK_ACCY(1)*(Kaccy*OP(2,1)*SK_ACCY(3) + Kaccy*OP(2,3)*SK_ACCY(2) + Kaccy*OP(2,4)*SK_ACCY(5) - Kaccy*OP(2,5)*SK_ACCY(7) - Kaccy*OP(2,6)*SK_ACCY(6) - 2*Kaccy*OP(2,23)*SK_ACCY(4) + Kaccy*OP(2,24)*SK_ACCY(7)); -Kfusion(3) = SK_ACCY(1)*(Kaccy*OP(3,1)*SK_ACCY(3) + Kaccy*OP(3,3)*SK_ACCY(2) + Kaccy*OP(3,4)*SK_ACCY(5) - Kaccy*OP(3,5)*SK_ACCY(7) - Kaccy*OP(3,6)*SK_ACCY(6) - 2*Kaccy*OP(3,23)*SK_ACCY(4) + Kaccy*OP(3,24)*SK_ACCY(7)); -Kfusion(4) = SK_ACCY(1)*(Kaccy*OP(4,1)*SK_ACCY(3) + Kaccy*OP(4,3)*SK_ACCY(2) + Kaccy*OP(4,4)*SK_ACCY(5) - Kaccy*OP(4,5)*SK_ACCY(7) - Kaccy*OP(4,6)*SK_ACCY(6) - 2*Kaccy*OP(4,23)*SK_ACCY(4) + Kaccy*OP(4,24)*SK_ACCY(7)); -Kfusion(5) = SK_ACCY(1)*(Kaccy*OP(5,1)*SK_ACCY(3) + Kaccy*OP(5,3)*SK_ACCY(2) + Kaccy*OP(5,4)*SK_ACCY(5) - Kaccy*OP(5,5)*SK_ACCY(7) - Kaccy*OP(5,6)*SK_ACCY(6) - 2*Kaccy*OP(5,23)*SK_ACCY(4) + Kaccy*OP(5,24)*SK_ACCY(7)); -Kfusion(6) = SK_ACCY(1)*(Kaccy*OP(6,1)*SK_ACCY(3) + Kaccy*OP(6,3)*SK_ACCY(2) + Kaccy*OP(6,4)*SK_ACCY(5) - Kaccy*OP(6,5)*SK_ACCY(7) - Kaccy*OP(6,6)*SK_ACCY(6) - 2*Kaccy*OP(6,23)*SK_ACCY(4) + Kaccy*OP(6,24)*SK_ACCY(7)); -Kfusion(7) = SK_ACCY(1)*(Kaccy*OP(7,1)*SK_ACCY(3) + Kaccy*OP(7,3)*SK_ACCY(2) + Kaccy*OP(7,4)*SK_ACCY(5) - Kaccy*OP(7,5)*SK_ACCY(7) - Kaccy*OP(7,6)*SK_ACCY(6) - 2*Kaccy*OP(7,23)*SK_ACCY(4) + Kaccy*OP(7,24)*SK_ACCY(7)); -Kfusion(8) = SK_ACCY(1)*(Kaccy*OP(8,1)*SK_ACCY(3) + Kaccy*OP(8,3)*SK_ACCY(2) + Kaccy*OP(8,4)*SK_ACCY(5) - Kaccy*OP(8,5)*SK_ACCY(7) - Kaccy*OP(8,6)*SK_ACCY(6) - 2*Kaccy*OP(8,23)*SK_ACCY(4) + Kaccy*OP(8,24)*SK_ACCY(7)); -Kfusion(9) = SK_ACCY(1)*(Kaccy*OP(9,1)*SK_ACCY(3) + Kaccy*OP(9,3)*SK_ACCY(2) + Kaccy*OP(9,4)*SK_ACCY(5) - Kaccy*OP(9,5)*SK_ACCY(7) - Kaccy*OP(9,6)*SK_ACCY(6) - 2*Kaccy*OP(9,23)*SK_ACCY(4) + Kaccy*OP(9,24)*SK_ACCY(7)); -Kfusion(10) = SK_ACCY(1)*(Kaccy*OP(10,1)*SK_ACCY(3) + Kaccy*OP(10,3)*SK_ACCY(2) + Kaccy*OP(10,4)*SK_ACCY(5) - Kaccy*OP(10,5)*SK_ACCY(7) - Kaccy*OP(10,6)*SK_ACCY(6) - 2*Kaccy*OP(10,23)*SK_ACCY(4) + Kaccy*OP(10,24)*SK_ACCY(7)); -Kfusion(11) = SK_ACCY(1)*(Kaccy*OP(11,1)*SK_ACCY(3) + Kaccy*OP(11,3)*SK_ACCY(2) + Kaccy*OP(11,4)*SK_ACCY(5) - Kaccy*OP(11,5)*SK_ACCY(7) - Kaccy*OP(11,6)*SK_ACCY(6) - 2*Kaccy*OP(11,23)*SK_ACCY(4) + Kaccy*OP(11,24)*SK_ACCY(7)); -Kfusion(12) = SK_ACCY(1)*(Kaccy*OP(12,1)*SK_ACCY(3) + Kaccy*OP(12,3)*SK_ACCY(2) + Kaccy*OP(12,4)*SK_ACCY(5) - Kaccy*OP(12,5)*SK_ACCY(7) - Kaccy*OP(12,6)*SK_ACCY(6) - 2*Kaccy*OP(12,23)*SK_ACCY(4) + Kaccy*OP(12,24)*SK_ACCY(7)); -Kfusion(13) = SK_ACCY(1)*(Kaccy*OP(13,1)*SK_ACCY(3) + Kaccy*OP(13,3)*SK_ACCY(2) + Kaccy*OP(13,4)*SK_ACCY(5) - Kaccy*OP(13,5)*SK_ACCY(7) - Kaccy*OP(13,6)*SK_ACCY(6) - 2*Kaccy*OP(13,23)*SK_ACCY(4) + Kaccy*OP(13,24)*SK_ACCY(7)); -Kfusion(14) = SK_ACCY(1)*(Kaccy*OP(14,1)*SK_ACCY(3) + Kaccy*OP(14,3)*SK_ACCY(2) + Kaccy*OP(14,4)*SK_ACCY(5) - Kaccy*OP(14,5)*SK_ACCY(7) - Kaccy*OP(14,6)*SK_ACCY(6) - 2*Kaccy*OP(14,23)*SK_ACCY(4) + Kaccy*OP(14,24)*SK_ACCY(7)); -Kfusion(15) = SK_ACCY(1)*(Kaccy*OP(15,1)*SK_ACCY(3) + Kaccy*OP(15,3)*SK_ACCY(2) + Kaccy*OP(15,4)*SK_ACCY(5) - Kaccy*OP(15,5)*SK_ACCY(7) - Kaccy*OP(15,6)*SK_ACCY(6) - 2*Kaccy*OP(15,23)*SK_ACCY(4) + Kaccy*OP(15,24)*SK_ACCY(7)); -Kfusion(16) = SK_ACCY(1)*(Kaccy*OP(16,1)*SK_ACCY(3) + Kaccy*OP(16,3)*SK_ACCY(2) + Kaccy*OP(16,4)*SK_ACCY(5) - Kaccy*OP(16,5)*SK_ACCY(7) - Kaccy*OP(16,6)*SK_ACCY(6) - 2*Kaccy*OP(16,23)*SK_ACCY(4) + Kaccy*OP(16,24)*SK_ACCY(7)); -Kfusion(17) = SK_ACCY(1)*(Kaccy*OP(17,1)*SK_ACCY(3) + Kaccy*OP(17,3)*SK_ACCY(2) + Kaccy*OP(17,4)*SK_ACCY(5) - Kaccy*OP(17,5)*SK_ACCY(7) - Kaccy*OP(17,6)*SK_ACCY(6) - 2*Kaccy*OP(17,23)*SK_ACCY(4) + Kaccy*OP(17,24)*SK_ACCY(7)); -Kfusion(18) = SK_ACCY(1)*(Kaccy*OP(18,1)*SK_ACCY(3) + Kaccy*OP(18,3)*SK_ACCY(2) + Kaccy*OP(18,4)*SK_ACCY(5) - Kaccy*OP(18,5)*SK_ACCY(7) - Kaccy*OP(18,6)*SK_ACCY(6) - 2*Kaccy*OP(18,23)*SK_ACCY(4) + Kaccy*OP(18,24)*SK_ACCY(7)); -Kfusion(19) = SK_ACCY(1)*(Kaccy*OP(19,1)*SK_ACCY(3) + Kaccy*OP(19,3)*SK_ACCY(2) + Kaccy*OP(19,4)*SK_ACCY(5) - Kaccy*OP(19,5)*SK_ACCY(7) - Kaccy*OP(19,6)*SK_ACCY(6) - 2*Kaccy*OP(19,23)*SK_ACCY(4) + Kaccy*OP(19,24)*SK_ACCY(7)); -Kfusion(20) = SK_ACCY(1)*(Kaccy*OP(20,1)*SK_ACCY(3) + Kaccy*OP(20,3)*SK_ACCY(2) + Kaccy*OP(20,4)*SK_ACCY(5) - Kaccy*OP(20,5)*SK_ACCY(7) - Kaccy*OP(20,6)*SK_ACCY(6) - 2*Kaccy*OP(20,23)*SK_ACCY(4) + Kaccy*OP(20,24)*SK_ACCY(7)); -Kfusion(21) = SK_ACCY(1)*(Kaccy*OP(21,1)*SK_ACCY(3) + Kaccy*OP(21,3)*SK_ACCY(2) + Kaccy*OP(21,4)*SK_ACCY(5) - Kaccy*OP(21,5)*SK_ACCY(7) - Kaccy*OP(21,6)*SK_ACCY(6) - 2*Kaccy*OP(21,23)*SK_ACCY(4) + Kaccy*OP(21,24)*SK_ACCY(7)); -Kfusion(22) = SK_ACCY(1)*(Kaccy*OP(22,1)*SK_ACCY(3) + Kaccy*OP(22,3)*SK_ACCY(2) + Kaccy*OP(22,4)*SK_ACCY(5) - Kaccy*OP(22,5)*SK_ACCY(7) - Kaccy*OP(22,6)*SK_ACCY(6) - 2*Kaccy*OP(22,23)*SK_ACCY(4) + Kaccy*OP(22,24)*SK_ACCY(7)); -Kfusion(23) = SK_ACCY(1)*(Kaccy*OP(23,1)*SK_ACCY(3) + Kaccy*OP(23,3)*SK_ACCY(2) + Kaccy*OP(23,4)*SK_ACCY(5) - Kaccy*OP(23,5)*SK_ACCY(7) - Kaccy*OP(23,6)*SK_ACCY(6) - 2*Kaccy*OP(23,23)*SK_ACCY(4) + Kaccy*OP(23,24)*SK_ACCY(7)); -Kfusion(24) = SK_ACCY(1)*(Kaccy*OP(24,1)*SK_ACCY(3) + Kaccy*OP(24,3)*SK_ACCY(2) + Kaccy*OP(24,4)*SK_ACCY(5) - Kaccy*OP(24,5)*SK_ACCY(7) - Kaccy*OP(24,6)*SK_ACCY(6) - 2*Kaccy*OP(24,23)*SK_ACCY(4) + Kaccy*OP(24,24)*SK_ACCY(7)); +Kfusion(1) = -SK_ACCY(1)*(Kaccy*OP(1,1)*SK_ACCY(4) + Kaccy*OP(1,2)*SK_ACCY(3) - Kaccy*OP(1,4)*SK_ACCY(2) + Kaccy*OP(1,3)*SK_ACCY(5) - Kaccy*OP(1,5)*SK_ACCY(6) + Kaccy*OP(1,6)*SK_ACCY(9) + Kaccy*OP(1,7)*SK_ACCY(8) + 2*Kaccy*OP(1,23)*SK_ACCY(7) - Kaccy*OP(1,24)*SK_ACCY(9)); +Kfusion(2) = -SK_ACCY(1)*(Kaccy*OP(2,1)*SK_ACCY(4) + Kaccy*OP(2,2)*SK_ACCY(3) - Kaccy*OP(2,4)*SK_ACCY(2) + Kaccy*OP(2,3)*SK_ACCY(5) - Kaccy*OP(2,5)*SK_ACCY(6) + Kaccy*OP(2,6)*SK_ACCY(9) + Kaccy*OP(2,7)*SK_ACCY(8) + 2*Kaccy*OP(2,23)*SK_ACCY(7) - Kaccy*OP(2,24)*SK_ACCY(9)); +Kfusion(3) = -SK_ACCY(1)*(Kaccy*OP(3,1)*SK_ACCY(4) + Kaccy*OP(3,2)*SK_ACCY(3) - Kaccy*OP(3,4)*SK_ACCY(2) + Kaccy*OP(3,3)*SK_ACCY(5) - Kaccy*OP(3,5)*SK_ACCY(6) + Kaccy*OP(3,6)*SK_ACCY(9) + Kaccy*OP(3,7)*SK_ACCY(8) + 2*Kaccy*OP(3,23)*SK_ACCY(7) - Kaccy*OP(3,24)*SK_ACCY(9)); +Kfusion(4) = -SK_ACCY(1)*(Kaccy*OP(4,1)*SK_ACCY(4) + Kaccy*OP(4,2)*SK_ACCY(3) - Kaccy*OP(4,4)*SK_ACCY(2) + Kaccy*OP(4,3)*SK_ACCY(5) - Kaccy*OP(4,5)*SK_ACCY(6) + Kaccy*OP(4,6)*SK_ACCY(9) + Kaccy*OP(4,7)*SK_ACCY(8) + 2*Kaccy*OP(4,23)*SK_ACCY(7) - Kaccy*OP(4,24)*SK_ACCY(9)); +Kfusion(5) = -SK_ACCY(1)*(Kaccy*OP(5,1)*SK_ACCY(4) + Kaccy*OP(5,2)*SK_ACCY(3) - Kaccy*OP(5,4)*SK_ACCY(2) + Kaccy*OP(5,3)*SK_ACCY(5) - Kaccy*OP(5,5)*SK_ACCY(6) + Kaccy*OP(5,6)*SK_ACCY(9) + Kaccy*OP(5,7)*SK_ACCY(8) + 2*Kaccy*OP(5,23)*SK_ACCY(7) - Kaccy*OP(5,24)*SK_ACCY(9)); +Kfusion(6) = -SK_ACCY(1)*(Kaccy*OP(6,1)*SK_ACCY(4) + Kaccy*OP(6,2)*SK_ACCY(3) - Kaccy*OP(6,4)*SK_ACCY(2) + Kaccy*OP(6,3)*SK_ACCY(5) - Kaccy*OP(6,5)*SK_ACCY(6) + Kaccy*OP(6,6)*SK_ACCY(9) + Kaccy*OP(6,7)*SK_ACCY(8) + 2*Kaccy*OP(6,23)*SK_ACCY(7) - Kaccy*OP(6,24)*SK_ACCY(9)); +Kfusion(7) = -SK_ACCY(1)*(Kaccy*OP(7,1)*SK_ACCY(4) + Kaccy*OP(7,2)*SK_ACCY(3) - Kaccy*OP(7,4)*SK_ACCY(2) + Kaccy*OP(7,3)*SK_ACCY(5) - Kaccy*OP(7,5)*SK_ACCY(6) + Kaccy*OP(7,6)*SK_ACCY(9) + Kaccy*OP(7,7)*SK_ACCY(8) + 2*Kaccy*OP(7,23)*SK_ACCY(7) - Kaccy*OP(7,24)*SK_ACCY(9)); +Kfusion(8) = -SK_ACCY(1)*(Kaccy*OP(8,1)*SK_ACCY(4) + Kaccy*OP(8,2)*SK_ACCY(3) - Kaccy*OP(8,4)*SK_ACCY(2) + Kaccy*OP(8,3)*SK_ACCY(5) - Kaccy*OP(8,5)*SK_ACCY(6) + Kaccy*OP(8,6)*SK_ACCY(9) + Kaccy*OP(8,7)*SK_ACCY(8) + 2*Kaccy*OP(8,23)*SK_ACCY(7) - Kaccy*OP(8,24)*SK_ACCY(9)); +Kfusion(9) = -SK_ACCY(1)*(Kaccy*OP(9,1)*SK_ACCY(4) + Kaccy*OP(9,2)*SK_ACCY(3) - Kaccy*OP(9,4)*SK_ACCY(2) + Kaccy*OP(9,3)*SK_ACCY(5) - Kaccy*OP(9,5)*SK_ACCY(6) + Kaccy*OP(9,6)*SK_ACCY(9) + Kaccy*OP(9,7)*SK_ACCY(8) + 2*Kaccy*OP(9,23)*SK_ACCY(7) - Kaccy*OP(9,24)*SK_ACCY(9)); +Kfusion(10) = -SK_ACCY(1)*(Kaccy*OP(10,1)*SK_ACCY(4) + Kaccy*OP(10,2)*SK_ACCY(3) - Kaccy*OP(10,4)*SK_ACCY(2) + Kaccy*OP(10,3)*SK_ACCY(5) - Kaccy*OP(10,5)*SK_ACCY(6) + Kaccy*OP(10,6)*SK_ACCY(9) + Kaccy*OP(10,7)*SK_ACCY(8) + 2*Kaccy*OP(10,23)*SK_ACCY(7) - Kaccy*OP(10,24)*SK_ACCY(9)); +Kfusion(11) = -SK_ACCY(1)*(Kaccy*OP(11,1)*SK_ACCY(4) + Kaccy*OP(11,2)*SK_ACCY(3) - Kaccy*OP(11,4)*SK_ACCY(2) + Kaccy*OP(11,3)*SK_ACCY(5) - Kaccy*OP(11,5)*SK_ACCY(6) + Kaccy*OP(11,6)*SK_ACCY(9) + Kaccy*OP(11,7)*SK_ACCY(8) + 2*Kaccy*OP(11,23)*SK_ACCY(7) - Kaccy*OP(11,24)*SK_ACCY(9)); +Kfusion(12) = -SK_ACCY(1)*(Kaccy*OP(12,1)*SK_ACCY(4) + Kaccy*OP(12,2)*SK_ACCY(3) - Kaccy*OP(12,4)*SK_ACCY(2) + Kaccy*OP(12,3)*SK_ACCY(5) - Kaccy*OP(12,5)*SK_ACCY(6) + Kaccy*OP(12,6)*SK_ACCY(9) + Kaccy*OP(12,7)*SK_ACCY(8) + 2*Kaccy*OP(12,23)*SK_ACCY(7) - Kaccy*OP(12,24)*SK_ACCY(9)); +Kfusion(13) = -SK_ACCY(1)*(Kaccy*OP(13,1)*SK_ACCY(4) + Kaccy*OP(13,2)*SK_ACCY(3) - Kaccy*OP(13,4)*SK_ACCY(2) + Kaccy*OP(13,3)*SK_ACCY(5) - Kaccy*OP(13,5)*SK_ACCY(6) + Kaccy*OP(13,6)*SK_ACCY(9) + Kaccy*OP(13,7)*SK_ACCY(8) + 2*Kaccy*OP(13,23)*SK_ACCY(7) - Kaccy*OP(13,24)*SK_ACCY(9)); +Kfusion(14) = -SK_ACCY(1)*(Kaccy*OP(14,1)*SK_ACCY(4) + Kaccy*OP(14,2)*SK_ACCY(3) - Kaccy*OP(14,4)*SK_ACCY(2) + Kaccy*OP(14,3)*SK_ACCY(5) - Kaccy*OP(14,5)*SK_ACCY(6) + Kaccy*OP(14,6)*SK_ACCY(9) + Kaccy*OP(14,7)*SK_ACCY(8) + 2*Kaccy*OP(14,23)*SK_ACCY(7) - Kaccy*OP(14,24)*SK_ACCY(9)); +Kfusion(15) = -SK_ACCY(1)*(Kaccy*OP(15,1)*SK_ACCY(4) + Kaccy*OP(15,2)*SK_ACCY(3) - Kaccy*OP(15,4)*SK_ACCY(2) + Kaccy*OP(15,3)*SK_ACCY(5) - Kaccy*OP(15,5)*SK_ACCY(6) + Kaccy*OP(15,6)*SK_ACCY(9) + Kaccy*OP(15,7)*SK_ACCY(8) + 2*Kaccy*OP(15,23)*SK_ACCY(7) - Kaccy*OP(15,24)*SK_ACCY(9)); +Kfusion(16) = -SK_ACCY(1)*(Kaccy*OP(16,1)*SK_ACCY(4) + Kaccy*OP(16,2)*SK_ACCY(3) - Kaccy*OP(16,4)*SK_ACCY(2) + Kaccy*OP(16,3)*SK_ACCY(5) - Kaccy*OP(16,5)*SK_ACCY(6) + Kaccy*OP(16,6)*SK_ACCY(9) + Kaccy*OP(16,7)*SK_ACCY(8) + 2*Kaccy*OP(16,23)*SK_ACCY(7) - Kaccy*OP(16,24)*SK_ACCY(9)); +Kfusion(17) = -SK_ACCY(1)*(Kaccy*OP(17,1)*SK_ACCY(4) + Kaccy*OP(17,2)*SK_ACCY(3) - Kaccy*OP(17,4)*SK_ACCY(2) + Kaccy*OP(17,3)*SK_ACCY(5) - Kaccy*OP(17,5)*SK_ACCY(6) + Kaccy*OP(17,6)*SK_ACCY(9) + Kaccy*OP(17,7)*SK_ACCY(8) + 2*Kaccy*OP(17,23)*SK_ACCY(7) - Kaccy*OP(17,24)*SK_ACCY(9)); +Kfusion(18) = -SK_ACCY(1)*(Kaccy*OP(18,1)*SK_ACCY(4) + Kaccy*OP(18,2)*SK_ACCY(3) - Kaccy*OP(18,4)*SK_ACCY(2) + Kaccy*OP(18,3)*SK_ACCY(5) - Kaccy*OP(18,5)*SK_ACCY(6) + Kaccy*OP(18,6)*SK_ACCY(9) + Kaccy*OP(18,7)*SK_ACCY(8) + 2*Kaccy*OP(18,23)*SK_ACCY(7) - Kaccy*OP(18,24)*SK_ACCY(9)); +Kfusion(19) = -SK_ACCY(1)*(Kaccy*OP(19,1)*SK_ACCY(4) + Kaccy*OP(19,2)*SK_ACCY(3) - Kaccy*OP(19,4)*SK_ACCY(2) + Kaccy*OP(19,3)*SK_ACCY(5) - Kaccy*OP(19,5)*SK_ACCY(6) + Kaccy*OP(19,6)*SK_ACCY(9) + Kaccy*OP(19,7)*SK_ACCY(8) + 2*Kaccy*OP(19,23)*SK_ACCY(7) - Kaccy*OP(19,24)*SK_ACCY(9)); +Kfusion(20) = -SK_ACCY(1)*(Kaccy*OP(20,1)*SK_ACCY(4) + Kaccy*OP(20,2)*SK_ACCY(3) - Kaccy*OP(20,4)*SK_ACCY(2) + Kaccy*OP(20,3)*SK_ACCY(5) - Kaccy*OP(20,5)*SK_ACCY(6) + Kaccy*OP(20,6)*SK_ACCY(9) + Kaccy*OP(20,7)*SK_ACCY(8) + 2*Kaccy*OP(20,23)*SK_ACCY(7) - Kaccy*OP(20,24)*SK_ACCY(9)); +Kfusion(21) = -SK_ACCY(1)*(Kaccy*OP(21,1)*SK_ACCY(4) + Kaccy*OP(21,2)*SK_ACCY(3) - Kaccy*OP(21,4)*SK_ACCY(2) + Kaccy*OP(21,3)*SK_ACCY(5) - Kaccy*OP(21,5)*SK_ACCY(6) + Kaccy*OP(21,6)*SK_ACCY(9) + Kaccy*OP(21,7)*SK_ACCY(8) + 2*Kaccy*OP(21,23)*SK_ACCY(7) - Kaccy*OP(21,24)*SK_ACCY(9)); +Kfusion(22) = -SK_ACCY(1)*(Kaccy*OP(22,1)*SK_ACCY(4) + Kaccy*OP(22,2)*SK_ACCY(3) - Kaccy*OP(22,4)*SK_ACCY(2) + Kaccy*OP(22,3)*SK_ACCY(5) - Kaccy*OP(22,5)*SK_ACCY(6) + Kaccy*OP(22,6)*SK_ACCY(9) + Kaccy*OP(22,7)*SK_ACCY(8) + 2*Kaccy*OP(22,23)*SK_ACCY(7) - Kaccy*OP(22,24)*SK_ACCY(9)); +Kfusion(23) = -SK_ACCY(1)*(Kaccy*OP(23,1)*SK_ACCY(4) + Kaccy*OP(23,2)*SK_ACCY(3) - Kaccy*OP(23,4)*SK_ACCY(2) + Kaccy*OP(23,3)*SK_ACCY(5) - Kaccy*OP(23,5)*SK_ACCY(6) + Kaccy*OP(23,6)*SK_ACCY(9) + Kaccy*OP(23,7)*SK_ACCY(8) + 2*Kaccy*OP(23,23)*SK_ACCY(7) - Kaccy*OP(23,24)*SK_ACCY(9)); +Kfusion(24) = -SK_ACCY(1)*(Kaccy*OP(24,1)*SK_ACCY(4) + Kaccy*OP(24,2)*SK_ACCY(3) - Kaccy*OP(24,4)*SK_ACCY(2) + Kaccy*OP(24,3)*SK_ACCY(5) - Kaccy*OP(24,5)*SK_ACCY(6) + Kaccy*OP(24,6)*SK_ACCY(9) + Kaccy*OP(24,7)*SK_ACCY(8) + 2*Kaccy*OP(24,23)*SK_ACCY(7) - Kaccy*OP(24,24)*SK_ACCY(9)); diff --git a/matlab/scripts/Inertial Nav EKF/Magnetometer.mat b/matlab/scripts/Inertial Nav EKF/Magnetometer.mat index a4525f2295..c302dd2ae0 100644 Binary files a/matlab/scripts/Inertial Nav EKF/Magnetometer.mat and b/matlab/scripts/Inertial Nav EKF/Magnetometer.mat differ diff --git a/matlab/scripts/Inertial Nav EKF/Sideslip.mat b/matlab/scripts/Inertial Nav EKF/Sideslip.mat index c0abe0b417..f6d8de8d86 100644 Binary files a/matlab/scripts/Inertial Nav EKF/Sideslip.mat and b/matlab/scripts/Inertial Nav EKF/Sideslip.mat differ diff --git a/matlab/scripts/Inertial Nav EKF/StateAndCovariancePrediction.mat b/matlab/scripts/Inertial Nav EKF/StateAndCovariancePrediction.mat index 76145de186..c87c9e4772 100644 Binary files a/matlab/scripts/Inertial Nav EKF/StateAndCovariancePrediction.mat and b/matlab/scripts/Inertial Nav EKF/StateAndCovariancePrediction.mat differ diff --git a/matlab/scripts/Inertial Nav EKF/StatePrediction.mat b/matlab/scripts/Inertial Nav EKF/StatePrediction.mat index 3f6faf511e..abf15241a0 100644 Binary files a/matlab/scripts/Inertial Nav EKF/StatePrediction.mat and b/matlab/scripts/Inertial Nav EKF/StatePrediction.mat differ diff --git a/matlab/scripts/Inertial Nav EKF/SymbolicOutput24.mat b/matlab/scripts/Inertial Nav EKF/SymbolicOutput24.mat index 3cd17e14f3..17edcd19d9 100644 Binary files a/matlab/scripts/Inertial Nav EKF/SymbolicOutput24.mat and b/matlab/scripts/Inertial Nav EKF/SymbolicOutput24.mat differ diff --git a/matlab/scripts/Inertial Nav EKF/SymbolicOutput24.txt b/matlab/scripts/Inertial Nav EKF/SymbolicOutput24.txt index 5c61fc39e1..5e225c8f2d 100644 --- a/matlab/scripts/Inertial Nav EKF/SymbolicOutput24.txt +++ b/matlab/scripts/Inertial Nav EKF/SymbolicOutput24.txt @@ -1,224 +1,212 @@ -SF = zeros(25,1); -SF(1) = daz_b/2 + dazNoise/2 - (daz*daz_s)/2; -SF(2) = day_b/2 + dayNoise/2 - (day*day_s)/2; -SF(3) = dax_b/2 + daxNoise/2 - (dax*dax_s)/2; -SF(4) = q3/2 - (q0*SF(1))/2 + (q1*SF(2))/2 - (q2*SF(3))/2; -SF(5) = q0/2 - (q1*SF(3))/2 - (q2*SF(2))/2 + (q3*SF(1))/2; -SF(6) = q1/2 + (q0*SF(3))/2 - (q2*SF(1))/2 - (q3*SF(2))/2; -SF(7) = q3/2 + (q0*SF(1))/2 - (q1*SF(2))/2 - (q2*SF(3))/2; -SF(8) = q0/2 - (q1*SF(3))/2 + (q2*SF(2))/2 - (q3*SF(1))/2; -SF(9) = q0/2 + (q1*SF(3))/2 - (q2*SF(2))/2 - (q3*SF(1))/2; -SF(10) = q2/2 + (q0*SF(2))/2 + (q1*SF(1))/2 + (q3*SF(3))/2; -SF(11) = q2/2 - (q0*SF(2))/2 - (q1*SF(1))/2 + (q3*SF(3))/2; -SF(12) = q2/2 + (q0*SF(2))/2 - (q1*SF(1))/2 - (q3*SF(3))/2; -SF(13) = q1/2 + (q0*SF(3))/2 + (q2*SF(1))/2 + (q3*SF(2))/2; -SF(14) = q1/2 - (q0*SF(3))/2 + (q2*SF(1))/2 - (q3*SF(2))/2; -SF(15) = q3/2 + (q0*SF(1))/2 + (q1*SF(2))/2 + (q2*SF(3))/2; -SF(16) = - q0^2 - q1^2 - q2^2 - q3^2; -SF(17) = dvz_b - dvz + dvzNoise; -SF(18) = dvx - dvxNoise; -SF(19) = dvy - dvyNoise; -SF(20) = q2^2; -SF(21) = SF(20) - q0^2 + q1^2 - q3^2; -SF(22) = SF(20) + q0^2 - q1^2 - q3^2; -SF(23) = 2*q0*q1 - 2*q2*q3; -SF(24) = SF(20) - q0^2 - q1^2 + q3^2; -SF(25) = 2*q1*q2; +SF = zeros(21,1); +SF(1) = dvz - dvz_b; +SF(2) = dvy - dvy_b; +SF(3) = dvx - dvx_b; +SF(4) = 2*q1*SF(3) + 2*q2*SF(2) + 2*q3*SF(1); +SF(5) = 2*q0*SF(2) - 2*q1*SF(1) + 2*q3*SF(3); +SF(6) = 2*q0*SF(3) + 2*q2*SF(1) - 2*q3*SF(2); +SF(7) = day/2 - day_b/2; +SF(8) = daz/2 - daz_b/2; +SF(9) = dax/2 - dax_b/2; +SF(10) = dax_b/2 - dax/2; +SF(11) = daz_b/2 - daz/2; +SF(12) = day_b/2 - day/2; +SF(13) = 2*q1*SF(2); +SF(14) = 2*q0*SF(1); +SF(15) = q1/2; +SF(16) = q2/2; +SF(17) = q3/2; +SF(18) = q3^2; +SF(19) = q2^2; +SF(20) = q1^2; +SF(21) = q0^2; -SG = zeros(5,1); -SG(1) = - q0^2 - q1^2 - q2^2 - q3^2; +SG = zeros(8,1); +SG(1) = q0/2; SG(2) = q3^2; SG(3) = q2^2; SG(4) = q1^2; SG(5) = q0^2; +SG(6) = 2*q2*q3; +SG(7) = 2*q1*q3; +SG(8) = 2*q1*q2; -SQ = zeros(10,1); -SQ(1) = - dvyNoise^2*(2*q0*q1 + 2*q2*q3)*(SG(2) - SG(3) + SG(4) - SG(5)) - dvzNoise^2*(2*q0*q1 - 2*q2*q3)*(SG(2) - SG(3) - SG(4) + SG(5)) - dvxNoise^2*(2*q0*q2 - 2*q1*q3)*(2*q0*q3 + 2*q1*q2); -SQ(2) = dvxNoise^2*(2*q0*q2 - 2*q1*q3)*(SG(2) + SG(3) - SG(4) - SG(5)) + dvzNoise^2*(2*q0*q2 + 2*q1*q3)*(SG(2) - SG(3) - SG(4) + SG(5)) - dvyNoise^2*(2*q0*q1 + 2*q2*q3)*(2*q0*q3 - 2*q1*q2); -SQ(3) = dvyNoise^2*(2*q0*q3 - 2*q1*q2)*(SG(2) - SG(3) + SG(4) - SG(5)) - dvxNoise^2*(2*q0*q3 + 2*q1*q2)*(SG(2) + SG(3) - SG(4) - SG(5)) - dvzNoise^2*(2*q0*q1 - 2*q2*q3)*(2*q0*q2 + 2*q1*q3); -SQ(4) = SG(1)^2; -SQ(5) = dvyNoise^2; -SQ(6) = dvzNoise^2; -SQ(7) = dvxNoise^2; -SQ(8) = 2*q2*q3; -SQ(9) = 2*q1*q3; -SQ(10) = 2*q1*q2; +SQ = zeros(11,1); +SQ(1) = dvzVar*(SG(6) - 2*q0*q1)*(SG(2) - SG(3) - SG(4) + SG(5)) - dvyVar*(SG(6) + 2*q0*q1)*(SG(2) - SG(3) + SG(4) - SG(5)) + dvxVar*(SG(7) - 2*q0*q2)*(SG(8) + 2*q0*q3); +SQ(2) = dvzVar*(SG(7) + 2*q0*q2)*(SG(2) - SG(3) - SG(4) + SG(5)) - dvxVar*(SG(7) - 2*q0*q2)*(SG(2) + SG(3) - SG(4) - SG(5)) + dvyVar*(SG(6) + 2*q0*q1)*(SG(8) - 2*q0*q3); +SQ(3) = dvzVar*(SG(6) - 2*q0*q1)*(SG(7) + 2*q0*q2) - dvyVar*(SG(8) - 2*q0*q3)*(SG(2) - SG(3) + SG(4) - SG(5)) - dvxVar*(SG(8) + 2*q0*q3)*(SG(2) + SG(3) - SG(4) - SG(5)); +SQ(4) = (dayVar*q1*SG(1))/2 - (dazVar*q1*SG(1))/2 - (daxVar*q2*q3)/4; +SQ(5) = (dazVar*q2*SG(1))/2 - (daxVar*q2*SG(1))/2 - (dayVar*q1*q3)/4; +SQ(6) = (daxVar*q3*SG(1))/2 - (dayVar*q3*SG(1))/2 - (dazVar*q1*q2)/4; +SQ(7) = (daxVar*q1*q2)/4 - (dazVar*q3*SG(1))/2 - (dayVar*q1*q2)/4; +SQ(8) = (dazVar*q1*q3)/4 - (daxVar*q1*q3)/4 - (dayVar*q2*SG(1))/2; +SQ(9) = (dayVar*q2*q3)/4 - (daxVar*q1*SG(1))/2 - (dazVar*q2*q3)/4; +SQ(10) = SG(1)^2; +SQ(11) = q1^2; -SPP = zeros(23,1); -SPP(1) = SF(18)*(2*q0*q1 + 2*q2*q3) + SF(19)*(2*q0*q2 - 2*q1*q3); -SPP(2) = SF(19)*(2*q0*q2 + 2*q1*q3) + SF(17)*(SF(25) - 2*q0*q3); -SPP(3) = 2*q3*SF(9) + 2*q1*SF(12) - 2*q0*SF(15) - 2*q2*SF(14); -SPP(4) = 2*q1*SF(8) + 2*q2*SF(7) - 2*q0*SF(13) - 2*q3*SF(11); -SPP(5) = 2*q0*SF(7) - 2*q3*SF(8) - 2*q1*SF(11) + 2*q2*SF(13); -SPP(6) = 2*q0*SF(9) + 2*q2*SF(12) + 2*q1*SF(14) + 2*q3*SF(15); -SPP(7) = 2*q0*SF(8) + 2*q3*SF(7) + 2*q2*SF(11) + 2*q1*SF(13); -SPP(8) = 2*q1*SF(4) - 2*q2*SF(5) - 2*q3*SF(6) + 2*q0*SF(10); -SPP(9) = 2*q0*SF(6) - 2*q1*SF(5) - 2*q2*SF(4) + 2*q3*SF(10); -SPP(10) = SF(19)*SF(21) - SF(17)*(2*q0*q1 + 2*q2*q3); -SPP(11) = SF(18)*SF(21) + SF(17)*(2*q0*q2 - 2*q1*q3); -SPP(12) = SF(18)*SF(22) - SF(19)*(SF(25) + 2*q0*q3); -SPP(13) = SF(18)*SF(23) - SF(17)*(SF(25) + 2*q0*q3); -SPP(14) = 2*q0*SF(5) + 2*q1*SF(6) + 2*q3*SF(4) + 2*q2*SF(10); -SPP(15) = 2*q2*SF(9) - 2*q0*SF(12) - 2*q1*SF(15) + 2*q3*SF(14); -SPP(16) = SF(19)*SF(24) + SF(18)*(SF(25) - 2*q0*q3); -SPP(17) = daz*SF(20) + daz*q0^2 + daz*q1^2 + daz*q3^2; -SPP(18) = day*SF(20) + day*q0^2 + day*q1^2 + day*q3^2; -SPP(19) = dax*SF(20) + dax*q0^2 + dax*q1^2 + dax*q3^2; -SPP(20) = SF(17)*SF(24) - SF(18)*(2*q0*q2 + 2*q1*q3); -SPP(21) = SF(17)*SF(22) - SF(19)*SF(23); -SPP(22) = 2*q0*q2 + 2*q1*q3; -SPP(23) = SF(16); +SPP = zeros(11,1); +SPP(1) = SF(13) + SF(14) - 2*q2*SF(3); +SPP(2) = SF(18) - SF(19) - SF(20) + SF(21); +SPP(3) = SF(18) - SF(19) + SF(20) - SF(21); +SPP(4) = SF(18) + SF(19) - SF(20) - SF(21); +SPP(5) = 2*q0*q2 - 2*q1*q3; +SPP(6) = 2*q0*q1 - 2*q2*q3; +SPP(7) = 2*q0*q3 - 2*q1*q2; +SPP(8) = 2*q0*q1 + 2*q2*q3; +SPP(9) = 2*q0*q3 + 2*q1*q2; +SPP(10) = 2*q0*q2 + 2*q1*q3; +SPP(11) = SF(17); nextP = zeros(24,24); -nextP(1,1) = SPP(6)*(OP_l_1_c_1_r_*SPP(6) - OP_l_2_c_1_r_*SPP(5) + OP_l_3_c_1_r_*SPP(8) + OP_l_10_c_1_r_*SPP(23) + OP_l_13_c_1_r_*SPP(19)) - SPP(5)*(OP_l_1_c_2_r_*SPP(6) - OP_l_2_c_2_r_*SPP(5) + OP_l_3_c_2_r_*SPP(8) + OP_l_10_c_2_r_*SPP(23) + OP_l_13_c_2_r_*SPP(19)) + SPP(8)*(OP_l_1_c_3_r_*SPP(6) - OP_l_2_c_3_r_*SPP(5) + OP_l_3_c_3_r_*SPP(8) + OP_l_10_c_3_r_*SPP(23) + OP_l_13_c_3_r_*SPP(19)) + SPP(23)*(OP_l_1_c_10_r_*SPP(6) - OP_l_2_c_10_r_*SPP(5) + OP_l_3_c_10_r_*SPP(8) + OP_l_10_c_10_r_*SPP(23) + OP_l_13_c_10_r_*SPP(19)) + SPP(19)*(OP_l_1_c_13_r_*SPP(6) - OP_l_2_c_13_r_*SPP(5) + OP_l_3_c_13_r_*SPP(8) + OP_l_10_c_13_r_*SPP(23) + OP_l_13_c_13_r_*SPP(19)) + daxNoise^2*SQ(4); -nextP(1,2) = SPP(7)*(OP_l_1_c_2_r_*SPP(6) - OP_l_2_c_2_r_*SPP(5) + OP_l_3_c_2_r_*SPP(8) + OP_l_10_c_2_r_*SPP(23) + OP_l_13_c_2_r_*SPP(19)) - SPP(3)*(OP_l_1_c_1_r_*SPP(6) - OP_l_2_c_1_r_*SPP(5) + OP_l_3_c_1_r_*SPP(8) + OP_l_10_c_1_r_*SPP(23) + OP_l_13_c_1_r_*SPP(19)) - SPP(9)*(OP_l_1_c_3_r_*SPP(6) - OP_l_2_c_3_r_*SPP(5) + OP_l_3_c_3_r_*SPP(8) + OP_l_10_c_3_r_*SPP(23) + OP_l_13_c_3_r_*SPP(19)) + SPP(23)*(OP_l_1_c_11_r_*SPP(6) - OP_l_2_c_11_r_*SPP(5) + OP_l_3_c_11_r_*SPP(8) + OP_l_10_c_11_r_*SPP(23) + OP_l_13_c_11_r_*SPP(19)) + SPP(18)*(OP_l_1_c_14_r_*SPP(6) - OP_l_2_c_14_r_*SPP(5) + OP_l_3_c_14_r_*SPP(8) + OP_l_10_c_14_r_*SPP(23) + OP_l_13_c_14_r_*SPP(19)); -nextP(2,2) = SPP(7)*(OP_l_2_c_2_r_*SPP(7) - OP_l_1_c_2_r_*SPP(3) - OP_l_3_c_2_r_*SPP(9) + OP_l_11_c_2_r_*SPP(23) + OP_l_14_c_2_r_*SPP(18)) - SPP(3)*(OP_l_2_c_1_r_*SPP(7) - OP_l_1_c_1_r_*SPP(3) - OP_l_3_c_1_r_*SPP(9) + OP_l_11_c_1_r_*SPP(23) + OP_l_14_c_1_r_*SPP(18)) - SPP(9)*(OP_l_2_c_3_r_*SPP(7) - OP_l_1_c_3_r_*SPP(3) - OP_l_3_c_3_r_*SPP(9) + OP_l_11_c_3_r_*SPP(23) + OP_l_14_c_3_r_*SPP(18)) + SPP(23)*(OP_l_2_c_11_r_*SPP(7) - OP_l_1_c_11_r_*SPP(3) - OP_l_3_c_11_r_*SPP(9) + OP_l_11_c_11_r_*SPP(23) + OP_l_14_c_11_r_*SPP(18)) + SPP(18)*(OP_l_2_c_14_r_*SPP(7) - OP_l_1_c_14_r_*SPP(3) - OP_l_3_c_14_r_*SPP(9) + OP_l_11_c_14_r_*SPP(23) + OP_l_14_c_14_r_*SPP(18)) + dayNoise^2*SQ(4); -nextP(1,3) = SPP(15)*(OP_l_1_c_1_r_*SPP(6) - OP_l_2_c_1_r_*SPP(5) + OP_l_3_c_1_r_*SPP(8) + OP_l_10_c_1_r_*SPP(23) + OP_l_13_c_1_r_*SPP(19)) - SPP(4)*(OP_l_1_c_2_r_*SPP(6) - OP_l_2_c_2_r_*SPP(5) + OP_l_3_c_2_r_*SPP(8) + OP_l_10_c_2_r_*SPP(23) + OP_l_13_c_2_r_*SPP(19)) + SPP(14)*(OP_l_1_c_3_r_*SPP(6) - OP_l_2_c_3_r_*SPP(5) + OP_l_3_c_3_r_*SPP(8) + OP_l_10_c_3_r_*SPP(23) + OP_l_13_c_3_r_*SPP(19)) + SPP(23)*(OP_l_1_c_12_r_*SPP(6) - OP_l_2_c_12_r_*SPP(5) + OP_l_3_c_12_r_*SPP(8) + OP_l_10_c_12_r_*SPP(23) + OP_l_13_c_12_r_*SPP(19)) + SPP(17)*(OP_l_1_c_15_r_*SPP(6) - OP_l_2_c_15_r_*SPP(5) + OP_l_3_c_15_r_*SPP(8) + OP_l_10_c_15_r_*SPP(23) + OP_l_13_c_15_r_*SPP(19)); -nextP(2,3) = SPP(15)*(OP_l_2_c_1_r_*SPP(7) - OP_l_1_c_1_r_*SPP(3) - OP_l_3_c_1_r_*SPP(9) + OP_l_11_c_1_r_*SPP(23) + OP_l_14_c_1_r_*SPP(18)) - SPP(4)*(OP_l_2_c_2_r_*SPP(7) - OP_l_1_c_2_r_*SPP(3) - OP_l_3_c_2_r_*SPP(9) + OP_l_11_c_2_r_*SPP(23) + OP_l_14_c_2_r_*SPP(18)) + SPP(14)*(OP_l_2_c_3_r_*SPP(7) - OP_l_1_c_3_r_*SPP(3) - OP_l_3_c_3_r_*SPP(9) + OP_l_11_c_3_r_*SPP(23) + OP_l_14_c_3_r_*SPP(18)) + SPP(23)*(OP_l_2_c_12_r_*SPP(7) - OP_l_1_c_12_r_*SPP(3) - OP_l_3_c_12_r_*SPP(9) + OP_l_11_c_12_r_*SPP(23) + OP_l_14_c_12_r_*SPP(18)) + SPP(17)*(OP_l_2_c_15_r_*SPP(7) - OP_l_1_c_15_r_*SPP(3) - OP_l_3_c_15_r_*SPP(9) + OP_l_11_c_15_r_*SPP(23) + OP_l_14_c_15_r_*SPP(18)); -nextP(3,3) = SPP(15)*(OP_l_1_c_1_r_*SPP(15) - OP_l_2_c_1_r_*SPP(4) + OP_l_3_c_1_r_*SPP(14) + OP_l_12_c_1_r_*SPP(23) + OP_l_15_c_1_r_*SPP(17)) - SPP(4)*(OP_l_1_c_2_r_*SPP(15) - OP_l_2_c_2_r_*SPP(4) + OP_l_3_c_2_r_*SPP(14) + OP_l_12_c_2_r_*SPP(23) + OP_l_15_c_2_r_*SPP(17)) + SPP(14)*(OP_l_1_c_3_r_*SPP(15) - OP_l_2_c_3_r_*SPP(4) + OP_l_3_c_3_r_*SPP(14) + OP_l_12_c_3_r_*SPP(23) + OP_l_15_c_3_r_*SPP(17)) + SPP(23)*(OP_l_1_c_12_r_*SPP(15) - OP_l_2_c_12_r_*SPP(4) + OP_l_3_c_12_r_*SPP(14) + OP_l_12_c_12_r_*SPP(23) + OP_l_15_c_12_r_*SPP(17)) + SPP(17)*(OP_l_1_c_15_r_*SPP(15) - OP_l_2_c_15_r_*SPP(4) + OP_l_3_c_15_r_*SPP(14) + OP_l_12_c_15_r_*SPP(23) + OP_l_15_c_15_r_*SPP(17)) + dazNoise^2*SQ(4); -nextP(1,4) = OP_l_1_c_4_r_*SPP(6) - OP_l_2_c_4_r_*SPP(5) + OP_l_3_c_4_r_*SPP(8) + OP_l_10_c_4_r_*SPP(23) + OP_l_13_c_4_r_*SPP(19) + SPP(2)*(OP_l_1_c_1_r_*SPP(6) - OP_l_2_c_1_r_*SPP(5) + OP_l_3_c_1_r_*SPP(8) + OP_l_10_c_1_r_*SPP(23) + OP_l_13_c_1_r_*SPP(19)) + SPP(20)*(OP_l_1_c_2_r_*SPP(6) - OP_l_2_c_2_r_*SPP(5) + OP_l_3_c_2_r_*SPP(8) + OP_l_10_c_2_r_*SPP(23) + OP_l_13_c_2_r_*SPP(19)) + SPP(16)*(OP_l_1_c_3_r_*SPP(6) - OP_l_2_c_3_r_*SPP(5) + OP_l_3_c_3_r_*SPP(8) + OP_l_10_c_3_r_*SPP(23) + OP_l_13_c_3_r_*SPP(19)) - SPP(22)*(OP_l_1_c_16_r_*SPP(6) - OP_l_2_c_16_r_*SPP(5) + OP_l_3_c_16_r_*SPP(8) + OP_l_10_c_16_r_*SPP(23) + OP_l_13_c_16_r_*SPP(19)); -nextP(2,4) = OP_l_2_c_4_r_*SPP(7) - OP_l_1_c_4_r_*SPP(3) - OP_l_3_c_4_r_*SPP(9) + OP_l_11_c_4_r_*SPP(23) + OP_l_14_c_4_r_*SPP(18) + SPP(2)*(OP_l_2_c_1_r_*SPP(7) - OP_l_1_c_1_r_*SPP(3) - OP_l_3_c_1_r_*SPP(9) + OP_l_11_c_1_r_*SPP(23) + OP_l_14_c_1_r_*SPP(18)) + SPP(20)*(OP_l_2_c_2_r_*SPP(7) - OP_l_1_c_2_r_*SPP(3) - OP_l_3_c_2_r_*SPP(9) + OP_l_11_c_2_r_*SPP(23) + OP_l_14_c_2_r_*SPP(18)) + SPP(16)*(OP_l_2_c_3_r_*SPP(7) - OP_l_1_c_3_r_*SPP(3) - OP_l_3_c_3_r_*SPP(9) + OP_l_11_c_3_r_*SPP(23) + OP_l_14_c_3_r_*SPP(18)) - SPP(22)*(OP_l_2_c_16_r_*SPP(7) - OP_l_1_c_16_r_*SPP(3) - OP_l_3_c_16_r_*SPP(9) + OP_l_11_c_16_r_*SPP(23) + OP_l_14_c_16_r_*SPP(18)); -nextP(3,4) = OP_l_1_c_4_r_*SPP(15) - OP_l_2_c_4_r_*SPP(4) + OP_l_3_c_4_r_*SPP(14) + OP_l_12_c_4_r_*SPP(23) + OP_l_15_c_4_r_*SPP(17) + SPP(2)*(OP_l_1_c_1_r_*SPP(15) - OP_l_2_c_1_r_*SPP(4) + OP_l_3_c_1_r_*SPP(14) + OP_l_12_c_1_r_*SPP(23) + OP_l_15_c_1_r_*SPP(17)) + SPP(20)*(OP_l_1_c_2_r_*SPP(15) - OP_l_2_c_2_r_*SPP(4) + OP_l_3_c_2_r_*SPP(14) + OP_l_12_c_2_r_*SPP(23) + OP_l_15_c_2_r_*SPP(17)) + SPP(16)*(OP_l_1_c_3_r_*SPP(15) - OP_l_2_c_3_r_*SPP(4) + OP_l_3_c_3_r_*SPP(14) + OP_l_12_c_3_r_*SPP(23) + OP_l_15_c_3_r_*SPP(17)) - SPP(22)*(OP_l_1_c_16_r_*SPP(15) - OP_l_2_c_16_r_*SPP(4) + OP_l_3_c_16_r_*SPP(14) + OP_l_12_c_16_r_*SPP(23) + OP_l_15_c_16_r_*SPP(17)); -nextP(4,4) = OP_l_4_c_4_r_ + OP_l_1_c_4_r_*SPP(2) + OP_l_2_c_4_r_*SPP(20) + OP_l_3_c_4_r_*SPP(16) - OP_l_16_c_4_r_*SPP(22) + SQ(6)*(SQ(9) + 2*q0*q2)^2 + SQ(5)*(SQ(10) - 2*q0*q3)^2 + SPP(2)*(OP_l_4_c_1_r_ + OP_l_1_c_1_r_*SPP(2) + OP_l_2_c_1_r_*SPP(20) + OP_l_3_c_1_r_*SPP(16) - OP_l_16_c_1_r_*SPP(22)) + SPP(20)*(OP_l_4_c_2_r_ + OP_l_1_c_2_r_*SPP(2) + OP_l_2_c_2_r_*SPP(20) + OP_l_3_c_2_r_*SPP(16) - OP_l_16_c_2_r_*SPP(22)) + SPP(16)*(OP_l_4_c_3_r_ + OP_l_1_c_3_r_*SPP(2) + OP_l_2_c_3_r_*SPP(20) + OP_l_3_c_3_r_*SPP(16) - OP_l_16_c_3_r_*SPP(22)) - SPP(22)*(OP_l_4_c_16_r_ + OP_l_1_c_16_r_*SPP(2) + OP_l_2_c_16_r_*SPP(20) + OP_l_3_c_16_r_*SPP(16) - OP_l_16_c_16_r_*SPP(22)) + SQ(7)*(SG(2) + SG(3) - SG(4) - SG(5))^2; -nextP(1,5) = OP_l_1_c_5_r_*SPP(6) - OP_l_2_c_5_r_*SPP(5) + OP_l_3_c_5_r_*SPP(8) + OP_l_10_c_5_r_*SPP(23) + OP_l_13_c_5_r_*SPP(19) + SF(23)*(OP_l_1_c_16_r_*SPP(6) - OP_l_2_c_16_r_*SPP(5) + OP_l_3_c_16_r_*SPP(8) + OP_l_10_c_16_r_*SPP(23) + OP_l_13_c_16_r_*SPP(19)) + SPP(13)*(OP_l_1_c_2_r_*SPP(6) - OP_l_2_c_2_r_*SPP(5) + OP_l_3_c_2_r_*SPP(8) + OP_l_10_c_2_r_*SPP(23) + OP_l_13_c_2_r_*SPP(19)) + SPP(21)*(OP_l_1_c_1_r_*SPP(6) - OP_l_2_c_1_r_*SPP(5) + OP_l_3_c_1_r_*SPP(8) + OP_l_10_c_1_r_*SPP(23) + OP_l_13_c_1_r_*SPP(19)) + SPP(12)*(OP_l_1_c_3_r_*SPP(6) - OP_l_2_c_3_r_*SPP(5) + OP_l_3_c_3_r_*SPP(8) + OP_l_10_c_3_r_*SPP(23) + OP_l_13_c_3_r_*SPP(19)); -nextP(2,5) = OP_l_2_c_5_r_*SPP(7) - OP_l_1_c_5_r_*SPP(3) - OP_l_3_c_5_r_*SPP(9) + OP_l_11_c_5_r_*SPP(23) + OP_l_14_c_5_r_*SPP(18) + SF(23)*(OP_l_2_c_16_r_*SPP(7) - OP_l_1_c_16_r_*SPP(3) - OP_l_3_c_16_r_*SPP(9) + OP_l_11_c_16_r_*SPP(23) + OP_l_14_c_16_r_*SPP(18)) + SPP(13)*(OP_l_2_c_2_r_*SPP(7) - OP_l_1_c_2_r_*SPP(3) - OP_l_3_c_2_r_*SPP(9) + OP_l_11_c_2_r_*SPP(23) + OP_l_14_c_2_r_*SPP(18)) + SPP(21)*(OP_l_2_c_1_r_*SPP(7) - OP_l_1_c_1_r_*SPP(3) - OP_l_3_c_1_r_*SPP(9) + OP_l_11_c_1_r_*SPP(23) + OP_l_14_c_1_r_*SPP(18)) + SPP(12)*(OP_l_2_c_3_r_*SPP(7) - OP_l_1_c_3_r_*SPP(3) - OP_l_3_c_3_r_*SPP(9) + OP_l_11_c_3_r_*SPP(23) + OP_l_14_c_3_r_*SPP(18)); -nextP(3,5) = OP_l_1_c_5_r_*SPP(15) - OP_l_2_c_5_r_*SPP(4) + OP_l_3_c_5_r_*SPP(14) + OP_l_12_c_5_r_*SPP(23) + OP_l_15_c_5_r_*SPP(17) + SF(23)*(OP_l_1_c_16_r_*SPP(15) - OP_l_2_c_16_r_*SPP(4) + OP_l_3_c_16_r_*SPP(14) + OP_l_12_c_16_r_*SPP(23) + OP_l_15_c_16_r_*SPP(17)) + SPP(13)*(OP_l_1_c_2_r_*SPP(15) - OP_l_2_c_2_r_*SPP(4) + OP_l_3_c_2_r_*SPP(14) + OP_l_12_c_2_r_*SPP(23) + OP_l_15_c_2_r_*SPP(17)) + SPP(21)*(OP_l_1_c_1_r_*SPP(15) - OP_l_2_c_1_r_*SPP(4) + OP_l_3_c_1_r_*SPP(14) + OP_l_12_c_1_r_*SPP(23) + OP_l_15_c_1_r_*SPP(17)) + SPP(12)*(OP_l_1_c_3_r_*SPP(15) - OP_l_2_c_3_r_*SPP(4) + OP_l_3_c_3_r_*SPP(14) + OP_l_12_c_3_r_*SPP(23) + OP_l_15_c_3_r_*SPP(17)); -nextP(4,5) = OP_l_4_c_5_r_ + SQ(3) + OP_l_1_c_5_r_*SPP(2) + OP_l_2_c_5_r_*SPP(20) + OP_l_3_c_5_r_*SPP(16) - OP_l_16_c_5_r_*SPP(22) + SF(23)*(OP_l_4_c_16_r_ + OP_l_1_c_16_r_*SPP(2) + OP_l_2_c_16_r_*SPP(20) + OP_l_3_c_16_r_*SPP(16) - OP_l_16_c_16_r_*SPP(22)) + SPP(13)*(OP_l_4_c_2_r_ + OP_l_1_c_2_r_*SPP(2) + OP_l_2_c_2_r_*SPP(20) + OP_l_3_c_2_r_*SPP(16) - OP_l_16_c_2_r_*SPP(22)) + SPP(21)*(OP_l_4_c_1_r_ + OP_l_1_c_1_r_*SPP(2) + OP_l_2_c_1_r_*SPP(20) + OP_l_3_c_1_r_*SPP(16) - OP_l_16_c_1_r_*SPP(22)) + SPP(12)*(OP_l_4_c_3_r_ + OP_l_1_c_3_r_*SPP(2) + OP_l_2_c_3_r_*SPP(20) + OP_l_3_c_3_r_*SPP(16) - OP_l_16_c_3_r_*SPP(22)); -nextP(5,5) = OP_l_5_c_5_r_ + OP_l_16_c_5_r_*SF(23) + OP_l_1_c_5_r_*SPP(21) + OP_l_2_c_5_r_*SPP(13) + OP_l_3_c_5_r_*SPP(12) + SQ(6)*(SQ(8) - 2*q0*q1)^2 + SQ(7)*(SQ(10) + 2*q0*q3)^2 + SF(23)*(OP_l_5_c_16_r_ + OP_l_16_c_16_r_*SF(23) + OP_l_1_c_16_r_*SPP(21) + OP_l_2_c_16_r_*SPP(13) + OP_l_3_c_16_r_*SPP(12)) + SPP(13)*(OP_l_5_c_2_r_ + OP_l_16_c_2_r_*SF(23) + OP_l_1_c_2_r_*SPP(21) + OP_l_2_c_2_r_*SPP(13) + OP_l_3_c_2_r_*SPP(12)) + SPP(21)*(OP_l_5_c_1_r_ + OP_l_16_c_1_r_*SF(23) + OP_l_1_c_1_r_*SPP(21) + OP_l_2_c_1_r_*SPP(13) + OP_l_3_c_1_r_*SPP(12)) + SPP(12)*(OP_l_5_c_3_r_ + OP_l_16_c_3_r_*SF(23) + OP_l_1_c_3_r_*SPP(21) + OP_l_2_c_3_r_*SPP(13) + OP_l_3_c_3_r_*SPP(12)) + SQ(5)*(SG(2) - SG(3) + SG(4) - SG(5))^2; -nextP(1,6) = OP_l_1_c_6_r_*SPP(6) - OP_l_2_c_6_r_*SPP(5) + OP_l_3_c_6_r_*SPP(8) + OP_l_10_c_6_r_*SPP(23) + OP_l_13_c_6_r_*SPP(19) + SF(21)*(OP_l_1_c_16_r_*SPP(6) - OP_l_2_c_16_r_*SPP(5) + OP_l_3_c_16_r_*SPP(8) + OP_l_10_c_16_r_*SPP(23) + OP_l_13_c_16_r_*SPP(19)) - SPP(10)*(OP_l_1_c_1_r_*SPP(6) - OP_l_2_c_1_r_*SPP(5) + OP_l_3_c_1_r_*SPP(8) + OP_l_10_c_1_r_*SPP(23) + OP_l_13_c_1_r_*SPP(19)) + SPP(1)*(OP_l_1_c_3_r_*SPP(6) - OP_l_2_c_3_r_*SPP(5) + OP_l_3_c_3_r_*SPP(8) + OP_l_10_c_3_r_*SPP(23) + OP_l_13_c_3_r_*SPP(19)) + SPP(11)*(OP_l_1_c_2_r_*SPP(6) - OP_l_2_c_2_r_*SPP(5) + OP_l_3_c_2_r_*SPP(8) + OP_l_10_c_2_r_*SPP(23) + OP_l_13_c_2_r_*SPP(19)); -nextP(2,6) = OP_l_2_c_6_r_*SPP(7) - OP_l_1_c_6_r_*SPP(3) - OP_l_3_c_6_r_*SPP(9) + OP_l_11_c_6_r_*SPP(23) + OP_l_14_c_6_r_*SPP(18) + SF(21)*(OP_l_2_c_16_r_*SPP(7) - OP_l_1_c_16_r_*SPP(3) - OP_l_3_c_16_r_*SPP(9) + OP_l_11_c_16_r_*SPP(23) + OP_l_14_c_16_r_*SPP(18)) - SPP(10)*(OP_l_2_c_1_r_*SPP(7) - OP_l_1_c_1_r_*SPP(3) - OP_l_3_c_1_r_*SPP(9) + OP_l_11_c_1_r_*SPP(23) + OP_l_14_c_1_r_*SPP(18)) + SPP(1)*(OP_l_2_c_3_r_*SPP(7) - OP_l_1_c_3_r_*SPP(3) - OP_l_3_c_3_r_*SPP(9) + OP_l_11_c_3_r_*SPP(23) + OP_l_14_c_3_r_*SPP(18)) + SPP(11)*(OP_l_2_c_2_r_*SPP(7) - OP_l_1_c_2_r_*SPP(3) - OP_l_3_c_2_r_*SPP(9) + OP_l_11_c_2_r_*SPP(23) + OP_l_14_c_2_r_*SPP(18)); -nextP(3,6) = OP_l_1_c_6_r_*SPP(15) - OP_l_2_c_6_r_*SPP(4) + OP_l_3_c_6_r_*SPP(14) + OP_l_12_c_6_r_*SPP(23) + OP_l_15_c_6_r_*SPP(17) + SF(21)*(OP_l_1_c_16_r_*SPP(15) - OP_l_2_c_16_r_*SPP(4) + OP_l_3_c_16_r_*SPP(14) + OP_l_12_c_16_r_*SPP(23) + OP_l_15_c_16_r_*SPP(17)) - SPP(10)*(OP_l_1_c_1_r_*SPP(15) - OP_l_2_c_1_r_*SPP(4) + OP_l_3_c_1_r_*SPP(14) + OP_l_12_c_1_r_*SPP(23) + OP_l_15_c_1_r_*SPP(17)) + SPP(1)*(OP_l_1_c_3_r_*SPP(15) - OP_l_2_c_3_r_*SPP(4) + OP_l_3_c_3_r_*SPP(14) + OP_l_12_c_3_r_*SPP(23) + OP_l_15_c_3_r_*SPP(17)) + SPP(11)*(OP_l_1_c_2_r_*SPP(15) - OP_l_2_c_2_r_*SPP(4) + OP_l_3_c_2_r_*SPP(14) + OP_l_12_c_2_r_*SPP(23) + OP_l_15_c_2_r_*SPP(17)); -nextP(4,6) = OP_l_4_c_6_r_ + SQ(2) + OP_l_1_c_6_r_*SPP(2) + OP_l_2_c_6_r_*SPP(20) + OP_l_3_c_6_r_*SPP(16) - OP_l_16_c_6_r_*SPP(22) + SF(21)*(OP_l_4_c_16_r_ + OP_l_1_c_16_r_*SPP(2) + OP_l_2_c_16_r_*SPP(20) + OP_l_3_c_16_r_*SPP(16) - OP_l_16_c_16_r_*SPP(22)) - SPP(10)*(OP_l_4_c_1_r_ + OP_l_1_c_1_r_*SPP(2) + OP_l_2_c_1_r_*SPP(20) + OP_l_3_c_1_r_*SPP(16) - OP_l_16_c_1_r_*SPP(22)) + SPP(1)*(OP_l_4_c_3_r_ + OP_l_1_c_3_r_*SPP(2) + OP_l_2_c_3_r_*SPP(20) + OP_l_3_c_3_r_*SPP(16) - OP_l_16_c_3_r_*SPP(22)) + SPP(11)*(OP_l_4_c_2_r_ + OP_l_1_c_2_r_*SPP(2) + OP_l_2_c_2_r_*SPP(20) + OP_l_3_c_2_r_*SPP(16) - OP_l_16_c_2_r_*SPP(22)); -nextP(5,6) = OP_l_5_c_6_r_ + SQ(1) + OP_l_16_c_6_r_*SF(23) + OP_l_1_c_6_r_*SPP(21) + OP_l_2_c_6_r_*SPP(13) + OP_l_3_c_6_r_*SPP(12) + SF(21)*(OP_l_5_c_16_r_ + OP_l_16_c_16_r_*SF(23) + OP_l_1_c_16_r_*SPP(21) + OP_l_2_c_16_r_*SPP(13) + OP_l_3_c_16_r_*SPP(12)) - SPP(10)*(OP_l_5_c_1_r_ + OP_l_16_c_1_r_*SF(23) + OP_l_1_c_1_r_*SPP(21) + OP_l_2_c_1_r_*SPP(13) + OP_l_3_c_1_r_*SPP(12)) + SPP(1)*(OP_l_5_c_3_r_ + OP_l_16_c_3_r_*SF(23) + OP_l_1_c_3_r_*SPP(21) + OP_l_2_c_3_r_*SPP(13) + OP_l_3_c_3_r_*SPP(12)) + SPP(11)*(OP_l_5_c_2_r_ + OP_l_16_c_2_r_*SF(23) + OP_l_1_c_2_r_*SPP(21) + OP_l_2_c_2_r_*SPP(13) + OP_l_3_c_2_r_*SPP(12)); -nextP(6,6) = OP_l_6_c_6_r_ + OP_l_16_c_6_r_*SF(21) - OP_l_1_c_6_r_*SPP(10) + OP_l_2_c_6_r_*SPP(11) + OP_l_3_c_6_r_*SPP(1) + SQ(5)*(SQ(8) + 2*q0*q1)^2 + SQ(7)*(SQ(9) - 2*q0*q2)^2 + SF(21)*(OP_l_6_c_16_r_ + OP_l_16_c_16_r_*SF(21) - OP_l_1_c_16_r_*SPP(10) + OP_l_2_c_16_r_*SPP(11) + OP_l_3_c_16_r_*SPP(1)) - SPP(10)*(OP_l_6_c_1_r_ + OP_l_16_c_1_r_*SF(21) - OP_l_1_c_1_r_*SPP(10) + OP_l_2_c_1_r_*SPP(11) + OP_l_3_c_1_r_*SPP(1)) + SPP(1)*(OP_l_6_c_3_r_ + OP_l_16_c_3_r_*SF(21) - OP_l_1_c_3_r_*SPP(10) + OP_l_2_c_3_r_*SPP(11) + OP_l_3_c_3_r_*SPP(1)) + SPP(11)*(OP_l_6_c_2_r_ + OP_l_16_c_2_r_*SF(21) - OP_l_1_c_2_r_*SPP(10) + OP_l_2_c_2_r_*SPP(11) + OP_l_3_c_2_r_*SPP(1)) + SQ(6)*(SG(2) - SG(3) - SG(4) + SG(5))^2; -nextP(1,7) = OP_l_1_c_7_r_*SPP(6) - OP_l_2_c_7_r_*SPP(5) + OP_l_3_c_7_r_*SPP(8) + OP_l_10_c_7_r_*SPP(23) + OP_l_13_c_7_r_*SPP(19) + dt*(OP_l_1_c_4_r_*SPP(6) - OP_l_2_c_4_r_*SPP(5) + OP_l_3_c_4_r_*SPP(8) + OP_l_10_c_4_r_*SPP(23) + OP_l_13_c_4_r_*SPP(19)); -nextP(2,7) = OP_l_2_c_7_r_*SPP(7) - OP_l_1_c_7_r_*SPP(3) - OP_l_3_c_7_r_*SPP(9) + OP_l_11_c_7_r_*SPP(23) + OP_l_14_c_7_r_*SPP(18) + dt*(OP_l_2_c_4_r_*SPP(7) - OP_l_1_c_4_r_*SPP(3) - OP_l_3_c_4_r_*SPP(9) + OP_l_11_c_4_r_*SPP(23) + OP_l_14_c_4_r_*SPP(18)); -nextP(3,7) = OP_l_1_c_7_r_*SPP(15) - OP_l_2_c_7_r_*SPP(4) + OP_l_3_c_7_r_*SPP(14) + OP_l_12_c_7_r_*SPP(23) + OP_l_15_c_7_r_*SPP(17) + dt*(OP_l_1_c_4_r_*SPP(15) - OP_l_2_c_4_r_*SPP(4) + OP_l_3_c_4_r_*SPP(14) + OP_l_12_c_4_r_*SPP(23) + OP_l_15_c_4_r_*SPP(17)); -nextP(4,7) = OP_l_4_c_7_r_ + OP_l_1_c_7_r_*SPP(2) + OP_l_2_c_7_r_*SPP(20) + OP_l_3_c_7_r_*SPP(16) - OP_l_16_c_7_r_*SPP(22) + dt*(OP_l_4_c_4_r_ + OP_l_1_c_4_r_*SPP(2) + OP_l_2_c_4_r_*SPP(20) + OP_l_3_c_4_r_*SPP(16) - OP_l_16_c_4_r_*SPP(22)); -nextP(5,7) = OP_l_5_c_7_r_ + OP_l_16_c_7_r_*SF(23) + OP_l_1_c_7_r_*SPP(21) + OP_l_2_c_7_r_*SPP(13) + OP_l_3_c_7_r_*SPP(12) + dt*(OP_l_5_c_4_r_ + OP_l_16_c_4_r_*SF(23) + OP_l_1_c_4_r_*SPP(21) + OP_l_2_c_4_r_*SPP(13) + OP_l_3_c_4_r_*SPP(12)); -nextP(6,7) = OP_l_6_c_7_r_ + OP_l_16_c_7_r_*SF(21) - OP_l_1_c_7_r_*SPP(10) + OP_l_2_c_7_r_*SPP(11) + OP_l_3_c_7_r_*SPP(1) + dt*(OP_l_6_c_4_r_ + OP_l_16_c_4_r_*SF(21) - OP_l_1_c_4_r_*SPP(10) + OP_l_2_c_4_r_*SPP(11) + OP_l_3_c_4_r_*SPP(1)); -nextP(7,7) = OP_l_7_c_7_r_ + OP_l_4_c_7_r_*dt + dt*(OP_l_7_c_4_r_ + OP_l_4_c_4_r_*dt); -nextP(1,8) = OP_l_1_c_8_r_*SPP(6) - OP_l_2_c_8_r_*SPP(5) + OP_l_3_c_8_r_*SPP(8) + OP_l_10_c_8_r_*SPP(23) + OP_l_13_c_8_r_*SPP(19) + dt*(OP_l_1_c_5_r_*SPP(6) - OP_l_2_c_5_r_*SPP(5) + OP_l_3_c_5_r_*SPP(8) + OP_l_10_c_5_r_*SPP(23) + OP_l_13_c_5_r_*SPP(19)); -nextP(2,8) = OP_l_2_c_8_r_*SPP(7) - OP_l_1_c_8_r_*SPP(3) - OP_l_3_c_8_r_*SPP(9) + OP_l_11_c_8_r_*SPP(23) + OP_l_14_c_8_r_*SPP(18) + dt*(OP_l_2_c_5_r_*SPP(7) - OP_l_1_c_5_r_*SPP(3) - OP_l_3_c_5_r_*SPP(9) + OP_l_11_c_5_r_*SPP(23) + OP_l_14_c_5_r_*SPP(18)); -nextP(3,8) = OP_l_1_c_8_r_*SPP(15) - OP_l_2_c_8_r_*SPP(4) + OP_l_3_c_8_r_*SPP(14) + OP_l_12_c_8_r_*SPP(23) + OP_l_15_c_8_r_*SPP(17) + dt*(OP_l_1_c_5_r_*SPP(15) - OP_l_2_c_5_r_*SPP(4) + OP_l_3_c_5_r_*SPP(14) + OP_l_12_c_5_r_*SPP(23) + OP_l_15_c_5_r_*SPP(17)); -nextP(4,8) = OP_l_4_c_8_r_ + OP_l_1_c_8_r_*SPP(2) + OP_l_2_c_8_r_*SPP(20) + OP_l_3_c_8_r_*SPP(16) - OP_l_16_c_8_r_*SPP(22) + dt*(OP_l_4_c_5_r_ + OP_l_1_c_5_r_*SPP(2) + OP_l_2_c_5_r_*SPP(20) + OP_l_3_c_5_r_*SPP(16) - OP_l_16_c_5_r_*SPP(22)); -nextP(5,8) = OP_l_5_c_8_r_ + OP_l_16_c_8_r_*SF(23) + OP_l_1_c_8_r_*SPP(21) + OP_l_2_c_8_r_*SPP(13) + OP_l_3_c_8_r_*SPP(12) + dt*(OP_l_5_c_5_r_ + OP_l_16_c_5_r_*SF(23) + OP_l_1_c_5_r_*SPP(21) + OP_l_2_c_5_r_*SPP(13) + OP_l_3_c_5_r_*SPP(12)); -nextP(6,8) = OP_l_6_c_8_r_ + OP_l_16_c_8_r_*SF(21) - OP_l_1_c_8_r_*SPP(10) + OP_l_2_c_8_r_*SPP(11) + OP_l_3_c_8_r_*SPP(1) + dt*(OP_l_6_c_5_r_ + OP_l_16_c_5_r_*SF(21) - OP_l_1_c_5_r_*SPP(10) + OP_l_2_c_5_r_*SPP(11) + OP_l_3_c_5_r_*SPP(1)); -nextP(7,8) = OP_l_7_c_8_r_ + OP_l_4_c_8_r_*dt + dt*(OP_l_7_c_5_r_ + OP_l_4_c_5_r_*dt); +nextP(1,1) = OP_l_1_c_1_r_ + OP_l_2_c_1_r_*SF(10) + OP_l_3_c_1_r_*SF(12) + OP_l_4_c_1_r_*SF(11) + OP_l_11_c_1_r_*SF(15) + OP_l_12_c_1_r_*SF(16) + OP_l_13_c_1_r_*SPP(11) + (daxVar*SQ(11))/4 + SF(10)*(OP_l_1_c_2_r_ + OP_l_2_c_2_r_*SF(10) + OP_l_3_c_2_r_*SF(12) + OP_l_4_c_2_r_*SF(11) + OP_l_11_c_2_r_*SF(15) + OP_l_12_c_2_r_*SF(16) + OP_l_13_c_2_r_*SPP(11)) + SF(12)*(OP_l_1_c_3_r_ + OP_l_2_c_3_r_*SF(10) + OP_l_3_c_3_r_*SF(12) + OP_l_4_c_3_r_*SF(11) + OP_l_11_c_3_r_*SF(15) + OP_l_12_c_3_r_*SF(16) + OP_l_13_c_3_r_*SPP(11)) + SF(11)*(OP_l_1_c_4_r_ + OP_l_2_c_4_r_*SF(10) + OP_l_3_c_4_r_*SF(12) + OP_l_4_c_4_r_*SF(11) + OP_l_11_c_4_r_*SF(15) + OP_l_12_c_4_r_*SF(16) + OP_l_13_c_4_r_*SPP(11)) + SF(15)*(OP_l_1_c_11_r_ + OP_l_2_c_11_r_*SF(10) + OP_l_3_c_11_r_*SF(12) + OP_l_4_c_11_r_*SF(11) + OP_l_11_c_11_r_*SF(15) + OP_l_12_c_11_r_*SF(16) + OP_l_13_c_11_r_*SPP(11)) + SF(16)*(OP_l_1_c_12_r_ + OP_l_2_c_12_r_*SF(10) + OP_l_3_c_12_r_*SF(12) + OP_l_4_c_12_r_*SF(11) + OP_l_11_c_12_r_*SF(15) + OP_l_12_c_12_r_*SF(16) + OP_l_13_c_12_r_*SPP(11)) + SPP(11)*(OP_l_1_c_13_r_ + OP_l_2_c_13_r_*SF(10) + OP_l_3_c_13_r_*SF(12) + OP_l_4_c_13_r_*SF(11) + OP_l_11_c_13_r_*SF(15) + OP_l_12_c_13_r_*SF(16) + OP_l_13_c_13_r_*SPP(11)) + (dayVar*q2^2)/4 + (dazVar*q3^2)/4; +nextP(1,2) = OP_l_1_c_2_r_ + SQ(9) + OP_l_2_c_2_r_*SF(10) + OP_l_3_c_2_r_*SF(12) + OP_l_4_c_2_r_*SF(11) + OP_l_11_c_2_r_*SF(15) + OP_l_12_c_2_r_*SF(16) + OP_l_13_c_2_r_*SPP(11) + SF(9)*(OP_l_1_c_1_r_ + OP_l_2_c_1_r_*SF(10) + OP_l_3_c_1_r_*SF(12) + OP_l_4_c_1_r_*SF(11) + OP_l_11_c_1_r_*SF(15) + OP_l_12_c_1_r_*SF(16) + OP_l_13_c_1_r_*SPP(11)) + SF(8)*(OP_l_1_c_3_r_ + OP_l_2_c_3_r_*SF(10) + OP_l_3_c_3_r_*SF(12) + OP_l_4_c_3_r_*SF(11) + OP_l_11_c_3_r_*SF(15) + OP_l_12_c_3_r_*SF(16) + OP_l_13_c_3_r_*SPP(11)) + SF(12)*(OP_l_1_c_4_r_ + OP_l_2_c_4_r_*SF(10) + OP_l_3_c_4_r_*SF(12) + OP_l_4_c_4_r_*SF(11) + OP_l_11_c_4_r_*SF(15) + OP_l_12_c_4_r_*SF(16) + OP_l_13_c_4_r_*SPP(11)) - SF(16)*(OP_l_1_c_13_r_ + OP_l_2_c_13_r_*SF(10) + OP_l_3_c_13_r_*SF(12) + OP_l_4_c_13_r_*SF(11) + OP_l_11_c_13_r_*SF(15) + OP_l_12_c_13_r_*SF(16) + OP_l_13_c_13_r_*SPP(11)) + SPP(11)*(OP_l_1_c_12_r_ + OP_l_2_c_12_r_*SF(10) + OP_l_3_c_12_r_*SF(12) + OP_l_4_c_12_r_*SF(11) + OP_l_11_c_12_r_*SF(15) + OP_l_12_c_12_r_*SF(16) + OP_l_13_c_12_r_*SPP(11)) - (q0*(OP_l_1_c_11_r_ + OP_l_2_c_11_r_*SF(10) + OP_l_3_c_11_r_*SF(12) + OP_l_4_c_11_r_*SF(11) + OP_l_11_c_11_r_*SF(15) + OP_l_12_c_11_r_*SF(16) + OP_l_13_c_11_r_*SPP(11)))/2; +nextP(2,2) = OP_l_2_c_2_r_ + OP_l_1_c_2_r_*SF(9) + OP_l_3_c_2_r_*SF(8) + OP_l_4_c_2_r_*SF(12) - OP_l_13_c_2_r_*SF(16) + OP_l_12_c_2_r_*SPP(11) + daxVar*SQ(10) - (OP_l_11_c_2_r_*q0)/2 + SF(9)*(OP_l_2_c_1_r_ + OP_l_1_c_1_r_*SF(9) + OP_l_3_c_1_r_*SF(8) + OP_l_4_c_1_r_*SF(12) - OP_l_13_c_1_r_*SF(16) + OP_l_12_c_1_r_*SPP(11) - (OP_l_11_c_1_r_*q0)/2) + SF(8)*(OP_l_2_c_3_r_ + OP_l_1_c_3_r_*SF(9) + OP_l_3_c_3_r_*SF(8) + OP_l_4_c_3_r_*SF(12) - OP_l_13_c_3_r_*SF(16) + OP_l_12_c_3_r_*SPP(11) - (OP_l_11_c_3_r_*q0)/2) + SF(12)*(OP_l_2_c_4_r_ + OP_l_1_c_4_r_*SF(9) + OP_l_3_c_4_r_*SF(8) + OP_l_4_c_4_r_*SF(12) - OP_l_13_c_4_r_*SF(16) + OP_l_12_c_4_r_*SPP(11) - (OP_l_11_c_4_r_*q0)/2) - SF(16)*(OP_l_2_c_13_r_ + OP_l_1_c_13_r_*SF(9) + OP_l_3_c_13_r_*SF(8) + OP_l_4_c_13_r_*SF(12) - OP_l_13_c_13_r_*SF(16) + OP_l_12_c_13_r_*SPP(11) - (OP_l_11_c_13_r_*q0)/2) + SPP(11)*(OP_l_2_c_12_r_ + OP_l_1_c_12_r_*SF(9) + OP_l_3_c_12_r_*SF(8) + OP_l_4_c_12_r_*SF(12) - OP_l_13_c_12_r_*SF(16) + OP_l_12_c_12_r_*SPP(11) - (OP_l_11_c_12_r_*q0)/2) + (dayVar*q3^2)/4 + (dazVar*q2^2)/4 - (q0*(OP_l_2_c_11_r_ + OP_l_1_c_11_r_*SF(9) + OP_l_3_c_11_r_*SF(8) + OP_l_4_c_11_r_*SF(12) - OP_l_13_c_11_r_*SF(16) + OP_l_12_c_11_r_*SPP(11) - (OP_l_11_c_11_r_*q0)/2))/2; +nextP(1,3) = OP_l_1_c_3_r_ + SQ(8) + OP_l_2_c_3_r_*SF(10) + OP_l_3_c_3_r_*SF(12) + OP_l_4_c_3_r_*SF(11) + OP_l_11_c_3_r_*SF(15) + OP_l_12_c_3_r_*SF(16) + OP_l_13_c_3_r_*SPP(11) + SF(7)*(OP_l_1_c_1_r_ + OP_l_2_c_1_r_*SF(10) + OP_l_3_c_1_r_*SF(12) + OP_l_4_c_1_r_*SF(11) + OP_l_11_c_1_r_*SF(15) + OP_l_12_c_1_r_*SF(16) + OP_l_13_c_1_r_*SPP(11)) + SF(11)*(OP_l_1_c_2_r_ + OP_l_2_c_2_r_*SF(10) + OP_l_3_c_2_r_*SF(12) + OP_l_4_c_2_r_*SF(11) + OP_l_11_c_2_r_*SF(15) + OP_l_12_c_2_r_*SF(16) + OP_l_13_c_2_r_*SPP(11)) + SF(9)*(OP_l_1_c_4_r_ + OP_l_2_c_4_r_*SF(10) + OP_l_3_c_4_r_*SF(12) + OP_l_4_c_4_r_*SF(11) + OP_l_11_c_4_r_*SF(15) + OP_l_12_c_4_r_*SF(16) + OP_l_13_c_4_r_*SPP(11)) + SF(15)*(OP_l_1_c_13_r_ + OP_l_2_c_13_r_*SF(10) + OP_l_3_c_13_r_*SF(12) + OP_l_4_c_13_r_*SF(11) + OP_l_11_c_13_r_*SF(15) + OP_l_12_c_13_r_*SF(16) + OP_l_13_c_13_r_*SPP(11)) - SPP(11)*(OP_l_1_c_11_r_ + OP_l_2_c_11_r_*SF(10) + OP_l_3_c_11_r_*SF(12) + OP_l_4_c_11_r_*SF(11) + OP_l_11_c_11_r_*SF(15) + OP_l_12_c_11_r_*SF(16) + OP_l_13_c_11_r_*SPP(11)) - (q0*(OP_l_1_c_12_r_ + OP_l_2_c_12_r_*SF(10) + OP_l_3_c_12_r_*SF(12) + OP_l_4_c_12_r_*SF(11) + OP_l_11_c_12_r_*SF(15) + OP_l_12_c_12_r_*SF(16) + OP_l_13_c_12_r_*SPP(11)))/2; +nextP(2,3) = OP_l_2_c_3_r_ + SQ(6) + OP_l_1_c_3_r_*SF(9) + OP_l_3_c_3_r_*SF(8) + OP_l_4_c_3_r_*SF(12) - OP_l_13_c_3_r_*SF(16) + OP_l_12_c_3_r_*SPP(11) - (OP_l_11_c_3_r_*q0)/2 + SF(7)*(OP_l_2_c_1_r_ + OP_l_1_c_1_r_*SF(9) + OP_l_3_c_1_r_*SF(8) + OP_l_4_c_1_r_*SF(12) - OP_l_13_c_1_r_*SF(16) + OP_l_12_c_1_r_*SPP(11) - (OP_l_11_c_1_r_*q0)/2) + SF(11)*(OP_l_2_c_2_r_ + OP_l_1_c_2_r_*SF(9) + OP_l_3_c_2_r_*SF(8) + OP_l_4_c_2_r_*SF(12) - OP_l_13_c_2_r_*SF(16) + OP_l_12_c_2_r_*SPP(11) - (OP_l_11_c_2_r_*q0)/2) + SF(9)*(OP_l_2_c_4_r_ + OP_l_1_c_4_r_*SF(9) + OP_l_3_c_4_r_*SF(8) + OP_l_4_c_4_r_*SF(12) - OP_l_13_c_4_r_*SF(16) + OP_l_12_c_4_r_*SPP(11) - (OP_l_11_c_4_r_*q0)/2) + SF(15)*(OP_l_2_c_13_r_ + OP_l_1_c_13_r_*SF(9) + OP_l_3_c_13_r_*SF(8) + OP_l_4_c_13_r_*SF(12) - OP_l_13_c_13_r_*SF(16) + OP_l_12_c_13_r_*SPP(11) - (OP_l_11_c_13_r_*q0)/2) - SPP(11)*(OP_l_2_c_11_r_ + OP_l_1_c_11_r_*SF(9) + OP_l_3_c_11_r_*SF(8) + OP_l_4_c_11_r_*SF(12) - OP_l_13_c_11_r_*SF(16) + OP_l_12_c_11_r_*SPP(11) - (OP_l_11_c_11_r_*q0)/2) - (q0*(OP_l_2_c_12_r_ + OP_l_1_c_12_r_*SF(9) + OP_l_3_c_12_r_*SF(8) + OP_l_4_c_12_r_*SF(12) - OP_l_13_c_12_r_*SF(16) + OP_l_12_c_12_r_*SPP(11) - (OP_l_11_c_12_r_*q0)/2))/2; +nextP(3,3) = OP_l_3_c_3_r_ + OP_l_1_c_3_r_*SF(7) + OP_l_2_c_3_r_*SF(11) + OP_l_4_c_3_r_*SF(9) + OP_l_13_c_3_r_*SF(15) - OP_l_11_c_3_r_*SPP(11) + dayVar*SQ(10) + (dazVar*SQ(11))/4 - (OP_l_12_c_3_r_*q0)/2 + SF(7)*(OP_l_3_c_1_r_ + OP_l_1_c_1_r_*SF(7) + OP_l_2_c_1_r_*SF(11) + OP_l_4_c_1_r_*SF(9) + OP_l_13_c_1_r_*SF(15) - OP_l_11_c_1_r_*SPP(11) - (OP_l_12_c_1_r_*q0)/2) + SF(11)*(OP_l_3_c_2_r_ + OP_l_1_c_2_r_*SF(7) + OP_l_2_c_2_r_*SF(11) + OP_l_4_c_2_r_*SF(9) + OP_l_13_c_2_r_*SF(15) - OP_l_11_c_2_r_*SPP(11) - (OP_l_12_c_2_r_*q0)/2) + SF(9)*(OP_l_3_c_4_r_ + OP_l_1_c_4_r_*SF(7) + OP_l_2_c_4_r_*SF(11) + OP_l_4_c_4_r_*SF(9) + OP_l_13_c_4_r_*SF(15) - OP_l_11_c_4_r_*SPP(11) - (OP_l_12_c_4_r_*q0)/2) + SF(15)*(OP_l_3_c_13_r_ + OP_l_1_c_13_r_*SF(7) + OP_l_2_c_13_r_*SF(11) + OP_l_4_c_13_r_*SF(9) + OP_l_13_c_13_r_*SF(15) - OP_l_11_c_13_r_*SPP(11) - (OP_l_12_c_13_r_*q0)/2) - SPP(11)*(OP_l_3_c_11_r_ + OP_l_1_c_11_r_*SF(7) + OP_l_2_c_11_r_*SF(11) + OP_l_4_c_11_r_*SF(9) + OP_l_13_c_11_r_*SF(15) - OP_l_11_c_11_r_*SPP(11) - (OP_l_12_c_11_r_*q0)/2) + (daxVar*q3^2)/4 - (q0*(OP_l_3_c_12_r_ + OP_l_1_c_12_r_*SF(7) + OP_l_2_c_12_r_*SF(11) + OP_l_4_c_12_r_*SF(9) + OP_l_13_c_12_r_*SF(15) - OP_l_11_c_12_r_*SPP(11) - (OP_l_12_c_12_r_*q0)/2))/2; +nextP(1,4) = OP_l_1_c_4_r_ + SQ(7) + OP_l_2_c_4_r_*SF(10) + OP_l_3_c_4_r_*SF(12) + OP_l_4_c_4_r_*SF(11) + OP_l_11_c_4_r_*SF(15) + OP_l_12_c_4_r_*SF(16) + OP_l_13_c_4_r_*SPP(11) + SF(8)*(OP_l_1_c_1_r_ + OP_l_2_c_1_r_*SF(10) + OP_l_3_c_1_r_*SF(12) + OP_l_4_c_1_r_*SF(11) + OP_l_11_c_1_r_*SF(15) + OP_l_12_c_1_r_*SF(16) + OP_l_13_c_1_r_*SPP(11)) + SF(7)*(OP_l_1_c_2_r_ + OP_l_2_c_2_r_*SF(10) + OP_l_3_c_2_r_*SF(12) + OP_l_4_c_2_r_*SF(11) + OP_l_11_c_2_r_*SF(15) + OP_l_12_c_2_r_*SF(16) + OP_l_13_c_2_r_*SPP(11)) + SF(10)*(OP_l_1_c_3_r_ + OP_l_2_c_3_r_*SF(10) + OP_l_3_c_3_r_*SF(12) + OP_l_4_c_3_r_*SF(11) + OP_l_11_c_3_r_*SF(15) + OP_l_12_c_3_r_*SF(16) + OP_l_13_c_3_r_*SPP(11)) + SF(16)*(OP_l_1_c_11_r_ + OP_l_2_c_11_r_*SF(10) + OP_l_3_c_11_r_*SF(12) + OP_l_4_c_11_r_*SF(11) + OP_l_11_c_11_r_*SF(15) + OP_l_12_c_11_r_*SF(16) + OP_l_13_c_11_r_*SPP(11)) - SF(15)*(OP_l_1_c_12_r_ + OP_l_2_c_12_r_*SF(10) + OP_l_3_c_12_r_*SF(12) + OP_l_4_c_12_r_*SF(11) + OP_l_11_c_12_r_*SF(15) + OP_l_12_c_12_r_*SF(16) + OP_l_13_c_12_r_*SPP(11)) - (q0*(OP_l_1_c_13_r_ + OP_l_2_c_13_r_*SF(10) + OP_l_3_c_13_r_*SF(12) + OP_l_4_c_13_r_*SF(11) + OP_l_11_c_13_r_*SF(15) + OP_l_12_c_13_r_*SF(16) + OP_l_13_c_13_r_*SPP(11)))/2; +nextP(2,4) = OP_l_2_c_4_r_ + SQ(5) + OP_l_1_c_4_r_*SF(9) + OP_l_3_c_4_r_*SF(8) + OP_l_4_c_4_r_*SF(12) - OP_l_13_c_4_r_*SF(16) + OP_l_12_c_4_r_*SPP(11) - (OP_l_11_c_4_r_*q0)/2 + SF(8)*(OP_l_2_c_1_r_ + OP_l_1_c_1_r_*SF(9) + OP_l_3_c_1_r_*SF(8) + OP_l_4_c_1_r_*SF(12) - OP_l_13_c_1_r_*SF(16) + OP_l_12_c_1_r_*SPP(11) - (OP_l_11_c_1_r_*q0)/2) + SF(7)*(OP_l_2_c_2_r_ + OP_l_1_c_2_r_*SF(9) + OP_l_3_c_2_r_*SF(8) + OP_l_4_c_2_r_*SF(12) - OP_l_13_c_2_r_*SF(16) + OP_l_12_c_2_r_*SPP(11) - (OP_l_11_c_2_r_*q0)/2) + SF(10)*(OP_l_2_c_3_r_ + OP_l_1_c_3_r_*SF(9) + OP_l_3_c_3_r_*SF(8) + OP_l_4_c_3_r_*SF(12) - OP_l_13_c_3_r_*SF(16) + OP_l_12_c_3_r_*SPP(11) - (OP_l_11_c_3_r_*q0)/2) + SF(16)*(OP_l_2_c_11_r_ + OP_l_1_c_11_r_*SF(9) + OP_l_3_c_11_r_*SF(8) + OP_l_4_c_11_r_*SF(12) - OP_l_13_c_11_r_*SF(16) + OP_l_12_c_11_r_*SPP(11) - (OP_l_11_c_11_r_*q0)/2) - SF(15)*(OP_l_2_c_12_r_ + OP_l_1_c_12_r_*SF(9) + OP_l_3_c_12_r_*SF(8) + OP_l_4_c_12_r_*SF(12) - OP_l_13_c_12_r_*SF(16) + OP_l_12_c_12_r_*SPP(11) - (OP_l_11_c_12_r_*q0)/2) - (q0*(OP_l_2_c_13_r_ + OP_l_1_c_13_r_*SF(9) + OP_l_3_c_13_r_*SF(8) + OP_l_4_c_13_r_*SF(12) - OP_l_13_c_13_r_*SF(16) + OP_l_12_c_13_r_*SPP(11) - (OP_l_11_c_13_r_*q0)/2))/2; +nextP(3,4) = OP_l_3_c_4_r_ + SQ(4) + OP_l_1_c_4_r_*SF(7) + OP_l_2_c_4_r_*SF(11) + OP_l_4_c_4_r_*SF(9) + OP_l_13_c_4_r_*SF(15) - OP_l_11_c_4_r_*SPP(11) - (OP_l_12_c_4_r_*q0)/2 + SF(8)*(OP_l_3_c_1_r_ + OP_l_1_c_1_r_*SF(7) + OP_l_2_c_1_r_*SF(11) + OP_l_4_c_1_r_*SF(9) + OP_l_13_c_1_r_*SF(15) - OP_l_11_c_1_r_*SPP(11) - (OP_l_12_c_1_r_*q0)/2) + SF(7)*(OP_l_3_c_2_r_ + OP_l_1_c_2_r_*SF(7) + OP_l_2_c_2_r_*SF(11) + OP_l_4_c_2_r_*SF(9) + OP_l_13_c_2_r_*SF(15) - OP_l_11_c_2_r_*SPP(11) - (OP_l_12_c_2_r_*q0)/2) + SF(10)*(OP_l_3_c_3_r_ + OP_l_1_c_3_r_*SF(7) + OP_l_2_c_3_r_*SF(11) + OP_l_4_c_3_r_*SF(9) + OP_l_13_c_3_r_*SF(15) - OP_l_11_c_3_r_*SPP(11) - (OP_l_12_c_3_r_*q0)/2) + SF(16)*(OP_l_3_c_11_r_ + OP_l_1_c_11_r_*SF(7) + OP_l_2_c_11_r_*SF(11) + OP_l_4_c_11_r_*SF(9) + OP_l_13_c_11_r_*SF(15) - OP_l_11_c_11_r_*SPP(11) - (OP_l_12_c_11_r_*q0)/2) - SF(15)*(OP_l_3_c_12_r_ + OP_l_1_c_12_r_*SF(7) + OP_l_2_c_12_r_*SF(11) + OP_l_4_c_12_r_*SF(9) + OP_l_13_c_12_r_*SF(15) - OP_l_11_c_12_r_*SPP(11) - (OP_l_12_c_12_r_*q0)/2) - (q0*(OP_l_3_c_13_r_ + OP_l_1_c_13_r_*SF(7) + OP_l_2_c_13_r_*SF(11) + OP_l_4_c_13_r_*SF(9) + OP_l_13_c_13_r_*SF(15) - OP_l_11_c_13_r_*SPP(11) - (OP_l_12_c_13_r_*q0)/2))/2; +nextP(4,4) = OP_l_4_c_4_r_ + OP_l_1_c_4_r_*SF(8) + OP_l_2_c_4_r_*SF(7) + OP_l_3_c_4_r_*SF(10) + OP_l_11_c_4_r_*SF(16) - OP_l_12_c_4_r_*SF(15) + (dayVar*SQ(11))/4 + dazVar*SQ(10) - (OP_l_13_c_4_r_*q0)/2 + SF(8)*(OP_l_4_c_1_r_ + OP_l_1_c_1_r_*SF(8) + OP_l_2_c_1_r_*SF(7) + OP_l_3_c_1_r_*SF(10) + OP_l_11_c_1_r_*SF(16) - OP_l_12_c_1_r_*SF(15) - (OP_l_13_c_1_r_*q0)/2) + SF(7)*(OP_l_4_c_2_r_ + OP_l_1_c_2_r_*SF(8) + OP_l_2_c_2_r_*SF(7) + OP_l_3_c_2_r_*SF(10) + OP_l_11_c_2_r_*SF(16) - OP_l_12_c_2_r_*SF(15) - (OP_l_13_c_2_r_*q0)/2) + SF(10)*(OP_l_4_c_3_r_ + OP_l_1_c_3_r_*SF(8) + OP_l_2_c_3_r_*SF(7) + OP_l_3_c_3_r_*SF(10) + OP_l_11_c_3_r_*SF(16) - OP_l_12_c_3_r_*SF(15) - (OP_l_13_c_3_r_*q0)/2) + SF(16)*(OP_l_4_c_11_r_ + OP_l_1_c_11_r_*SF(8) + OP_l_2_c_11_r_*SF(7) + OP_l_3_c_11_r_*SF(10) + OP_l_11_c_11_r_*SF(16) - OP_l_12_c_11_r_*SF(15) - (OP_l_13_c_11_r_*q0)/2) - SF(15)*(OP_l_4_c_12_r_ + OP_l_1_c_12_r_*SF(8) + OP_l_2_c_12_r_*SF(7) + OP_l_3_c_12_r_*SF(10) + OP_l_11_c_12_r_*SF(16) - OP_l_12_c_12_r_*SF(15) - (OP_l_13_c_12_r_*q0)/2) + (daxVar*q2^2)/4 - (q0*(OP_l_4_c_13_r_ + OP_l_1_c_13_r_*SF(8) + OP_l_2_c_13_r_*SF(7) + OP_l_3_c_13_r_*SF(10) + OP_l_11_c_13_r_*SF(16) - OP_l_12_c_13_r_*SF(15) - (OP_l_13_c_13_r_*q0)/2))/2; +nextP(1,5) = OP_l_1_c_5_r_ + OP_l_2_c_5_r_*SF(10) + OP_l_3_c_5_r_*SF(12) + OP_l_4_c_5_r_*SF(11) + OP_l_11_c_5_r_*SF(15) + OP_l_12_c_5_r_*SF(16) + OP_l_13_c_5_r_*SPP(11) + SF(6)*(OP_l_1_c_1_r_ + OP_l_2_c_1_r_*SF(10) + OP_l_3_c_1_r_*SF(12) + OP_l_4_c_1_r_*SF(11) + OP_l_11_c_1_r_*SF(15) + OP_l_12_c_1_r_*SF(16) + OP_l_13_c_1_r_*SPP(11)) + SF(4)*(OP_l_1_c_2_r_ + OP_l_2_c_2_r_*SF(10) + OP_l_3_c_2_r_*SF(12) + OP_l_4_c_2_r_*SF(11) + OP_l_11_c_2_r_*SF(15) + OP_l_12_c_2_r_*SF(16) + OP_l_13_c_2_r_*SPP(11)) - SF(5)*(OP_l_1_c_4_r_ + OP_l_2_c_4_r_*SF(10) + OP_l_3_c_4_r_*SF(12) + OP_l_4_c_4_r_*SF(11) + OP_l_11_c_4_r_*SF(15) + OP_l_12_c_4_r_*SF(16) + OP_l_13_c_4_r_*SPP(11)) + SPP(1)*(OP_l_1_c_3_r_ + OP_l_2_c_3_r_*SF(10) + OP_l_3_c_3_r_*SF(12) + OP_l_4_c_3_r_*SF(11) + OP_l_11_c_3_r_*SF(15) + OP_l_12_c_3_r_*SF(16) + OP_l_13_c_3_r_*SPP(11)) + SPP(4)*(OP_l_1_c_14_r_ + OP_l_2_c_14_r_*SF(10) + OP_l_3_c_14_r_*SF(12) + OP_l_4_c_14_r_*SF(11) + OP_l_11_c_14_r_*SF(15) + OP_l_12_c_14_r_*SF(16) + OP_l_13_c_14_r_*SPP(11)) + SPP(7)*(OP_l_1_c_15_r_ + OP_l_2_c_15_r_*SF(10) + OP_l_3_c_15_r_*SF(12) + OP_l_4_c_15_r_*SF(11) + OP_l_11_c_15_r_*SF(15) + OP_l_12_c_15_r_*SF(16) + OP_l_13_c_15_r_*SPP(11)) - SPP(10)*(OP_l_1_c_16_r_ + OP_l_2_c_16_r_*SF(10) + OP_l_3_c_16_r_*SF(12) + OP_l_4_c_16_r_*SF(11) + OP_l_11_c_16_r_*SF(15) + OP_l_12_c_16_r_*SF(16) + OP_l_13_c_16_r_*SPP(11)); +nextP(2,5) = OP_l_2_c_5_r_ + OP_l_1_c_5_r_*SF(9) + OP_l_3_c_5_r_*SF(8) + OP_l_4_c_5_r_*SF(12) - OP_l_13_c_5_r_*SF(16) + OP_l_12_c_5_r_*SPP(11) - (OP_l_11_c_5_r_*q0)/2 + SF(6)*(OP_l_2_c_1_r_ + OP_l_1_c_1_r_*SF(9) + OP_l_3_c_1_r_*SF(8) + OP_l_4_c_1_r_*SF(12) - OP_l_13_c_1_r_*SF(16) + OP_l_12_c_1_r_*SPP(11) - (OP_l_11_c_1_r_*q0)/2) + SF(4)*(OP_l_2_c_2_r_ + OP_l_1_c_2_r_*SF(9) + OP_l_3_c_2_r_*SF(8) + OP_l_4_c_2_r_*SF(12) - OP_l_13_c_2_r_*SF(16) + OP_l_12_c_2_r_*SPP(11) - (OP_l_11_c_2_r_*q0)/2) - SF(5)*(OP_l_2_c_4_r_ + OP_l_1_c_4_r_*SF(9) + OP_l_3_c_4_r_*SF(8) + OP_l_4_c_4_r_*SF(12) - OP_l_13_c_4_r_*SF(16) + OP_l_12_c_4_r_*SPP(11) - (OP_l_11_c_4_r_*q0)/2) + SPP(1)*(OP_l_2_c_3_r_ + OP_l_1_c_3_r_*SF(9) + OP_l_3_c_3_r_*SF(8) + OP_l_4_c_3_r_*SF(12) - OP_l_13_c_3_r_*SF(16) + OP_l_12_c_3_r_*SPP(11) - (OP_l_11_c_3_r_*q0)/2) + SPP(4)*(OP_l_2_c_14_r_ + OP_l_1_c_14_r_*SF(9) + OP_l_3_c_14_r_*SF(8) + OP_l_4_c_14_r_*SF(12) - OP_l_13_c_14_r_*SF(16) + OP_l_12_c_14_r_*SPP(11) - (OP_l_11_c_14_r_*q0)/2) + SPP(7)*(OP_l_2_c_15_r_ + OP_l_1_c_15_r_*SF(9) + OP_l_3_c_15_r_*SF(8) + OP_l_4_c_15_r_*SF(12) - OP_l_13_c_15_r_*SF(16) + OP_l_12_c_15_r_*SPP(11) - (OP_l_11_c_15_r_*q0)/2) - SPP(10)*(OP_l_2_c_16_r_ + OP_l_1_c_16_r_*SF(9) + OP_l_3_c_16_r_*SF(8) + OP_l_4_c_16_r_*SF(12) - OP_l_13_c_16_r_*SF(16) + OP_l_12_c_16_r_*SPP(11) - (OP_l_11_c_16_r_*q0)/2); +nextP(3,5) = OP_l_3_c_5_r_ + OP_l_1_c_5_r_*SF(7) + OP_l_2_c_5_r_*SF(11) + OP_l_4_c_5_r_*SF(9) + OP_l_13_c_5_r_*SF(15) - OP_l_11_c_5_r_*SPP(11) - (OP_l_12_c_5_r_*q0)/2 + SF(6)*(OP_l_3_c_1_r_ + OP_l_1_c_1_r_*SF(7) + OP_l_2_c_1_r_*SF(11) + OP_l_4_c_1_r_*SF(9) + OP_l_13_c_1_r_*SF(15) - OP_l_11_c_1_r_*SPP(11) - (OP_l_12_c_1_r_*q0)/2) + SF(4)*(OP_l_3_c_2_r_ + OP_l_1_c_2_r_*SF(7) + OP_l_2_c_2_r_*SF(11) + OP_l_4_c_2_r_*SF(9) + OP_l_13_c_2_r_*SF(15) - OP_l_11_c_2_r_*SPP(11) - (OP_l_12_c_2_r_*q0)/2) - SF(5)*(OP_l_3_c_4_r_ + OP_l_1_c_4_r_*SF(7) + OP_l_2_c_4_r_*SF(11) + OP_l_4_c_4_r_*SF(9) + OP_l_13_c_4_r_*SF(15) - OP_l_11_c_4_r_*SPP(11) - (OP_l_12_c_4_r_*q0)/2) + SPP(1)*(OP_l_3_c_3_r_ + OP_l_1_c_3_r_*SF(7) + OP_l_2_c_3_r_*SF(11) + OP_l_4_c_3_r_*SF(9) + OP_l_13_c_3_r_*SF(15) - OP_l_11_c_3_r_*SPP(11) - (OP_l_12_c_3_r_*q0)/2) + SPP(4)*(OP_l_3_c_14_r_ + OP_l_1_c_14_r_*SF(7) + OP_l_2_c_14_r_*SF(11) + OP_l_4_c_14_r_*SF(9) + OP_l_13_c_14_r_*SF(15) - OP_l_11_c_14_r_*SPP(11) - (OP_l_12_c_14_r_*q0)/2) + SPP(7)*(OP_l_3_c_15_r_ + OP_l_1_c_15_r_*SF(7) + OP_l_2_c_15_r_*SF(11) + OP_l_4_c_15_r_*SF(9) + OP_l_13_c_15_r_*SF(15) - OP_l_11_c_15_r_*SPP(11) - (OP_l_12_c_15_r_*q0)/2) - SPP(10)*(OP_l_3_c_16_r_ + OP_l_1_c_16_r_*SF(7) + OP_l_2_c_16_r_*SF(11) + OP_l_4_c_16_r_*SF(9) + OP_l_13_c_16_r_*SF(15) - OP_l_11_c_16_r_*SPP(11) - (OP_l_12_c_16_r_*q0)/2); +nextP(4,5) = OP_l_4_c_5_r_ + OP_l_1_c_5_r_*SF(8) + OP_l_2_c_5_r_*SF(7) + OP_l_3_c_5_r_*SF(10) + OP_l_11_c_5_r_*SF(16) - OP_l_12_c_5_r_*SF(15) - (OP_l_13_c_5_r_*q0)/2 + SF(6)*(OP_l_4_c_1_r_ + OP_l_1_c_1_r_*SF(8) + OP_l_2_c_1_r_*SF(7) + OP_l_3_c_1_r_*SF(10) + OP_l_11_c_1_r_*SF(16) - OP_l_12_c_1_r_*SF(15) - (OP_l_13_c_1_r_*q0)/2) + SF(4)*(OP_l_4_c_2_r_ + OP_l_1_c_2_r_*SF(8) + OP_l_2_c_2_r_*SF(7) + OP_l_3_c_2_r_*SF(10) + OP_l_11_c_2_r_*SF(16) - OP_l_12_c_2_r_*SF(15) - (OP_l_13_c_2_r_*q0)/2) - SF(5)*(OP_l_4_c_4_r_ + OP_l_1_c_4_r_*SF(8) + OP_l_2_c_4_r_*SF(7) + OP_l_3_c_4_r_*SF(10) + OP_l_11_c_4_r_*SF(16) - OP_l_12_c_4_r_*SF(15) - (OP_l_13_c_4_r_*q0)/2) + SPP(1)*(OP_l_4_c_3_r_ + OP_l_1_c_3_r_*SF(8) + OP_l_2_c_3_r_*SF(7) + OP_l_3_c_3_r_*SF(10) + OP_l_11_c_3_r_*SF(16) - OP_l_12_c_3_r_*SF(15) - (OP_l_13_c_3_r_*q0)/2) + SPP(4)*(OP_l_4_c_14_r_ + OP_l_1_c_14_r_*SF(8) + OP_l_2_c_14_r_*SF(7) + OP_l_3_c_14_r_*SF(10) + OP_l_11_c_14_r_*SF(16) - OP_l_12_c_14_r_*SF(15) - (OP_l_13_c_14_r_*q0)/2) + SPP(7)*(OP_l_4_c_15_r_ + OP_l_1_c_15_r_*SF(8) + OP_l_2_c_15_r_*SF(7) + OP_l_3_c_15_r_*SF(10) + OP_l_11_c_15_r_*SF(16) - OP_l_12_c_15_r_*SF(15) - (OP_l_13_c_15_r_*q0)/2) - SPP(10)*(OP_l_4_c_16_r_ + OP_l_1_c_16_r_*SF(8) + OP_l_2_c_16_r_*SF(7) + OP_l_3_c_16_r_*SF(10) + OP_l_11_c_16_r_*SF(16) - OP_l_12_c_16_r_*SF(15) - (OP_l_13_c_16_r_*q0)/2); +nextP(5,5) = OP_l_5_c_5_r_ + OP_l_1_c_5_r_*SF(6) + OP_l_2_c_5_r_*SF(4) - OP_l_4_c_5_r_*SF(5) + OP_l_3_c_5_r_*SPP(1) + OP_l_14_c_5_r_*SPP(4) + OP_l_15_c_5_r_*SPP(7) - OP_l_16_c_5_r_*SPP(10) + dvyVar*(SG(8) - 2*q0*q3)^2 + dvzVar*(SG(7) + 2*q0*q2)^2 + SF(6)*(OP_l_5_c_1_r_ + OP_l_1_c_1_r_*SF(6) + OP_l_2_c_1_r_*SF(4) - OP_l_4_c_1_r_*SF(5) + OP_l_3_c_1_r_*SPP(1) + OP_l_14_c_1_r_*SPP(4) + OP_l_15_c_1_r_*SPP(7) - OP_l_16_c_1_r_*SPP(10)) + SF(4)*(OP_l_5_c_2_r_ + OP_l_1_c_2_r_*SF(6) + OP_l_2_c_2_r_*SF(4) - OP_l_4_c_2_r_*SF(5) + OP_l_3_c_2_r_*SPP(1) + OP_l_14_c_2_r_*SPP(4) + OP_l_15_c_2_r_*SPP(7) - OP_l_16_c_2_r_*SPP(10)) - SF(5)*(OP_l_5_c_4_r_ + OP_l_1_c_4_r_*SF(6) + OP_l_2_c_4_r_*SF(4) - OP_l_4_c_4_r_*SF(5) + OP_l_3_c_4_r_*SPP(1) + OP_l_14_c_4_r_*SPP(4) + OP_l_15_c_4_r_*SPP(7) - OP_l_16_c_4_r_*SPP(10)) + SPP(1)*(OP_l_5_c_3_r_ + OP_l_1_c_3_r_*SF(6) + OP_l_2_c_3_r_*SF(4) - OP_l_4_c_3_r_*SF(5) + OP_l_3_c_3_r_*SPP(1) + OP_l_14_c_3_r_*SPP(4) + OP_l_15_c_3_r_*SPP(7) - OP_l_16_c_3_r_*SPP(10)) + SPP(4)*(OP_l_5_c_14_r_ + OP_l_1_c_14_r_*SF(6) + OP_l_2_c_14_r_*SF(4) - OP_l_4_c_14_r_*SF(5) + OP_l_3_c_14_r_*SPP(1) + OP_l_14_c_14_r_*SPP(4) + OP_l_15_c_14_r_*SPP(7) - OP_l_16_c_14_r_*SPP(10)) + SPP(7)*(OP_l_5_c_15_r_ + OP_l_1_c_15_r_*SF(6) + OP_l_2_c_15_r_*SF(4) - OP_l_4_c_15_r_*SF(5) + OP_l_3_c_15_r_*SPP(1) + OP_l_14_c_15_r_*SPP(4) + OP_l_15_c_15_r_*SPP(7) - OP_l_16_c_15_r_*SPP(10)) - SPP(10)*(OP_l_5_c_16_r_ + OP_l_1_c_16_r_*SF(6) + OP_l_2_c_16_r_*SF(4) - OP_l_4_c_16_r_*SF(5) + OP_l_3_c_16_r_*SPP(1) + OP_l_14_c_16_r_*SPP(4) + OP_l_15_c_16_r_*SPP(7) - OP_l_16_c_16_r_*SPP(10)) + dvxVar*(SG(2) + SG(3) - SG(4) - SG(5))^2; +nextP(1,6) = OP_l_1_c_6_r_ + OP_l_2_c_6_r_*SF(10) + OP_l_3_c_6_r_*SF(12) + OP_l_4_c_6_r_*SF(11) + OP_l_11_c_6_r_*SF(15) + OP_l_12_c_6_r_*SF(16) + OP_l_13_c_6_r_*SPP(11) + SF(5)*(OP_l_1_c_1_r_ + OP_l_2_c_1_r_*SF(10) + OP_l_3_c_1_r_*SF(12) + OP_l_4_c_1_r_*SF(11) + OP_l_11_c_1_r_*SF(15) + OP_l_12_c_1_r_*SF(16) + OP_l_13_c_1_r_*SPP(11)) + SF(4)*(OP_l_1_c_3_r_ + OP_l_2_c_3_r_*SF(10) + OP_l_3_c_3_r_*SF(12) + OP_l_4_c_3_r_*SF(11) + OP_l_11_c_3_r_*SF(15) + OP_l_12_c_3_r_*SF(16) + OP_l_13_c_3_r_*SPP(11)) + SF(6)*(OP_l_1_c_4_r_ + OP_l_2_c_4_r_*SF(10) + OP_l_3_c_4_r_*SF(12) + OP_l_4_c_4_r_*SF(11) + OP_l_11_c_4_r_*SF(15) + OP_l_12_c_4_r_*SF(16) + OP_l_13_c_4_r_*SPP(11)) - SPP(1)*(OP_l_1_c_2_r_ + OP_l_2_c_2_r_*SF(10) + OP_l_3_c_2_r_*SF(12) + OP_l_4_c_2_r_*SF(11) + OP_l_11_c_2_r_*SF(15) + OP_l_12_c_2_r_*SF(16) + OP_l_13_c_2_r_*SPP(11)) - SPP(9)*(OP_l_1_c_14_r_ + OP_l_2_c_14_r_*SF(10) + OP_l_3_c_14_r_*SF(12) + OP_l_4_c_14_r_*SF(11) + OP_l_11_c_14_r_*SF(15) + OP_l_12_c_14_r_*SF(16) + OP_l_13_c_14_r_*SPP(11)) + SPP(3)*(OP_l_1_c_15_r_ + OP_l_2_c_15_r_*SF(10) + OP_l_3_c_15_r_*SF(12) + OP_l_4_c_15_r_*SF(11) + OP_l_11_c_15_r_*SF(15) + OP_l_12_c_15_r_*SF(16) + OP_l_13_c_15_r_*SPP(11)) + SPP(6)*(OP_l_1_c_16_r_ + OP_l_2_c_16_r_*SF(10) + OP_l_3_c_16_r_*SF(12) + OP_l_4_c_16_r_*SF(11) + OP_l_11_c_16_r_*SF(15) + OP_l_12_c_16_r_*SF(16) + OP_l_13_c_16_r_*SPP(11)); +nextP(2,6) = OP_l_2_c_6_r_ + OP_l_1_c_6_r_*SF(9) + OP_l_3_c_6_r_*SF(8) + OP_l_4_c_6_r_*SF(12) - OP_l_13_c_6_r_*SF(16) + OP_l_12_c_6_r_*SPP(11) - (OP_l_11_c_6_r_*q0)/2 + SF(5)*(OP_l_2_c_1_r_ + OP_l_1_c_1_r_*SF(9) + OP_l_3_c_1_r_*SF(8) + OP_l_4_c_1_r_*SF(12) - OP_l_13_c_1_r_*SF(16) + OP_l_12_c_1_r_*SPP(11) - (OP_l_11_c_1_r_*q0)/2) + SF(4)*(OP_l_2_c_3_r_ + OP_l_1_c_3_r_*SF(9) + OP_l_3_c_3_r_*SF(8) + OP_l_4_c_3_r_*SF(12) - OP_l_13_c_3_r_*SF(16) + OP_l_12_c_3_r_*SPP(11) - (OP_l_11_c_3_r_*q0)/2) + SF(6)*(OP_l_2_c_4_r_ + OP_l_1_c_4_r_*SF(9) + OP_l_3_c_4_r_*SF(8) + OP_l_4_c_4_r_*SF(12) - OP_l_13_c_4_r_*SF(16) + OP_l_12_c_4_r_*SPP(11) - (OP_l_11_c_4_r_*q0)/2) - SPP(1)*(OP_l_2_c_2_r_ + OP_l_1_c_2_r_*SF(9) + OP_l_3_c_2_r_*SF(8) + OP_l_4_c_2_r_*SF(12) - OP_l_13_c_2_r_*SF(16) + OP_l_12_c_2_r_*SPP(11) - (OP_l_11_c_2_r_*q0)/2) - SPP(9)*(OP_l_2_c_14_r_ + OP_l_1_c_14_r_*SF(9) + OP_l_3_c_14_r_*SF(8) + OP_l_4_c_14_r_*SF(12) - OP_l_13_c_14_r_*SF(16) + OP_l_12_c_14_r_*SPP(11) - (OP_l_11_c_14_r_*q0)/2) + SPP(3)*(OP_l_2_c_15_r_ + OP_l_1_c_15_r_*SF(9) + OP_l_3_c_15_r_*SF(8) + OP_l_4_c_15_r_*SF(12) - OP_l_13_c_15_r_*SF(16) + OP_l_12_c_15_r_*SPP(11) - (OP_l_11_c_15_r_*q0)/2) + SPP(6)*(OP_l_2_c_16_r_ + OP_l_1_c_16_r_*SF(9) + OP_l_3_c_16_r_*SF(8) + OP_l_4_c_16_r_*SF(12) - OP_l_13_c_16_r_*SF(16) + OP_l_12_c_16_r_*SPP(11) - (OP_l_11_c_16_r_*q0)/2); +nextP(3,6) = OP_l_3_c_6_r_ + OP_l_1_c_6_r_*SF(7) + OP_l_2_c_6_r_*SF(11) + OP_l_4_c_6_r_*SF(9) + OP_l_13_c_6_r_*SF(15) - OP_l_11_c_6_r_*SPP(11) - (OP_l_12_c_6_r_*q0)/2 + SF(5)*(OP_l_3_c_1_r_ + OP_l_1_c_1_r_*SF(7) + OP_l_2_c_1_r_*SF(11) + OP_l_4_c_1_r_*SF(9) + OP_l_13_c_1_r_*SF(15) - OP_l_11_c_1_r_*SPP(11) - (OP_l_12_c_1_r_*q0)/2) + SF(4)*(OP_l_3_c_3_r_ + OP_l_1_c_3_r_*SF(7) + OP_l_2_c_3_r_*SF(11) + OP_l_4_c_3_r_*SF(9) + OP_l_13_c_3_r_*SF(15) - OP_l_11_c_3_r_*SPP(11) - (OP_l_12_c_3_r_*q0)/2) + SF(6)*(OP_l_3_c_4_r_ + OP_l_1_c_4_r_*SF(7) + OP_l_2_c_4_r_*SF(11) + OP_l_4_c_4_r_*SF(9) + OP_l_13_c_4_r_*SF(15) - OP_l_11_c_4_r_*SPP(11) - (OP_l_12_c_4_r_*q0)/2) - SPP(1)*(OP_l_3_c_2_r_ + OP_l_1_c_2_r_*SF(7) + OP_l_2_c_2_r_*SF(11) + OP_l_4_c_2_r_*SF(9) + OP_l_13_c_2_r_*SF(15) - OP_l_11_c_2_r_*SPP(11) - (OP_l_12_c_2_r_*q0)/2) - SPP(9)*(OP_l_3_c_14_r_ + OP_l_1_c_14_r_*SF(7) + OP_l_2_c_14_r_*SF(11) + OP_l_4_c_14_r_*SF(9) + OP_l_13_c_14_r_*SF(15) - OP_l_11_c_14_r_*SPP(11) - (OP_l_12_c_14_r_*q0)/2) + SPP(3)*(OP_l_3_c_15_r_ + OP_l_1_c_15_r_*SF(7) + OP_l_2_c_15_r_*SF(11) + OP_l_4_c_15_r_*SF(9) + OP_l_13_c_15_r_*SF(15) - OP_l_11_c_15_r_*SPP(11) - (OP_l_12_c_15_r_*q0)/2) + SPP(6)*(OP_l_3_c_16_r_ + OP_l_1_c_16_r_*SF(7) + OP_l_2_c_16_r_*SF(11) + OP_l_4_c_16_r_*SF(9) + OP_l_13_c_16_r_*SF(15) - OP_l_11_c_16_r_*SPP(11) - (OP_l_12_c_16_r_*q0)/2); +nextP(4,6) = OP_l_4_c_6_r_ + OP_l_1_c_6_r_*SF(8) + OP_l_2_c_6_r_*SF(7) + OP_l_3_c_6_r_*SF(10) + OP_l_11_c_6_r_*SF(16) - OP_l_12_c_6_r_*SF(15) - (OP_l_13_c_6_r_*q0)/2 + SF(5)*(OP_l_4_c_1_r_ + OP_l_1_c_1_r_*SF(8) + OP_l_2_c_1_r_*SF(7) + OP_l_3_c_1_r_*SF(10) + OP_l_11_c_1_r_*SF(16) - OP_l_12_c_1_r_*SF(15) - (OP_l_13_c_1_r_*q0)/2) + SF(4)*(OP_l_4_c_3_r_ + OP_l_1_c_3_r_*SF(8) + OP_l_2_c_3_r_*SF(7) + OP_l_3_c_3_r_*SF(10) + OP_l_11_c_3_r_*SF(16) - OP_l_12_c_3_r_*SF(15) - (OP_l_13_c_3_r_*q0)/2) + SF(6)*(OP_l_4_c_4_r_ + OP_l_1_c_4_r_*SF(8) + OP_l_2_c_4_r_*SF(7) + OP_l_3_c_4_r_*SF(10) + OP_l_11_c_4_r_*SF(16) - OP_l_12_c_4_r_*SF(15) - (OP_l_13_c_4_r_*q0)/2) - SPP(1)*(OP_l_4_c_2_r_ + OP_l_1_c_2_r_*SF(8) + OP_l_2_c_2_r_*SF(7) + OP_l_3_c_2_r_*SF(10) + OP_l_11_c_2_r_*SF(16) - OP_l_12_c_2_r_*SF(15) - (OP_l_13_c_2_r_*q0)/2) - SPP(9)*(OP_l_4_c_14_r_ + OP_l_1_c_14_r_*SF(8) + OP_l_2_c_14_r_*SF(7) + OP_l_3_c_14_r_*SF(10) + OP_l_11_c_14_r_*SF(16) - OP_l_12_c_14_r_*SF(15) - (OP_l_13_c_14_r_*q0)/2) + SPP(3)*(OP_l_4_c_15_r_ + OP_l_1_c_15_r_*SF(8) + OP_l_2_c_15_r_*SF(7) + OP_l_3_c_15_r_*SF(10) + OP_l_11_c_15_r_*SF(16) - OP_l_12_c_15_r_*SF(15) - (OP_l_13_c_15_r_*q0)/2) + SPP(6)*(OP_l_4_c_16_r_ + OP_l_1_c_16_r_*SF(8) + OP_l_2_c_16_r_*SF(7) + OP_l_3_c_16_r_*SF(10) + OP_l_11_c_16_r_*SF(16) - OP_l_12_c_16_r_*SF(15) - (OP_l_13_c_16_r_*q0)/2); +nextP(5,6) = OP_l_5_c_6_r_ + SQ(3) + OP_l_1_c_6_r_*SF(6) + OP_l_2_c_6_r_*SF(4) - OP_l_4_c_6_r_*SF(5) + OP_l_3_c_6_r_*SPP(1) + OP_l_14_c_6_r_*SPP(4) + OP_l_15_c_6_r_*SPP(7) - OP_l_16_c_6_r_*SPP(10) + SF(5)*(OP_l_5_c_1_r_ + OP_l_1_c_1_r_*SF(6) + OP_l_2_c_1_r_*SF(4) - OP_l_4_c_1_r_*SF(5) + OP_l_3_c_1_r_*SPP(1) + OP_l_14_c_1_r_*SPP(4) + OP_l_15_c_1_r_*SPP(7) - OP_l_16_c_1_r_*SPP(10)) + SF(4)*(OP_l_5_c_3_r_ + OP_l_1_c_3_r_*SF(6) + OP_l_2_c_3_r_*SF(4) - OP_l_4_c_3_r_*SF(5) + OP_l_3_c_3_r_*SPP(1) + OP_l_14_c_3_r_*SPP(4) + OP_l_15_c_3_r_*SPP(7) - OP_l_16_c_3_r_*SPP(10)) + SF(6)*(OP_l_5_c_4_r_ + OP_l_1_c_4_r_*SF(6) + OP_l_2_c_4_r_*SF(4) - OP_l_4_c_4_r_*SF(5) + OP_l_3_c_4_r_*SPP(1) + OP_l_14_c_4_r_*SPP(4) + OP_l_15_c_4_r_*SPP(7) - OP_l_16_c_4_r_*SPP(10)) - SPP(1)*(OP_l_5_c_2_r_ + OP_l_1_c_2_r_*SF(6) + OP_l_2_c_2_r_*SF(4) - OP_l_4_c_2_r_*SF(5) + OP_l_3_c_2_r_*SPP(1) + OP_l_14_c_2_r_*SPP(4) + OP_l_15_c_2_r_*SPP(7) - OP_l_16_c_2_r_*SPP(10)) - SPP(9)*(OP_l_5_c_14_r_ + OP_l_1_c_14_r_*SF(6) + OP_l_2_c_14_r_*SF(4) - OP_l_4_c_14_r_*SF(5) + OP_l_3_c_14_r_*SPP(1) + OP_l_14_c_14_r_*SPP(4) + OP_l_15_c_14_r_*SPP(7) - OP_l_16_c_14_r_*SPP(10)) + SPP(3)*(OP_l_5_c_15_r_ + OP_l_1_c_15_r_*SF(6) + OP_l_2_c_15_r_*SF(4) - OP_l_4_c_15_r_*SF(5) + OP_l_3_c_15_r_*SPP(1) + OP_l_14_c_15_r_*SPP(4) + OP_l_15_c_15_r_*SPP(7) - OP_l_16_c_15_r_*SPP(10)) + SPP(6)*(OP_l_5_c_16_r_ + OP_l_1_c_16_r_*SF(6) + OP_l_2_c_16_r_*SF(4) - OP_l_4_c_16_r_*SF(5) + OP_l_3_c_16_r_*SPP(1) + OP_l_14_c_16_r_*SPP(4) + OP_l_15_c_16_r_*SPP(7) - OP_l_16_c_16_r_*SPP(10)); +nextP(6,6) = OP_l_6_c_6_r_ + OP_l_1_c_6_r_*SF(5) + OP_l_3_c_6_r_*SF(4) + OP_l_4_c_6_r_*SF(6) - OP_l_2_c_6_r_*SPP(1) - OP_l_14_c_6_r_*SPP(9) + OP_l_15_c_6_r_*SPP(3) + OP_l_16_c_6_r_*SPP(6) + dvxVar*(SG(8) + 2*q0*q3)^2 + dvzVar*(SG(6) - 2*q0*q1)^2 + SF(5)*(OP_l_6_c_1_r_ + OP_l_1_c_1_r_*SF(5) + OP_l_3_c_1_r_*SF(4) + OP_l_4_c_1_r_*SF(6) - OP_l_2_c_1_r_*SPP(1) - OP_l_14_c_1_r_*SPP(9) + OP_l_15_c_1_r_*SPP(3) + OP_l_16_c_1_r_*SPP(6)) + SF(4)*(OP_l_6_c_3_r_ + OP_l_1_c_3_r_*SF(5) + OP_l_3_c_3_r_*SF(4) + OP_l_4_c_3_r_*SF(6) - OP_l_2_c_3_r_*SPP(1) - OP_l_14_c_3_r_*SPP(9) + OP_l_15_c_3_r_*SPP(3) + OP_l_16_c_3_r_*SPP(6)) + SF(6)*(OP_l_6_c_4_r_ + OP_l_1_c_4_r_*SF(5) + OP_l_3_c_4_r_*SF(4) + OP_l_4_c_4_r_*SF(6) - OP_l_2_c_4_r_*SPP(1) - OP_l_14_c_4_r_*SPP(9) + OP_l_15_c_4_r_*SPP(3) + OP_l_16_c_4_r_*SPP(6)) - SPP(1)*(OP_l_6_c_2_r_ + OP_l_1_c_2_r_*SF(5) + OP_l_3_c_2_r_*SF(4) + OP_l_4_c_2_r_*SF(6) - OP_l_2_c_2_r_*SPP(1) - OP_l_14_c_2_r_*SPP(9) + OP_l_15_c_2_r_*SPP(3) + OP_l_16_c_2_r_*SPP(6)) - SPP(9)*(OP_l_6_c_14_r_ + OP_l_1_c_14_r_*SF(5) + OP_l_3_c_14_r_*SF(4) + OP_l_4_c_14_r_*SF(6) - OP_l_2_c_14_r_*SPP(1) - OP_l_14_c_14_r_*SPP(9) + OP_l_15_c_14_r_*SPP(3) + OP_l_16_c_14_r_*SPP(6)) + SPP(3)*(OP_l_6_c_15_r_ + OP_l_1_c_15_r_*SF(5) + OP_l_3_c_15_r_*SF(4) + OP_l_4_c_15_r_*SF(6) - OP_l_2_c_15_r_*SPP(1) - OP_l_14_c_15_r_*SPP(9) + OP_l_15_c_15_r_*SPP(3) + OP_l_16_c_15_r_*SPP(6)) + SPP(6)*(OP_l_6_c_16_r_ + OP_l_1_c_16_r_*SF(5) + OP_l_3_c_16_r_*SF(4) + OP_l_4_c_16_r_*SF(6) - OP_l_2_c_16_r_*SPP(1) - OP_l_14_c_16_r_*SPP(9) + OP_l_15_c_16_r_*SPP(3) + OP_l_16_c_16_r_*SPP(6)) + dvyVar*(SG(2) - SG(3) + SG(4) - SG(5))^2; +nextP(1,7) = OP_l_1_c_7_r_ + OP_l_2_c_7_r_*SF(10) + OP_l_3_c_7_r_*SF(12) + OP_l_4_c_7_r_*SF(11) + OP_l_11_c_7_r_*SF(15) + OP_l_12_c_7_r_*SF(16) + OP_l_13_c_7_r_*SPP(11) + SF(5)*(OP_l_1_c_2_r_ + OP_l_2_c_2_r_*SF(10) + OP_l_3_c_2_r_*SF(12) + OP_l_4_c_2_r_*SF(11) + OP_l_11_c_2_r_*SF(15) + OP_l_12_c_2_r_*SF(16) + OP_l_13_c_2_r_*SPP(11)) - SF(6)*(OP_l_1_c_3_r_ + OP_l_2_c_3_r_*SF(10) + OP_l_3_c_3_r_*SF(12) + OP_l_4_c_3_r_*SF(11) + OP_l_11_c_3_r_*SF(15) + OP_l_12_c_3_r_*SF(16) + OP_l_13_c_3_r_*SPP(11)) + SF(4)*(OP_l_1_c_4_r_ + OP_l_2_c_4_r_*SF(10) + OP_l_3_c_4_r_*SF(12) + OP_l_4_c_4_r_*SF(11) + OP_l_11_c_4_r_*SF(15) + OP_l_12_c_4_r_*SF(16) + OP_l_13_c_4_r_*SPP(11)) + SPP(1)*(OP_l_1_c_1_r_ + OP_l_2_c_1_r_*SF(10) + OP_l_3_c_1_r_*SF(12) + OP_l_4_c_1_r_*SF(11) + OP_l_11_c_1_r_*SF(15) + OP_l_12_c_1_r_*SF(16) + OP_l_13_c_1_r_*SPP(11)) + SPP(5)*(OP_l_1_c_14_r_ + OP_l_2_c_14_r_*SF(10) + OP_l_3_c_14_r_*SF(12) + OP_l_4_c_14_r_*SF(11) + OP_l_11_c_14_r_*SF(15) + OP_l_12_c_14_r_*SF(16) + OP_l_13_c_14_r_*SPP(11)) - SPP(8)*(OP_l_1_c_15_r_ + OP_l_2_c_15_r_*SF(10) + OP_l_3_c_15_r_*SF(12) + OP_l_4_c_15_r_*SF(11) + OP_l_11_c_15_r_*SF(15) + OP_l_12_c_15_r_*SF(16) + OP_l_13_c_15_r_*SPP(11)) - SPP(2)*(OP_l_1_c_16_r_ + OP_l_2_c_16_r_*SF(10) + OP_l_3_c_16_r_*SF(12) + OP_l_4_c_16_r_*SF(11) + OP_l_11_c_16_r_*SF(15) + OP_l_12_c_16_r_*SF(16) + OP_l_13_c_16_r_*SPP(11)); +nextP(2,7) = OP_l_2_c_7_r_ + OP_l_1_c_7_r_*SF(9) + OP_l_3_c_7_r_*SF(8) + OP_l_4_c_7_r_*SF(12) - OP_l_13_c_7_r_*SF(16) + OP_l_12_c_7_r_*SPP(11) - (OP_l_11_c_7_r_*q0)/2 + SF(5)*(OP_l_2_c_2_r_ + OP_l_1_c_2_r_*SF(9) + OP_l_3_c_2_r_*SF(8) + OP_l_4_c_2_r_*SF(12) - OP_l_13_c_2_r_*SF(16) + OP_l_12_c_2_r_*SPP(11) - (OP_l_11_c_2_r_*q0)/2) - SF(6)*(OP_l_2_c_3_r_ + OP_l_1_c_3_r_*SF(9) + OP_l_3_c_3_r_*SF(8) + OP_l_4_c_3_r_*SF(12) - OP_l_13_c_3_r_*SF(16) + OP_l_12_c_3_r_*SPP(11) - (OP_l_11_c_3_r_*q0)/2) + SF(4)*(OP_l_2_c_4_r_ + OP_l_1_c_4_r_*SF(9) + OP_l_3_c_4_r_*SF(8) + OP_l_4_c_4_r_*SF(12) - OP_l_13_c_4_r_*SF(16) + OP_l_12_c_4_r_*SPP(11) - (OP_l_11_c_4_r_*q0)/2) + SPP(1)*(OP_l_2_c_1_r_ + OP_l_1_c_1_r_*SF(9) + OP_l_3_c_1_r_*SF(8) + OP_l_4_c_1_r_*SF(12) - OP_l_13_c_1_r_*SF(16) + OP_l_12_c_1_r_*SPP(11) - (OP_l_11_c_1_r_*q0)/2) + SPP(5)*(OP_l_2_c_14_r_ + OP_l_1_c_14_r_*SF(9) + OP_l_3_c_14_r_*SF(8) + OP_l_4_c_14_r_*SF(12) - OP_l_13_c_14_r_*SF(16) + OP_l_12_c_14_r_*SPP(11) - (OP_l_11_c_14_r_*q0)/2) - SPP(8)*(OP_l_2_c_15_r_ + OP_l_1_c_15_r_*SF(9) + OP_l_3_c_15_r_*SF(8) + OP_l_4_c_15_r_*SF(12) - OP_l_13_c_15_r_*SF(16) + OP_l_12_c_15_r_*SPP(11) - (OP_l_11_c_15_r_*q0)/2) - SPP(2)*(OP_l_2_c_16_r_ + OP_l_1_c_16_r_*SF(9) + OP_l_3_c_16_r_*SF(8) + OP_l_4_c_16_r_*SF(12) - OP_l_13_c_16_r_*SF(16) + OP_l_12_c_16_r_*SPP(11) - (OP_l_11_c_16_r_*q0)/2); +nextP(3,7) = OP_l_3_c_7_r_ + OP_l_1_c_7_r_*SF(7) + OP_l_2_c_7_r_*SF(11) + OP_l_4_c_7_r_*SF(9) + OP_l_13_c_7_r_*SF(15) - OP_l_11_c_7_r_*SPP(11) - (OP_l_12_c_7_r_*q0)/2 + SF(5)*(OP_l_3_c_2_r_ + OP_l_1_c_2_r_*SF(7) + OP_l_2_c_2_r_*SF(11) + OP_l_4_c_2_r_*SF(9) + OP_l_13_c_2_r_*SF(15) - OP_l_11_c_2_r_*SPP(11) - (OP_l_12_c_2_r_*q0)/2) - SF(6)*(OP_l_3_c_3_r_ + OP_l_1_c_3_r_*SF(7) + OP_l_2_c_3_r_*SF(11) + OP_l_4_c_3_r_*SF(9) + OP_l_13_c_3_r_*SF(15) - OP_l_11_c_3_r_*SPP(11) - (OP_l_12_c_3_r_*q0)/2) + SF(4)*(OP_l_3_c_4_r_ + OP_l_1_c_4_r_*SF(7) + OP_l_2_c_4_r_*SF(11) + OP_l_4_c_4_r_*SF(9) + OP_l_13_c_4_r_*SF(15) - OP_l_11_c_4_r_*SPP(11) - (OP_l_12_c_4_r_*q0)/2) + SPP(1)*(OP_l_3_c_1_r_ + OP_l_1_c_1_r_*SF(7) + OP_l_2_c_1_r_*SF(11) + OP_l_4_c_1_r_*SF(9) + OP_l_13_c_1_r_*SF(15) - OP_l_11_c_1_r_*SPP(11) - (OP_l_12_c_1_r_*q0)/2) + SPP(5)*(OP_l_3_c_14_r_ + OP_l_1_c_14_r_*SF(7) + OP_l_2_c_14_r_*SF(11) + OP_l_4_c_14_r_*SF(9) + OP_l_13_c_14_r_*SF(15) - OP_l_11_c_14_r_*SPP(11) - (OP_l_12_c_14_r_*q0)/2) - SPP(8)*(OP_l_3_c_15_r_ + OP_l_1_c_15_r_*SF(7) + OP_l_2_c_15_r_*SF(11) + OP_l_4_c_15_r_*SF(9) + OP_l_13_c_15_r_*SF(15) - OP_l_11_c_15_r_*SPP(11) - (OP_l_12_c_15_r_*q0)/2) - SPP(2)*(OP_l_3_c_16_r_ + OP_l_1_c_16_r_*SF(7) + OP_l_2_c_16_r_*SF(11) + OP_l_4_c_16_r_*SF(9) + OP_l_13_c_16_r_*SF(15) - OP_l_11_c_16_r_*SPP(11) - (OP_l_12_c_16_r_*q0)/2); +nextP(4,7) = OP_l_4_c_7_r_ + OP_l_1_c_7_r_*SF(8) + OP_l_2_c_7_r_*SF(7) + OP_l_3_c_7_r_*SF(10) + OP_l_11_c_7_r_*SF(16) - OP_l_12_c_7_r_*SF(15) - (OP_l_13_c_7_r_*q0)/2 + SF(5)*(OP_l_4_c_2_r_ + OP_l_1_c_2_r_*SF(8) + OP_l_2_c_2_r_*SF(7) + OP_l_3_c_2_r_*SF(10) + OP_l_11_c_2_r_*SF(16) - OP_l_12_c_2_r_*SF(15) - (OP_l_13_c_2_r_*q0)/2) - SF(6)*(OP_l_4_c_3_r_ + OP_l_1_c_3_r_*SF(8) + OP_l_2_c_3_r_*SF(7) + OP_l_3_c_3_r_*SF(10) + OP_l_11_c_3_r_*SF(16) - OP_l_12_c_3_r_*SF(15) - (OP_l_13_c_3_r_*q0)/2) + SF(4)*(OP_l_4_c_4_r_ + OP_l_1_c_4_r_*SF(8) + OP_l_2_c_4_r_*SF(7) + OP_l_3_c_4_r_*SF(10) + OP_l_11_c_4_r_*SF(16) - OP_l_12_c_4_r_*SF(15) - (OP_l_13_c_4_r_*q0)/2) + SPP(1)*(OP_l_4_c_1_r_ + OP_l_1_c_1_r_*SF(8) + OP_l_2_c_1_r_*SF(7) + OP_l_3_c_1_r_*SF(10) + OP_l_11_c_1_r_*SF(16) - OP_l_12_c_1_r_*SF(15) - (OP_l_13_c_1_r_*q0)/2) + SPP(5)*(OP_l_4_c_14_r_ + OP_l_1_c_14_r_*SF(8) + OP_l_2_c_14_r_*SF(7) + OP_l_3_c_14_r_*SF(10) + OP_l_11_c_14_r_*SF(16) - OP_l_12_c_14_r_*SF(15) - (OP_l_13_c_14_r_*q0)/2) - SPP(8)*(OP_l_4_c_15_r_ + OP_l_1_c_15_r_*SF(8) + OP_l_2_c_15_r_*SF(7) + OP_l_3_c_15_r_*SF(10) + OP_l_11_c_15_r_*SF(16) - OP_l_12_c_15_r_*SF(15) - (OP_l_13_c_15_r_*q0)/2) - SPP(2)*(OP_l_4_c_16_r_ + OP_l_1_c_16_r_*SF(8) + OP_l_2_c_16_r_*SF(7) + OP_l_3_c_16_r_*SF(10) + OP_l_11_c_16_r_*SF(16) - OP_l_12_c_16_r_*SF(15) - (OP_l_13_c_16_r_*q0)/2); +nextP(5,7) = OP_l_5_c_7_r_ + SQ(2) + OP_l_1_c_7_r_*SF(6) + OP_l_2_c_7_r_*SF(4) - OP_l_4_c_7_r_*SF(5) + OP_l_3_c_7_r_*SPP(1) + OP_l_14_c_7_r_*SPP(4) + OP_l_15_c_7_r_*SPP(7) - OP_l_16_c_7_r_*SPP(10) + SF(5)*(OP_l_5_c_2_r_ + OP_l_1_c_2_r_*SF(6) + OP_l_2_c_2_r_*SF(4) - OP_l_4_c_2_r_*SF(5) + OP_l_3_c_2_r_*SPP(1) + OP_l_14_c_2_r_*SPP(4) + OP_l_15_c_2_r_*SPP(7) - OP_l_16_c_2_r_*SPP(10)) - SF(6)*(OP_l_5_c_3_r_ + OP_l_1_c_3_r_*SF(6) + OP_l_2_c_3_r_*SF(4) - OP_l_4_c_3_r_*SF(5) + OP_l_3_c_3_r_*SPP(1) + OP_l_14_c_3_r_*SPP(4) + OP_l_15_c_3_r_*SPP(7) - OP_l_16_c_3_r_*SPP(10)) + SF(4)*(OP_l_5_c_4_r_ + OP_l_1_c_4_r_*SF(6) + OP_l_2_c_4_r_*SF(4) - OP_l_4_c_4_r_*SF(5) + OP_l_3_c_4_r_*SPP(1) + OP_l_14_c_4_r_*SPP(4) + OP_l_15_c_4_r_*SPP(7) - OP_l_16_c_4_r_*SPP(10)) + SPP(1)*(OP_l_5_c_1_r_ + OP_l_1_c_1_r_*SF(6) + OP_l_2_c_1_r_*SF(4) - OP_l_4_c_1_r_*SF(5) + OP_l_3_c_1_r_*SPP(1) + OP_l_14_c_1_r_*SPP(4) + OP_l_15_c_1_r_*SPP(7) - OP_l_16_c_1_r_*SPP(10)) + SPP(5)*(OP_l_5_c_14_r_ + OP_l_1_c_14_r_*SF(6) + OP_l_2_c_14_r_*SF(4) - OP_l_4_c_14_r_*SF(5) + OP_l_3_c_14_r_*SPP(1) + OP_l_14_c_14_r_*SPP(4) + OP_l_15_c_14_r_*SPP(7) - OP_l_16_c_14_r_*SPP(10)) - SPP(8)*(OP_l_5_c_15_r_ + OP_l_1_c_15_r_*SF(6) + OP_l_2_c_15_r_*SF(4) - OP_l_4_c_15_r_*SF(5) + OP_l_3_c_15_r_*SPP(1) + OP_l_14_c_15_r_*SPP(4) + OP_l_15_c_15_r_*SPP(7) - OP_l_16_c_15_r_*SPP(10)) - SPP(2)*(OP_l_5_c_16_r_ + OP_l_1_c_16_r_*SF(6) + OP_l_2_c_16_r_*SF(4) - OP_l_4_c_16_r_*SF(5) + OP_l_3_c_16_r_*SPP(1) + OP_l_14_c_16_r_*SPP(4) + OP_l_15_c_16_r_*SPP(7) - OP_l_16_c_16_r_*SPP(10)); +nextP(6,7) = OP_l_6_c_7_r_ + SQ(1) + OP_l_1_c_7_r_*SF(5) + OP_l_3_c_7_r_*SF(4) + OP_l_4_c_7_r_*SF(6) - OP_l_2_c_7_r_*SPP(1) - OP_l_14_c_7_r_*SPP(9) + OP_l_15_c_7_r_*SPP(3) + OP_l_16_c_7_r_*SPP(6) + SF(5)*(OP_l_6_c_2_r_ + OP_l_1_c_2_r_*SF(5) + OP_l_3_c_2_r_*SF(4) + OP_l_4_c_2_r_*SF(6) - OP_l_2_c_2_r_*SPP(1) - OP_l_14_c_2_r_*SPP(9) + OP_l_15_c_2_r_*SPP(3) + OP_l_16_c_2_r_*SPP(6)) - SF(6)*(OP_l_6_c_3_r_ + OP_l_1_c_3_r_*SF(5) + OP_l_3_c_3_r_*SF(4) + OP_l_4_c_3_r_*SF(6) - OP_l_2_c_3_r_*SPP(1) - OP_l_14_c_3_r_*SPP(9) + OP_l_15_c_3_r_*SPP(3) + OP_l_16_c_3_r_*SPP(6)) + SF(4)*(OP_l_6_c_4_r_ + OP_l_1_c_4_r_*SF(5) + OP_l_3_c_4_r_*SF(4) + OP_l_4_c_4_r_*SF(6) - OP_l_2_c_4_r_*SPP(1) - OP_l_14_c_4_r_*SPP(9) + OP_l_15_c_4_r_*SPP(3) + OP_l_16_c_4_r_*SPP(6)) + SPP(1)*(OP_l_6_c_1_r_ + OP_l_1_c_1_r_*SF(5) + OP_l_3_c_1_r_*SF(4) + OP_l_4_c_1_r_*SF(6) - OP_l_2_c_1_r_*SPP(1) - OP_l_14_c_1_r_*SPP(9) + OP_l_15_c_1_r_*SPP(3) + OP_l_16_c_1_r_*SPP(6)) + SPP(5)*(OP_l_6_c_14_r_ + OP_l_1_c_14_r_*SF(5) + OP_l_3_c_14_r_*SF(4) + OP_l_4_c_14_r_*SF(6) - OP_l_2_c_14_r_*SPP(1) - OP_l_14_c_14_r_*SPP(9) + OP_l_15_c_14_r_*SPP(3) + OP_l_16_c_14_r_*SPP(6)) - SPP(8)*(OP_l_6_c_15_r_ + OP_l_1_c_15_r_*SF(5) + OP_l_3_c_15_r_*SF(4) + OP_l_4_c_15_r_*SF(6) - OP_l_2_c_15_r_*SPP(1) - OP_l_14_c_15_r_*SPP(9) + OP_l_15_c_15_r_*SPP(3) + OP_l_16_c_15_r_*SPP(6)) - SPP(2)*(OP_l_6_c_16_r_ + OP_l_1_c_16_r_*SF(5) + OP_l_3_c_16_r_*SF(4) + OP_l_4_c_16_r_*SF(6) - OP_l_2_c_16_r_*SPP(1) - OP_l_14_c_16_r_*SPP(9) + OP_l_15_c_16_r_*SPP(3) + OP_l_16_c_16_r_*SPP(6)); +nextP(7,7) = OP_l_7_c_7_r_ + OP_l_2_c_7_r_*SF(5) - OP_l_3_c_7_r_*SF(6) + OP_l_4_c_7_r_*SF(4) + OP_l_1_c_7_r_*SPP(1) + OP_l_14_c_7_r_*SPP(5) - OP_l_15_c_7_r_*SPP(8) - OP_l_16_c_7_r_*SPP(2) + dvxVar*(SG(7) - 2*q0*q2)^2 + dvyVar*(SG(6) + 2*q0*q1)^2 + SF(5)*(OP_l_7_c_2_r_ + OP_l_2_c_2_r_*SF(5) - OP_l_3_c_2_r_*SF(6) + OP_l_4_c_2_r_*SF(4) + OP_l_1_c_2_r_*SPP(1) + OP_l_14_c_2_r_*SPP(5) - OP_l_15_c_2_r_*SPP(8) - OP_l_16_c_2_r_*SPP(2)) - SF(6)*(OP_l_7_c_3_r_ + OP_l_2_c_3_r_*SF(5) - OP_l_3_c_3_r_*SF(6) + OP_l_4_c_3_r_*SF(4) + OP_l_1_c_3_r_*SPP(1) + OP_l_14_c_3_r_*SPP(5) - OP_l_15_c_3_r_*SPP(8) - OP_l_16_c_3_r_*SPP(2)) + SF(4)*(OP_l_7_c_4_r_ + OP_l_2_c_4_r_*SF(5) - OP_l_3_c_4_r_*SF(6) + OP_l_4_c_4_r_*SF(4) + OP_l_1_c_4_r_*SPP(1) + OP_l_14_c_4_r_*SPP(5) - OP_l_15_c_4_r_*SPP(8) - OP_l_16_c_4_r_*SPP(2)) + SPP(1)*(OP_l_7_c_1_r_ + OP_l_2_c_1_r_*SF(5) - OP_l_3_c_1_r_*SF(6) + OP_l_4_c_1_r_*SF(4) + OP_l_1_c_1_r_*SPP(1) + OP_l_14_c_1_r_*SPP(5) - OP_l_15_c_1_r_*SPP(8) - OP_l_16_c_1_r_*SPP(2)) + SPP(5)*(OP_l_7_c_14_r_ + OP_l_2_c_14_r_*SF(5) - OP_l_3_c_14_r_*SF(6) + OP_l_4_c_14_r_*SF(4) + OP_l_1_c_14_r_*SPP(1) + OP_l_14_c_14_r_*SPP(5) - OP_l_15_c_14_r_*SPP(8) - OP_l_16_c_14_r_*SPP(2)) - SPP(8)*(OP_l_7_c_15_r_ + OP_l_2_c_15_r_*SF(5) - OP_l_3_c_15_r_*SF(6) + OP_l_4_c_15_r_*SF(4) + OP_l_1_c_15_r_*SPP(1) + OP_l_14_c_15_r_*SPP(5) - OP_l_15_c_15_r_*SPP(8) - OP_l_16_c_15_r_*SPP(2)) - SPP(2)*(OP_l_7_c_16_r_ + OP_l_2_c_16_r_*SF(5) - OP_l_3_c_16_r_*SF(6) + OP_l_4_c_16_r_*SF(4) + OP_l_1_c_16_r_*SPP(1) + OP_l_14_c_16_r_*SPP(5) - OP_l_15_c_16_r_*SPP(8) - OP_l_16_c_16_r_*SPP(2)) + dvzVar*(SG(2) - SG(3) - SG(4) + SG(5))^2; +nextP(1,8) = OP_l_1_c_8_r_ + OP_l_2_c_8_r_*SF(10) + OP_l_3_c_8_r_*SF(12) + OP_l_4_c_8_r_*SF(11) + OP_l_11_c_8_r_*SF(15) + OP_l_12_c_8_r_*SF(16) + OP_l_13_c_8_r_*SPP(11) + dt*(OP_l_1_c_5_r_ + OP_l_2_c_5_r_*SF(10) + OP_l_3_c_5_r_*SF(12) + OP_l_4_c_5_r_*SF(11) + OP_l_11_c_5_r_*SF(15) + OP_l_12_c_5_r_*SF(16) + OP_l_13_c_5_r_*SPP(11)); +nextP(2,8) = OP_l_2_c_8_r_ + OP_l_1_c_8_r_*SF(9) + OP_l_3_c_8_r_*SF(8) + OP_l_4_c_8_r_*SF(12) - OP_l_13_c_8_r_*SF(16) + OP_l_12_c_8_r_*SPP(11) - (OP_l_11_c_8_r_*q0)/2 + dt*(OP_l_2_c_5_r_ + OP_l_1_c_5_r_*SF(9) + OP_l_3_c_5_r_*SF(8) + OP_l_4_c_5_r_*SF(12) - OP_l_13_c_5_r_*SF(16) + OP_l_12_c_5_r_*SPP(11) - (OP_l_11_c_5_r_*q0)/2); +nextP(3,8) = OP_l_3_c_8_r_ + OP_l_1_c_8_r_*SF(7) + OP_l_2_c_8_r_*SF(11) + OP_l_4_c_8_r_*SF(9) + OP_l_13_c_8_r_*SF(15) - OP_l_11_c_8_r_*SPP(11) - (OP_l_12_c_8_r_*q0)/2 + dt*(OP_l_3_c_5_r_ + OP_l_1_c_5_r_*SF(7) + OP_l_2_c_5_r_*SF(11) + OP_l_4_c_5_r_*SF(9) + OP_l_13_c_5_r_*SF(15) - OP_l_11_c_5_r_*SPP(11) - (OP_l_12_c_5_r_*q0)/2); +nextP(4,8) = OP_l_4_c_8_r_ + OP_l_1_c_8_r_*SF(8) + OP_l_2_c_8_r_*SF(7) + OP_l_3_c_8_r_*SF(10) + OP_l_11_c_8_r_*SF(16) - OP_l_12_c_8_r_*SF(15) - (OP_l_13_c_8_r_*q0)/2 + dt*(OP_l_4_c_5_r_ + OP_l_1_c_5_r_*SF(8) + OP_l_2_c_5_r_*SF(7) + OP_l_3_c_5_r_*SF(10) + OP_l_11_c_5_r_*SF(16) - OP_l_12_c_5_r_*SF(15) - (OP_l_13_c_5_r_*q0)/2); +nextP(5,8) = OP_l_5_c_8_r_ + OP_l_1_c_8_r_*SF(6) + OP_l_2_c_8_r_*SF(4) - OP_l_4_c_8_r_*SF(5) + OP_l_3_c_8_r_*SPP(1) + OP_l_14_c_8_r_*SPP(4) + OP_l_15_c_8_r_*SPP(7) - OP_l_16_c_8_r_*SPP(10) + dt*(OP_l_5_c_5_r_ + OP_l_1_c_5_r_*SF(6) + OP_l_2_c_5_r_*SF(4) - OP_l_4_c_5_r_*SF(5) + OP_l_3_c_5_r_*SPP(1) + OP_l_14_c_5_r_*SPP(4) + OP_l_15_c_5_r_*SPP(7) - OP_l_16_c_5_r_*SPP(10)); +nextP(6,8) = OP_l_6_c_8_r_ + OP_l_1_c_8_r_*SF(5) + OP_l_3_c_8_r_*SF(4) + OP_l_4_c_8_r_*SF(6) - OP_l_2_c_8_r_*SPP(1) - OP_l_14_c_8_r_*SPP(9) + OP_l_15_c_8_r_*SPP(3) + OP_l_16_c_8_r_*SPP(6) + dt*(OP_l_6_c_5_r_ + OP_l_1_c_5_r_*SF(5) + OP_l_3_c_5_r_*SF(4) + OP_l_4_c_5_r_*SF(6) - OP_l_2_c_5_r_*SPP(1) - OP_l_14_c_5_r_*SPP(9) + OP_l_15_c_5_r_*SPP(3) + OP_l_16_c_5_r_*SPP(6)); +nextP(7,8) = OP_l_7_c_8_r_ + OP_l_2_c_8_r_*SF(5) - OP_l_3_c_8_r_*SF(6) + OP_l_4_c_8_r_*SF(4) + OP_l_1_c_8_r_*SPP(1) + OP_l_14_c_8_r_*SPP(5) - OP_l_15_c_8_r_*SPP(8) - OP_l_16_c_8_r_*SPP(2) + dt*(OP_l_7_c_5_r_ + OP_l_2_c_5_r_*SF(5) - OP_l_3_c_5_r_*SF(6) + OP_l_4_c_5_r_*SF(4) + OP_l_1_c_5_r_*SPP(1) + OP_l_14_c_5_r_*SPP(5) - OP_l_15_c_5_r_*SPP(8) - OP_l_16_c_5_r_*SPP(2)); nextP(8,8) = OP_l_8_c_8_r_ + OP_l_5_c_8_r_*dt + dt*(OP_l_8_c_5_r_ + OP_l_5_c_5_r_*dt); -nextP(1,9) = OP_l_1_c_9_r_*SPP(6) - OP_l_2_c_9_r_*SPP(5) + OP_l_3_c_9_r_*SPP(8) + OP_l_10_c_9_r_*SPP(23) + OP_l_13_c_9_r_*SPP(19) + dt*(OP_l_1_c_6_r_*SPP(6) - OP_l_2_c_6_r_*SPP(5) + OP_l_3_c_6_r_*SPP(8) + OP_l_10_c_6_r_*SPP(23) + OP_l_13_c_6_r_*SPP(19)); -nextP(2,9) = OP_l_2_c_9_r_*SPP(7) - OP_l_1_c_9_r_*SPP(3) - OP_l_3_c_9_r_*SPP(9) + OP_l_11_c_9_r_*SPP(23) + OP_l_14_c_9_r_*SPP(18) + dt*(OP_l_2_c_6_r_*SPP(7) - OP_l_1_c_6_r_*SPP(3) - OP_l_3_c_6_r_*SPP(9) + OP_l_11_c_6_r_*SPP(23) + OP_l_14_c_6_r_*SPP(18)); -nextP(3,9) = OP_l_1_c_9_r_*SPP(15) - OP_l_2_c_9_r_*SPP(4) + OP_l_3_c_9_r_*SPP(14) + OP_l_12_c_9_r_*SPP(23) + OP_l_15_c_9_r_*SPP(17) + dt*(OP_l_1_c_6_r_*SPP(15) - OP_l_2_c_6_r_*SPP(4) + OP_l_3_c_6_r_*SPP(14) + OP_l_12_c_6_r_*SPP(23) + OP_l_15_c_6_r_*SPP(17)); -nextP(4,9) = OP_l_4_c_9_r_ + OP_l_1_c_9_r_*SPP(2) + OP_l_2_c_9_r_*SPP(20) + OP_l_3_c_9_r_*SPP(16) - OP_l_16_c_9_r_*SPP(22) + dt*(OP_l_4_c_6_r_ + OP_l_1_c_6_r_*SPP(2) + OP_l_2_c_6_r_*SPP(20) + OP_l_3_c_6_r_*SPP(16) - OP_l_16_c_6_r_*SPP(22)); -nextP(5,9) = OP_l_5_c_9_r_ + OP_l_16_c_9_r_*SF(23) + OP_l_1_c_9_r_*SPP(21) + OP_l_2_c_9_r_*SPP(13) + OP_l_3_c_9_r_*SPP(12) + dt*(OP_l_5_c_6_r_ + OP_l_16_c_6_r_*SF(23) + OP_l_1_c_6_r_*SPP(21) + OP_l_2_c_6_r_*SPP(13) + OP_l_3_c_6_r_*SPP(12)); -nextP(6,9) = OP_l_6_c_9_r_ + OP_l_16_c_9_r_*SF(21) - OP_l_1_c_9_r_*SPP(10) + OP_l_2_c_9_r_*SPP(11) + OP_l_3_c_9_r_*SPP(1) + dt*(OP_l_6_c_6_r_ + OP_l_16_c_6_r_*SF(21) - OP_l_1_c_6_r_*SPP(10) + OP_l_2_c_6_r_*SPP(11) + OP_l_3_c_6_r_*SPP(1)); -nextP(7,9) = OP_l_7_c_9_r_ + OP_l_4_c_9_r_*dt + dt*(OP_l_7_c_6_r_ + OP_l_4_c_6_r_*dt); +nextP(1,9) = OP_l_1_c_9_r_ + OP_l_2_c_9_r_*SF(10) + OP_l_3_c_9_r_*SF(12) + OP_l_4_c_9_r_*SF(11) + OP_l_11_c_9_r_*SF(15) + OP_l_12_c_9_r_*SF(16) + OP_l_13_c_9_r_*SPP(11) + dt*(OP_l_1_c_6_r_ + OP_l_2_c_6_r_*SF(10) + OP_l_3_c_6_r_*SF(12) + OP_l_4_c_6_r_*SF(11) + OP_l_11_c_6_r_*SF(15) + OP_l_12_c_6_r_*SF(16) + OP_l_13_c_6_r_*SPP(11)); +nextP(2,9) = OP_l_2_c_9_r_ + OP_l_1_c_9_r_*SF(9) + OP_l_3_c_9_r_*SF(8) + OP_l_4_c_9_r_*SF(12) - OP_l_13_c_9_r_*SF(16) + OP_l_12_c_9_r_*SPP(11) - (OP_l_11_c_9_r_*q0)/2 + dt*(OP_l_2_c_6_r_ + OP_l_1_c_6_r_*SF(9) + OP_l_3_c_6_r_*SF(8) + OP_l_4_c_6_r_*SF(12) - OP_l_13_c_6_r_*SF(16) + OP_l_12_c_6_r_*SPP(11) - (OP_l_11_c_6_r_*q0)/2); +nextP(3,9) = OP_l_3_c_9_r_ + OP_l_1_c_9_r_*SF(7) + OP_l_2_c_9_r_*SF(11) + OP_l_4_c_9_r_*SF(9) + OP_l_13_c_9_r_*SF(15) - OP_l_11_c_9_r_*SPP(11) - (OP_l_12_c_9_r_*q0)/2 + dt*(OP_l_3_c_6_r_ + OP_l_1_c_6_r_*SF(7) + OP_l_2_c_6_r_*SF(11) + OP_l_4_c_6_r_*SF(9) + OP_l_13_c_6_r_*SF(15) - OP_l_11_c_6_r_*SPP(11) - (OP_l_12_c_6_r_*q0)/2); +nextP(4,9) = OP_l_4_c_9_r_ + OP_l_1_c_9_r_*SF(8) + OP_l_2_c_9_r_*SF(7) + OP_l_3_c_9_r_*SF(10) + OP_l_11_c_9_r_*SF(16) - OP_l_12_c_9_r_*SF(15) - (OP_l_13_c_9_r_*q0)/2 + dt*(OP_l_4_c_6_r_ + OP_l_1_c_6_r_*SF(8) + OP_l_2_c_6_r_*SF(7) + OP_l_3_c_6_r_*SF(10) + OP_l_11_c_6_r_*SF(16) - OP_l_12_c_6_r_*SF(15) - (OP_l_13_c_6_r_*q0)/2); +nextP(5,9) = OP_l_5_c_9_r_ + OP_l_1_c_9_r_*SF(6) + OP_l_2_c_9_r_*SF(4) - OP_l_4_c_9_r_*SF(5) + OP_l_3_c_9_r_*SPP(1) + OP_l_14_c_9_r_*SPP(4) + OP_l_15_c_9_r_*SPP(7) - OP_l_16_c_9_r_*SPP(10) + dt*(OP_l_5_c_6_r_ + OP_l_1_c_6_r_*SF(6) + OP_l_2_c_6_r_*SF(4) - OP_l_4_c_6_r_*SF(5) + OP_l_3_c_6_r_*SPP(1) + OP_l_14_c_6_r_*SPP(4) + OP_l_15_c_6_r_*SPP(7) - OP_l_16_c_6_r_*SPP(10)); +nextP(6,9) = OP_l_6_c_9_r_ + OP_l_1_c_9_r_*SF(5) + OP_l_3_c_9_r_*SF(4) + OP_l_4_c_9_r_*SF(6) - OP_l_2_c_9_r_*SPP(1) - OP_l_14_c_9_r_*SPP(9) + OP_l_15_c_9_r_*SPP(3) + OP_l_16_c_9_r_*SPP(6) + dt*(OP_l_6_c_6_r_ + OP_l_1_c_6_r_*SF(5) + OP_l_3_c_6_r_*SF(4) + OP_l_4_c_6_r_*SF(6) - OP_l_2_c_6_r_*SPP(1) - OP_l_14_c_6_r_*SPP(9) + OP_l_15_c_6_r_*SPP(3) + OP_l_16_c_6_r_*SPP(6)); +nextP(7,9) = OP_l_7_c_9_r_ + OP_l_2_c_9_r_*SF(5) - OP_l_3_c_9_r_*SF(6) + OP_l_4_c_9_r_*SF(4) + OP_l_1_c_9_r_*SPP(1) + OP_l_14_c_9_r_*SPP(5) - OP_l_15_c_9_r_*SPP(8) - OP_l_16_c_9_r_*SPP(2) + dt*(OP_l_7_c_6_r_ + OP_l_2_c_6_r_*SF(5) - OP_l_3_c_6_r_*SF(6) + OP_l_4_c_6_r_*SF(4) + OP_l_1_c_6_r_*SPP(1) + OP_l_14_c_6_r_*SPP(5) - OP_l_15_c_6_r_*SPP(8) - OP_l_16_c_6_r_*SPP(2)); nextP(8,9) = OP_l_8_c_9_r_ + OP_l_5_c_9_r_*dt + dt*(OP_l_8_c_6_r_ + OP_l_5_c_6_r_*dt); nextP(9,9) = OP_l_9_c_9_r_ + OP_l_6_c_9_r_*dt + dt*(OP_l_9_c_6_r_ + OP_l_6_c_6_r_*dt); -nextP(1,10) = OP_l_1_c_10_r_*SPP(6) - OP_l_2_c_10_r_*SPP(5) + OP_l_3_c_10_r_*SPP(8) + OP_l_10_c_10_r_*SPP(23) + OP_l_13_c_10_r_*SPP(19); -nextP(2,10) = OP_l_2_c_10_r_*SPP(7) - OP_l_1_c_10_r_*SPP(3) - OP_l_3_c_10_r_*SPP(9) + OP_l_11_c_10_r_*SPP(23) + OP_l_14_c_10_r_*SPP(18); -nextP(3,10) = OP_l_1_c_10_r_*SPP(15) - OP_l_2_c_10_r_*SPP(4) + OP_l_3_c_10_r_*SPP(14) + OP_l_12_c_10_r_*SPP(23) + OP_l_15_c_10_r_*SPP(17); -nextP(4,10) = OP_l_4_c_10_r_ + OP_l_1_c_10_r_*SPP(2) + OP_l_2_c_10_r_*SPP(20) + OP_l_3_c_10_r_*SPP(16) - OP_l_16_c_10_r_*SPP(22); -nextP(5,10) = OP_l_5_c_10_r_ + OP_l_16_c_10_r_*SF(23) + OP_l_1_c_10_r_*SPP(21) + OP_l_2_c_10_r_*SPP(13) + OP_l_3_c_10_r_*SPP(12); -nextP(6,10) = OP_l_6_c_10_r_ + OP_l_16_c_10_r_*SF(21) - OP_l_1_c_10_r_*SPP(10) + OP_l_2_c_10_r_*SPP(11) + OP_l_3_c_10_r_*SPP(1); -nextP(7,10) = OP_l_7_c_10_r_ + OP_l_4_c_10_r_*dt; -nextP(8,10) = OP_l_8_c_10_r_ + OP_l_5_c_10_r_*dt; -nextP(9,10) = OP_l_9_c_10_r_ + OP_l_6_c_10_r_*dt; -nextP(10,10) = OP_l_10_c_10_r_; -nextP(1,11) = OP_l_1_c_11_r_*SPP(6) - OP_l_2_c_11_r_*SPP(5) + OP_l_3_c_11_r_*SPP(8) + OP_l_10_c_11_r_*SPP(23) + OP_l_13_c_11_r_*SPP(19); -nextP(2,11) = OP_l_2_c_11_r_*SPP(7) - OP_l_1_c_11_r_*SPP(3) - OP_l_3_c_11_r_*SPP(9) + OP_l_11_c_11_r_*SPP(23) + OP_l_14_c_11_r_*SPP(18); -nextP(3,11) = OP_l_1_c_11_r_*SPP(15) - OP_l_2_c_11_r_*SPP(4) + OP_l_3_c_11_r_*SPP(14) + OP_l_12_c_11_r_*SPP(23) + OP_l_15_c_11_r_*SPP(17); -nextP(4,11) = OP_l_4_c_11_r_ + OP_l_1_c_11_r_*SPP(2) + OP_l_2_c_11_r_*SPP(20) + OP_l_3_c_11_r_*SPP(16) - OP_l_16_c_11_r_*SPP(22); -nextP(5,11) = OP_l_5_c_11_r_ + OP_l_16_c_11_r_*SF(23) + OP_l_1_c_11_r_*SPP(21) + OP_l_2_c_11_r_*SPP(13) + OP_l_3_c_11_r_*SPP(12); -nextP(6,11) = OP_l_6_c_11_r_ + OP_l_16_c_11_r_*SF(21) - OP_l_1_c_11_r_*SPP(10) + OP_l_2_c_11_r_*SPP(11) + OP_l_3_c_11_r_*SPP(1); -nextP(7,11) = OP_l_7_c_11_r_ + OP_l_4_c_11_r_*dt; +nextP(1,10) = OP_l_1_c_10_r_ + OP_l_2_c_10_r_*SF(10) + OP_l_3_c_10_r_*SF(12) + OP_l_4_c_10_r_*SF(11) + OP_l_11_c_10_r_*SF(15) + OP_l_12_c_10_r_*SF(16) + OP_l_13_c_10_r_*SPP(11) + dt*(OP_l_1_c_7_r_ + OP_l_2_c_7_r_*SF(10) + OP_l_3_c_7_r_*SF(12) + OP_l_4_c_7_r_*SF(11) + OP_l_11_c_7_r_*SF(15) + OP_l_12_c_7_r_*SF(16) + OP_l_13_c_7_r_*SPP(11)); +nextP(2,10) = OP_l_2_c_10_r_ + OP_l_1_c_10_r_*SF(9) + OP_l_3_c_10_r_*SF(8) + OP_l_4_c_10_r_*SF(12) - OP_l_13_c_10_r_*SF(16) + OP_l_12_c_10_r_*SPP(11) - (OP_l_11_c_10_r_*q0)/2 + dt*(OP_l_2_c_7_r_ + OP_l_1_c_7_r_*SF(9) + OP_l_3_c_7_r_*SF(8) + OP_l_4_c_7_r_*SF(12) - OP_l_13_c_7_r_*SF(16) + OP_l_12_c_7_r_*SPP(11) - (OP_l_11_c_7_r_*q0)/2); +nextP(3,10) = OP_l_3_c_10_r_ + OP_l_1_c_10_r_*SF(7) + OP_l_2_c_10_r_*SF(11) + OP_l_4_c_10_r_*SF(9) + OP_l_13_c_10_r_*SF(15) - OP_l_11_c_10_r_*SPP(11) - (OP_l_12_c_10_r_*q0)/2 + dt*(OP_l_3_c_7_r_ + OP_l_1_c_7_r_*SF(7) + OP_l_2_c_7_r_*SF(11) + OP_l_4_c_7_r_*SF(9) + OP_l_13_c_7_r_*SF(15) - OP_l_11_c_7_r_*SPP(11) - (OP_l_12_c_7_r_*q0)/2); +nextP(4,10) = OP_l_4_c_10_r_ + OP_l_1_c_10_r_*SF(8) + OP_l_2_c_10_r_*SF(7) + OP_l_3_c_10_r_*SF(10) + OP_l_11_c_10_r_*SF(16) - OP_l_12_c_10_r_*SF(15) - (OP_l_13_c_10_r_*q0)/2 + dt*(OP_l_4_c_7_r_ + OP_l_1_c_7_r_*SF(8) + OP_l_2_c_7_r_*SF(7) + OP_l_3_c_7_r_*SF(10) + OP_l_11_c_7_r_*SF(16) - OP_l_12_c_7_r_*SF(15) - (OP_l_13_c_7_r_*q0)/2); +nextP(5,10) = OP_l_5_c_10_r_ + OP_l_1_c_10_r_*SF(6) + OP_l_2_c_10_r_*SF(4) - OP_l_4_c_10_r_*SF(5) + OP_l_3_c_10_r_*SPP(1) + OP_l_14_c_10_r_*SPP(4) + OP_l_15_c_10_r_*SPP(7) - OP_l_16_c_10_r_*SPP(10) + dt*(OP_l_5_c_7_r_ + OP_l_1_c_7_r_*SF(6) + OP_l_2_c_7_r_*SF(4) - OP_l_4_c_7_r_*SF(5) + OP_l_3_c_7_r_*SPP(1) + OP_l_14_c_7_r_*SPP(4) + OP_l_15_c_7_r_*SPP(7) - OP_l_16_c_7_r_*SPP(10)); +nextP(6,10) = OP_l_6_c_10_r_ + OP_l_1_c_10_r_*SF(5) + OP_l_3_c_10_r_*SF(4) + OP_l_4_c_10_r_*SF(6) - OP_l_2_c_10_r_*SPP(1) - OP_l_14_c_10_r_*SPP(9) + OP_l_15_c_10_r_*SPP(3) + OP_l_16_c_10_r_*SPP(6) + dt*(OP_l_6_c_7_r_ + OP_l_1_c_7_r_*SF(5) + OP_l_3_c_7_r_*SF(4) + OP_l_4_c_7_r_*SF(6) - OP_l_2_c_7_r_*SPP(1) - OP_l_14_c_7_r_*SPP(9) + OP_l_15_c_7_r_*SPP(3) + OP_l_16_c_7_r_*SPP(6)); +nextP(7,10) = OP_l_7_c_10_r_ + OP_l_2_c_10_r_*SF(5) - OP_l_3_c_10_r_*SF(6) + OP_l_4_c_10_r_*SF(4) + OP_l_1_c_10_r_*SPP(1) + OP_l_14_c_10_r_*SPP(5) - OP_l_15_c_10_r_*SPP(8) - OP_l_16_c_10_r_*SPP(2) + dt*(OP_l_7_c_7_r_ + OP_l_2_c_7_r_*SF(5) - OP_l_3_c_7_r_*SF(6) + OP_l_4_c_7_r_*SF(4) + OP_l_1_c_7_r_*SPP(1) + OP_l_14_c_7_r_*SPP(5) - OP_l_15_c_7_r_*SPP(8) - OP_l_16_c_7_r_*SPP(2)); +nextP(8,10) = OP_l_8_c_10_r_ + OP_l_5_c_10_r_*dt + dt*(OP_l_8_c_7_r_ + OP_l_5_c_7_r_*dt); +nextP(9,10) = OP_l_9_c_10_r_ + OP_l_6_c_10_r_*dt + dt*(OP_l_9_c_7_r_ + OP_l_6_c_7_r_*dt); +nextP(10,10) = OP_l_10_c_10_r_ + OP_l_7_c_10_r_*dt + dt*(OP_l_10_c_7_r_ + OP_l_7_c_7_r_*dt); +nextP(1,11) = OP_l_1_c_11_r_ + OP_l_2_c_11_r_*SF(10) + OP_l_3_c_11_r_*SF(12) + OP_l_4_c_11_r_*SF(11) + OP_l_11_c_11_r_*SF(15) + OP_l_12_c_11_r_*SF(16) + OP_l_13_c_11_r_*SPP(11); +nextP(2,11) = OP_l_2_c_11_r_ + OP_l_1_c_11_r_*SF(9) + OP_l_3_c_11_r_*SF(8) + OP_l_4_c_11_r_*SF(12) - OP_l_13_c_11_r_*SF(16) + OP_l_12_c_11_r_*SPP(11) - (OP_l_11_c_11_r_*q0)/2; +nextP(3,11) = OP_l_3_c_11_r_ + OP_l_1_c_11_r_*SF(7) + OP_l_2_c_11_r_*SF(11) + OP_l_4_c_11_r_*SF(9) + OP_l_13_c_11_r_*SF(15) - OP_l_11_c_11_r_*SPP(11) - (OP_l_12_c_11_r_*q0)/2; +nextP(4,11) = OP_l_4_c_11_r_ + OP_l_1_c_11_r_*SF(8) + OP_l_2_c_11_r_*SF(7) + OP_l_3_c_11_r_*SF(10) + OP_l_11_c_11_r_*SF(16) - OP_l_12_c_11_r_*SF(15) - (OP_l_13_c_11_r_*q0)/2; +nextP(5,11) = OP_l_5_c_11_r_ + OP_l_1_c_11_r_*SF(6) + OP_l_2_c_11_r_*SF(4) - OP_l_4_c_11_r_*SF(5) + OP_l_3_c_11_r_*SPP(1) + OP_l_14_c_11_r_*SPP(4) + OP_l_15_c_11_r_*SPP(7) - OP_l_16_c_11_r_*SPP(10); +nextP(6,11) = OP_l_6_c_11_r_ + OP_l_1_c_11_r_*SF(5) + OP_l_3_c_11_r_*SF(4) + OP_l_4_c_11_r_*SF(6) - OP_l_2_c_11_r_*SPP(1) - OP_l_14_c_11_r_*SPP(9) + OP_l_15_c_11_r_*SPP(3) + OP_l_16_c_11_r_*SPP(6); +nextP(7,11) = OP_l_7_c_11_r_ + OP_l_2_c_11_r_*SF(5) - OP_l_3_c_11_r_*SF(6) + OP_l_4_c_11_r_*SF(4) + OP_l_1_c_11_r_*SPP(1) + OP_l_14_c_11_r_*SPP(5) - OP_l_15_c_11_r_*SPP(8) - OP_l_16_c_11_r_*SPP(2); nextP(8,11) = OP_l_8_c_11_r_ + OP_l_5_c_11_r_*dt; nextP(9,11) = OP_l_9_c_11_r_ + OP_l_6_c_11_r_*dt; -nextP(10,11) = OP_l_10_c_11_r_; +nextP(10,11) = OP_l_10_c_11_r_ + OP_l_7_c_11_r_*dt; nextP(11,11) = OP_l_11_c_11_r_; -nextP(1,12) = OP_l_1_c_12_r_*SPP(6) - OP_l_2_c_12_r_*SPP(5) + OP_l_3_c_12_r_*SPP(8) + OP_l_10_c_12_r_*SPP(23) + OP_l_13_c_12_r_*SPP(19); -nextP(2,12) = OP_l_2_c_12_r_*SPP(7) - OP_l_1_c_12_r_*SPP(3) - OP_l_3_c_12_r_*SPP(9) + OP_l_11_c_12_r_*SPP(23) + OP_l_14_c_12_r_*SPP(18); -nextP(3,12) = OP_l_1_c_12_r_*SPP(15) - OP_l_2_c_12_r_*SPP(4) + OP_l_3_c_12_r_*SPP(14) + OP_l_12_c_12_r_*SPP(23) + OP_l_15_c_12_r_*SPP(17); -nextP(4,12) = OP_l_4_c_12_r_ + OP_l_1_c_12_r_*SPP(2) + OP_l_2_c_12_r_*SPP(20) + OP_l_3_c_12_r_*SPP(16) - OP_l_16_c_12_r_*SPP(22); -nextP(5,12) = OP_l_5_c_12_r_ + OP_l_16_c_12_r_*SF(23) + OP_l_1_c_12_r_*SPP(21) + OP_l_2_c_12_r_*SPP(13) + OP_l_3_c_12_r_*SPP(12); -nextP(6,12) = OP_l_6_c_12_r_ + OP_l_16_c_12_r_*SF(21) - OP_l_1_c_12_r_*SPP(10) + OP_l_2_c_12_r_*SPP(11) + OP_l_3_c_12_r_*SPP(1); -nextP(7,12) = OP_l_7_c_12_r_ + OP_l_4_c_12_r_*dt; +nextP(1,12) = OP_l_1_c_12_r_ + OP_l_2_c_12_r_*SF(10) + OP_l_3_c_12_r_*SF(12) + OP_l_4_c_12_r_*SF(11) + OP_l_11_c_12_r_*SF(15) + OP_l_12_c_12_r_*SF(16) + OP_l_13_c_12_r_*SPP(11); +nextP(2,12) = OP_l_2_c_12_r_ + OP_l_1_c_12_r_*SF(9) + OP_l_3_c_12_r_*SF(8) + OP_l_4_c_12_r_*SF(12) - OP_l_13_c_12_r_*SF(16) + OP_l_12_c_12_r_*SPP(11) - (OP_l_11_c_12_r_*q0)/2; +nextP(3,12) = OP_l_3_c_12_r_ + OP_l_1_c_12_r_*SF(7) + OP_l_2_c_12_r_*SF(11) + OP_l_4_c_12_r_*SF(9) + OP_l_13_c_12_r_*SF(15) - OP_l_11_c_12_r_*SPP(11) - (OP_l_12_c_12_r_*q0)/2; +nextP(4,12) = OP_l_4_c_12_r_ + OP_l_1_c_12_r_*SF(8) + OP_l_2_c_12_r_*SF(7) + OP_l_3_c_12_r_*SF(10) + OP_l_11_c_12_r_*SF(16) - OP_l_12_c_12_r_*SF(15) - (OP_l_13_c_12_r_*q0)/2; +nextP(5,12) = OP_l_5_c_12_r_ + OP_l_1_c_12_r_*SF(6) + OP_l_2_c_12_r_*SF(4) - OP_l_4_c_12_r_*SF(5) + OP_l_3_c_12_r_*SPP(1) + OP_l_14_c_12_r_*SPP(4) + OP_l_15_c_12_r_*SPP(7) - OP_l_16_c_12_r_*SPP(10); +nextP(6,12) = OP_l_6_c_12_r_ + OP_l_1_c_12_r_*SF(5) + OP_l_3_c_12_r_*SF(4) + OP_l_4_c_12_r_*SF(6) - OP_l_2_c_12_r_*SPP(1) - OP_l_14_c_12_r_*SPP(9) + OP_l_15_c_12_r_*SPP(3) + OP_l_16_c_12_r_*SPP(6); +nextP(7,12) = OP_l_7_c_12_r_ + OP_l_2_c_12_r_*SF(5) - OP_l_3_c_12_r_*SF(6) + OP_l_4_c_12_r_*SF(4) + OP_l_1_c_12_r_*SPP(1) + OP_l_14_c_12_r_*SPP(5) - OP_l_15_c_12_r_*SPP(8) - OP_l_16_c_12_r_*SPP(2); nextP(8,12) = OP_l_8_c_12_r_ + OP_l_5_c_12_r_*dt; nextP(9,12) = OP_l_9_c_12_r_ + OP_l_6_c_12_r_*dt; -nextP(10,12) = OP_l_10_c_12_r_; +nextP(10,12) = OP_l_10_c_12_r_ + OP_l_7_c_12_r_*dt; nextP(11,12) = OP_l_11_c_12_r_; nextP(12,12) = OP_l_12_c_12_r_; -nextP(1,13) = OP_l_1_c_13_r_*SPP(6) - OP_l_2_c_13_r_*SPP(5) + OP_l_3_c_13_r_*SPP(8) + OP_l_10_c_13_r_*SPP(23) + OP_l_13_c_13_r_*SPP(19); -nextP(2,13) = OP_l_2_c_13_r_*SPP(7) - OP_l_1_c_13_r_*SPP(3) - OP_l_3_c_13_r_*SPP(9) + OP_l_11_c_13_r_*SPP(23) + OP_l_14_c_13_r_*SPP(18); -nextP(3,13) = OP_l_1_c_13_r_*SPP(15) - OP_l_2_c_13_r_*SPP(4) + OP_l_3_c_13_r_*SPP(14) + OP_l_12_c_13_r_*SPP(23) + OP_l_15_c_13_r_*SPP(17); -nextP(4,13) = OP_l_4_c_13_r_ + OP_l_1_c_13_r_*SPP(2) + OP_l_2_c_13_r_*SPP(20) + OP_l_3_c_13_r_*SPP(16) - OP_l_16_c_13_r_*SPP(22); -nextP(5,13) = OP_l_5_c_13_r_ + OP_l_16_c_13_r_*SF(23) + OP_l_1_c_13_r_*SPP(21) + OP_l_2_c_13_r_*SPP(13) + OP_l_3_c_13_r_*SPP(12); -nextP(6,13) = OP_l_6_c_13_r_ + OP_l_16_c_13_r_*SF(21) - OP_l_1_c_13_r_*SPP(10) + OP_l_2_c_13_r_*SPP(11) + OP_l_3_c_13_r_*SPP(1); -nextP(7,13) = OP_l_7_c_13_r_ + OP_l_4_c_13_r_*dt; +nextP(1,13) = OP_l_1_c_13_r_ + OP_l_2_c_13_r_*SF(10) + OP_l_3_c_13_r_*SF(12) + OP_l_4_c_13_r_*SF(11) + OP_l_11_c_13_r_*SF(15) + OP_l_12_c_13_r_*SF(16) + OP_l_13_c_13_r_*SPP(11); +nextP(2,13) = OP_l_2_c_13_r_ + OP_l_1_c_13_r_*SF(9) + OP_l_3_c_13_r_*SF(8) + OP_l_4_c_13_r_*SF(12) - OP_l_13_c_13_r_*SF(16) + OP_l_12_c_13_r_*SPP(11) - (OP_l_11_c_13_r_*q0)/2; +nextP(3,13) = OP_l_3_c_13_r_ + OP_l_1_c_13_r_*SF(7) + OP_l_2_c_13_r_*SF(11) + OP_l_4_c_13_r_*SF(9) + OP_l_13_c_13_r_*SF(15) - OP_l_11_c_13_r_*SPP(11) - (OP_l_12_c_13_r_*q0)/2; +nextP(4,13) = OP_l_4_c_13_r_ + OP_l_1_c_13_r_*SF(8) + OP_l_2_c_13_r_*SF(7) + OP_l_3_c_13_r_*SF(10) + OP_l_11_c_13_r_*SF(16) - OP_l_12_c_13_r_*SF(15) - (OP_l_13_c_13_r_*q0)/2; +nextP(5,13) = OP_l_5_c_13_r_ + OP_l_1_c_13_r_*SF(6) + OP_l_2_c_13_r_*SF(4) - OP_l_4_c_13_r_*SF(5) + OP_l_3_c_13_r_*SPP(1) + OP_l_14_c_13_r_*SPP(4) + OP_l_15_c_13_r_*SPP(7) - OP_l_16_c_13_r_*SPP(10); +nextP(6,13) = OP_l_6_c_13_r_ + OP_l_1_c_13_r_*SF(5) + OP_l_3_c_13_r_*SF(4) + OP_l_4_c_13_r_*SF(6) - OP_l_2_c_13_r_*SPP(1) - OP_l_14_c_13_r_*SPP(9) + OP_l_15_c_13_r_*SPP(3) + OP_l_16_c_13_r_*SPP(6); +nextP(7,13) = OP_l_7_c_13_r_ + OP_l_2_c_13_r_*SF(5) - OP_l_3_c_13_r_*SF(6) + OP_l_4_c_13_r_*SF(4) + OP_l_1_c_13_r_*SPP(1) + OP_l_14_c_13_r_*SPP(5) - OP_l_15_c_13_r_*SPP(8) - OP_l_16_c_13_r_*SPP(2); nextP(8,13) = OP_l_8_c_13_r_ + OP_l_5_c_13_r_*dt; nextP(9,13) = OP_l_9_c_13_r_ + OP_l_6_c_13_r_*dt; -nextP(10,13) = OP_l_10_c_13_r_; +nextP(10,13) = OP_l_10_c_13_r_ + OP_l_7_c_13_r_*dt; nextP(11,13) = OP_l_11_c_13_r_; nextP(12,13) = OP_l_12_c_13_r_; nextP(13,13) = OP_l_13_c_13_r_; -nextP(1,14) = OP_l_1_c_14_r_*SPP(6) - OP_l_2_c_14_r_*SPP(5) + OP_l_3_c_14_r_*SPP(8) + OP_l_10_c_14_r_*SPP(23) + OP_l_13_c_14_r_*SPP(19); -nextP(2,14) = OP_l_2_c_14_r_*SPP(7) - OP_l_1_c_14_r_*SPP(3) - OP_l_3_c_14_r_*SPP(9) + OP_l_11_c_14_r_*SPP(23) + OP_l_14_c_14_r_*SPP(18); -nextP(3,14) = OP_l_1_c_14_r_*SPP(15) - OP_l_2_c_14_r_*SPP(4) + OP_l_3_c_14_r_*SPP(14) + OP_l_12_c_14_r_*SPP(23) + OP_l_15_c_14_r_*SPP(17); -nextP(4,14) = OP_l_4_c_14_r_ + OP_l_1_c_14_r_*SPP(2) + OP_l_2_c_14_r_*SPP(20) + OP_l_3_c_14_r_*SPP(16) - OP_l_16_c_14_r_*SPP(22); -nextP(5,14) = OP_l_5_c_14_r_ + OP_l_16_c_14_r_*SF(23) + OP_l_1_c_14_r_*SPP(21) + OP_l_2_c_14_r_*SPP(13) + OP_l_3_c_14_r_*SPP(12); -nextP(6,14) = OP_l_6_c_14_r_ + OP_l_16_c_14_r_*SF(21) - OP_l_1_c_14_r_*SPP(10) + OP_l_2_c_14_r_*SPP(11) + OP_l_3_c_14_r_*SPP(1); -nextP(7,14) = OP_l_7_c_14_r_ + OP_l_4_c_14_r_*dt; +nextP(1,14) = OP_l_1_c_14_r_ + OP_l_2_c_14_r_*SF(10) + OP_l_3_c_14_r_*SF(12) + OP_l_4_c_14_r_*SF(11) + OP_l_11_c_14_r_*SF(15) + OP_l_12_c_14_r_*SF(16) + OP_l_13_c_14_r_*SPP(11); +nextP(2,14) = OP_l_2_c_14_r_ + OP_l_1_c_14_r_*SF(9) + OP_l_3_c_14_r_*SF(8) + OP_l_4_c_14_r_*SF(12) - OP_l_13_c_14_r_*SF(16) + OP_l_12_c_14_r_*SPP(11) - (OP_l_11_c_14_r_*q0)/2; +nextP(3,14) = OP_l_3_c_14_r_ + OP_l_1_c_14_r_*SF(7) + OP_l_2_c_14_r_*SF(11) + OP_l_4_c_14_r_*SF(9) + OP_l_13_c_14_r_*SF(15) - OP_l_11_c_14_r_*SPP(11) - (OP_l_12_c_14_r_*q0)/2; +nextP(4,14) = OP_l_4_c_14_r_ + OP_l_1_c_14_r_*SF(8) + OP_l_2_c_14_r_*SF(7) + OP_l_3_c_14_r_*SF(10) + OP_l_11_c_14_r_*SF(16) - OP_l_12_c_14_r_*SF(15) - (OP_l_13_c_14_r_*q0)/2; +nextP(5,14) = OP_l_5_c_14_r_ + OP_l_1_c_14_r_*SF(6) + OP_l_2_c_14_r_*SF(4) - OP_l_4_c_14_r_*SF(5) + OP_l_3_c_14_r_*SPP(1) + OP_l_14_c_14_r_*SPP(4) + OP_l_15_c_14_r_*SPP(7) - OP_l_16_c_14_r_*SPP(10); +nextP(6,14) = OP_l_6_c_14_r_ + OP_l_1_c_14_r_*SF(5) + OP_l_3_c_14_r_*SF(4) + OP_l_4_c_14_r_*SF(6) - OP_l_2_c_14_r_*SPP(1) - OP_l_14_c_14_r_*SPP(9) + OP_l_15_c_14_r_*SPP(3) + OP_l_16_c_14_r_*SPP(6); +nextP(7,14) = OP_l_7_c_14_r_ + OP_l_2_c_14_r_*SF(5) - OP_l_3_c_14_r_*SF(6) + OP_l_4_c_14_r_*SF(4) + OP_l_1_c_14_r_*SPP(1) + OP_l_14_c_14_r_*SPP(5) - OP_l_15_c_14_r_*SPP(8) - OP_l_16_c_14_r_*SPP(2); nextP(8,14) = OP_l_8_c_14_r_ + OP_l_5_c_14_r_*dt; nextP(9,14) = OP_l_9_c_14_r_ + OP_l_6_c_14_r_*dt; -nextP(10,14) = OP_l_10_c_14_r_; +nextP(10,14) = OP_l_10_c_14_r_ + OP_l_7_c_14_r_*dt; nextP(11,14) = OP_l_11_c_14_r_; nextP(12,14) = OP_l_12_c_14_r_; nextP(13,14) = OP_l_13_c_14_r_; nextP(14,14) = OP_l_14_c_14_r_; -nextP(1,15) = OP_l_1_c_15_r_*SPP(6) - OP_l_2_c_15_r_*SPP(5) + OP_l_3_c_15_r_*SPP(8) + OP_l_10_c_15_r_*SPP(23) + OP_l_13_c_15_r_*SPP(19); -nextP(2,15) = OP_l_2_c_15_r_*SPP(7) - OP_l_1_c_15_r_*SPP(3) - OP_l_3_c_15_r_*SPP(9) + OP_l_11_c_15_r_*SPP(23) + OP_l_14_c_15_r_*SPP(18); -nextP(3,15) = OP_l_1_c_15_r_*SPP(15) - OP_l_2_c_15_r_*SPP(4) + OP_l_3_c_15_r_*SPP(14) + OP_l_12_c_15_r_*SPP(23) + OP_l_15_c_15_r_*SPP(17); -nextP(4,15) = OP_l_4_c_15_r_ + OP_l_1_c_15_r_*SPP(2) + OP_l_2_c_15_r_*SPP(20) + OP_l_3_c_15_r_*SPP(16) - OP_l_16_c_15_r_*SPP(22); -nextP(5,15) = OP_l_5_c_15_r_ + OP_l_16_c_15_r_*SF(23) + OP_l_1_c_15_r_*SPP(21) + OP_l_2_c_15_r_*SPP(13) + OP_l_3_c_15_r_*SPP(12); -nextP(6,15) = OP_l_6_c_15_r_ + OP_l_16_c_15_r_*SF(21) - OP_l_1_c_15_r_*SPP(10) + OP_l_2_c_15_r_*SPP(11) + OP_l_3_c_15_r_*SPP(1); -nextP(7,15) = OP_l_7_c_15_r_ + OP_l_4_c_15_r_*dt; +nextP(1,15) = OP_l_1_c_15_r_ + OP_l_2_c_15_r_*SF(10) + OP_l_3_c_15_r_*SF(12) + OP_l_4_c_15_r_*SF(11) + OP_l_11_c_15_r_*SF(15) + OP_l_12_c_15_r_*SF(16) + OP_l_13_c_15_r_*SPP(11); +nextP(2,15) = OP_l_2_c_15_r_ + OP_l_1_c_15_r_*SF(9) + OP_l_3_c_15_r_*SF(8) + OP_l_4_c_15_r_*SF(12) - OP_l_13_c_15_r_*SF(16) + OP_l_12_c_15_r_*SPP(11) - (OP_l_11_c_15_r_*q0)/2; +nextP(3,15) = OP_l_3_c_15_r_ + OP_l_1_c_15_r_*SF(7) + OP_l_2_c_15_r_*SF(11) + OP_l_4_c_15_r_*SF(9) + OP_l_13_c_15_r_*SF(15) - OP_l_11_c_15_r_*SPP(11) - (OP_l_12_c_15_r_*q0)/2; +nextP(4,15) = OP_l_4_c_15_r_ + OP_l_1_c_15_r_*SF(8) + OP_l_2_c_15_r_*SF(7) + OP_l_3_c_15_r_*SF(10) + OP_l_11_c_15_r_*SF(16) - OP_l_12_c_15_r_*SF(15) - (OP_l_13_c_15_r_*q0)/2; +nextP(5,15) = OP_l_5_c_15_r_ + OP_l_1_c_15_r_*SF(6) + OP_l_2_c_15_r_*SF(4) - OP_l_4_c_15_r_*SF(5) + OP_l_3_c_15_r_*SPP(1) + OP_l_14_c_15_r_*SPP(4) + OP_l_15_c_15_r_*SPP(7) - OP_l_16_c_15_r_*SPP(10); +nextP(6,15) = OP_l_6_c_15_r_ + OP_l_1_c_15_r_*SF(5) + OP_l_3_c_15_r_*SF(4) + OP_l_4_c_15_r_*SF(6) - OP_l_2_c_15_r_*SPP(1) - OP_l_14_c_15_r_*SPP(9) + OP_l_15_c_15_r_*SPP(3) + OP_l_16_c_15_r_*SPP(6); +nextP(7,15) = OP_l_7_c_15_r_ + OP_l_2_c_15_r_*SF(5) - OP_l_3_c_15_r_*SF(6) + OP_l_4_c_15_r_*SF(4) + OP_l_1_c_15_r_*SPP(1) + OP_l_14_c_15_r_*SPP(5) - OP_l_15_c_15_r_*SPP(8) - OP_l_16_c_15_r_*SPP(2); nextP(8,15) = OP_l_8_c_15_r_ + OP_l_5_c_15_r_*dt; nextP(9,15) = OP_l_9_c_15_r_ + OP_l_6_c_15_r_*dt; -nextP(10,15) = OP_l_10_c_15_r_; +nextP(10,15) = OP_l_10_c_15_r_ + OP_l_7_c_15_r_*dt; nextP(11,15) = OP_l_11_c_15_r_; nextP(12,15) = OP_l_12_c_15_r_; nextP(13,15) = OP_l_13_c_15_r_; nextP(14,15) = OP_l_14_c_15_r_; nextP(15,15) = OP_l_15_c_15_r_; -nextP(1,16) = OP_l_1_c_16_r_*SPP(6) - OP_l_2_c_16_r_*SPP(5) + OP_l_3_c_16_r_*SPP(8) + OP_l_10_c_16_r_*SPP(23) + OP_l_13_c_16_r_*SPP(19); -nextP(2,16) = OP_l_2_c_16_r_*SPP(7) - OP_l_1_c_16_r_*SPP(3) - OP_l_3_c_16_r_*SPP(9) + OP_l_11_c_16_r_*SPP(23) + OP_l_14_c_16_r_*SPP(18); -nextP(3,16) = OP_l_1_c_16_r_*SPP(15) - OP_l_2_c_16_r_*SPP(4) + OP_l_3_c_16_r_*SPP(14) + OP_l_12_c_16_r_*SPP(23) + OP_l_15_c_16_r_*SPP(17); -nextP(4,16) = OP_l_4_c_16_r_ + OP_l_1_c_16_r_*SPP(2) + OP_l_2_c_16_r_*SPP(20) + OP_l_3_c_16_r_*SPP(16) - OP_l_16_c_16_r_*SPP(22); -nextP(5,16) = OP_l_5_c_16_r_ + OP_l_16_c_16_r_*SF(23) + OP_l_1_c_16_r_*SPP(21) + OP_l_2_c_16_r_*SPP(13) + OP_l_3_c_16_r_*SPP(12); -nextP(6,16) = OP_l_6_c_16_r_ + OP_l_16_c_16_r_*SF(21) - OP_l_1_c_16_r_*SPP(10) + OP_l_2_c_16_r_*SPP(11) + OP_l_3_c_16_r_*SPP(1); -nextP(7,16) = OP_l_7_c_16_r_ + OP_l_4_c_16_r_*dt; +nextP(1,16) = OP_l_1_c_16_r_ + OP_l_2_c_16_r_*SF(10) + OP_l_3_c_16_r_*SF(12) + OP_l_4_c_16_r_*SF(11) + OP_l_11_c_16_r_*SF(15) + OP_l_12_c_16_r_*SF(16) + OP_l_13_c_16_r_*SPP(11); +nextP(2,16) = OP_l_2_c_16_r_ + OP_l_1_c_16_r_*SF(9) + OP_l_3_c_16_r_*SF(8) + OP_l_4_c_16_r_*SF(12) - OP_l_13_c_16_r_*SF(16) + OP_l_12_c_16_r_*SPP(11) - (OP_l_11_c_16_r_*q0)/2; +nextP(3,16) = OP_l_3_c_16_r_ + OP_l_1_c_16_r_*SF(7) + OP_l_2_c_16_r_*SF(11) + OP_l_4_c_16_r_*SF(9) + OP_l_13_c_16_r_*SF(15) - OP_l_11_c_16_r_*SPP(11) - (OP_l_12_c_16_r_*q0)/2; +nextP(4,16) = OP_l_4_c_16_r_ + OP_l_1_c_16_r_*SF(8) + OP_l_2_c_16_r_*SF(7) + OP_l_3_c_16_r_*SF(10) + OP_l_11_c_16_r_*SF(16) - OP_l_12_c_16_r_*SF(15) - (OP_l_13_c_16_r_*q0)/2; +nextP(5,16) = OP_l_5_c_16_r_ + OP_l_1_c_16_r_*SF(6) + OP_l_2_c_16_r_*SF(4) - OP_l_4_c_16_r_*SF(5) + OP_l_3_c_16_r_*SPP(1) + OP_l_14_c_16_r_*SPP(4) + OP_l_15_c_16_r_*SPP(7) - OP_l_16_c_16_r_*SPP(10); +nextP(6,16) = OP_l_6_c_16_r_ + OP_l_1_c_16_r_*SF(5) + OP_l_3_c_16_r_*SF(4) + OP_l_4_c_16_r_*SF(6) - OP_l_2_c_16_r_*SPP(1) - OP_l_14_c_16_r_*SPP(9) + OP_l_15_c_16_r_*SPP(3) + OP_l_16_c_16_r_*SPP(6); +nextP(7,16) = OP_l_7_c_16_r_ + OP_l_2_c_16_r_*SF(5) - OP_l_3_c_16_r_*SF(6) + OP_l_4_c_16_r_*SF(4) + OP_l_1_c_16_r_*SPP(1) + OP_l_14_c_16_r_*SPP(5) - OP_l_15_c_16_r_*SPP(8) - OP_l_16_c_16_r_*SPP(2); nextP(8,16) = OP_l_8_c_16_r_ + OP_l_5_c_16_r_*dt; nextP(9,16) = OP_l_9_c_16_r_ + OP_l_6_c_16_r_*dt; -nextP(10,16) = OP_l_10_c_16_r_; +nextP(10,16) = OP_l_10_c_16_r_ + OP_l_7_c_16_r_*dt; nextP(11,16) = OP_l_11_c_16_r_; nextP(12,16) = OP_l_12_c_16_r_; nextP(13,16) = OP_l_13_c_16_r_; nextP(14,16) = OP_l_14_c_16_r_; nextP(15,16) = OP_l_15_c_16_r_; nextP(16,16) = OP_l_16_c_16_r_; -nextP(1,17) = OP_l_1_c_17_r_*SPP(6) - OP_l_2_c_17_r_*SPP(5) + OP_l_3_c_17_r_*SPP(8) + OP_l_10_c_17_r_*SPP(23) + OP_l_13_c_17_r_*SPP(19); -nextP(2,17) = OP_l_2_c_17_r_*SPP(7) - OP_l_1_c_17_r_*SPP(3) - OP_l_3_c_17_r_*SPP(9) + OP_l_11_c_17_r_*SPP(23) + OP_l_14_c_17_r_*SPP(18); -nextP(3,17) = OP_l_1_c_17_r_*SPP(15) - OP_l_2_c_17_r_*SPP(4) + OP_l_3_c_17_r_*SPP(14) + OP_l_12_c_17_r_*SPP(23) + OP_l_15_c_17_r_*SPP(17); -nextP(4,17) = OP_l_4_c_17_r_ + OP_l_1_c_17_r_*SPP(2) + OP_l_2_c_17_r_*SPP(20) + OP_l_3_c_17_r_*SPP(16) - OP_l_16_c_17_r_*SPP(22); -nextP(5,17) = OP_l_5_c_17_r_ + OP_l_16_c_17_r_*SF(23) + OP_l_1_c_17_r_*SPP(21) + OP_l_2_c_17_r_*SPP(13) + OP_l_3_c_17_r_*SPP(12); -nextP(6,17) = OP_l_6_c_17_r_ + OP_l_16_c_17_r_*SF(21) - OP_l_1_c_17_r_*SPP(10) + OP_l_2_c_17_r_*SPP(11) + OP_l_3_c_17_r_*SPP(1); -nextP(7,17) = OP_l_7_c_17_r_ + OP_l_4_c_17_r_*dt; +nextP(1,17) = OP_l_1_c_17_r_ + OP_l_2_c_17_r_*SF(10) + OP_l_3_c_17_r_*SF(12) + OP_l_4_c_17_r_*SF(11) + OP_l_11_c_17_r_*SF(15) + OP_l_12_c_17_r_*SF(16) + OP_l_13_c_17_r_*SPP(11); +nextP(2,17) = OP_l_2_c_17_r_ + OP_l_1_c_17_r_*SF(9) + OP_l_3_c_17_r_*SF(8) + OP_l_4_c_17_r_*SF(12) - OP_l_13_c_17_r_*SF(16) + OP_l_12_c_17_r_*SPP(11) - (OP_l_11_c_17_r_*q0)/2; +nextP(3,17) = OP_l_3_c_17_r_ + OP_l_1_c_17_r_*SF(7) + OP_l_2_c_17_r_*SF(11) + OP_l_4_c_17_r_*SF(9) + OP_l_13_c_17_r_*SF(15) - OP_l_11_c_17_r_*SPP(11) - (OP_l_12_c_17_r_*q0)/2; +nextP(4,17) = OP_l_4_c_17_r_ + OP_l_1_c_17_r_*SF(8) + OP_l_2_c_17_r_*SF(7) + OP_l_3_c_17_r_*SF(10) + OP_l_11_c_17_r_*SF(16) - OP_l_12_c_17_r_*SF(15) - (OP_l_13_c_17_r_*q0)/2; +nextP(5,17) = OP_l_5_c_17_r_ + OP_l_1_c_17_r_*SF(6) + OP_l_2_c_17_r_*SF(4) - OP_l_4_c_17_r_*SF(5) + OP_l_3_c_17_r_*SPP(1) + OP_l_14_c_17_r_*SPP(4) + OP_l_15_c_17_r_*SPP(7) - OP_l_16_c_17_r_*SPP(10); +nextP(6,17) = OP_l_6_c_17_r_ + OP_l_1_c_17_r_*SF(5) + OP_l_3_c_17_r_*SF(4) + OP_l_4_c_17_r_*SF(6) - OP_l_2_c_17_r_*SPP(1) - OP_l_14_c_17_r_*SPP(9) + OP_l_15_c_17_r_*SPP(3) + OP_l_16_c_17_r_*SPP(6); +nextP(7,17) = OP_l_7_c_17_r_ + OP_l_2_c_17_r_*SF(5) - OP_l_3_c_17_r_*SF(6) + OP_l_4_c_17_r_*SF(4) + OP_l_1_c_17_r_*SPP(1) + OP_l_14_c_17_r_*SPP(5) - OP_l_15_c_17_r_*SPP(8) - OP_l_16_c_17_r_*SPP(2); nextP(8,17) = OP_l_8_c_17_r_ + OP_l_5_c_17_r_*dt; nextP(9,17) = OP_l_9_c_17_r_ + OP_l_6_c_17_r_*dt; -nextP(10,17) = OP_l_10_c_17_r_; +nextP(10,17) = OP_l_10_c_17_r_ + OP_l_7_c_17_r_*dt; nextP(11,17) = OP_l_11_c_17_r_; nextP(12,17) = OP_l_12_c_17_r_; nextP(13,17) = OP_l_13_c_17_r_; @@ -226,16 +214,16 @@ nextP(14,17) = OP_l_14_c_17_r_; nextP(15,17) = OP_l_15_c_17_r_; nextP(16,17) = OP_l_16_c_17_r_; nextP(17,17) = OP_l_17_c_17_r_; -nextP(1,18) = OP_l_1_c_18_r_*SPP(6) - OP_l_2_c_18_r_*SPP(5) + OP_l_3_c_18_r_*SPP(8) + OP_l_10_c_18_r_*SPP(23) + OP_l_13_c_18_r_*SPP(19); -nextP(2,18) = OP_l_2_c_18_r_*SPP(7) - OP_l_1_c_18_r_*SPP(3) - OP_l_3_c_18_r_*SPP(9) + OP_l_11_c_18_r_*SPP(23) + OP_l_14_c_18_r_*SPP(18); -nextP(3,18) = OP_l_1_c_18_r_*SPP(15) - OP_l_2_c_18_r_*SPP(4) + OP_l_3_c_18_r_*SPP(14) + OP_l_12_c_18_r_*SPP(23) + OP_l_15_c_18_r_*SPP(17); -nextP(4,18) = OP_l_4_c_18_r_ + OP_l_1_c_18_r_*SPP(2) + OP_l_2_c_18_r_*SPP(20) + OP_l_3_c_18_r_*SPP(16) - OP_l_16_c_18_r_*SPP(22); -nextP(5,18) = OP_l_5_c_18_r_ + OP_l_16_c_18_r_*SF(23) + OP_l_1_c_18_r_*SPP(21) + OP_l_2_c_18_r_*SPP(13) + OP_l_3_c_18_r_*SPP(12); -nextP(6,18) = OP_l_6_c_18_r_ + OP_l_16_c_18_r_*SF(21) - OP_l_1_c_18_r_*SPP(10) + OP_l_2_c_18_r_*SPP(11) + OP_l_3_c_18_r_*SPP(1); -nextP(7,18) = OP_l_7_c_18_r_ + OP_l_4_c_18_r_*dt; +nextP(1,18) = OP_l_1_c_18_r_ + OP_l_2_c_18_r_*SF(10) + OP_l_3_c_18_r_*SF(12) + OP_l_4_c_18_r_*SF(11) + OP_l_11_c_18_r_*SF(15) + OP_l_12_c_18_r_*SF(16) + OP_l_13_c_18_r_*SPP(11); +nextP(2,18) = OP_l_2_c_18_r_ + OP_l_1_c_18_r_*SF(9) + OP_l_3_c_18_r_*SF(8) + OP_l_4_c_18_r_*SF(12) - OP_l_13_c_18_r_*SF(16) + OP_l_12_c_18_r_*SPP(11) - (OP_l_11_c_18_r_*q0)/2; +nextP(3,18) = OP_l_3_c_18_r_ + OP_l_1_c_18_r_*SF(7) + OP_l_2_c_18_r_*SF(11) + OP_l_4_c_18_r_*SF(9) + OP_l_13_c_18_r_*SF(15) - OP_l_11_c_18_r_*SPP(11) - (OP_l_12_c_18_r_*q0)/2; +nextP(4,18) = OP_l_4_c_18_r_ + OP_l_1_c_18_r_*SF(8) + OP_l_2_c_18_r_*SF(7) + OP_l_3_c_18_r_*SF(10) + OP_l_11_c_18_r_*SF(16) - OP_l_12_c_18_r_*SF(15) - (OP_l_13_c_18_r_*q0)/2; +nextP(5,18) = OP_l_5_c_18_r_ + OP_l_1_c_18_r_*SF(6) + OP_l_2_c_18_r_*SF(4) - OP_l_4_c_18_r_*SF(5) + OP_l_3_c_18_r_*SPP(1) + OP_l_14_c_18_r_*SPP(4) + OP_l_15_c_18_r_*SPP(7) - OP_l_16_c_18_r_*SPP(10); +nextP(6,18) = OP_l_6_c_18_r_ + OP_l_1_c_18_r_*SF(5) + OP_l_3_c_18_r_*SF(4) + OP_l_4_c_18_r_*SF(6) - OP_l_2_c_18_r_*SPP(1) - OP_l_14_c_18_r_*SPP(9) + OP_l_15_c_18_r_*SPP(3) + OP_l_16_c_18_r_*SPP(6); +nextP(7,18) = OP_l_7_c_18_r_ + OP_l_2_c_18_r_*SF(5) - OP_l_3_c_18_r_*SF(6) + OP_l_4_c_18_r_*SF(4) + OP_l_1_c_18_r_*SPP(1) + OP_l_14_c_18_r_*SPP(5) - OP_l_15_c_18_r_*SPP(8) - OP_l_16_c_18_r_*SPP(2); nextP(8,18) = OP_l_8_c_18_r_ + OP_l_5_c_18_r_*dt; nextP(9,18) = OP_l_9_c_18_r_ + OP_l_6_c_18_r_*dt; -nextP(10,18) = OP_l_10_c_18_r_; +nextP(10,18) = OP_l_10_c_18_r_ + OP_l_7_c_18_r_*dt; nextP(11,18) = OP_l_11_c_18_r_; nextP(12,18) = OP_l_12_c_18_r_; nextP(13,18) = OP_l_13_c_18_r_; @@ -244,16 +232,16 @@ nextP(15,18) = OP_l_15_c_18_r_; nextP(16,18) = OP_l_16_c_18_r_; nextP(17,18) = OP_l_17_c_18_r_; nextP(18,18) = OP_l_18_c_18_r_; -nextP(1,19) = OP_l_1_c_19_r_*SPP(6) - OP_l_2_c_19_r_*SPP(5) + OP_l_3_c_19_r_*SPP(8) + OP_l_10_c_19_r_*SPP(23) + OP_l_13_c_19_r_*SPP(19); -nextP(2,19) = OP_l_2_c_19_r_*SPP(7) - OP_l_1_c_19_r_*SPP(3) - OP_l_3_c_19_r_*SPP(9) + OP_l_11_c_19_r_*SPP(23) + OP_l_14_c_19_r_*SPP(18); -nextP(3,19) = OP_l_1_c_19_r_*SPP(15) - OP_l_2_c_19_r_*SPP(4) + OP_l_3_c_19_r_*SPP(14) + OP_l_12_c_19_r_*SPP(23) + OP_l_15_c_19_r_*SPP(17); -nextP(4,19) = OP_l_4_c_19_r_ + OP_l_1_c_19_r_*SPP(2) + OP_l_2_c_19_r_*SPP(20) + OP_l_3_c_19_r_*SPP(16) - OP_l_16_c_19_r_*SPP(22); -nextP(5,19) = OP_l_5_c_19_r_ + OP_l_16_c_19_r_*SF(23) + OP_l_1_c_19_r_*SPP(21) + OP_l_2_c_19_r_*SPP(13) + OP_l_3_c_19_r_*SPP(12); -nextP(6,19) = OP_l_6_c_19_r_ + OP_l_16_c_19_r_*SF(21) - OP_l_1_c_19_r_*SPP(10) + OP_l_2_c_19_r_*SPP(11) + OP_l_3_c_19_r_*SPP(1); -nextP(7,19) = OP_l_7_c_19_r_ + OP_l_4_c_19_r_*dt; +nextP(1,19) = OP_l_1_c_19_r_ + OP_l_2_c_19_r_*SF(10) + OP_l_3_c_19_r_*SF(12) + OP_l_4_c_19_r_*SF(11) + OP_l_11_c_19_r_*SF(15) + OP_l_12_c_19_r_*SF(16) + OP_l_13_c_19_r_*SPP(11); +nextP(2,19) = OP_l_2_c_19_r_ + OP_l_1_c_19_r_*SF(9) + OP_l_3_c_19_r_*SF(8) + OP_l_4_c_19_r_*SF(12) - OP_l_13_c_19_r_*SF(16) + OP_l_12_c_19_r_*SPP(11) - (OP_l_11_c_19_r_*q0)/2; +nextP(3,19) = OP_l_3_c_19_r_ + OP_l_1_c_19_r_*SF(7) + OP_l_2_c_19_r_*SF(11) + OP_l_4_c_19_r_*SF(9) + OP_l_13_c_19_r_*SF(15) - OP_l_11_c_19_r_*SPP(11) - (OP_l_12_c_19_r_*q0)/2; +nextP(4,19) = OP_l_4_c_19_r_ + OP_l_1_c_19_r_*SF(8) + OP_l_2_c_19_r_*SF(7) + OP_l_3_c_19_r_*SF(10) + OP_l_11_c_19_r_*SF(16) - OP_l_12_c_19_r_*SF(15) - (OP_l_13_c_19_r_*q0)/2; +nextP(5,19) = OP_l_5_c_19_r_ + OP_l_1_c_19_r_*SF(6) + OP_l_2_c_19_r_*SF(4) - OP_l_4_c_19_r_*SF(5) + OP_l_3_c_19_r_*SPP(1) + OP_l_14_c_19_r_*SPP(4) + OP_l_15_c_19_r_*SPP(7) - OP_l_16_c_19_r_*SPP(10); +nextP(6,19) = OP_l_6_c_19_r_ + OP_l_1_c_19_r_*SF(5) + OP_l_3_c_19_r_*SF(4) + OP_l_4_c_19_r_*SF(6) - OP_l_2_c_19_r_*SPP(1) - OP_l_14_c_19_r_*SPP(9) + OP_l_15_c_19_r_*SPP(3) + OP_l_16_c_19_r_*SPP(6); +nextP(7,19) = OP_l_7_c_19_r_ + OP_l_2_c_19_r_*SF(5) - OP_l_3_c_19_r_*SF(6) + OP_l_4_c_19_r_*SF(4) + OP_l_1_c_19_r_*SPP(1) + OP_l_14_c_19_r_*SPP(5) - OP_l_15_c_19_r_*SPP(8) - OP_l_16_c_19_r_*SPP(2); nextP(8,19) = OP_l_8_c_19_r_ + OP_l_5_c_19_r_*dt; nextP(9,19) = OP_l_9_c_19_r_ + OP_l_6_c_19_r_*dt; -nextP(10,19) = OP_l_10_c_19_r_; +nextP(10,19) = OP_l_10_c_19_r_ + OP_l_7_c_19_r_*dt; nextP(11,19) = OP_l_11_c_19_r_; nextP(12,19) = OP_l_12_c_19_r_; nextP(13,19) = OP_l_13_c_19_r_; @@ -263,16 +251,16 @@ nextP(16,19) = OP_l_16_c_19_r_; nextP(17,19) = OP_l_17_c_19_r_; nextP(18,19) = OP_l_18_c_19_r_; nextP(19,19) = OP_l_19_c_19_r_; -nextP(1,20) = OP_l_1_c_20_r_*SPP(6) - OP_l_2_c_20_r_*SPP(5) + OP_l_3_c_20_r_*SPP(8) + OP_l_10_c_20_r_*SPP(23) + OP_l_13_c_20_r_*SPP(19); -nextP(2,20) = OP_l_2_c_20_r_*SPP(7) - OP_l_1_c_20_r_*SPP(3) - OP_l_3_c_20_r_*SPP(9) + OP_l_11_c_20_r_*SPP(23) + OP_l_14_c_20_r_*SPP(18); -nextP(3,20) = OP_l_1_c_20_r_*SPP(15) - OP_l_2_c_20_r_*SPP(4) + OP_l_3_c_20_r_*SPP(14) + OP_l_12_c_20_r_*SPP(23) + OP_l_15_c_20_r_*SPP(17); -nextP(4,20) = OP_l_4_c_20_r_ + OP_l_1_c_20_r_*SPP(2) + OP_l_2_c_20_r_*SPP(20) + OP_l_3_c_20_r_*SPP(16) - OP_l_16_c_20_r_*SPP(22); -nextP(5,20) = OP_l_5_c_20_r_ + OP_l_16_c_20_r_*SF(23) + OP_l_1_c_20_r_*SPP(21) + OP_l_2_c_20_r_*SPP(13) + OP_l_3_c_20_r_*SPP(12); -nextP(6,20) = OP_l_6_c_20_r_ + OP_l_16_c_20_r_*SF(21) - OP_l_1_c_20_r_*SPP(10) + OP_l_2_c_20_r_*SPP(11) + OP_l_3_c_20_r_*SPP(1); -nextP(7,20) = OP_l_7_c_20_r_ + OP_l_4_c_20_r_*dt; +nextP(1,20) = OP_l_1_c_20_r_ + OP_l_2_c_20_r_*SF(10) + OP_l_3_c_20_r_*SF(12) + OP_l_4_c_20_r_*SF(11) + OP_l_11_c_20_r_*SF(15) + OP_l_12_c_20_r_*SF(16) + OP_l_13_c_20_r_*SPP(11); +nextP(2,20) = OP_l_2_c_20_r_ + OP_l_1_c_20_r_*SF(9) + OP_l_3_c_20_r_*SF(8) + OP_l_4_c_20_r_*SF(12) - OP_l_13_c_20_r_*SF(16) + OP_l_12_c_20_r_*SPP(11) - (OP_l_11_c_20_r_*q0)/2; +nextP(3,20) = OP_l_3_c_20_r_ + OP_l_1_c_20_r_*SF(7) + OP_l_2_c_20_r_*SF(11) + OP_l_4_c_20_r_*SF(9) + OP_l_13_c_20_r_*SF(15) - OP_l_11_c_20_r_*SPP(11) - (OP_l_12_c_20_r_*q0)/2; +nextP(4,20) = OP_l_4_c_20_r_ + OP_l_1_c_20_r_*SF(8) + OP_l_2_c_20_r_*SF(7) + OP_l_3_c_20_r_*SF(10) + OP_l_11_c_20_r_*SF(16) - OP_l_12_c_20_r_*SF(15) - (OP_l_13_c_20_r_*q0)/2; +nextP(5,20) = OP_l_5_c_20_r_ + OP_l_1_c_20_r_*SF(6) + OP_l_2_c_20_r_*SF(4) - OP_l_4_c_20_r_*SF(5) + OP_l_3_c_20_r_*SPP(1) + OP_l_14_c_20_r_*SPP(4) + OP_l_15_c_20_r_*SPP(7) - OP_l_16_c_20_r_*SPP(10); +nextP(6,20) = OP_l_6_c_20_r_ + OP_l_1_c_20_r_*SF(5) + OP_l_3_c_20_r_*SF(4) + OP_l_4_c_20_r_*SF(6) - OP_l_2_c_20_r_*SPP(1) - OP_l_14_c_20_r_*SPP(9) + OP_l_15_c_20_r_*SPP(3) + OP_l_16_c_20_r_*SPP(6); +nextP(7,20) = OP_l_7_c_20_r_ + OP_l_2_c_20_r_*SF(5) - OP_l_3_c_20_r_*SF(6) + OP_l_4_c_20_r_*SF(4) + OP_l_1_c_20_r_*SPP(1) + OP_l_14_c_20_r_*SPP(5) - OP_l_15_c_20_r_*SPP(8) - OP_l_16_c_20_r_*SPP(2); nextP(8,20) = OP_l_8_c_20_r_ + OP_l_5_c_20_r_*dt; nextP(9,20) = OP_l_9_c_20_r_ + OP_l_6_c_20_r_*dt; -nextP(10,20) = OP_l_10_c_20_r_; +nextP(10,20) = OP_l_10_c_20_r_ + OP_l_7_c_20_r_*dt; nextP(11,20) = OP_l_11_c_20_r_; nextP(12,20) = OP_l_12_c_20_r_; nextP(13,20) = OP_l_13_c_20_r_; @@ -283,16 +271,16 @@ nextP(17,20) = OP_l_17_c_20_r_; nextP(18,20) = OP_l_18_c_20_r_; nextP(19,20) = OP_l_19_c_20_r_; nextP(20,20) = OP_l_20_c_20_r_; -nextP(1,21) = OP_l_1_c_21_r_*SPP(6) - OP_l_2_c_21_r_*SPP(5) + OP_l_3_c_21_r_*SPP(8) + OP_l_10_c_21_r_*SPP(23) + OP_l_13_c_21_r_*SPP(19); -nextP(2,21) = OP_l_2_c_21_r_*SPP(7) - OP_l_1_c_21_r_*SPP(3) - OP_l_3_c_21_r_*SPP(9) + OP_l_11_c_21_r_*SPP(23) + OP_l_14_c_21_r_*SPP(18); -nextP(3,21) = OP_l_1_c_21_r_*SPP(15) - OP_l_2_c_21_r_*SPP(4) + OP_l_3_c_21_r_*SPP(14) + OP_l_12_c_21_r_*SPP(23) + OP_l_15_c_21_r_*SPP(17); -nextP(4,21) = OP_l_4_c_21_r_ + OP_l_1_c_21_r_*SPP(2) + OP_l_2_c_21_r_*SPP(20) + OP_l_3_c_21_r_*SPP(16) - OP_l_16_c_21_r_*SPP(22); -nextP(5,21) = OP_l_5_c_21_r_ + OP_l_16_c_21_r_*SF(23) + OP_l_1_c_21_r_*SPP(21) + OP_l_2_c_21_r_*SPP(13) + OP_l_3_c_21_r_*SPP(12); -nextP(6,21) = OP_l_6_c_21_r_ + OP_l_16_c_21_r_*SF(21) - OP_l_1_c_21_r_*SPP(10) + OP_l_2_c_21_r_*SPP(11) + OP_l_3_c_21_r_*SPP(1); -nextP(7,21) = OP_l_7_c_21_r_ + OP_l_4_c_21_r_*dt; +nextP(1,21) = OP_l_1_c_21_r_ + OP_l_2_c_21_r_*SF(10) + OP_l_3_c_21_r_*SF(12) + OP_l_4_c_21_r_*SF(11) + OP_l_11_c_21_r_*SF(15) + OP_l_12_c_21_r_*SF(16) + OP_l_13_c_21_r_*SPP(11); +nextP(2,21) = OP_l_2_c_21_r_ + OP_l_1_c_21_r_*SF(9) + OP_l_3_c_21_r_*SF(8) + OP_l_4_c_21_r_*SF(12) - OP_l_13_c_21_r_*SF(16) + OP_l_12_c_21_r_*SPP(11) - (OP_l_11_c_21_r_*q0)/2; +nextP(3,21) = OP_l_3_c_21_r_ + OP_l_1_c_21_r_*SF(7) + OP_l_2_c_21_r_*SF(11) + OP_l_4_c_21_r_*SF(9) + OP_l_13_c_21_r_*SF(15) - OP_l_11_c_21_r_*SPP(11) - (OP_l_12_c_21_r_*q0)/2; +nextP(4,21) = OP_l_4_c_21_r_ + OP_l_1_c_21_r_*SF(8) + OP_l_2_c_21_r_*SF(7) + OP_l_3_c_21_r_*SF(10) + OP_l_11_c_21_r_*SF(16) - OP_l_12_c_21_r_*SF(15) - (OP_l_13_c_21_r_*q0)/2; +nextP(5,21) = OP_l_5_c_21_r_ + OP_l_1_c_21_r_*SF(6) + OP_l_2_c_21_r_*SF(4) - OP_l_4_c_21_r_*SF(5) + OP_l_3_c_21_r_*SPP(1) + OP_l_14_c_21_r_*SPP(4) + OP_l_15_c_21_r_*SPP(7) - OP_l_16_c_21_r_*SPP(10); +nextP(6,21) = OP_l_6_c_21_r_ + OP_l_1_c_21_r_*SF(5) + OP_l_3_c_21_r_*SF(4) + OP_l_4_c_21_r_*SF(6) - OP_l_2_c_21_r_*SPP(1) - OP_l_14_c_21_r_*SPP(9) + OP_l_15_c_21_r_*SPP(3) + OP_l_16_c_21_r_*SPP(6); +nextP(7,21) = OP_l_7_c_21_r_ + OP_l_2_c_21_r_*SF(5) - OP_l_3_c_21_r_*SF(6) + OP_l_4_c_21_r_*SF(4) + OP_l_1_c_21_r_*SPP(1) + OP_l_14_c_21_r_*SPP(5) - OP_l_15_c_21_r_*SPP(8) - OP_l_16_c_21_r_*SPP(2); nextP(8,21) = OP_l_8_c_21_r_ + OP_l_5_c_21_r_*dt; nextP(9,21) = OP_l_9_c_21_r_ + OP_l_6_c_21_r_*dt; -nextP(10,21) = OP_l_10_c_21_r_; +nextP(10,21) = OP_l_10_c_21_r_ + OP_l_7_c_21_r_*dt; nextP(11,21) = OP_l_11_c_21_r_; nextP(12,21) = OP_l_12_c_21_r_; nextP(13,21) = OP_l_13_c_21_r_; @@ -304,16 +292,16 @@ nextP(18,21) = OP_l_18_c_21_r_; nextP(19,21) = OP_l_19_c_21_r_; nextP(20,21) = OP_l_20_c_21_r_; nextP(21,21) = OP_l_21_c_21_r_; -nextP(1,22) = OP_l_1_c_22_r_*SPP(6) - OP_l_2_c_22_r_*SPP(5) + OP_l_3_c_22_r_*SPP(8) + OP_l_10_c_22_r_*SPP(23) + OP_l_13_c_22_r_*SPP(19); -nextP(2,22) = OP_l_2_c_22_r_*SPP(7) - OP_l_1_c_22_r_*SPP(3) - OP_l_3_c_22_r_*SPP(9) + OP_l_11_c_22_r_*SPP(23) + OP_l_14_c_22_r_*SPP(18); -nextP(3,22) = OP_l_1_c_22_r_*SPP(15) - OP_l_2_c_22_r_*SPP(4) + OP_l_3_c_22_r_*SPP(14) + OP_l_12_c_22_r_*SPP(23) + OP_l_15_c_22_r_*SPP(17); -nextP(4,22) = OP_l_4_c_22_r_ + OP_l_1_c_22_r_*SPP(2) + OP_l_2_c_22_r_*SPP(20) + OP_l_3_c_22_r_*SPP(16) - OP_l_16_c_22_r_*SPP(22); -nextP(5,22) = OP_l_5_c_22_r_ + OP_l_16_c_22_r_*SF(23) + OP_l_1_c_22_r_*SPP(21) + OP_l_2_c_22_r_*SPP(13) + OP_l_3_c_22_r_*SPP(12); -nextP(6,22) = OP_l_6_c_22_r_ + OP_l_16_c_22_r_*SF(21) - OP_l_1_c_22_r_*SPP(10) + OP_l_2_c_22_r_*SPP(11) + OP_l_3_c_22_r_*SPP(1); -nextP(7,22) = OP_l_7_c_22_r_ + OP_l_4_c_22_r_*dt; +nextP(1,22) = OP_l_1_c_22_r_ + OP_l_2_c_22_r_*SF(10) + OP_l_3_c_22_r_*SF(12) + OP_l_4_c_22_r_*SF(11) + OP_l_11_c_22_r_*SF(15) + OP_l_12_c_22_r_*SF(16) + OP_l_13_c_22_r_*SPP(11); +nextP(2,22) = OP_l_2_c_22_r_ + OP_l_1_c_22_r_*SF(9) + OP_l_3_c_22_r_*SF(8) + OP_l_4_c_22_r_*SF(12) - OP_l_13_c_22_r_*SF(16) + OP_l_12_c_22_r_*SPP(11) - (OP_l_11_c_22_r_*q0)/2; +nextP(3,22) = OP_l_3_c_22_r_ + OP_l_1_c_22_r_*SF(7) + OP_l_2_c_22_r_*SF(11) + OP_l_4_c_22_r_*SF(9) + OP_l_13_c_22_r_*SF(15) - OP_l_11_c_22_r_*SPP(11) - (OP_l_12_c_22_r_*q0)/2; +nextP(4,22) = OP_l_4_c_22_r_ + OP_l_1_c_22_r_*SF(8) + OP_l_2_c_22_r_*SF(7) + OP_l_3_c_22_r_*SF(10) + OP_l_11_c_22_r_*SF(16) - OP_l_12_c_22_r_*SF(15) - (OP_l_13_c_22_r_*q0)/2; +nextP(5,22) = OP_l_5_c_22_r_ + OP_l_1_c_22_r_*SF(6) + OP_l_2_c_22_r_*SF(4) - OP_l_4_c_22_r_*SF(5) + OP_l_3_c_22_r_*SPP(1) + OP_l_14_c_22_r_*SPP(4) + OP_l_15_c_22_r_*SPP(7) - OP_l_16_c_22_r_*SPP(10); +nextP(6,22) = OP_l_6_c_22_r_ + OP_l_1_c_22_r_*SF(5) + OP_l_3_c_22_r_*SF(4) + OP_l_4_c_22_r_*SF(6) - OP_l_2_c_22_r_*SPP(1) - OP_l_14_c_22_r_*SPP(9) + OP_l_15_c_22_r_*SPP(3) + OP_l_16_c_22_r_*SPP(6); +nextP(7,22) = OP_l_7_c_22_r_ + OP_l_2_c_22_r_*SF(5) - OP_l_3_c_22_r_*SF(6) + OP_l_4_c_22_r_*SF(4) + OP_l_1_c_22_r_*SPP(1) + OP_l_14_c_22_r_*SPP(5) - OP_l_15_c_22_r_*SPP(8) - OP_l_16_c_22_r_*SPP(2); nextP(8,22) = OP_l_8_c_22_r_ + OP_l_5_c_22_r_*dt; nextP(9,22) = OP_l_9_c_22_r_ + OP_l_6_c_22_r_*dt; -nextP(10,22) = OP_l_10_c_22_r_; +nextP(10,22) = OP_l_10_c_22_r_ + OP_l_7_c_22_r_*dt; nextP(11,22) = OP_l_11_c_22_r_; nextP(12,22) = OP_l_12_c_22_r_; nextP(13,22) = OP_l_13_c_22_r_; @@ -326,16 +314,16 @@ nextP(19,22) = OP_l_19_c_22_r_; nextP(20,22) = OP_l_20_c_22_r_; nextP(21,22) = OP_l_21_c_22_r_; nextP(22,22) = OP_l_22_c_22_r_; -nextP(1,23) = OP_l_1_c_23_r_*SPP(6) - OP_l_2_c_23_r_*SPP(5) + OP_l_3_c_23_r_*SPP(8) + OP_l_10_c_23_r_*SPP(23) + OP_l_13_c_23_r_*SPP(19); -nextP(2,23) = OP_l_2_c_23_r_*SPP(7) - OP_l_1_c_23_r_*SPP(3) - OP_l_3_c_23_r_*SPP(9) + OP_l_11_c_23_r_*SPP(23) + OP_l_14_c_23_r_*SPP(18); -nextP(3,23) = OP_l_1_c_23_r_*SPP(15) - OP_l_2_c_23_r_*SPP(4) + OP_l_3_c_23_r_*SPP(14) + OP_l_12_c_23_r_*SPP(23) + OP_l_15_c_23_r_*SPP(17); -nextP(4,23) = OP_l_4_c_23_r_ + OP_l_1_c_23_r_*SPP(2) + OP_l_2_c_23_r_*SPP(20) + OP_l_3_c_23_r_*SPP(16) - OP_l_16_c_23_r_*SPP(22); -nextP(5,23) = OP_l_5_c_23_r_ + OP_l_16_c_23_r_*SF(23) + OP_l_1_c_23_r_*SPP(21) + OP_l_2_c_23_r_*SPP(13) + OP_l_3_c_23_r_*SPP(12); -nextP(6,23) = OP_l_6_c_23_r_ + OP_l_16_c_23_r_*SF(21) - OP_l_1_c_23_r_*SPP(10) + OP_l_2_c_23_r_*SPP(11) + OP_l_3_c_23_r_*SPP(1); -nextP(7,23) = OP_l_7_c_23_r_ + OP_l_4_c_23_r_*dt; +nextP(1,23) = OP_l_1_c_23_r_ + OP_l_2_c_23_r_*SF(10) + OP_l_3_c_23_r_*SF(12) + OP_l_4_c_23_r_*SF(11) + OP_l_11_c_23_r_*SF(15) + OP_l_12_c_23_r_*SF(16) + OP_l_13_c_23_r_*SPP(11); +nextP(2,23) = OP_l_2_c_23_r_ + OP_l_1_c_23_r_*SF(9) + OP_l_3_c_23_r_*SF(8) + OP_l_4_c_23_r_*SF(12) - OP_l_13_c_23_r_*SF(16) + OP_l_12_c_23_r_*SPP(11) - (OP_l_11_c_23_r_*q0)/2; +nextP(3,23) = OP_l_3_c_23_r_ + OP_l_1_c_23_r_*SF(7) + OP_l_2_c_23_r_*SF(11) + OP_l_4_c_23_r_*SF(9) + OP_l_13_c_23_r_*SF(15) - OP_l_11_c_23_r_*SPP(11) - (OP_l_12_c_23_r_*q0)/2; +nextP(4,23) = OP_l_4_c_23_r_ + OP_l_1_c_23_r_*SF(8) + OP_l_2_c_23_r_*SF(7) + OP_l_3_c_23_r_*SF(10) + OP_l_11_c_23_r_*SF(16) - OP_l_12_c_23_r_*SF(15) - (OP_l_13_c_23_r_*q0)/2; +nextP(5,23) = OP_l_5_c_23_r_ + OP_l_1_c_23_r_*SF(6) + OP_l_2_c_23_r_*SF(4) - OP_l_4_c_23_r_*SF(5) + OP_l_3_c_23_r_*SPP(1) + OP_l_14_c_23_r_*SPP(4) + OP_l_15_c_23_r_*SPP(7) - OP_l_16_c_23_r_*SPP(10); +nextP(6,23) = OP_l_6_c_23_r_ + OP_l_1_c_23_r_*SF(5) + OP_l_3_c_23_r_*SF(4) + OP_l_4_c_23_r_*SF(6) - OP_l_2_c_23_r_*SPP(1) - OP_l_14_c_23_r_*SPP(9) + OP_l_15_c_23_r_*SPP(3) + OP_l_16_c_23_r_*SPP(6); +nextP(7,23) = OP_l_7_c_23_r_ + OP_l_2_c_23_r_*SF(5) - OP_l_3_c_23_r_*SF(6) + OP_l_4_c_23_r_*SF(4) + OP_l_1_c_23_r_*SPP(1) + OP_l_14_c_23_r_*SPP(5) - OP_l_15_c_23_r_*SPP(8) - OP_l_16_c_23_r_*SPP(2); nextP(8,23) = OP_l_8_c_23_r_ + OP_l_5_c_23_r_*dt; nextP(9,23) = OP_l_9_c_23_r_ + OP_l_6_c_23_r_*dt; -nextP(10,23) = OP_l_10_c_23_r_; +nextP(10,23) = OP_l_10_c_23_r_ + OP_l_7_c_23_r_*dt; nextP(11,23) = OP_l_11_c_23_r_; nextP(12,23) = OP_l_12_c_23_r_; nextP(13,23) = OP_l_13_c_23_r_; @@ -349,16 +337,16 @@ nextP(20,23) = OP_l_20_c_23_r_; nextP(21,23) = OP_l_21_c_23_r_; nextP(22,23) = OP_l_22_c_23_r_; nextP(23,23) = OP_l_23_c_23_r_; -nextP(1,24) = OP_l_1_c_24_r_*SPP(6) - OP_l_2_c_24_r_*SPP(5) + OP_l_3_c_24_r_*SPP(8) + OP_l_10_c_24_r_*SPP(23) + OP_l_13_c_24_r_*SPP(19); -nextP(2,24) = OP_l_2_c_24_r_*SPP(7) - OP_l_1_c_24_r_*SPP(3) - OP_l_3_c_24_r_*SPP(9) + OP_l_11_c_24_r_*SPP(23) + OP_l_14_c_24_r_*SPP(18); -nextP(3,24) = OP_l_1_c_24_r_*SPP(15) - OP_l_2_c_24_r_*SPP(4) + OP_l_3_c_24_r_*SPP(14) + OP_l_12_c_24_r_*SPP(23) + OP_l_15_c_24_r_*SPP(17); -nextP(4,24) = OP_l_4_c_24_r_ + OP_l_1_c_24_r_*SPP(2) + OP_l_2_c_24_r_*SPP(20) + OP_l_3_c_24_r_*SPP(16) - OP_l_16_c_24_r_*SPP(22); -nextP(5,24) = OP_l_5_c_24_r_ + OP_l_16_c_24_r_*SF(23) + OP_l_1_c_24_r_*SPP(21) + OP_l_2_c_24_r_*SPP(13) + OP_l_3_c_24_r_*SPP(12); -nextP(6,24) = OP_l_6_c_24_r_ + OP_l_16_c_24_r_*SF(21) - OP_l_1_c_24_r_*SPP(10) + OP_l_2_c_24_r_*SPP(11) + OP_l_3_c_24_r_*SPP(1); -nextP(7,24) = OP_l_7_c_24_r_ + OP_l_4_c_24_r_*dt; +nextP(1,24) = OP_l_1_c_24_r_ + OP_l_2_c_24_r_*SF(10) + OP_l_3_c_24_r_*SF(12) + OP_l_4_c_24_r_*SF(11) + OP_l_11_c_24_r_*SF(15) + OP_l_12_c_24_r_*SF(16) + OP_l_13_c_24_r_*SPP(11); +nextP(2,24) = OP_l_2_c_24_r_ + OP_l_1_c_24_r_*SF(9) + OP_l_3_c_24_r_*SF(8) + OP_l_4_c_24_r_*SF(12) - OP_l_13_c_24_r_*SF(16) + OP_l_12_c_24_r_*SPP(11) - (OP_l_11_c_24_r_*q0)/2; +nextP(3,24) = OP_l_3_c_24_r_ + OP_l_1_c_24_r_*SF(7) + OP_l_2_c_24_r_*SF(11) + OP_l_4_c_24_r_*SF(9) + OP_l_13_c_24_r_*SF(15) - OP_l_11_c_24_r_*SPP(11) - (OP_l_12_c_24_r_*q0)/2; +nextP(4,24) = OP_l_4_c_24_r_ + OP_l_1_c_24_r_*SF(8) + OP_l_2_c_24_r_*SF(7) + OP_l_3_c_24_r_*SF(10) + OP_l_11_c_24_r_*SF(16) - OP_l_12_c_24_r_*SF(15) - (OP_l_13_c_24_r_*q0)/2; +nextP(5,24) = OP_l_5_c_24_r_ + OP_l_1_c_24_r_*SF(6) + OP_l_2_c_24_r_*SF(4) - OP_l_4_c_24_r_*SF(5) + OP_l_3_c_24_r_*SPP(1) + OP_l_14_c_24_r_*SPP(4) + OP_l_15_c_24_r_*SPP(7) - OP_l_16_c_24_r_*SPP(10); +nextP(6,24) = OP_l_6_c_24_r_ + OP_l_1_c_24_r_*SF(5) + OP_l_3_c_24_r_*SF(4) + OP_l_4_c_24_r_*SF(6) - OP_l_2_c_24_r_*SPP(1) - OP_l_14_c_24_r_*SPP(9) + OP_l_15_c_24_r_*SPP(3) + OP_l_16_c_24_r_*SPP(6); +nextP(7,24) = OP_l_7_c_24_r_ + OP_l_2_c_24_r_*SF(5) - OP_l_3_c_24_r_*SF(6) + OP_l_4_c_24_r_*SF(4) + OP_l_1_c_24_r_*SPP(1) + OP_l_14_c_24_r_*SPP(5) - OP_l_15_c_24_r_*SPP(8) - OP_l_16_c_24_r_*SPP(2); nextP(8,24) = OP_l_8_c_24_r_ + OP_l_5_c_24_r_*dt; nextP(9,24) = OP_l_9_c_24_r_ + OP_l_6_c_24_r_*dt; -nextP(10,24) = OP_l_10_c_24_r_; +nextP(10,24) = OP_l_10_c_24_r_ + OP_l_7_c_24_r_*dt; nextP(11,24) = OP_l_11_c_24_r_; nextP(12,24) = OP_l_12_c_24_r_; nextP(13,24) = OP_l_13_c_24_r_; @@ -381,357 +369,374 @@ SH_TAS(2) = (SH_TAS(1)*(2*ve - 2*vwe))/2; SH_TAS(3) = (SH_TAS(1)*(2*vn - 2*vwn))/2; H_TAS = zeros(1,24); -H_TAS(1,4) = SH_TAS(3); -H_TAS(1,5) = SH_TAS(2); -H_TAS(1,6) = vd*SH_TAS(1); +H_TAS(1,5) = SH_TAS(3); +H_TAS(1,6) = SH_TAS(2); +H_TAS(1,7) = vd*SH_TAS(1); H_TAS(1,23) = -SH_TAS(3); H_TAS(1,24) = -SH_TAS(2); SK_TAS = zeros(2,1); -SK_TAS(1) = 1/(R_TAS + SH_TAS(3)*(OP_l_4_c_4_r_*SH_TAS(3) + OP_l_5_c_4_r_*SH_TAS(2) - OP_l_23_c_4_r_*SH_TAS(3) - OP_l_24_c_4_r_*SH_TAS(2) + OP_l_6_c_4_r_*vd*SH_TAS(1)) + SH_TAS(2)*(OP_l_4_c_5_r_*SH_TAS(3) + OP_l_5_c_5_r_*SH_TAS(2) - OP_l_23_c_5_r_*SH_TAS(3) - OP_l_24_c_5_r_*SH_TAS(2) + OP_l_6_c_5_r_*vd*SH_TAS(1)) - SH_TAS(3)*(OP_l_4_c_23_r_*SH_TAS(3) + OP_l_5_c_23_r_*SH_TAS(2) - OP_l_23_c_23_r_*SH_TAS(3) - OP_l_24_c_23_r_*SH_TAS(2) + OP_l_6_c_23_r_*vd*SH_TAS(1)) - SH_TAS(2)*(OP_l_4_c_24_r_*SH_TAS(3) + OP_l_5_c_24_r_*SH_TAS(2) - OP_l_23_c_24_r_*SH_TAS(3) - OP_l_24_c_24_r_*SH_TAS(2) + OP_l_6_c_24_r_*vd*SH_TAS(1)) + vd*SH_TAS(1)*(OP_l_4_c_6_r_*SH_TAS(3) + OP_l_5_c_6_r_*SH_TAS(2) - OP_l_23_c_6_r_*SH_TAS(3) - OP_l_24_c_6_r_*SH_TAS(2) + OP_l_6_c_6_r_*vd*SH_TAS(1))); +SK_TAS(1) = 1/(R_TAS + SH_TAS(3)*(OP_l_5_c_5_r_*SH_TAS(3) + OP_l_6_c_5_r_*SH_TAS(2) - OP_l_23_c_5_r_*SH_TAS(3) - OP_l_24_c_5_r_*SH_TAS(2) + OP_l_7_c_5_r_*vd*SH_TAS(1)) + SH_TAS(2)*(OP_l_5_c_6_r_*SH_TAS(3) + OP_l_6_c_6_r_*SH_TAS(2) - OP_l_23_c_6_r_*SH_TAS(3) - OP_l_24_c_6_r_*SH_TAS(2) + OP_l_7_c_6_r_*vd*SH_TAS(1)) - SH_TAS(3)*(OP_l_5_c_23_r_*SH_TAS(3) + OP_l_6_c_23_r_*SH_TAS(2) - OP_l_23_c_23_r_*SH_TAS(3) - OP_l_24_c_23_r_*SH_TAS(2) + OP_l_7_c_23_r_*vd*SH_TAS(1)) - SH_TAS(2)*(OP_l_5_c_24_r_*SH_TAS(3) + OP_l_6_c_24_r_*SH_TAS(2) - OP_l_23_c_24_r_*SH_TAS(3) - OP_l_24_c_24_r_*SH_TAS(2) + OP_l_7_c_24_r_*vd*SH_TAS(1)) + vd*SH_TAS(1)*(OP_l_5_c_7_r_*SH_TAS(3) + OP_l_6_c_7_r_*SH_TAS(2) - OP_l_23_c_7_r_*SH_TAS(3) - OP_l_24_c_7_r_*SH_TAS(2) + OP_l_7_c_7_r_*vd*SH_TAS(1))); SK_TAS(2) = SH_TAS(2); Kfusion = zeros(24,1); Kfusion = zeros(1,1); -Kfusion(1) = SK_TAS(1)*(OP_l_1_c_4_r_*SH_TAS(3) - OP_l_1_c_23_r_*SH_TAS(3) + OP_l_1_c_5_r_*SK_TAS(2) - OP_l_1_c_24_r_*SK_TAS(2) + OP_l_1_c_6_r_*vd*SH_TAS(1)); -Kfusion(2) = SK_TAS(1)*(OP_l_2_c_4_r_*SH_TAS(3) - OP_l_2_c_23_r_*SH_TAS(3) + OP_l_2_c_5_r_*SK_TAS(2) - OP_l_2_c_24_r_*SK_TAS(2) + OP_l_2_c_6_r_*vd*SH_TAS(1)); -Kfusion(3) = SK_TAS(1)*(OP_l_3_c_4_r_*SH_TAS(3) - OP_l_3_c_23_r_*SH_TAS(3) + OP_l_3_c_5_r_*SK_TAS(2) - OP_l_3_c_24_r_*SK_TAS(2) + OP_l_3_c_6_r_*vd*SH_TAS(1)); -Kfusion(4) = SK_TAS(1)*(OP_l_4_c_4_r_*SH_TAS(3) - OP_l_4_c_23_r_*SH_TAS(3) + OP_l_4_c_5_r_*SK_TAS(2) - OP_l_4_c_24_r_*SK_TAS(2) + OP_l_4_c_6_r_*vd*SH_TAS(1)); -Kfusion(5) = SK_TAS(1)*(OP_l_5_c_4_r_*SH_TAS(3) - OP_l_5_c_23_r_*SH_TAS(3) + OP_l_5_c_5_r_*SK_TAS(2) - OP_l_5_c_24_r_*SK_TAS(2) + OP_l_5_c_6_r_*vd*SH_TAS(1)); -Kfusion(6) = SK_TAS(1)*(OP_l_6_c_4_r_*SH_TAS(3) - OP_l_6_c_23_r_*SH_TAS(3) + OP_l_6_c_5_r_*SK_TAS(2) - OP_l_6_c_24_r_*SK_TAS(2) + OP_l_6_c_6_r_*vd*SH_TAS(1)); -Kfusion(7) = SK_TAS(1)*(OP_l_7_c_4_r_*SH_TAS(3) - OP_l_7_c_23_r_*SH_TAS(3) + OP_l_7_c_5_r_*SK_TAS(2) - OP_l_7_c_24_r_*SK_TAS(2) + OP_l_7_c_6_r_*vd*SH_TAS(1)); -Kfusion(8) = SK_TAS(1)*(OP_l_8_c_4_r_*SH_TAS(3) - OP_l_8_c_23_r_*SH_TAS(3) + OP_l_8_c_5_r_*SK_TAS(2) - OP_l_8_c_24_r_*SK_TAS(2) + OP_l_8_c_6_r_*vd*SH_TAS(1)); -Kfusion(9) = SK_TAS(1)*(OP_l_9_c_4_r_*SH_TAS(3) - OP_l_9_c_23_r_*SH_TAS(3) + OP_l_9_c_5_r_*SK_TAS(2) - OP_l_9_c_24_r_*SK_TAS(2) + OP_l_9_c_6_r_*vd*SH_TAS(1)); -Kfusion(10) = SK_TAS(1)*(OP_l_10_c_4_r_*SH_TAS(3) - OP_l_10_c_23_r_*SH_TAS(3) + OP_l_10_c_5_r_*SK_TAS(2) - OP_l_10_c_24_r_*SK_TAS(2) + OP_l_10_c_6_r_*vd*SH_TAS(1)); -Kfusion(11) = SK_TAS(1)*(OP_l_11_c_4_r_*SH_TAS(3) - OP_l_11_c_23_r_*SH_TAS(3) + OP_l_11_c_5_r_*SK_TAS(2) - OP_l_11_c_24_r_*SK_TAS(2) + OP_l_11_c_6_r_*vd*SH_TAS(1)); -Kfusion(12) = SK_TAS(1)*(OP_l_12_c_4_r_*SH_TAS(3) - OP_l_12_c_23_r_*SH_TAS(3) + OP_l_12_c_5_r_*SK_TAS(2) - OP_l_12_c_24_r_*SK_TAS(2) + OP_l_12_c_6_r_*vd*SH_TAS(1)); -Kfusion(13) = SK_TAS(1)*(OP_l_13_c_4_r_*SH_TAS(3) - OP_l_13_c_23_r_*SH_TAS(3) + OP_l_13_c_5_r_*SK_TAS(2) - OP_l_13_c_24_r_*SK_TAS(2) + OP_l_13_c_6_r_*vd*SH_TAS(1)); -Kfusion(14) = SK_TAS(1)*(OP_l_14_c_4_r_*SH_TAS(3) - OP_l_14_c_23_r_*SH_TAS(3) + OP_l_14_c_5_r_*SK_TAS(2) - OP_l_14_c_24_r_*SK_TAS(2) + OP_l_14_c_6_r_*vd*SH_TAS(1)); -Kfusion(15) = SK_TAS(1)*(OP_l_15_c_4_r_*SH_TAS(3) - OP_l_15_c_23_r_*SH_TAS(3) + OP_l_15_c_5_r_*SK_TAS(2) - OP_l_15_c_24_r_*SK_TAS(2) + OP_l_15_c_6_r_*vd*SH_TAS(1)); -Kfusion(16) = SK_TAS(1)*(OP_l_16_c_4_r_*SH_TAS(3) - OP_l_16_c_23_r_*SH_TAS(3) + OP_l_16_c_5_r_*SK_TAS(2) - OP_l_16_c_24_r_*SK_TAS(2) + OP_l_16_c_6_r_*vd*SH_TAS(1)); -Kfusion(17) = SK_TAS(1)*(OP_l_17_c_4_r_*SH_TAS(3) - OP_l_17_c_23_r_*SH_TAS(3) + OP_l_17_c_5_r_*SK_TAS(2) - OP_l_17_c_24_r_*SK_TAS(2) + OP_l_17_c_6_r_*vd*SH_TAS(1)); -Kfusion(18) = SK_TAS(1)*(OP_l_18_c_4_r_*SH_TAS(3) - OP_l_18_c_23_r_*SH_TAS(3) + OP_l_18_c_5_r_*SK_TAS(2) - OP_l_18_c_24_r_*SK_TAS(2) + OP_l_18_c_6_r_*vd*SH_TAS(1)); -Kfusion(19) = SK_TAS(1)*(OP_l_19_c_4_r_*SH_TAS(3) - OP_l_19_c_23_r_*SH_TAS(3) + OP_l_19_c_5_r_*SK_TAS(2) - OP_l_19_c_24_r_*SK_TAS(2) + OP_l_19_c_6_r_*vd*SH_TAS(1)); -Kfusion(20) = SK_TAS(1)*(OP_l_20_c_4_r_*SH_TAS(3) - OP_l_20_c_23_r_*SH_TAS(3) + OP_l_20_c_5_r_*SK_TAS(2) - OP_l_20_c_24_r_*SK_TAS(2) + OP_l_20_c_6_r_*vd*SH_TAS(1)); -Kfusion(21) = SK_TAS(1)*(OP_l_21_c_4_r_*SH_TAS(3) - OP_l_21_c_23_r_*SH_TAS(3) + OP_l_21_c_5_r_*SK_TAS(2) - OP_l_21_c_24_r_*SK_TAS(2) + OP_l_21_c_6_r_*vd*SH_TAS(1)); -Kfusion(22) = SK_TAS(1)*(OP_l_22_c_4_r_*SH_TAS(3) - OP_l_22_c_23_r_*SH_TAS(3) + OP_l_22_c_5_r_*SK_TAS(2) - OP_l_22_c_24_r_*SK_TAS(2) + OP_l_22_c_6_r_*vd*SH_TAS(1)); -Kfusion(23) = SK_TAS(1)*(OP_l_23_c_4_r_*SH_TAS(3) - OP_l_23_c_23_r_*SH_TAS(3) + OP_l_23_c_5_r_*SK_TAS(2) - OP_l_23_c_24_r_*SK_TAS(2) + OP_l_23_c_6_r_*vd*SH_TAS(1)); -Kfusion(24) = SK_TAS(1)*(OP_l_24_c_4_r_*SH_TAS(3) - OP_l_24_c_23_r_*SH_TAS(3) + OP_l_24_c_5_r_*SK_TAS(2) - OP_l_24_c_24_r_*SK_TAS(2) + OP_l_24_c_6_r_*vd*SH_TAS(1)); +Kfusion(1) = SK_TAS(1)*(OP_l_1_c_5_r_*SH_TAS(3) - OP_l_1_c_23_r_*SH_TAS(3) + OP_l_1_c_6_r_*SK_TAS(2) - OP_l_1_c_24_r_*SK_TAS(2) + OP_l_1_c_7_r_*vd*SH_TAS(1)); +Kfusion(2) = SK_TAS(1)*(OP_l_2_c_5_r_*SH_TAS(3) - OP_l_2_c_23_r_*SH_TAS(3) + OP_l_2_c_6_r_*SK_TAS(2) - OP_l_2_c_24_r_*SK_TAS(2) + OP_l_2_c_7_r_*vd*SH_TAS(1)); +Kfusion(3) = SK_TAS(1)*(OP_l_3_c_5_r_*SH_TAS(3) - OP_l_3_c_23_r_*SH_TAS(3) + OP_l_3_c_6_r_*SK_TAS(2) - OP_l_3_c_24_r_*SK_TAS(2) + OP_l_3_c_7_r_*vd*SH_TAS(1)); +Kfusion(4) = SK_TAS(1)*(OP_l_4_c_5_r_*SH_TAS(3) - OP_l_4_c_23_r_*SH_TAS(3) + OP_l_4_c_6_r_*SK_TAS(2) - OP_l_4_c_24_r_*SK_TAS(2) + OP_l_4_c_7_r_*vd*SH_TAS(1)); +Kfusion(5) = SK_TAS(1)*(OP_l_5_c_5_r_*SH_TAS(3) - OP_l_5_c_23_r_*SH_TAS(3) + OP_l_5_c_6_r_*SK_TAS(2) - OP_l_5_c_24_r_*SK_TAS(2) + OP_l_5_c_7_r_*vd*SH_TAS(1)); +Kfusion(6) = SK_TAS(1)*(OP_l_6_c_5_r_*SH_TAS(3) - OP_l_6_c_23_r_*SH_TAS(3) + OP_l_6_c_6_r_*SK_TAS(2) - OP_l_6_c_24_r_*SK_TAS(2) + OP_l_6_c_7_r_*vd*SH_TAS(1)); +Kfusion(7) = SK_TAS(1)*(OP_l_7_c_5_r_*SH_TAS(3) - OP_l_7_c_23_r_*SH_TAS(3) + OP_l_7_c_6_r_*SK_TAS(2) - OP_l_7_c_24_r_*SK_TAS(2) + OP_l_7_c_7_r_*vd*SH_TAS(1)); +Kfusion(8) = SK_TAS(1)*(OP_l_8_c_5_r_*SH_TAS(3) - OP_l_8_c_23_r_*SH_TAS(3) + OP_l_8_c_6_r_*SK_TAS(2) - OP_l_8_c_24_r_*SK_TAS(2) + OP_l_8_c_7_r_*vd*SH_TAS(1)); +Kfusion(9) = SK_TAS(1)*(OP_l_9_c_5_r_*SH_TAS(3) - OP_l_9_c_23_r_*SH_TAS(3) + OP_l_9_c_6_r_*SK_TAS(2) - OP_l_9_c_24_r_*SK_TAS(2) + OP_l_9_c_7_r_*vd*SH_TAS(1)); +Kfusion(10) = SK_TAS(1)*(OP_l_10_c_5_r_*SH_TAS(3) - OP_l_10_c_23_r_*SH_TAS(3) + OP_l_10_c_6_r_*SK_TAS(2) - OP_l_10_c_24_r_*SK_TAS(2) + OP_l_10_c_7_r_*vd*SH_TAS(1)); +Kfusion(11) = SK_TAS(1)*(OP_l_11_c_5_r_*SH_TAS(3) - OP_l_11_c_23_r_*SH_TAS(3) + OP_l_11_c_6_r_*SK_TAS(2) - OP_l_11_c_24_r_*SK_TAS(2) + OP_l_11_c_7_r_*vd*SH_TAS(1)); +Kfusion(12) = SK_TAS(1)*(OP_l_12_c_5_r_*SH_TAS(3) - OP_l_12_c_23_r_*SH_TAS(3) + OP_l_12_c_6_r_*SK_TAS(2) - OP_l_12_c_24_r_*SK_TAS(2) + OP_l_12_c_7_r_*vd*SH_TAS(1)); +Kfusion(13) = SK_TAS(1)*(OP_l_13_c_5_r_*SH_TAS(3) - OP_l_13_c_23_r_*SH_TAS(3) + OP_l_13_c_6_r_*SK_TAS(2) - OP_l_13_c_24_r_*SK_TAS(2) + OP_l_13_c_7_r_*vd*SH_TAS(1)); +Kfusion(14) = SK_TAS(1)*(OP_l_14_c_5_r_*SH_TAS(3) - OP_l_14_c_23_r_*SH_TAS(3) + OP_l_14_c_6_r_*SK_TAS(2) - OP_l_14_c_24_r_*SK_TAS(2) + OP_l_14_c_7_r_*vd*SH_TAS(1)); +Kfusion(15) = SK_TAS(1)*(OP_l_15_c_5_r_*SH_TAS(3) - OP_l_15_c_23_r_*SH_TAS(3) + OP_l_15_c_6_r_*SK_TAS(2) - OP_l_15_c_24_r_*SK_TAS(2) + OP_l_15_c_7_r_*vd*SH_TAS(1)); +Kfusion(16) = SK_TAS(1)*(OP_l_16_c_5_r_*SH_TAS(3) - OP_l_16_c_23_r_*SH_TAS(3) + OP_l_16_c_6_r_*SK_TAS(2) - OP_l_16_c_24_r_*SK_TAS(2) + OP_l_16_c_7_r_*vd*SH_TAS(1)); +Kfusion(17) = SK_TAS(1)*(OP_l_17_c_5_r_*SH_TAS(3) - OP_l_17_c_23_r_*SH_TAS(3) + OP_l_17_c_6_r_*SK_TAS(2) - OP_l_17_c_24_r_*SK_TAS(2) + OP_l_17_c_7_r_*vd*SH_TAS(1)); +Kfusion(18) = SK_TAS(1)*(OP_l_18_c_5_r_*SH_TAS(3) - OP_l_18_c_23_r_*SH_TAS(3) + OP_l_18_c_6_r_*SK_TAS(2) - OP_l_18_c_24_r_*SK_TAS(2) + OP_l_18_c_7_r_*vd*SH_TAS(1)); +Kfusion(19) = SK_TAS(1)*(OP_l_19_c_5_r_*SH_TAS(3) - OP_l_19_c_23_r_*SH_TAS(3) + OP_l_19_c_6_r_*SK_TAS(2) - OP_l_19_c_24_r_*SK_TAS(2) + OP_l_19_c_7_r_*vd*SH_TAS(1)); +Kfusion(20) = SK_TAS(1)*(OP_l_20_c_5_r_*SH_TAS(3) - OP_l_20_c_23_r_*SH_TAS(3) + OP_l_20_c_6_r_*SK_TAS(2) - OP_l_20_c_24_r_*SK_TAS(2) + OP_l_20_c_7_r_*vd*SH_TAS(1)); +Kfusion(21) = SK_TAS(1)*(OP_l_21_c_5_r_*SH_TAS(3) - OP_l_21_c_23_r_*SH_TAS(3) + OP_l_21_c_6_r_*SK_TAS(2) - OP_l_21_c_24_r_*SK_TAS(2) + OP_l_21_c_7_r_*vd*SH_TAS(1)); +Kfusion(22) = SK_TAS(1)*(OP_l_22_c_5_r_*SH_TAS(3) - OP_l_22_c_23_r_*SH_TAS(3) + OP_l_22_c_6_r_*SK_TAS(2) - OP_l_22_c_24_r_*SK_TAS(2) + OP_l_22_c_7_r_*vd*SH_TAS(1)); +Kfusion(23) = SK_TAS(1)*(OP_l_23_c_5_r_*SH_TAS(3) - OP_l_23_c_23_r_*SH_TAS(3) + OP_l_23_c_6_r_*SK_TAS(2) - OP_l_23_c_24_r_*SK_TAS(2) + OP_l_23_c_7_r_*vd*SH_TAS(1)); +Kfusion(24) = SK_TAS(1)*(OP_l_24_c_5_r_*SH_TAS(3) - OP_l_24_c_23_r_*SH_TAS(3) + OP_l_24_c_6_r_*SK_TAS(2) - OP_l_24_c_24_r_*SK_TAS(2) + OP_l_24_c_7_r_*vd*SH_TAS(1)); -SH_BETA = zeros(10,1); -SH_BETA(1) = (2*conj(q0)*conj(q3) + 2*conj(q1)*conj(q2))*(ve - vwe) - vd*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3)) + (vn - vwn)*(conj(q0)^2 + conj(q1)^2 - conj(q2)^2 - conj(q3)^2); -SH_BETA(2) = vd*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) - (2*conj(q0)*conj(q3) - 2*conj(q1)*conj(q2))*(vn - vwn) + (ve - vwe)*(conj(q0)^2 - conj(q1)^2 + conj(q2)^2 - conj(q3)^2); -SH_BETA(3) = (2*conj(q0)*conj(q2) + 2*conj(q1)*conj(q3))*(vn - vwn) - (2*conj(q0)*conj(q1) - 2*conj(q2)*conj(q3))*(ve - vwe) + vd*(conj(q0)^2 - conj(q1)^2 - conj(q2)^2 + conj(q3)^2); -SH_BETA(4) = (conj(q0)^2 - conj(q1)^2 + conj(q2)^2 - conj(q3)^2)/SH_BETA(1); +SH_BETA = zeros(13,1); +SH_BETA(1) = (vn - vwn)*(q0^2 + q1^2 - q2^2 - q3^2) - vd*(2*q0*q2 - 2*q1*q3) + (ve - vwe)*(2*q0*q3 + 2*q1*q2); +SH_BETA(2) = (ve - vwe)*(q0^2 - q1^2 + q2^2 - q3^2) + vd*(2*q0*q1 + 2*q2*q3) - (vn - vwn)*(2*q0*q3 - 2*q1*q2); +SH_BETA(3) = vn - vwn; +SH_BETA(4) = ve - vwe; SH_BETA(5) = 1/SH_BETA(1)^2; -SH_BETA(6) = conj(q0)^2 + conj(q1)^2 - conj(q2)^2 - conj(q3)^2; -SH_BETA(7) = 2*conj(q0)*conj(q3); -SH_BETA(8) = 1/SH_BETA(1); -SH_BETA(9) = SH_BETA(7) + 2*conj(q1)*conj(q2); -SH_BETA(10) = SH_BETA(7) - 2*conj(q1)*conj(q2); +SH_BETA(6) = 1/SH_BETA(1); +SH_BETA(7) = SH_BETA(6)*(q0^2 - q1^2 + q2^2 - q3^2); +SH_BETA(8) = q0^2 + q1^2 - q2^2 - q3^2; +SH_BETA(9) = 2*q0*SH_BETA(4) - 2*q3*SH_BETA(3) + 2*q1*vd; +SH_BETA(10) = 2*q0*SH_BETA(3) + 2*q3*SH_BETA(4) - 2*q2*vd; +SH_BETA(11) = 2*q2*SH_BETA(3) - 2*q1*SH_BETA(4) + 2*q0*vd; +SH_BETA(12) = 2*q1*SH_BETA(3) + 2*q2*SH_BETA(4) + 2*q3*vd; +SH_BETA(13) = 2*q0*q3; H_BETA = zeros(1,24); -H_BETA(1,1) = SH_BETA(3)*SH_BETA(8); -H_BETA(1,2) = SH_BETA(2)*SH_BETA(3)*SH_BETA(5); -H_BETA(1,3) = - SH_BETA(2)^2*SH_BETA(5) - 1; -H_BETA(1,4) = - SH_BETA(8)*SH_BETA(10) - SH_BETA(2)*SH_BETA(5)*SH_BETA(6); -H_BETA(1,5) = SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9); -H_BETA(1,6) = SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3)); -H_BETA(1,23) = SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6); -H_BETA(1,24) = SH_BETA(2)*SH_BETA(5)*SH_BETA(9) - SH_BETA(4); +H_BETA(1,1) = SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10); +H_BETA(1,2) = SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12); +H_BETA(1,3) = SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11); +H_BETA(1,4) = - SH_BETA(6)*SH_BETA(10) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9); +H_BETA(1,5) = - SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) - SH_BETA(2)*SH_BETA(5)*SH_BETA(8); +H_BETA(1,6) = SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2); +H_BETA(1,7) = SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3); +H_BETA(1,23) = SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8); +H_BETA(1,24) = SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2) - SH_BETA(7); -SK_BETA = zeros(6,1); -SK_BETA(1) = 1/(R_BETA + (SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3)))*(OP_l_23_c_6_r_*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP_l_4_c_6_r_*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP_l_3_c_6_r_*(SH_BETA(2)^2*SH_BETA(5) + 1) + OP_l_6_c_6_r_*(SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + OP_l_5_c_6_r_*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) - OP_l_24_c_6_r_*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP_l_1_c_6_r_*SH_BETA(3)*SH_BETA(8) + OP_l_2_c_6_r_*SH_BETA(2)*SH_BETA(3)*SH_BETA(5)) + (SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9))*(OP_l_23_c_5_r_*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP_l_4_c_5_r_*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP_l_3_c_5_r_*(SH_BETA(2)^2*SH_BETA(5) + 1) + OP_l_6_c_5_r_*(SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + OP_l_5_c_5_r_*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) - OP_l_24_c_5_r_*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP_l_1_c_5_r_*SH_BETA(3)*SH_BETA(8) + OP_l_2_c_5_r_*SH_BETA(2)*SH_BETA(3)*SH_BETA(5)) - (SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9))*(OP_l_23_c_24_r_*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP_l_4_c_24_r_*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP_l_3_c_24_r_*(SH_BETA(2)^2*SH_BETA(5) + 1) + OP_l_6_c_24_r_*(SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + OP_l_5_c_24_r_*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) - OP_l_24_c_24_r_*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP_l_1_c_24_r_*SH_BETA(3)*SH_BETA(8) + OP_l_2_c_24_r_*SH_BETA(2)*SH_BETA(3)*SH_BETA(5)) - (SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6))*(OP_l_23_c_4_r_*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP_l_4_c_4_r_*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP_l_3_c_4_r_*(SH_BETA(2)^2*SH_BETA(5) + 1) + OP_l_6_c_4_r_*(SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + OP_l_5_c_4_r_*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) - OP_l_24_c_4_r_*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP_l_1_c_4_r_*SH_BETA(3)*SH_BETA(8) + OP_l_2_c_4_r_*SH_BETA(2)*SH_BETA(3)*SH_BETA(5)) + (SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6))*(OP_l_23_c_23_r_*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP_l_4_c_23_r_*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP_l_3_c_23_r_*(SH_BETA(2)^2*SH_BETA(5) + 1) + OP_l_6_c_23_r_*(SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + OP_l_5_c_23_r_*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) - OP_l_24_c_23_r_*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP_l_1_c_23_r_*SH_BETA(3)*SH_BETA(8) + OP_l_2_c_23_r_*SH_BETA(2)*SH_BETA(3)*SH_BETA(5)) - (SH_BETA(2)^2*SH_BETA(5) + 1)*(OP_l_23_c_3_r_*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP_l_4_c_3_r_*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP_l_3_c_3_r_*(SH_BETA(2)^2*SH_BETA(5) + 1) + OP_l_6_c_3_r_*(SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + OP_l_5_c_3_r_*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) - OP_l_24_c_3_r_*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP_l_1_c_3_r_*SH_BETA(3)*SH_BETA(8) + OP_l_2_c_3_r_*SH_BETA(2)*SH_BETA(3)*SH_BETA(5)) + SH_BETA(3)*SH_BETA(8)*(OP_l_23_c_1_r_*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP_l_4_c_1_r_*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP_l_3_c_1_r_*(SH_BETA(2)^2*SH_BETA(5) + 1) + OP_l_6_c_1_r_*(SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + OP_l_5_c_1_r_*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) - OP_l_24_c_1_r_*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP_l_1_c_1_r_*SH_BETA(3)*SH_BETA(8) + OP_l_2_c_1_r_*SH_BETA(2)*SH_BETA(3)*SH_BETA(5)) + SH_BETA(2)*SH_BETA(3)*SH_BETA(5)*(OP_l_23_c_2_r_*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP_l_4_c_2_r_*(SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6)) - OP_l_3_c_2_r_*(SH_BETA(2)^2*SH_BETA(5) + 1) + OP_l_6_c_2_r_*(SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3))) + OP_l_5_c_2_r_*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) - OP_l_24_c_2_r_*(SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP_l_1_c_2_r_*SH_BETA(3)*SH_BETA(8) + OP_l_2_c_2_r_*SH_BETA(2)*SH_BETA(3)*SH_BETA(5))); -SK_BETA(2) = SH_BETA(8)*(2*conj(q0)*conj(q1) + 2*conj(q2)*conj(q3)) + SH_BETA(2)*SH_BETA(5)*(2*conj(q0)*conj(q2) - 2*conj(q1)*conj(q3)); -SK_BETA(3) = SH_BETA(8)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(6); -SK_BETA(4) = SH_BETA(4) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9); -SK_BETA(5) = SH_BETA(2)^2*SH_BETA(5) + 1; -SK_BETA(6) = SH_BETA(3); +SK_BETA = zeros(8,1); +SK_BETA(1) = 1/(R_BETA - (SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8))*(OP_l_23_c_5_r_*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) - OP_l_5_c_5_r_*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) + OP_l_6_c_5_r_*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) - OP_l_24_c_5_r_*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) + OP_l_1_c_5_r_*(SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10)) + OP_l_2_c_5_r_*(SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12)) + OP_l_3_c_5_r_*(SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11)) - OP_l_4_c_5_r_*(SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP_l_7_c_5_r_*(SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3))) + (SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8))*(OP_l_23_c_23_r_*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) - OP_l_5_c_23_r_*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) + OP_l_6_c_23_r_*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) - OP_l_24_c_23_r_*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) + OP_l_1_c_23_r_*(SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10)) + OP_l_2_c_23_r_*(SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12)) + OP_l_3_c_23_r_*(SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11)) - OP_l_4_c_23_r_*(SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP_l_7_c_23_r_*(SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3))) + (SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2))*(OP_l_23_c_6_r_*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) - OP_l_5_c_6_r_*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) + OP_l_6_c_6_r_*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) - OP_l_24_c_6_r_*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) + OP_l_1_c_6_r_*(SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10)) + OP_l_2_c_6_r_*(SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12)) + OP_l_3_c_6_r_*(SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11)) - OP_l_4_c_6_r_*(SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP_l_7_c_6_r_*(SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3))) - (SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2))*(OP_l_23_c_24_r_*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) - OP_l_5_c_24_r_*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) + OP_l_6_c_24_r_*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) - OP_l_24_c_24_r_*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) + OP_l_1_c_24_r_*(SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10)) + OP_l_2_c_24_r_*(SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12)) + OP_l_3_c_24_r_*(SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11)) - OP_l_4_c_24_r_*(SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP_l_7_c_24_r_*(SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3))) + (SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10))*(OP_l_23_c_1_r_*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) - OP_l_5_c_1_r_*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) + OP_l_6_c_1_r_*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) - OP_l_24_c_1_r_*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) + OP_l_1_c_1_r_*(SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10)) + OP_l_2_c_1_r_*(SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12)) + OP_l_3_c_1_r_*(SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11)) - OP_l_4_c_1_r_*(SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP_l_7_c_1_r_*(SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3))) + (SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12))*(OP_l_23_c_2_r_*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) - OP_l_5_c_2_r_*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) + OP_l_6_c_2_r_*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) - OP_l_24_c_2_r_*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) + OP_l_1_c_2_r_*(SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10)) + OP_l_2_c_2_r_*(SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12)) + OP_l_3_c_2_r_*(SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11)) - OP_l_4_c_2_r_*(SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP_l_7_c_2_r_*(SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3))) + (SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11))*(OP_l_23_c_3_r_*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) - OP_l_5_c_3_r_*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) + OP_l_6_c_3_r_*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) - OP_l_24_c_3_r_*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) + OP_l_1_c_3_r_*(SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10)) + OP_l_2_c_3_r_*(SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12)) + OP_l_3_c_3_r_*(SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11)) - OP_l_4_c_3_r_*(SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP_l_7_c_3_r_*(SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3))) - (SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9))*(OP_l_23_c_4_r_*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) - OP_l_5_c_4_r_*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) + OP_l_6_c_4_r_*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) - OP_l_24_c_4_r_*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) + OP_l_1_c_4_r_*(SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10)) + OP_l_2_c_4_r_*(SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12)) + OP_l_3_c_4_r_*(SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11)) - OP_l_4_c_4_r_*(SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP_l_7_c_4_r_*(SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3))) + (SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3))*(OP_l_23_c_7_r_*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) - OP_l_5_c_7_r_*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) + OP_l_6_c_7_r_*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) - OP_l_24_c_7_r_*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) + OP_l_1_c_7_r_*(SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10)) + OP_l_2_c_7_r_*(SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12)) + OP_l_3_c_7_r_*(SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11)) - OP_l_4_c_7_r_*(SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP_l_7_c_7_r_*(SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3)))); +SK_BETA(2) = SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8); +SK_BETA(3) = SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2); +SK_BETA(4) = SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3); +SK_BETA(5) = SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12); +SK_BETA(6) = SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10); +SK_BETA(7) = SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11); +SK_BETA(8) = SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9); Kfusion = zeros(24,1); Kfusion = zeros(1,1); -Kfusion(1) = SK_BETA(1)*(OP_l_1_c_6_r_*SK_BETA(2) - OP_l_1_c_3_r_*SK_BETA(5) - OP_l_1_c_4_r_*SK_BETA(3) + OP_l_1_c_5_r_*SK_BETA(4) + OP_l_1_c_23_r_*SK_BETA(3) - OP_l_1_c_24_r_*SK_BETA(4) + OP_l_1_c_1_r_*SH_BETA(8)*SK_BETA(6) + OP_l_1_c_2_r_*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(2) = SK_BETA(1)*(OP_l_2_c_6_r_*SK_BETA(2) - OP_l_2_c_3_r_*SK_BETA(5) - OP_l_2_c_4_r_*SK_BETA(3) + OP_l_2_c_5_r_*SK_BETA(4) + OP_l_2_c_23_r_*SK_BETA(3) - OP_l_2_c_24_r_*SK_BETA(4) + OP_l_2_c_1_r_*SH_BETA(8)*SK_BETA(6) + OP_l_2_c_2_r_*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(3) = SK_BETA(1)*(OP_l_3_c_6_r_*SK_BETA(2) - OP_l_3_c_3_r_*SK_BETA(5) - OP_l_3_c_4_r_*SK_BETA(3) + OP_l_3_c_5_r_*SK_BETA(4) + OP_l_3_c_23_r_*SK_BETA(3) - OP_l_3_c_24_r_*SK_BETA(4) + OP_l_3_c_1_r_*SH_BETA(8)*SK_BETA(6) + OP_l_3_c_2_r_*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(4) = SK_BETA(1)*(OP_l_4_c_6_r_*SK_BETA(2) - OP_l_4_c_3_r_*SK_BETA(5) - OP_l_4_c_4_r_*SK_BETA(3) + OP_l_4_c_5_r_*SK_BETA(4) + OP_l_4_c_23_r_*SK_BETA(3) - OP_l_4_c_24_r_*SK_BETA(4) + OP_l_4_c_1_r_*SH_BETA(8)*SK_BETA(6) + OP_l_4_c_2_r_*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(5) = SK_BETA(1)*(OP_l_5_c_6_r_*SK_BETA(2) - OP_l_5_c_3_r_*SK_BETA(5) - OP_l_5_c_4_r_*SK_BETA(3) + OP_l_5_c_5_r_*SK_BETA(4) + OP_l_5_c_23_r_*SK_BETA(3) - OP_l_5_c_24_r_*SK_BETA(4) + OP_l_5_c_1_r_*SH_BETA(8)*SK_BETA(6) + OP_l_5_c_2_r_*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(6) = SK_BETA(1)*(OP_l_6_c_6_r_*SK_BETA(2) - OP_l_6_c_3_r_*SK_BETA(5) - OP_l_6_c_4_r_*SK_BETA(3) + OP_l_6_c_5_r_*SK_BETA(4) + OP_l_6_c_23_r_*SK_BETA(3) - OP_l_6_c_24_r_*SK_BETA(4) + OP_l_6_c_1_r_*SH_BETA(8)*SK_BETA(6) + OP_l_6_c_2_r_*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(7) = SK_BETA(1)*(OP_l_7_c_6_r_*SK_BETA(2) - OP_l_7_c_3_r_*SK_BETA(5) - OP_l_7_c_4_r_*SK_BETA(3) + OP_l_7_c_5_r_*SK_BETA(4) + OP_l_7_c_23_r_*SK_BETA(3) - OP_l_7_c_24_r_*SK_BETA(4) + OP_l_7_c_1_r_*SH_BETA(8)*SK_BETA(6) + OP_l_7_c_2_r_*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(8) = SK_BETA(1)*(OP_l_8_c_6_r_*SK_BETA(2) - OP_l_8_c_3_r_*SK_BETA(5) - OP_l_8_c_4_r_*SK_BETA(3) + OP_l_8_c_5_r_*SK_BETA(4) + OP_l_8_c_23_r_*SK_BETA(3) - OP_l_8_c_24_r_*SK_BETA(4) + OP_l_8_c_1_r_*SH_BETA(8)*SK_BETA(6) + OP_l_8_c_2_r_*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(9) = SK_BETA(1)*(OP_l_9_c_6_r_*SK_BETA(2) - OP_l_9_c_3_r_*SK_BETA(5) - OP_l_9_c_4_r_*SK_BETA(3) + OP_l_9_c_5_r_*SK_BETA(4) + OP_l_9_c_23_r_*SK_BETA(3) - OP_l_9_c_24_r_*SK_BETA(4) + OP_l_9_c_1_r_*SH_BETA(8)*SK_BETA(6) + OP_l_9_c_2_r_*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(10) = SK_BETA(1)*(OP_l_10_c_6_r_*SK_BETA(2) - OP_l_10_c_3_r_*SK_BETA(5) - OP_l_10_c_4_r_*SK_BETA(3) + OP_l_10_c_5_r_*SK_BETA(4) + OP_l_10_c_23_r_*SK_BETA(3) - OP_l_10_c_24_r_*SK_BETA(4) + OP_l_10_c_1_r_*SH_BETA(8)*SK_BETA(6) + OP_l_10_c_2_r_*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(11) = SK_BETA(1)*(OP_l_11_c_6_r_*SK_BETA(2) - OP_l_11_c_3_r_*SK_BETA(5) - OP_l_11_c_4_r_*SK_BETA(3) + OP_l_11_c_5_r_*SK_BETA(4) + OP_l_11_c_23_r_*SK_BETA(3) - OP_l_11_c_24_r_*SK_BETA(4) + OP_l_11_c_1_r_*SH_BETA(8)*SK_BETA(6) + OP_l_11_c_2_r_*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(12) = SK_BETA(1)*(OP_l_12_c_6_r_*SK_BETA(2) - OP_l_12_c_3_r_*SK_BETA(5) - OP_l_12_c_4_r_*SK_BETA(3) + OP_l_12_c_5_r_*SK_BETA(4) + OP_l_12_c_23_r_*SK_BETA(3) - OP_l_12_c_24_r_*SK_BETA(4) + OP_l_12_c_1_r_*SH_BETA(8)*SK_BETA(6) + OP_l_12_c_2_r_*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(13) = SK_BETA(1)*(OP_l_13_c_6_r_*SK_BETA(2) - OP_l_13_c_3_r_*SK_BETA(5) - OP_l_13_c_4_r_*SK_BETA(3) + OP_l_13_c_5_r_*SK_BETA(4) + OP_l_13_c_23_r_*SK_BETA(3) - OP_l_13_c_24_r_*SK_BETA(4) + OP_l_13_c_1_r_*SH_BETA(8)*SK_BETA(6) + OP_l_13_c_2_r_*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(14) = SK_BETA(1)*(OP_l_14_c_6_r_*SK_BETA(2) - OP_l_14_c_3_r_*SK_BETA(5) - OP_l_14_c_4_r_*SK_BETA(3) + OP_l_14_c_5_r_*SK_BETA(4) + OP_l_14_c_23_r_*SK_BETA(3) - OP_l_14_c_24_r_*SK_BETA(4) + OP_l_14_c_1_r_*SH_BETA(8)*SK_BETA(6) + OP_l_14_c_2_r_*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(15) = SK_BETA(1)*(OP_l_15_c_6_r_*SK_BETA(2) - OP_l_15_c_3_r_*SK_BETA(5) - OP_l_15_c_4_r_*SK_BETA(3) + OP_l_15_c_5_r_*SK_BETA(4) + OP_l_15_c_23_r_*SK_BETA(3) - OP_l_15_c_24_r_*SK_BETA(4) + OP_l_15_c_1_r_*SH_BETA(8)*SK_BETA(6) + OP_l_15_c_2_r_*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(16) = SK_BETA(1)*(OP_l_16_c_6_r_*SK_BETA(2) - OP_l_16_c_3_r_*SK_BETA(5) - OP_l_16_c_4_r_*SK_BETA(3) + OP_l_16_c_5_r_*SK_BETA(4) + OP_l_16_c_23_r_*SK_BETA(3) - OP_l_16_c_24_r_*SK_BETA(4) + OP_l_16_c_1_r_*SH_BETA(8)*SK_BETA(6) + OP_l_16_c_2_r_*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(17) = SK_BETA(1)*(OP_l_17_c_6_r_*SK_BETA(2) - OP_l_17_c_3_r_*SK_BETA(5) - OP_l_17_c_4_r_*SK_BETA(3) + OP_l_17_c_5_r_*SK_BETA(4) + OP_l_17_c_23_r_*SK_BETA(3) - OP_l_17_c_24_r_*SK_BETA(4) + OP_l_17_c_1_r_*SH_BETA(8)*SK_BETA(6) + OP_l_17_c_2_r_*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(18) = SK_BETA(1)*(OP_l_18_c_6_r_*SK_BETA(2) - OP_l_18_c_3_r_*SK_BETA(5) - OP_l_18_c_4_r_*SK_BETA(3) + OP_l_18_c_5_r_*SK_BETA(4) + OP_l_18_c_23_r_*SK_BETA(3) - OP_l_18_c_24_r_*SK_BETA(4) + OP_l_18_c_1_r_*SH_BETA(8)*SK_BETA(6) + OP_l_18_c_2_r_*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(19) = SK_BETA(1)*(OP_l_19_c_6_r_*SK_BETA(2) - OP_l_19_c_3_r_*SK_BETA(5) - OP_l_19_c_4_r_*SK_BETA(3) + OP_l_19_c_5_r_*SK_BETA(4) + OP_l_19_c_23_r_*SK_BETA(3) - OP_l_19_c_24_r_*SK_BETA(4) + OP_l_19_c_1_r_*SH_BETA(8)*SK_BETA(6) + OP_l_19_c_2_r_*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(20) = SK_BETA(1)*(OP_l_20_c_6_r_*SK_BETA(2) - OP_l_20_c_3_r_*SK_BETA(5) - OP_l_20_c_4_r_*SK_BETA(3) + OP_l_20_c_5_r_*SK_BETA(4) + OP_l_20_c_23_r_*SK_BETA(3) - OP_l_20_c_24_r_*SK_BETA(4) + OP_l_20_c_1_r_*SH_BETA(8)*SK_BETA(6) + OP_l_20_c_2_r_*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(21) = SK_BETA(1)*(OP_l_21_c_6_r_*SK_BETA(2) - OP_l_21_c_3_r_*SK_BETA(5) - OP_l_21_c_4_r_*SK_BETA(3) + OP_l_21_c_5_r_*SK_BETA(4) + OP_l_21_c_23_r_*SK_BETA(3) - OP_l_21_c_24_r_*SK_BETA(4) + OP_l_21_c_1_r_*SH_BETA(8)*SK_BETA(6) + OP_l_21_c_2_r_*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(22) = SK_BETA(1)*(OP_l_22_c_6_r_*SK_BETA(2) - OP_l_22_c_3_r_*SK_BETA(5) - OP_l_22_c_4_r_*SK_BETA(3) + OP_l_22_c_5_r_*SK_BETA(4) + OP_l_22_c_23_r_*SK_BETA(3) - OP_l_22_c_24_r_*SK_BETA(4) + OP_l_22_c_1_r_*SH_BETA(8)*SK_BETA(6) + OP_l_22_c_2_r_*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(23) = SK_BETA(1)*(OP_l_23_c_6_r_*SK_BETA(2) - OP_l_23_c_3_r_*SK_BETA(5) - OP_l_23_c_4_r_*SK_BETA(3) + OP_l_23_c_5_r_*SK_BETA(4) + OP_l_23_c_23_r_*SK_BETA(3) - OP_l_23_c_24_r_*SK_BETA(4) + OP_l_23_c_1_r_*SH_BETA(8)*SK_BETA(6) + OP_l_23_c_2_r_*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); -Kfusion(24) = SK_BETA(1)*(OP_l_24_c_6_r_*SK_BETA(2) - OP_l_24_c_3_r_*SK_BETA(5) - OP_l_24_c_4_r_*SK_BETA(3) + OP_l_24_c_5_r_*SK_BETA(4) + OP_l_24_c_23_r_*SK_BETA(3) - OP_l_24_c_24_r_*SK_BETA(4) + OP_l_24_c_1_r_*SH_BETA(8)*SK_BETA(6) + OP_l_24_c_2_r_*SH_BETA(2)*SH_BETA(5)*SK_BETA(6)); +Kfusion(1) = SK_BETA(1)*(OP_l_1_c_1_r_*SK_BETA(6) + OP_l_1_c_2_r_*SK_BETA(5) - OP_l_1_c_5_r_*SK_BETA(2) + OP_l_1_c_6_r_*SK_BETA(3) + OP_l_1_c_3_r_*SK_BETA(7) + OP_l_1_c_7_r_*SK_BETA(4) - OP_l_1_c_4_r_*SK_BETA(8) + OP_l_1_c_23_r_*SK_BETA(2) - OP_l_1_c_24_r_*SK_BETA(3)); +Kfusion(2) = SK_BETA(1)*(OP_l_2_c_1_r_*SK_BETA(6) + OP_l_2_c_2_r_*SK_BETA(5) - OP_l_2_c_5_r_*SK_BETA(2) + OP_l_2_c_6_r_*SK_BETA(3) + OP_l_2_c_3_r_*SK_BETA(7) + OP_l_2_c_7_r_*SK_BETA(4) - OP_l_2_c_4_r_*SK_BETA(8) + OP_l_2_c_23_r_*SK_BETA(2) - OP_l_2_c_24_r_*SK_BETA(3)); +Kfusion(3) = SK_BETA(1)*(OP_l_3_c_1_r_*SK_BETA(6) + OP_l_3_c_2_r_*SK_BETA(5) - OP_l_3_c_5_r_*SK_BETA(2) + OP_l_3_c_6_r_*SK_BETA(3) + OP_l_3_c_3_r_*SK_BETA(7) + OP_l_3_c_7_r_*SK_BETA(4) - OP_l_3_c_4_r_*SK_BETA(8) + OP_l_3_c_23_r_*SK_BETA(2) - OP_l_3_c_24_r_*SK_BETA(3)); +Kfusion(4) = SK_BETA(1)*(OP_l_4_c_1_r_*SK_BETA(6) + OP_l_4_c_2_r_*SK_BETA(5) - OP_l_4_c_5_r_*SK_BETA(2) + OP_l_4_c_6_r_*SK_BETA(3) + OP_l_4_c_3_r_*SK_BETA(7) + OP_l_4_c_7_r_*SK_BETA(4) - OP_l_4_c_4_r_*SK_BETA(8) + OP_l_4_c_23_r_*SK_BETA(2) - OP_l_4_c_24_r_*SK_BETA(3)); +Kfusion(5) = SK_BETA(1)*(OP_l_5_c_1_r_*SK_BETA(6) + OP_l_5_c_2_r_*SK_BETA(5) - OP_l_5_c_5_r_*SK_BETA(2) + OP_l_5_c_6_r_*SK_BETA(3) + OP_l_5_c_3_r_*SK_BETA(7) + OP_l_5_c_7_r_*SK_BETA(4) - OP_l_5_c_4_r_*SK_BETA(8) + OP_l_5_c_23_r_*SK_BETA(2) - OP_l_5_c_24_r_*SK_BETA(3)); +Kfusion(6) = SK_BETA(1)*(OP_l_6_c_1_r_*SK_BETA(6) + OP_l_6_c_2_r_*SK_BETA(5) - OP_l_6_c_5_r_*SK_BETA(2) + OP_l_6_c_6_r_*SK_BETA(3) + OP_l_6_c_3_r_*SK_BETA(7) + OP_l_6_c_7_r_*SK_BETA(4) - OP_l_6_c_4_r_*SK_BETA(8) + OP_l_6_c_23_r_*SK_BETA(2) - OP_l_6_c_24_r_*SK_BETA(3)); +Kfusion(7) = SK_BETA(1)*(OP_l_7_c_1_r_*SK_BETA(6) + OP_l_7_c_2_r_*SK_BETA(5) - OP_l_7_c_5_r_*SK_BETA(2) + OP_l_7_c_6_r_*SK_BETA(3) + OP_l_7_c_3_r_*SK_BETA(7) + OP_l_7_c_7_r_*SK_BETA(4) - OP_l_7_c_4_r_*SK_BETA(8) + OP_l_7_c_23_r_*SK_BETA(2) - OP_l_7_c_24_r_*SK_BETA(3)); +Kfusion(8) = SK_BETA(1)*(OP_l_8_c_1_r_*SK_BETA(6) + OP_l_8_c_2_r_*SK_BETA(5) - OP_l_8_c_5_r_*SK_BETA(2) + OP_l_8_c_6_r_*SK_BETA(3) + OP_l_8_c_3_r_*SK_BETA(7) + OP_l_8_c_7_r_*SK_BETA(4) - OP_l_8_c_4_r_*SK_BETA(8) + OP_l_8_c_23_r_*SK_BETA(2) - OP_l_8_c_24_r_*SK_BETA(3)); +Kfusion(9) = SK_BETA(1)*(OP_l_9_c_1_r_*SK_BETA(6) + OP_l_9_c_2_r_*SK_BETA(5) - OP_l_9_c_5_r_*SK_BETA(2) + OP_l_9_c_6_r_*SK_BETA(3) + OP_l_9_c_3_r_*SK_BETA(7) + OP_l_9_c_7_r_*SK_BETA(4) - OP_l_9_c_4_r_*SK_BETA(8) + OP_l_9_c_23_r_*SK_BETA(2) - OP_l_9_c_24_r_*SK_BETA(3)); +Kfusion(10) = SK_BETA(1)*(OP_l_10_c_1_r_*SK_BETA(6) + OP_l_10_c_2_r_*SK_BETA(5) - OP_l_10_c_5_r_*SK_BETA(2) + OP_l_10_c_6_r_*SK_BETA(3) + OP_l_10_c_3_r_*SK_BETA(7) + OP_l_10_c_7_r_*SK_BETA(4) - OP_l_10_c_4_r_*SK_BETA(8) + OP_l_10_c_23_r_*SK_BETA(2) - OP_l_10_c_24_r_*SK_BETA(3)); +Kfusion(11) = SK_BETA(1)*(OP_l_11_c_1_r_*SK_BETA(6) + OP_l_11_c_2_r_*SK_BETA(5) - OP_l_11_c_5_r_*SK_BETA(2) + OP_l_11_c_6_r_*SK_BETA(3) + OP_l_11_c_3_r_*SK_BETA(7) + OP_l_11_c_7_r_*SK_BETA(4) - OP_l_11_c_4_r_*SK_BETA(8) + OP_l_11_c_23_r_*SK_BETA(2) - OP_l_11_c_24_r_*SK_BETA(3)); +Kfusion(12) = SK_BETA(1)*(OP_l_12_c_1_r_*SK_BETA(6) + OP_l_12_c_2_r_*SK_BETA(5) - OP_l_12_c_5_r_*SK_BETA(2) + OP_l_12_c_6_r_*SK_BETA(3) + OP_l_12_c_3_r_*SK_BETA(7) + OP_l_12_c_7_r_*SK_BETA(4) - OP_l_12_c_4_r_*SK_BETA(8) + OP_l_12_c_23_r_*SK_BETA(2) - OP_l_12_c_24_r_*SK_BETA(3)); +Kfusion(13) = SK_BETA(1)*(OP_l_13_c_1_r_*SK_BETA(6) + OP_l_13_c_2_r_*SK_BETA(5) - OP_l_13_c_5_r_*SK_BETA(2) + OP_l_13_c_6_r_*SK_BETA(3) + OP_l_13_c_3_r_*SK_BETA(7) + OP_l_13_c_7_r_*SK_BETA(4) - OP_l_13_c_4_r_*SK_BETA(8) + OP_l_13_c_23_r_*SK_BETA(2) - OP_l_13_c_24_r_*SK_BETA(3)); +Kfusion(14) = SK_BETA(1)*(OP_l_14_c_1_r_*SK_BETA(6) + OP_l_14_c_2_r_*SK_BETA(5) - OP_l_14_c_5_r_*SK_BETA(2) + OP_l_14_c_6_r_*SK_BETA(3) + OP_l_14_c_3_r_*SK_BETA(7) + OP_l_14_c_7_r_*SK_BETA(4) - OP_l_14_c_4_r_*SK_BETA(8) + OP_l_14_c_23_r_*SK_BETA(2) - OP_l_14_c_24_r_*SK_BETA(3)); +Kfusion(15) = SK_BETA(1)*(OP_l_15_c_1_r_*SK_BETA(6) + OP_l_15_c_2_r_*SK_BETA(5) - OP_l_15_c_5_r_*SK_BETA(2) + OP_l_15_c_6_r_*SK_BETA(3) + OP_l_15_c_3_r_*SK_BETA(7) + OP_l_15_c_7_r_*SK_BETA(4) - OP_l_15_c_4_r_*SK_BETA(8) + OP_l_15_c_23_r_*SK_BETA(2) - OP_l_15_c_24_r_*SK_BETA(3)); +Kfusion(16) = SK_BETA(1)*(OP_l_16_c_1_r_*SK_BETA(6) + OP_l_16_c_2_r_*SK_BETA(5) - OP_l_16_c_5_r_*SK_BETA(2) + OP_l_16_c_6_r_*SK_BETA(3) + OP_l_16_c_3_r_*SK_BETA(7) + OP_l_16_c_7_r_*SK_BETA(4) - OP_l_16_c_4_r_*SK_BETA(8) + OP_l_16_c_23_r_*SK_BETA(2) - OP_l_16_c_24_r_*SK_BETA(3)); +Kfusion(17) = SK_BETA(1)*(OP_l_17_c_1_r_*SK_BETA(6) + OP_l_17_c_2_r_*SK_BETA(5) - OP_l_17_c_5_r_*SK_BETA(2) + OP_l_17_c_6_r_*SK_BETA(3) + OP_l_17_c_3_r_*SK_BETA(7) + OP_l_17_c_7_r_*SK_BETA(4) - OP_l_17_c_4_r_*SK_BETA(8) + OP_l_17_c_23_r_*SK_BETA(2) - OP_l_17_c_24_r_*SK_BETA(3)); +Kfusion(18) = SK_BETA(1)*(OP_l_18_c_1_r_*SK_BETA(6) + OP_l_18_c_2_r_*SK_BETA(5) - OP_l_18_c_5_r_*SK_BETA(2) + OP_l_18_c_6_r_*SK_BETA(3) + OP_l_18_c_3_r_*SK_BETA(7) + OP_l_18_c_7_r_*SK_BETA(4) - OP_l_18_c_4_r_*SK_BETA(8) + OP_l_18_c_23_r_*SK_BETA(2) - OP_l_18_c_24_r_*SK_BETA(3)); +Kfusion(19) = SK_BETA(1)*(OP_l_19_c_1_r_*SK_BETA(6) + OP_l_19_c_2_r_*SK_BETA(5) - OP_l_19_c_5_r_*SK_BETA(2) + OP_l_19_c_6_r_*SK_BETA(3) + OP_l_19_c_3_r_*SK_BETA(7) + OP_l_19_c_7_r_*SK_BETA(4) - OP_l_19_c_4_r_*SK_BETA(8) + OP_l_19_c_23_r_*SK_BETA(2) - OP_l_19_c_24_r_*SK_BETA(3)); +Kfusion(20) = SK_BETA(1)*(OP_l_20_c_1_r_*SK_BETA(6) + OP_l_20_c_2_r_*SK_BETA(5) - OP_l_20_c_5_r_*SK_BETA(2) + OP_l_20_c_6_r_*SK_BETA(3) + OP_l_20_c_3_r_*SK_BETA(7) + OP_l_20_c_7_r_*SK_BETA(4) - OP_l_20_c_4_r_*SK_BETA(8) + OP_l_20_c_23_r_*SK_BETA(2) - OP_l_20_c_24_r_*SK_BETA(3)); +Kfusion(21) = SK_BETA(1)*(OP_l_21_c_1_r_*SK_BETA(6) + OP_l_21_c_2_r_*SK_BETA(5) - OP_l_21_c_5_r_*SK_BETA(2) + OP_l_21_c_6_r_*SK_BETA(3) + OP_l_21_c_3_r_*SK_BETA(7) + OP_l_21_c_7_r_*SK_BETA(4) - OP_l_21_c_4_r_*SK_BETA(8) + OP_l_21_c_23_r_*SK_BETA(2) - OP_l_21_c_24_r_*SK_BETA(3)); +Kfusion(22) = SK_BETA(1)*(OP_l_22_c_1_r_*SK_BETA(6) + OP_l_22_c_2_r_*SK_BETA(5) - OP_l_22_c_5_r_*SK_BETA(2) + OP_l_22_c_6_r_*SK_BETA(3) + OP_l_22_c_3_r_*SK_BETA(7) + OP_l_22_c_7_r_*SK_BETA(4) - OP_l_22_c_4_r_*SK_BETA(8) + OP_l_22_c_23_r_*SK_BETA(2) - OP_l_22_c_24_r_*SK_BETA(3)); +Kfusion(23) = SK_BETA(1)*(OP_l_23_c_1_r_*SK_BETA(6) + OP_l_23_c_2_r_*SK_BETA(5) - OP_l_23_c_5_r_*SK_BETA(2) + OP_l_23_c_6_r_*SK_BETA(3) + OP_l_23_c_3_r_*SK_BETA(7) + OP_l_23_c_7_r_*SK_BETA(4) - OP_l_23_c_4_r_*SK_BETA(8) + OP_l_23_c_23_r_*SK_BETA(2) - OP_l_23_c_24_r_*SK_BETA(3)); +Kfusion(24) = SK_BETA(1)*(OP_l_24_c_1_r_*SK_BETA(6) + OP_l_24_c_2_r_*SK_BETA(5) - OP_l_24_c_5_r_*SK_BETA(2) + OP_l_24_c_6_r_*SK_BETA(3) + OP_l_24_c_3_r_*SK_BETA(7) + OP_l_24_c_7_r_*SK_BETA(4) - OP_l_24_c_4_r_*SK_BETA(8) + OP_l_24_c_23_r_*SK_BETA(2) - OP_l_24_c_24_r_*SK_BETA(3)); SH_MAG = zeros(9,1); -SH_MAG(1) = q0^2 - q1^2 + q2^2 - q3^2; -SH_MAG(2) = q0^2 + q1^2 - q2^2 - q3^2; -SH_MAG(3) = q0^2 - q1^2 - q2^2 + q3^2; -SH_MAG(4) = 2*q0*q1 + 2*q2*q3; -SH_MAG(5) = 2*q0*q3 + 2*q1*q2; -SH_MAG(6) = 2*q0*q2 + 2*q1*q3; -SH_MAG(7) = magE*(2*q0*q1 - 2*q2*q3); -SH_MAG(8) = 2*q1*q3 - 2*q0*q2; -SH_MAG(9) = 2*q0*q3; +SH_MAG(1) = 2*magD*q3 + 2*magE*q2 + 2*magN*q1; +SH_MAG(2) = 2*magD*q0 - 2*magE*q1 + 2*magN*q2; +SH_MAG(3) = 2*magD*q1 + 2*magE*q0 - 2*magN*q3; +SH_MAG(4) = q3^2; +SH_MAG(5) = q2^2; +SH_MAG(6) = q1^2; +SH_MAG(7) = q0^2; +SH_MAG(8) = 2*magN*q0; +SH_MAG(9) = 2*magE*q3; H_MAG = zeros(1,24); -H_MAG(2) = SH_MAG(7) - magD*SH_MAG(3) - magN*SH_MAG(6); -H_MAG(3) = magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2); -H_MAG(17) = SH_MAG(2); -H_MAG(18) = SH_MAG(5); -H_MAG(19) = SH_MAG(8); +H_MAG(1) = SH_MAG(8) + SH_MAG(9) - 2*magD*q2; +H_MAG(2) = SH_MAG(1); +H_MAG(3) = -SH_MAG(2); +H_MAG(4) = SH_MAG(3); +H_MAG(17) = SH_MAG(6) - SH_MAG(5) - SH_MAG(4) + SH_MAG(7); +H_MAG(18) = 2*q0*q3 + 2*q1*q2; +H_MAG(19) = 2*q1*q3 - 2*q0*q2; H_MAG(20) = 1; -SK_MX = zeros(4,1); -SK_MX(1) = 1/(OP_l_20_c_20_r_ + R_MAG - OP_l_2_c_20_r_*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP_l_17_c_20_r_*SH_MAG(2) + OP_l_18_c_20_r_*SH_MAG(5) + OP_l_19_c_20_r_*SH_MAG(8) + OP_l_3_c_20_r_*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2)) - (magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6))*(OP_l_20_c_2_r_ - OP_l_2_c_2_r_*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP_l_17_c_2_r_*SH_MAG(2) + OP_l_18_c_2_r_*SH_MAG(5) + OP_l_19_c_2_r_*SH_MAG(8) + OP_l_3_c_2_r_*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2))) + SH_MAG(2)*(OP_l_20_c_17_r_ - OP_l_2_c_17_r_*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP_l_17_c_17_r_*SH_MAG(2) + OP_l_18_c_17_r_*SH_MAG(5) + OP_l_19_c_17_r_*SH_MAG(8) + OP_l_3_c_17_r_*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2))) + SH_MAG(5)*(OP_l_20_c_18_r_ - OP_l_2_c_18_r_*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP_l_17_c_18_r_*SH_MAG(2) + OP_l_18_c_18_r_*SH_MAG(5) + OP_l_19_c_18_r_*SH_MAG(8) + OP_l_3_c_18_r_*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2))) + SH_MAG(8)*(OP_l_20_c_19_r_ - OP_l_2_c_19_r_*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP_l_17_c_19_r_*SH_MAG(2) + OP_l_18_c_19_r_*SH_MAG(5) + OP_l_19_c_19_r_*SH_MAG(8) + OP_l_3_c_19_r_*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2))) + (magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2))*(OP_l_20_c_3_r_ - OP_l_2_c_3_r_*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP_l_17_c_3_r_*SH_MAG(2) + OP_l_18_c_3_r_*SH_MAG(5) + OP_l_19_c_3_r_*SH_MAG(8) + OP_l_3_c_3_r_*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2)))); -SK_MX(2) = magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2); -SK_MX(3) = magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6); -SK_MX(4) = SH_MAG(8); +SK_MX = zeros(5,1); +SK_MX(1) = 1/(OP_l_20_c_20_r_ + R_MAG + OP_l_2_c_20_r_*SH_MAG(1) - OP_l_3_c_20_r_*SH_MAG(2) + OP_l_4_c_20_r_*SH_MAG(3) - OP_l_17_c_20_r_*(SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7)) + (2*q0*q3 + 2*q1*q2)*(OP_l_20_c_18_r_ + OP_l_2_c_18_r_*SH_MAG(1) - OP_l_3_c_18_r_*SH_MAG(2) + OP_l_4_c_18_r_*SH_MAG(3) - OP_l_17_c_18_r_*(SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7)) + OP_l_18_c_18_r_*(2*q0*q3 + 2*q1*q2) - OP_l_19_c_18_r_*(2*q0*q2 - 2*q1*q3) + OP_l_1_c_18_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) - (2*q0*q2 - 2*q1*q3)*(OP_l_20_c_19_r_ + OP_l_2_c_19_r_*SH_MAG(1) - OP_l_3_c_19_r_*SH_MAG(2) + OP_l_4_c_19_r_*SH_MAG(3) - OP_l_17_c_19_r_*(SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7)) + OP_l_18_c_19_r_*(2*q0*q3 + 2*q1*q2) - OP_l_19_c_19_r_*(2*q0*q2 - 2*q1*q3) + OP_l_1_c_19_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + (SH_MAG(8) + SH_MAG(9) - 2*magD*q2)*(OP_l_20_c_1_r_ + OP_l_2_c_1_r_*SH_MAG(1) - OP_l_3_c_1_r_*SH_MAG(2) + OP_l_4_c_1_r_*SH_MAG(3) - OP_l_17_c_1_r_*(SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7)) + OP_l_18_c_1_r_*(2*q0*q3 + 2*q1*q2) - OP_l_19_c_1_r_*(2*q0*q2 - 2*q1*q3) + OP_l_1_c_1_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + OP_l_18_c_20_r_*(2*q0*q3 + 2*q1*q2) - OP_l_19_c_20_r_*(2*q0*q2 - 2*q1*q3) + SH_MAG(1)*(OP_l_20_c_2_r_ + OP_l_2_c_2_r_*SH_MAG(1) - OP_l_3_c_2_r_*SH_MAG(2) + OP_l_4_c_2_r_*SH_MAG(3) - OP_l_17_c_2_r_*(SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7)) + OP_l_18_c_2_r_*(2*q0*q3 + 2*q1*q2) - OP_l_19_c_2_r_*(2*q0*q2 - 2*q1*q3) + OP_l_1_c_2_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) - SH_MAG(2)*(OP_l_20_c_3_r_ + OP_l_2_c_3_r_*SH_MAG(1) - OP_l_3_c_3_r_*SH_MAG(2) + OP_l_4_c_3_r_*SH_MAG(3) - OP_l_17_c_3_r_*(SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7)) + OP_l_18_c_3_r_*(2*q0*q3 + 2*q1*q2) - OP_l_19_c_3_r_*(2*q0*q2 - 2*q1*q3) + OP_l_1_c_3_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + SH_MAG(3)*(OP_l_20_c_4_r_ + OP_l_2_c_4_r_*SH_MAG(1) - OP_l_3_c_4_r_*SH_MAG(2) + OP_l_4_c_4_r_*SH_MAG(3) - OP_l_17_c_4_r_*(SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7)) + OP_l_18_c_4_r_*(2*q0*q3 + 2*q1*q2) - OP_l_19_c_4_r_*(2*q0*q2 - 2*q1*q3) + OP_l_1_c_4_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) - (SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7))*(OP_l_20_c_17_r_ + OP_l_2_c_17_r_*SH_MAG(1) - OP_l_3_c_17_r_*SH_MAG(2) + OP_l_4_c_17_r_*SH_MAG(3) - OP_l_17_c_17_r_*(SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7)) + OP_l_18_c_17_r_*(2*q0*q3 + 2*q1*q2) - OP_l_19_c_17_r_*(2*q0*q2 - 2*q1*q3) + OP_l_1_c_17_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + OP_l_1_c_20_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)); +SK_MX(2) = SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7); +SK_MX(3) = SH_MAG(8) + SH_MAG(9) - 2*magD*q2; +SK_MX(4) = 2*q0*q2 - 2*q1*q3; +SK_MX(5) = 2*q0*q3 + 2*q1*q2; Kfusion = zeros(24,1); Kfusion = zeros(1,1); -Kfusion(1) = SK_MX(1)*(OP_l_1_c_20_r_ + OP_l_1_c_17_r_*SH_MAG(2) + OP_l_1_c_18_r_*SH_MAG(5) - OP_l_1_c_2_r_*SK_MX(3) + OP_l_1_c_3_r_*SK_MX(2) + OP_l_1_c_19_r_*SK_MX(4)); -Kfusion(2) = SK_MX(1)*(OP_l_2_c_20_r_ + OP_l_2_c_17_r_*SH_MAG(2) + OP_l_2_c_18_r_*SH_MAG(5) - OP_l_2_c_2_r_*SK_MX(3) + OP_l_2_c_3_r_*SK_MX(2) + OP_l_2_c_19_r_*SK_MX(4)); -Kfusion(3) = SK_MX(1)*(OP_l_3_c_20_r_ + OP_l_3_c_17_r_*SH_MAG(2) + OP_l_3_c_18_r_*SH_MAG(5) - OP_l_3_c_2_r_*SK_MX(3) + OP_l_3_c_3_r_*SK_MX(2) + OP_l_3_c_19_r_*SK_MX(4)); -Kfusion(4) = SK_MX(1)*(OP_l_4_c_20_r_ + OP_l_4_c_17_r_*SH_MAG(2) + OP_l_4_c_18_r_*SH_MAG(5) - OP_l_4_c_2_r_*SK_MX(3) + OP_l_4_c_3_r_*SK_MX(2) + OP_l_4_c_19_r_*SK_MX(4)); -Kfusion(5) = SK_MX(1)*(OP_l_5_c_20_r_ + OP_l_5_c_17_r_*SH_MAG(2) + OP_l_5_c_18_r_*SH_MAG(5) - OP_l_5_c_2_r_*SK_MX(3) + OP_l_5_c_3_r_*SK_MX(2) + OP_l_5_c_19_r_*SK_MX(4)); -Kfusion(6) = SK_MX(1)*(OP_l_6_c_20_r_ + OP_l_6_c_17_r_*SH_MAG(2) + OP_l_6_c_18_r_*SH_MAG(5) - OP_l_6_c_2_r_*SK_MX(3) + OP_l_6_c_3_r_*SK_MX(2) + OP_l_6_c_19_r_*SK_MX(4)); -Kfusion(7) = SK_MX(1)*(OP_l_7_c_20_r_ + OP_l_7_c_17_r_*SH_MAG(2) + OP_l_7_c_18_r_*SH_MAG(5) - OP_l_7_c_2_r_*SK_MX(3) + OP_l_7_c_3_r_*SK_MX(2) + OP_l_7_c_19_r_*SK_MX(4)); -Kfusion(8) = SK_MX(1)*(OP_l_8_c_20_r_ + OP_l_8_c_17_r_*SH_MAG(2) + OP_l_8_c_18_r_*SH_MAG(5) - OP_l_8_c_2_r_*SK_MX(3) + OP_l_8_c_3_r_*SK_MX(2) + OP_l_8_c_19_r_*SK_MX(4)); -Kfusion(9) = SK_MX(1)*(OP_l_9_c_20_r_ + OP_l_9_c_17_r_*SH_MAG(2) + OP_l_9_c_18_r_*SH_MAG(5) - OP_l_9_c_2_r_*SK_MX(3) + OP_l_9_c_3_r_*SK_MX(2) + OP_l_9_c_19_r_*SK_MX(4)); -Kfusion(10) = SK_MX(1)*(OP_l_10_c_20_r_ + OP_l_10_c_17_r_*SH_MAG(2) + OP_l_10_c_18_r_*SH_MAG(5) - OP_l_10_c_2_r_*SK_MX(3) + OP_l_10_c_3_r_*SK_MX(2) + OP_l_10_c_19_r_*SK_MX(4)); -Kfusion(11) = SK_MX(1)*(OP_l_11_c_20_r_ + OP_l_11_c_17_r_*SH_MAG(2) + OP_l_11_c_18_r_*SH_MAG(5) - OP_l_11_c_2_r_*SK_MX(3) + OP_l_11_c_3_r_*SK_MX(2) + OP_l_11_c_19_r_*SK_MX(4)); -Kfusion(12) = SK_MX(1)*(OP_l_12_c_20_r_ + OP_l_12_c_17_r_*SH_MAG(2) + OP_l_12_c_18_r_*SH_MAG(5) - OP_l_12_c_2_r_*SK_MX(3) + OP_l_12_c_3_r_*SK_MX(2) + OP_l_12_c_19_r_*SK_MX(4)); -Kfusion(13) = SK_MX(1)*(OP_l_13_c_20_r_ + OP_l_13_c_17_r_*SH_MAG(2) + OP_l_13_c_18_r_*SH_MAG(5) - OP_l_13_c_2_r_*SK_MX(3) + OP_l_13_c_3_r_*SK_MX(2) + OP_l_13_c_19_r_*SK_MX(4)); -Kfusion(14) = SK_MX(1)*(OP_l_14_c_20_r_ + OP_l_14_c_17_r_*SH_MAG(2) + OP_l_14_c_18_r_*SH_MAG(5) - OP_l_14_c_2_r_*SK_MX(3) + OP_l_14_c_3_r_*SK_MX(2) + OP_l_14_c_19_r_*SK_MX(4)); -Kfusion(15) = SK_MX(1)*(OP_l_15_c_20_r_ + OP_l_15_c_17_r_*SH_MAG(2) + OP_l_15_c_18_r_*SH_MAG(5) - OP_l_15_c_2_r_*SK_MX(3) + OP_l_15_c_3_r_*SK_MX(2) + OP_l_15_c_19_r_*SK_MX(4)); -Kfusion(16) = SK_MX(1)*(OP_l_16_c_20_r_ + OP_l_16_c_17_r_*SH_MAG(2) + OP_l_16_c_18_r_*SH_MAG(5) - OP_l_16_c_2_r_*SK_MX(3) + OP_l_16_c_3_r_*SK_MX(2) + OP_l_16_c_19_r_*SK_MX(4)); -Kfusion(17) = SK_MX(1)*(OP_l_17_c_20_r_ + OP_l_17_c_17_r_*SH_MAG(2) + OP_l_17_c_18_r_*SH_MAG(5) - OP_l_17_c_2_r_*SK_MX(3) + OP_l_17_c_3_r_*SK_MX(2) + OP_l_17_c_19_r_*SK_MX(4)); -Kfusion(18) = SK_MX(1)*(OP_l_18_c_20_r_ + OP_l_18_c_17_r_*SH_MAG(2) + OP_l_18_c_18_r_*SH_MAG(5) - OP_l_18_c_2_r_*SK_MX(3) + OP_l_18_c_3_r_*SK_MX(2) + OP_l_18_c_19_r_*SK_MX(4)); -Kfusion(19) = SK_MX(1)*(OP_l_19_c_20_r_ + OP_l_19_c_17_r_*SH_MAG(2) + OP_l_19_c_18_r_*SH_MAG(5) - OP_l_19_c_2_r_*SK_MX(3) + OP_l_19_c_3_r_*SK_MX(2) + OP_l_19_c_19_r_*SK_MX(4)); -Kfusion(20) = SK_MX(1)*(OP_l_20_c_20_r_ + OP_l_20_c_17_r_*SH_MAG(2) + OP_l_20_c_18_r_*SH_MAG(5) - OP_l_20_c_2_r_*SK_MX(3) + OP_l_20_c_3_r_*SK_MX(2) + OP_l_20_c_19_r_*SK_MX(4)); -Kfusion(21) = SK_MX(1)*(OP_l_21_c_20_r_ + OP_l_21_c_17_r_*SH_MAG(2) + OP_l_21_c_18_r_*SH_MAG(5) - OP_l_21_c_2_r_*SK_MX(3) + OP_l_21_c_3_r_*SK_MX(2) + OP_l_21_c_19_r_*SK_MX(4)); -Kfusion(22) = SK_MX(1)*(OP_l_22_c_20_r_ + OP_l_22_c_17_r_*SH_MAG(2) + OP_l_22_c_18_r_*SH_MAG(5) - OP_l_22_c_2_r_*SK_MX(3) + OP_l_22_c_3_r_*SK_MX(2) + OP_l_22_c_19_r_*SK_MX(4)); -Kfusion(23) = SK_MX(1)*(OP_l_23_c_20_r_ + OP_l_23_c_17_r_*SH_MAG(2) + OP_l_23_c_18_r_*SH_MAG(5) - OP_l_23_c_2_r_*SK_MX(3) + OP_l_23_c_3_r_*SK_MX(2) + OP_l_23_c_19_r_*SK_MX(4)); -Kfusion(24) = SK_MX(1)*(OP_l_24_c_20_r_ + OP_l_24_c_17_r_*SH_MAG(2) + OP_l_24_c_18_r_*SH_MAG(5) - OP_l_24_c_2_r_*SK_MX(3) + OP_l_24_c_3_r_*SK_MX(2) + OP_l_24_c_19_r_*SK_MX(4)); +Kfusion(1) = SK_MX(1)*(OP_l_1_c_20_r_ + OP_l_1_c_2_r_*SH_MAG(1) - OP_l_1_c_3_r_*SH_MAG(2) + OP_l_1_c_4_r_*SH_MAG(3) + OP_l_1_c_1_r_*SK_MX(3) - OP_l_1_c_17_r_*SK_MX(2) + OP_l_1_c_18_r_*SK_MX(5) - OP_l_1_c_19_r_*SK_MX(4)); +Kfusion(2) = SK_MX(1)*(OP_l_2_c_20_r_ + OP_l_2_c_2_r_*SH_MAG(1) - OP_l_2_c_3_r_*SH_MAG(2) + OP_l_2_c_4_r_*SH_MAG(3) + OP_l_2_c_1_r_*SK_MX(3) - OP_l_2_c_17_r_*SK_MX(2) + OP_l_2_c_18_r_*SK_MX(5) - OP_l_2_c_19_r_*SK_MX(4)); +Kfusion(3) = SK_MX(1)*(OP_l_3_c_20_r_ + OP_l_3_c_2_r_*SH_MAG(1) - OP_l_3_c_3_r_*SH_MAG(2) + OP_l_3_c_4_r_*SH_MAG(3) + OP_l_3_c_1_r_*SK_MX(3) - OP_l_3_c_17_r_*SK_MX(2) + OP_l_3_c_18_r_*SK_MX(5) - OP_l_3_c_19_r_*SK_MX(4)); +Kfusion(4) = SK_MX(1)*(OP_l_4_c_20_r_ + OP_l_4_c_2_r_*SH_MAG(1) - OP_l_4_c_3_r_*SH_MAG(2) + OP_l_4_c_4_r_*SH_MAG(3) + OP_l_4_c_1_r_*SK_MX(3) - OP_l_4_c_17_r_*SK_MX(2) + OP_l_4_c_18_r_*SK_MX(5) - OP_l_4_c_19_r_*SK_MX(4)); +Kfusion(5) = SK_MX(1)*(OP_l_5_c_20_r_ + OP_l_5_c_2_r_*SH_MAG(1) - OP_l_5_c_3_r_*SH_MAG(2) + OP_l_5_c_4_r_*SH_MAG(3) + OP_l_5_c_1_r_*SK_MX(3) - OP_l_5_c_17_r_*SK_MX(2) + OP_l_5_c_18_r_*SK_MX(5) - OP_l_5_c_19_r_*SK_MX(4)); +Kfusion(6) = SK_MX(1)*(OP_l_6_c_20_r_ + OP_l_6_c_2_r_*SH_MAG(1) - OP_l_6_c_3_r_*SH_MAG(2) + OP_l_6_c_4_r_*SH_MAG(3) + OP_l_6_c_1_r_*SK_MX(3) - OP_l_6_c_17_r_*SK_MX(2) + OP_l_6_c_18_r_*SK_MX(5) - OP_l_6_c_19_r_*SK_MX(4)); +Kfusion(7) = SK_MX(1)*(OP_l_7_c_20_r_ + OP_l_7_c_2_r_*SH_MAG(1) - OP_l_7_c_3_r_*SH_MAG(2) + OP_l_7_c_4_r_*SH_MAG(3) + OP_l_7_c_1_r_*SK_MX(3) - OP_l_7_c_17_r_*SK_MX(2) + OP_l_7_c_18_r_*SK_MX(5) - OP_l_7_c_19_r_*SK_MX(4)); +Kfusion(8) = SK_MX(1)*(OP_l_8_c_20_r_ + OP_l_8_c_2_r_*SH_MAG(1) - OP_l_8_c_3_r_*SH_MAG(2) + OP_l_8_c_4_r_*SH_MAG(3) + OP_l_8_c_1_r_*SK_MX(3) - OP_l_8_c_17_r_*SK_MX(2) + OP_l_8_c_18_r_*SK_MX(5) - OP_l_8_c_19_r_*SK_MX(4)); +Kfusion(9) = SK_MX(1)*(OP_l_9_c_20_r_ + OP_l_9_c_2_r_*SH_MAG(1) - OP_l_9_c_3_r_*SH_MAG(2) + OP_l_9_c_4_r_*SH_MAG(3) + OP_l_9_c_1_r_*SK_MX(3) - OP_l_9_c_17_r_*SK_MX(2) + OP_l_9_c_18_r_*SK_MX(5) - OP_l_9_c_19_r_*SK_MX(4)); +Kfusion(10) = SK_MX(1)*(OP_l_10_c_20_r_ + OP_l_10_c_2_r_*SH_MAG(1) - OP_l_10_c_3_r_*SH_MAG(2) + OP_l_10_c_4_r_*SH_MAG(3) + OP_l_10_c_1_r_*SK_MX(3) - OP_l_10_c_17_r_*SK_MX(2) + OP_l_10_c_18_r_*SK_MX(5) - OP_l_10_c_19_r_*SK_MX(4)); +Kfusion(11) = SK_MX(1)*(OP_l_11_c_20_r_ + OP_l_11_c_2_r_*SH_MAG(1) - OP_l_11_c_3_r_*SH_MAG(2) + OP_l_11_c_4_r_*SH_MAG(3) + OP_l_11_c_1_r_*SK_MX(3) - OP_l_11_c_17_r_*SK_MX(2) + OP_l_11_c_18_r_*SK_MX(5) - OP_l_11_c_19_r_*SK_MX(4)); +Kfusion(12) = SK_MX(1)*(OP_l_12_c_20_r_ + OP_l_12_c_2_r_*SH_MAG(1) - OP_l_12_c_3_r_*SH_MAG(2) + OP_l_12_c_4_r_*SH_MAG(3) + OP_l_12_c_1_r_*SK_MX(3) - OP_l_12_c_17_r_*SK_MX(2) + OP_l_12_c_18_r_*SK_MX(5) - OP_l_12_c_19_r_*SK_MX(4)); +Kfusion(13) = SK_MX(1)*(OP_l_13_c_20_r_ + OP_l_13_c_2_r_*SH_MAG(1) - OP_l_13_c_3_r_*SH_MAG(2) + OP_l_13_c_4_r_*SH_MAG(3) + OP_l_13_c_1_r_*SK_MX(3) - OP_l_13_c_17_r_*SK_MX(2) + OP_l_13_c_18_r_*SK_MX(5) - OP_l_13_c_19_r_*SK_MX(4)); +Kfusion(14) = SK_MX(1)*(OP_l_14_c_20_r_ + OP_l_14_c_2_r_*SH_MAG(1) - OP_l_14_c_3_r_*SH_MAG(2) + OP_l_14_c_4_r_*SH_MAG(3) + OP_l_14_c_1_r_*SK_MX(3) - OP_l_14_c_17_r_*SK_MX(2) + OP_l_14_c_18_r_*SK_MX(5) - OP_l_14_c_19_r_*SK_MX(4)); +Kfusion(15) = SK_MX(1)*(OP_l_15_c_20_r_ + OP_l_15_c_2_r_*SH_MAG(1) - OP_l_15_c_3_r_*SH_MAG(2) + OP_l_15_c_4_r_*SH_MAG(3) + OP_l_15_c_1_r_*SK_MX(3) - OP_l_15_c_17_r_*SK_MX(2) + OP_l_15_c_18_r_*SK_MX(5) - OP_l_15_c_19_r_*SK_MX(4)); +Kfusion(16) = SK_MX(1)*(OP_l_16_c_20_r_ + OP_l_16_c_2_r_*SH_MAG(1) - OP_l_16_c_3_r_*SH_MAG(2) + OP_l_16_c_4_r_*SH_MAG(3) + OP_l_16_c_1_r_*SK_MX(3) - OP_l_16_c_17_r_*SK_MX(2) + OP_l_16_c_18_r_*SK_MX(5) - OP_l_16_c_19_r_*SK_MX(4)); +Kfusion(17) = SK_MX(1)*(OP_l_17_c_20_r_ + OP_l_17_c_2_r_*SH_MAG(1) - OP_l_17_c_3_r_*SH_MAG(2) + OP_l_17_c_4_r_*SH_MAG(3) + OP_l_17_c_1_r_*SK_MX(3) - OP_l_17_c_17_r_*SK_MX(2) + OP_l_17_c_18_r_*SK_MX(5) - OP_l_17_c_19_r_*SK_MX(4)); +Kfusion(18) = SK_MX(1)*(OP_l_18_c_20_r_ + OP_l_18_c_2_r_*SH_MAG(1) - OP_l_18_c_3_r_*SH_MAG(2) + OP_l_18_c_4_r_*SH_MAG(3) + OP_l_18_c_1_r_*SK_MX(3) - OP_l_18_c_17_r_*SK_MX(2) + OP_l_18_c_18_r_*SK_MX(5) - OP_l_18_c_19_r_*SK_MX(4)); +Kfusion(19) = SK_MX(1)*(OP_l_19_c_20_r_ + OP_l_19_c_2_r_*SH_MAG(1) - OP_l_19_c_3_r_*SH_MAG(2) + OP_l_19_c_4_r_*SH_MAG(3) + OP_l_19_c_1_r_*SK_MX(3) - OP_l_19_c_17_r_*SK_MX(2) + OP_l_19_c_18_r_*SK_MX(5) - OP_l_19_c_19_r_*SK_MX(4)); +Kfusion(20) = SK_MX(1)*(OP_l_20_c_20_r_ + OP_l_20_c_2_r_*SH_MAG(1) - OP_l_20_c_3_r_*SH_MAG(2) + OP_l_20_c_4_r_*SH_MAG(3) + OP_l_20_c_1_r_*SK_MX(3) - OP_l_20_c_17_r_*SK_MX(2) + OP_l_20_c_18_r_*SK_MX(5) - OP_l_20_c_19_r_*SK_MX(4)); +Kfusion(21) = SK_MX(1)*(OP_l_21_c_20_r_ + OP_l_21_c_2_r_*SH_MAG(1) - OP_l_21_c_3_r_*SH_MAG(2) + OP_l_21_c_4_r_*SH_MAG(3) + OP_l_21_c_1_r_*SK_MX(3) - OP_l_21_c_17_r_*SK_MX(2) + OP_l_21_c_18_r_*SK_MX(5) - OP_l_21_c_19_r_*SK_MX(4)); +Kfusion(22) = SK_MX(1)*(OP_l_22_c_20_r_ + OP_l_22_c_2_r_*SH_MAG(1) - OP_l_22_c_3_r_*SH_MAG(2) + OP_l_22_c_4_r_*SH_MAG(3) + OP_l_22_c_1_r_*SK_MX(3) - OP_l_22_c_17_r_*SK_MX(2) + OP_l_22_c_18_r_*SK_MX(5) - OP_l_22_c_19_r_*SK_MX(4)); +Kfusion(23) = SK_MX(1)*(OP_l_23_c_20_r_ + OP_l_23_c_2_r_*SH_MAG(1) - OP_l_23_c_3_r_*SH_MAG(2) + OP_l_23_c_4_r_*SH_MAG(3) + OP_l_23_c_1_r_*SK_MX(3) - OP_l_23_c_17_r_*SK_MX(2) + OP_l_23_c_18_r_*SK_MX(5) - OP_l_23_c_19_r_*SK_MX(4)); +Kfusion(24) = SK_MX(1)*(OP_l_24_c_20_r_ + OP_l_24_c_2_r_*SH_MAG(1) - OP_l_24_c_3_r_*SH_MAG(2) + OP_l_24_c_4_r_*SH_MAG(3) + OP_l_24_c_1_r_*SK_MX(3) - OP_l_24_c_17_r_*SK_MX(2) + OP_l_24_c_18_r_*SK_MX(5) - OP_l_24_c_19_r_*SK_MX(4)); H_MAG = zeros(1,24); -H_MAG(1) = magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6); -H_MAG(3) = - magE*SH_MAG(5) - magD*SH_MAG(8) - magN*SH_MAG(2); -H_MAG(17) = 2*q1*q2 - SH_MAG(9); -H_MAG(18) = SH_MAG(1); -H_MAG(19) = SH_MAG(4); +H_MAG(1) = SH_MAG(3); +H_MAG(2) = SH_MAG(2); +H_MAG(3) = SH_MAG(1); +H_MAG(4) = 2*magD*q2 - SH_MAG(9) - SH_MAG(8); +H_MAG(17) = 2*q1*q2 - 2*q0*q3; +H_MAG(18) = SH_MAG(5) - SH_MAG(4) - SH_MAG(6) + SH_MAG(7); +H_MAG(19) = 2*q0*q1 + 2*q2*q3; H_MAG(21) = 1; -SK_MY = zeros(4,1); -SK_MY(1) = 1/(OP_l_21_c_21_r_ + R_MAG + OP_l_1_c_21_r_*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP_l_18_c_21_r_*SH_MAG(1) + OP_l_19_c_21_r_*SH_MAG(4) - (SH_MAG(9) - 2*q1*q2)*(OP_l_21_c_17_r_ + OP_l_1_c_17_r_*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP_l_18_c_17_r_*SH_MAG(1) + OP_l_19_c_17_r_*SH_MAG(4) - OP_l_3_c_17_r_*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) - OP_l_17_c_17_r_*(SH_MAG(9) - 2*q1*q2)) - OP_l_3_c_21_r_*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) + (magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6))*(OP_l_21_c_1_r_ + OP_l_1_c_1_r_*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP_l_18_c_1_r_*SH_MAG(1) + OP_l_19_c_1_r_*SH_MAG(4) - OP_l_3_c_1_r_*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) - OP_l_17_c_1_r_*(SH_MAG(9) - 2*q1*q2)) + SH_MAG(1)*(OP_l_21_c_18_r_ + OP_l_1_c_18_r_*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP_l_18_c_18_r_*SH_MAG(1) + OP_l_19_c_18_r_*SH_MAG(4) - OP_l_3_c_18_r_*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) - OP_l_17_c_18_r_*(SH_MAG(9) - 2*q1*q2)) + SH_MAG(4)*(OP_l_21_c_19_r_ + OP_l_1_c_19_r_*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP_l_18_c_19_r_*SH_MAG(1) + OP_l_19_c_19_r_*SH_MAG(4) - OP_l_3_c_19_r_*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) - OP_l_17_c_19_r_*(SH_MAG(9) - 2*q1*q2)) - OP_l_17_c_21_r_*(SH_MAG(9) - 2*q1*q2) - (magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2))*(OP_l_21_c_3_r_ + OP_l_1_c_3_r_*(magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6)) + OP_l_18_c_3_r_*SH_MAG(1) + OP_l_19_c_3_r_*SH_MAG(4) - OP_l_3_c_3_r_*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) - OP_l_17_c_3_r_*(SH_MAG(9) - 2*q1*q2))); -SK_MY(2) = magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2); -SK_MY(3) = magD*SH_MAG(3) - SH_MAG(7) + magN*SH_MAG(6); -SK_MY(4) = SH_MAG(9) - 2*q1*q2; +SK_MY = zeros(5,1); +SK_MY(1) = 1/(OP_l_21_c_21_r_ + R_MAG + OP_l_1_c_21_r_*SH_MAG(3) + OP_l_2_c_21_r_*SH_MAG(2) + OP_l_3_c_21_r_*SH_MAG(1) - OP_l_18_c_21_r_*(SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7)) - (2*q0*q3 - 2*q1*q2)*(OP_l_21_c_17_r_ + OP_l_1_c_17_r_*SH_MAG(3) + OP_l_2_c_17_r_*SH_MAG(2) + OP_l_3_c_17_r_*SH_MAG(1) - OP_l_18_c_17_r_*(SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7)) - OP_l_17_c_17_r_*(2*q0*q3 - 2*q1*q2) + OP_l_19_c_17_r_*(2*q0*q1 + 2*q2*q3) - OP_l_4_c_17_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + (2*q0*q1 + 2*q2*q3)*(OP_l_21_c_19_r_ + OP_l_1_c_19_r_*SH_MAG(3) + OP_l_2_c_19_r_*SH_MAG(2) + OP_l_3_c_19_r_*SH_MAG(1) - OP_l_18_c_19_r_*(SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7)) - OP_l_17_c_19_r_*(2*q0*q3 - 2*q1*q2) + OP_l_19_c_19_r_*(2*q0*q1 + 2*q2*q3) - OP_l_4_c_19_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) - (SH_MAG(8) + SH_MAG(9) - 2*magD*q2)*(OP_l_21_c_4_r_ + OP_l_1_c_4_r_*SH_MAG(3) + OP_l_2_c_4_r_*SH_MAG(2) + OP_l_3_c_4_r_*SH_MAG(1) - OP_l_18_c_4_r_*(SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7)) - OP_l_17_c_4_r_*(2*q0*q3 - 2*q1*q2) + OP_l_19_c_4_r_*(2*q0*q1 + 2*q2*q3) - OP_l_4_c_4_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) - OP_l_17_c_21_r_*(2*q0*q3 - 2*q1*q2) + OP_l_19_c_21_r_*(2*q0*q1 + 2*q2*q3) + SH_MAG(3)*(OP_l_21_c_1_r_ + OP_l_1_c_1_r_*SH_MAG(3) + OP_l_2_c_1_r_*SH_MAG(2) + OP_l_3_c_1_r_*SH_MAG(1) - OP_l_18_c_1_r_*(SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7)) - OP_l_17_c_1_r_*(2*q0*q3 - 2*q1*q2) + OP_l_19_c_1_r_*(2*q0*q1 + 2*q2*q3) - OP_l_4_c_1_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + SH_MAG(2)*(OP_l_21_c_2_r_ + OP_l_1_c_2_r_*SH_MAG(3) + OP_l_2_c_2_r_*SH_MAG(2) + OP_l_3_c_2_r_*SH_MAG(1) - OP_l_18_c_2_r_*(SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7)) - OP_l_17_c_2_r_*(2*q0*q3 - 2*q1*q2) + OP_l_19_c_2_r_*(2*q0*q1 + 2*q2*q3) - OP_l_4_c_2_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + SH_MAG(1)*(OP_l_21_c_3_r_ + OP_l_1_c_3_r_*SH_MAG(3) + OP_l_2_c_3_r_*SH_MAG(2) + OP_l_3_c_3_r_*SH_MAG(1) - OP_l_18_c_3_r_*(SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7)) - OP_l_17_c_3_r_*(2*q0*q3 - 2*q1*q2) + OP_l_19_c_3_r_*(2*q0*q1 + 2*q2*q3) - OP_l_4_c_3_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) - (SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7))*(OP_l_21_c_18_r_ + OP_l_1_c_18_r_*SH_MAG(3) + OP_l_2_c_18_r_*SH_MAG(2) + OP_l_3_c_18_r_*SH_MAG(1) - OP_l_18_c_18_r_*(SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7)) - OP_l_17_c_18_r_*(2*q0*q3 - 2*q1*q2) + OP_l_19_c_18_r_*(2*q0*q1 + 2*q2*q3) - OP_l_4_c_18_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) - OP_l_4_c_21_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)); +SK_MY(2) = SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7); +SK_MY(3) = SH_MAG(8) + SH_MAG(9) - 2*magD*q2; +SK_MY(4) = 2*q0*q3 - 2*q1*q2; +SK_MY(5) = 2*q0*q1 + 2*q2*q3; Kfusion = zeros(24,1); Kfusion = zeros(1,1); -Kfusion(1) = SK_MY(1)*(OP_l_1_c_21_r_ + OP_l_1_c_18_r_*SH_MAG(1) + OP_l_1_c_19_r_*SH_MAG(4) + OP_l_1_c_1_r_*SK_MY(3) - OP_l_1_c_3_r_*SK_MY(2) - OP_l_1_c_17_r_*SK_MY(4)); -Kfusion(2) = SK_MY(1)*(OP_l_2_c_21_r_ + OP_l_2_c_18_r_*SH_MAG(1) + OP_l_2_c_19_r_*SH_MAG(4) + OP_l_2_c_1_r_*SK_MY(3) - OP_l_2_c_3_r_*SK_MY(2) - OP_l_2_c_17_r_*SK_MY(4)); -Kfusion(3) = SK_MY(1)*(OP_l_3_c_21_r_ + OP_l_3_c_18_r_*SH_MAG(1) + OP_l_3_c_19_r_*SH_MAG(4) + OP_l_3_c_1_r_*SK_MY(3) - OP_l_3_c_3_r_*SK_MY(2) - OP_l_3_c_17_r_*SK_MY(4)); -Kfusion(4) = SK_MY(1)*(OP_l_4_c_21_r_ + OP_l_4_c_18_r_*SH_MAG(1) + OP_l_4_c_19_r_*SH_MAG(4) + OP_l_4_c_1_r_*SK_MY(3) - OP_l_4_c_3_r_*SK_MY(2) - OP_l_4_c_17_r_*SK_MY(4)); -Kfusion(5) = SK_MY(1)*(OP_l_5_c_21_r_ + OP_l_5_c_18_r_*SH_MAG(1) + OP_l_5_c_19_r_*SH_MAG(4) + OP_l_5_c_1_r_*SK_MY(3) - OP_l_5_c_3_r_*SK_MY(2) - OP_l_5_c_17_r_*SK_MY(4)); -Kfusion(6) = SK_MY(1)*(OP_l_6_c_21_r_ + OP_l_6_c_18_r_*SH_MAG(1) + OP_l_6_c_19_r_*SH_MAG(4) + OP_l_6_c_1_r_*SK_MY(3) - OP_l_6_c_3_r_*SK_MY(2) - OP_l_6_c_17_r_*SK_MY(4)); -Kfusion(7) = SK_MY(1)*(OP_l_7_c_21_r_ + OP_l_7_c_18_r_*SH_MAG(1) + OP_l_7_c_19_r_*SH_MAG(4) + OP_l_7_c_1_r_*SK_MY(3) - OP_l_7_c_3_r_*SK_MY(2) - OP_l_7_c_17_r_*SK_MY(4)); -Kfusion(8) = SK_MY(1)*(OP_l_8_c_21_r_ + OP_l_8_c_18_r_*SH_MAG(1) + OP_l_8_c_19_r_*SH_MAG(4) + OP_l_8_c_1_r_*SK_MY(3) - OP_l_8_c_3_r_*SK_MY(2) - OP_l_8_c_17_r_*SK_MY(4)); -Kfusion(9) = SK_MY(1)*(OP_l_9_c_21_r_ + OP_l_9_c_18_r_*SH_MAG(1) + OP_l_9_c_19_r_*SH_MAG(4) + OP_l_9_c_1_r_*SK_MY(3) - OP_l_9_c_3_r_*SK_MY(2) - OP_l_9_c_17_r_*SK_MY(4)); -Kfusion(10) = SK_MY(1)*(OP_l_10_c_21_r_ + OP_l_10_c_18_r_*SH_MAG(1) + OP_l_10_c_19_r_*SH_MAG(4) + OP_l_10_c_1_r_*SK_MY(3) - OP_l_10_c_3_r_*SK_MY(2) - OP_l_10_c_17_r_*SK_MY(4)); -Kfusion(11) = SK_MY(1)*(OP_l_11_c_21_r_ + OP_l_11_c_18_r_*SH_MAG(1) + OP_l_11_c_19_r_*SH_MAG(4) + OP_l_11_c_1_r_*SK_MY(3) - OP_l_11_c_3_r_*SK_MY(2) - OP_l_11_c_17_r_*SK_MY(4)); -Kfusion(12) = SK_MY(1)*(OP_l_12_c_21_r_ + OP_l_12_c_18_r_*SH_MAG(1) + OP_l_12_c_19_r_*SH_MAG(4) + OP_l_12_c_1_r_*SK_MY(3) - OP_l_12_c_3_r_*SK_MY(2) - OP_l_12_c_17_r_*SK_MY(4)); -Kfusion(13) = SK_MY(1)*(OP_l_13_c_21_r_ + OP_l_13_c_18_r_*SH_MAG(1) + OP_l_13_c_19_r_*SH_MAG(4) + OP_l_13_c_1_r_*SK_MY(3) - OP_l_13_c_3_r_*SK_MY(2) - OP_l_13_c_17_r_*SK_MY(4)); -Kfusion(14) = SK_MY(1)*(OP_l_14_c_21_r_ + OP_l_14_c_18_r_*SH_MAG(1) + OP_l_14_c_19_r_*SH_MAG(4) + OP_l_14_c_1_r_*SK_MY(3) - OP_l_14_c_3_r_*SK_MY(2) - OP_l_14_c_17_r_*SK_MY(4)); -Kfusion(15) = SK_MY(1)*(OP_l_15_c_21_r_ + OP_l_15_c_18_r_*SH_MAG(1) + OP_l_15_c_19_r_*SH_MAG(4) + OP_l_15_c_1_r_*SK_MY(3) - OP_l_15_c_3_r_*SK_MY(2) - OP_l_15_c_17_r_*SK_MY(4)); -Kfusion(16) = SK_MY(1)*(OP_l_16_c_21_r_ + OP_l_16_c_18_r_*SH_MAG(1) + OP_l_16_c_19_r_*SH_MAG(4) + OP_l_16_c_1_r_*SK_MY(3) - OP_l_16_c_3_r_*SK_MY(2) - OP_l_16_c_17_r_*SK_MY(4)); -Kfusion(17) = SK_MY(1)*(OP_l_17_c_21_r_ + OP_l_17_c_18_r_*SH_MAG(1) + OP_l_17_c_19_r_*SH_MAG(4) + OP_l_17_c_1_r_*SK_MY(3) - OP_l_17_c_3_r_*SK_MY(2) - OP_l_17_c_17_r_*SK_MY(4)); -Kfusion(18) = SK_MY(1)*(OP_l_18_c_21_r_ + OP_l_18_c_18_r_*SH_MAG(1) + OP_l_18_c_19_r_*SH_MAG(4) + OP_l_18_c_1_r_*SK_MY(3) - OP_l_18_c_3_r_*SK_MY(2) - OP_l_18_c_17_r_*SK_MY(4)); -Kfusion(19) = SK_MY(1)*(OP_l_19_c_21_r_ + OP_l_19_c_18_r_*SH_MAG(1) + OP_l_19_c_19_r_*SH_MAG(4) + OP_l_19_c_1_r_*SK_MY(3) - OP_l_19_c_3_r_*SK_MY(2) - OP_l_19_c_17_r_*SK_MY(4)); -Kfusion(20) = SK_MY(1)*(OP_l_20_c_21_r_ + OP_l_20_c_18_r_*SH_MAG(1) + OP_l_20_c_19_r_*SH_MAG(4) + OP_l_20_c_1_r_*SK_MY(3) - OP_l_20_c_3_r_*SK_MY(2) - OP_l_20_c_17_r_*SK_MY(4)); -Kfusion(21) = SK_MY(1)*(OP_l_21_c_21_r_ + OP_l_21_c_18_r_*SH_MAG(1) + OP_l_21_c_19_r_*SH_MAG(4) + OP_l_21_c_1_r_*SK_MY(3) - OP_l_21_c_3_r_*SK_MY(2) - OP_l_21_c_17_r_*SK_MY(4)); -Kfusion(22) = SK_MY(1)*(OP_l_22_c_21_r_ + OP_l_22_c_18_r_*SH_MAG(1) + OP_l_22_c_19_r_*SH_MAG(4) + OP_l_22_c_1_r_*SK_MY(3) - OP_l_22_c_3_r_*SK_MY(2) - OP_l_22_c_17_r_*SK_MY(4)); -Kfusion(23) = SK_MY(1)*(OP_l_23_c_21_r_ + OP_l_23_c_18_r_*SH_MAG(1) + OP_l_23_c_19_r_*SH_MAG(4) + OP_l_23_c_1_r_*SK_MY(3) - OP_l_23_c_3_r_*SK_MY(2) - OP_l_23_c_17_r_*SK_MY(4)); -Kfusion(24) = SK_MY(1)*(OP_l_24_c_21_r_ + OP_l_24_c_18_r_*SH_MAG(1) + OP_l_24_c_19_r_*SH_MAG(4) + OP_l_24_c_1_r_*SK_MY(3) - OP_l_24_c_3_r_*SK_MY(2) - OP_l_24_c_17_r_*SK_MY(4)); +Kfusion(1) = SK_MY(1)*(OP_l_1_c_21_r_ + OP_l_1_c_1_r_*SH_MAG(3) + OP_l_1_c_2_r_*SH_MAG(2) + OP_l_1_c_3_r_*SH_MAG(1) - OP_l_1_c_4_r_*SK_MY(3) - OP_l_1_c_18_r_*SK_MY(2) - OP_l_1_c_17_r_*SK_MY(4) + OP_l_1_c_19_r_*SK_MY(5)); +Kfusion(2) = SK_MY(1)*(OP_l_2_c_21_r_ + OP_l_2_c_1_r_*SH_MAG(3) + OP_l_2_c_2_r_*SH_MAG(2) + OP_l_2_c_3_r_*SH_MAG(1) - OP_l_2_c_4_r_*SK_MY(3) - OP_l_2_c_18_r_*SK_MY(2) - OP_l_2_c_17_r_*SK_MY(4) + OP_l_2_c_19_r_*SK_MY(5)); +Kfusion(3) = SK_MY(1)*(OP_l_3_c_21_r_ + OP_l_3_c_1_r_*SH_MAG(3) + OP_l_3_c_2_r_*SH_MAG(2) + OP_l_3_c_3_r_*SH_MAG(1) - OP_l_3_c_4_r_*SK_MY(3) - OP_l_3_c_18_r_*SK_MY(2) - OP_l_3_c_17_r_*SK_MY(4) + OP_l_3_c_19_r_*SK_MY(5)); +Kfusion(4) = SK_MY(1)*(OP_l_4_c_21_r_ + OP_l_4_c_1_r_*SH_MAG(3) + OP_l_4_c_2_r_*SH_MAG(2) + OP_l_4_c_3_r_*SH_MAG(1) - OP_l_4_c_4_r_*SK_MY(3) - OP_l_4_c_18_r_*SK_MY(2) - OP_l_4_c_17_r_*SK_MY(4) + OP_l_4_c_19_r_*SK_MY(5)); +Kfusion(5) = SK_MY(1)*(OP_l_5_c_21_r_ + OP_l_5_c_1_r_*SH_MAG(3) + OP_l_5_c_2_r_*SH_MAG(2) + OP_l_5_c_3_r_*SH_MAG(1) - OP_l_5_c_4_r_*SK_MY(3) - OP_l_5_c_18_r_*SK_MY(2) - OP_l_5_c_17_r_*SK_MY(4) + OP_l_5_c_19_r_*SK_MY(5)); +Kfusion(6) = SK_MY(1)*(OP_l_6_c_21_r_ + OP_l_6_c_1_r_*SH_MAG(3) + OP_l_6_c_2_r_*SH_MAG(2) + OP_l_6_c_3_r_*SH_MAG(1) - OP_l_6_c_4_r_*SK_MY(3) - OP_l_6_c_18_r_*SK_MY(2) - OP_l_6_c_17_r_*SK_MY(4) + OP_l_6_c_19_r_*SK_MY(5)); +Kfusion(7) = SK_MY(1)*(OP_l_7_c_21_r_ + OP_l_7_c_1_r_*SH_MAG(3) + OP_l_7_c_2_r_*SH_MAG(2) + OP_l_7_c_3_r_*SH_MAG(1) - OP_l_7_c_4_r_*SK_MY(3) - OP_l_7_c_18_r_*SK_MY(2) - OP_l_7_c_17_r_*SK_MY(4) + OP_l_7_c_19_r_*SK_MY(5)); +Kfusion(8) = SK_MY(1)*(OP_l_8_c_21_r_ + OP_l_8_c_1_r_*SH_MAG(3) + OP_l_8_c_2_r_*SH_MAG(2) + OP_l_8_c_3_r_*SH_MAG(1) - OP_l_8_c_4_r_*SK_MY(3) - OP_l_8_c_18_r_*SK_MY(2) - OP_l_8_c_17_r_*SK_MY(4) + OP_l_8_c_19_r_*SK_MY(5)); +Kfusion(9) = SK_MY(1)*(OP_l_9_c_21_r_ + OP_l_9_c_1_r_*SH_MAG(3) + OP_l_9_c_2_r_*SH_MAG(2) + OP_l_9_c_3_r_*SH_MAG(1) - OP_l_9_c_4_r_*SK_MY(3) - OP_l_9_c_18_r_*SK_MY(2) - OP_l_9_c_17_r_*SK_MY(4) + OP_l_9_c_19_r_*SK_MY(5)); +Kfusion(10) = SK_MY(1)*(OP_l_10_c_21_r_ + OP_l_10_c_1_r_*SH_MAG(3) + OP_l_10_c_2_r_*SH_MAG(2) + OP_l_10_c_3_r_*SH_MAG(1) - OP_l_10_c_4_r_*SK_MY(3) - OP_l_10_c_18_r_*SK_MY(2) - OP_l_10_c_17_r_*SK_MY(4) + OP_l_10_c_19_r_*SK_MY(5)); +Kfusion(11) = SK_MY(1)*(OP_l_11_c_21_r_ + OP_l_11_c_1_r_*SH_MAG(3) + OP_l_11_c_2_r_*SH_MAG(2) + OP_l_11_c_3_r_*SH_MAG(1) - OP_l_11_c_4_r_*SK_MY(3) - OP_l_11_c_18_r_*SK_MY(2) - OP_l_11_c_17_r_*SK_MY(4) + OP_l_11_c_19_r_*SK_MY(5)); +Kfusion(12) = SK_MY(1)*(OP_l_12_c_21_r_ + OP_l_12_c_1_r_*SH_MAG(3) + OP_l_12_c_2_r_*SH_MAG(2) + OP_l_12_c_3_r_*SH_MAG(1) - OP_l_12_c_4_r_*SK_MY(3) - OP_l_12_c_18_r_*SK_MY(2) - OP_l_12_c_17_r_*SK_MY(4) + OP_l_12_c_19_r_*SK_MY(5)); +Kfusion(13) = SK_MY(1)*(OP_l_13_c_21_r_ + OP_l_13_c_1_r_*SH_MAG(3) + OP_l_13_c_2_r_*SH_MAG(2) + OP_l_13_c_3_r_*SH_MAG(1) - OP_l_13_c_4_r_*SK_MY(3) - OP_l_13_c_18_r_*SK_MY(2) - OP_l_13_c_17_r_*SK_MY(4) + OP_l_13_c_19_r_*SK_MY(5)); +Kfusion(14) = SK_MY(1)*(OP_l_14_c_21_r_ + OP_l_14_c_1_r_*SH_MAG(3) + OP_l_14_c_2_r_*SH_MAG(2) + OP_l_14_c_3_r_*SH_MAG(1) - OP_l_14_c_4_r_*SK_MY(3) - OP_l_14_c_18_r_*SK_MY(2) - OP_l_14_c_17_r_*SK_MY(4) + OP_l_14_c_19_r_*SK_MY(5)); +Kfusion(15) = SK_MY(1)*(OP_l_15_c_21_r_ + OP_l_15_c_1_r_*SH_MAG(3) + OP_l_15_c_2_r_*SH_MAG(2) + OP_l_15_c_3_r_*SH_MAG(1) - OP_l_15_c_4_r_*SK_MY(3) - OP_l_15_c_18_r_*SK_MY(2) - OP_l_15_c_17_r_*SK_MY(4) + OP_l_15_c_19_r_*SK_MY(5)); +Kfusion(16) = SK_MY(1)*(OP_l_16_c_21_r_ + OP_l_16_c_1_r_*SH_MAG(3) + OP_l_16_c_2_r_*SH_MAG(2) + OP_l_16_c_3_r_*SH_MAG(1) - OP_l_16_c_4_r_*SK_MY(3) - OP_l_16_c_18_r_*SK_MY(2) - OP_l_16_c_17_r_*SK_MY(4) + OP_l_16_c_19_r_*SK_MY(5)); +Kfusion(17) = SK_MY(1)*(OP_l_17_c_21_r_ + OP_l_17_c_1_r_*SH_MAG(3) + OP_l_17_c_2_r_*SH_MAG(2) + OP_l_17_c_3_r_*SH_MAG(1) - OP_l_17_c_4_r_*SK_MY(3) - OP_l_17_c_18_r_*SK_MY(2) - OP_l_17_c_17_r_*SK_MY(4) + OP_l_17_c_19_r_*SK_MY(5)); +Kfusion(18) = SK_MY(1)*(OP_l_18_c_21_r_ + OP_l_18_c_1_r_*SH_MAG(3) + OP_l_18_c_2_r_*SH_MAG(2) + OP_l_18_c_3_r_*SH_MAG(1) - OP_l_18_c_4_r_*SK_MY(3) - OP_l_18_c_18_r_*SK_MY(2) - OP_l_18_c_17_r_*SK_MY(4) + OP_l_18_c_19_r_*SK_MY(5)); +Kfusion(19) = SK_MY(1)*(OP_l_19_c_21_r_ + OP_l_19_c_1_r_*SH_MAG(3) + OP_l_19_c_2_r_*SH_MAG(2) + OP_l_19_c_3_r_*SH_MAG(1) - OP_l_19_c_4_r_*SK_MY(3) - OP_l_19_c_18_r_*SK_MY(2) - OP_l_19_c_17_r_*SK_MY(4) + OP_l_19_c_19_r_*SK_MY(5)); +Kfusion(20) = SK_MY(1)*(OP_l_20_c_21_r_ + OP_l_20_c_1_r_*SH_MAG(3) + OP_l_20_c_2_r_*SH_MAG(2) + OP_l_20_c_3_r_*SH_MAG(1) - OP_l_20_c_4_r_*SK_MY(3) - OP_l_20_c_18_r_*SK_MY(2) - OP_l_20_c_17_r_*SK_MY(4) + OP_l_20_c_19_r_*SK_MY(5)); +Kfusion(21) = SK_MY(1)*(OP_l_21_c_21_r_ + OP_l_21_c_1_r_*SH_MAG(3) + OP_l_21_c_2_r_*SH_MAG(2) + OP_l_21_c_3_r_*SH_MAG(1) - OP_l_21_c_4_r_*SK_MY(3) - OP_l_21_c_18_r_*SK_MY(2) - OP_l_21_c_17_r_*SK_MY(4) + OP_l_21_c_19_r_*SK_MY(5)); +Kfusion(22) = SK_MY(1)*(OP_l_22_c_21_r_ + OP_l_22_c_1_r_*SH_MAG(3) + OP_l_22_c_2_r_*SH_MAG(2) + OP_l_22_c_3_r_*SH_MAG(1) - OP_l_22_c_4_r_*SK_MY(3) - OP_l_22_c_18_r_*SK_MY(2) - OP_l_22_c_17_r_*SK_MY(4) + OP_l_22_c_19_r_*SK_MY(5)); +Kfusion(23) = SK_MY(1)*(OP_l_23_c_21_r_ + OP_l_23_c_1_r_*SH_MAG(3) + OP_l_23_c_2_r_*SH_MAG(2) + OP_l_23_c_3_r_*SH_MAG(1) - OP_l_23_c_4_r_*SK_MY(3) - OP_l_23_c_18_r_*SK_MY(2) - OP_l_23_c_17_r_*SK_MY(4) + OP_l_23_c_19_r_*SK_MY(5)); +Kfusion(24) = SK_MY(1)*(OP_l_24_c_21_r_ + OP_l_24_c_1_r_*SH_MAG(3) + OP_l_24_c_2_r_*SH_MAG(2) + OP_l_24_c_3_r_*SH_MAG(1) - OP_l_24_c_4_r_*SK_MY(3) - OP_l_24_c_18_r_*SK_MY(2) - OP_l_24_c_17_r_*SK_MY(4) + OP_l_24_c_19_r_*SK_MY(5)); H_MAG = zeros(1,24); -H_MAG(1) = magN*(SH_MAG(9) - 2*q1*q2) - magD*SH_MAG(4) - magE*SH_MAG(1); -H_MAG(2) = magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2); -H_MAG(17) = SH_MAG(6); +H_MAG(1) = SH_MAG(2); +H_MAG(2) = -SH_MAG(3); +H_MAG(3) = SH_MAG(8) + SH_MAG(9) - 2*magD*q2; +H_MAG(4) = SH_MAG(1); +H_MAG(17) = 2*q0*q2 + 2*q1*q3; H_MAG(18) = 2*q2*q3 - 2*q0*q1; -H_MAG(19) = SH_MAG(3); +H_MAG(19) = SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7); H_MAG(22) = 1; -SK_MZ = zeros(4,1); -SK_MZ(1) = 1/(OP_l_22_c_22_r_ + R_MAG + OP_l_17_c_22_r_*SH_MAG(6) + OP_l_19_c_22_r_*SH_MAG(3) - (2*q0*q1 - 2*q2*q3)*(OP_l_22_c_18_r_ + OP_l_17_c_18_r_*SH_MAG(6) + OP_l_19_c_18_r_*SH_MAG(3) - OP_l_1_c_18_r_*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2)) + OP_l_2_c_18_r_*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) - OP_l_18_c_18_r_*(2*q0*q1 - 2*q2*q3)) - OP_l_1_c_22_r_*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2)) + OP_l_2_c_22_r_*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) + SH_MAG(6)*(OP_l_22_c_17_r_ + OP_l_17_c_17_r_*SH_MAG(6) + OP_l_19_c_17_r_*SH_MAG(3) - OP_l_1_c_17_r_*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2)) + OP_l_2_c_17_r_*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) - OP_l_18_c_17_r_*(2*q0*q1 - 2*q2*q3)) + SH_MAG(3)*(OP_l_22_c_19_r_ + OP_l_17_c_19_r_*SH_MAG(6) + OP_l_19_c_19_r_*SH_MAG(3) - OP_l_1_c_19_r_*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2)) + OP_l_2_c_19_r_*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) - OP_l_18_c_19_r_*(2*q0*q1 - 2*q2*q3)) - (magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2))*(OP_l_22_c_1_r_ + OP_l_17_c_1_r_*SH_MAG(6) + OP_l_19_c_1_r_*SH_MAG(3) - OP_l_1_c_1_r_*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2)) + OP_l_2_c_1_r_*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) - OP_l_18_c_1_r_*(2*q0*q1 - 2*q2*q3)) - OP_l_18_c_22_r_*(2*q0*q1 - 2*q2*q3) + (magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2))*(OP_l_22_c_2_r_ + OP_l_17_c_2_r_*SH_MAG(6) + OP_l_19_c_2_r_*SH_MAG(3) - OP_l_1_c_2_r_*(magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2)) + OP_l_2_c_2_r_*(magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2)) - OP_l_18_c_2_r_*(2*q0*q1 - 2*q2*q3))); -SK_MZ(2) = magE*SH_MAG(1) + magD*SH_MAG(4) - magN*(SH_MAG(9) - 2*q1*q2); -SK_MZ(3) = magE*SH_MAG(5) + magD*SH_MAG(8) + magN*SH_MAG(2); +SK_MZ = zeros(5,1); +SK_MZ(1) = 1/(OP_l_22_c_22_r_ + R_MAG + OP_l_1_c_22_r_*SH_MAG(2) - OP_l_2_c_22_r_*SH_MAG(3) + OP_l_4_c_22_r_*SH_MAG(1) + OP_l_19_c_22_r_*(SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7)) + (2*q0*q2 + 2*q1*q3)*(OP_l_22_c_17_r_ + OP_l_1_c_17_r_*SH_MAG(2) - OP_l_2_c_17_r_*SH_MAG(3) + OP_l_4_c_17_r_*SH_MAG(1) + OP_l_19_c_17_r_*(SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7)) + OP_l_17_c_17_r_*(2*q0*q2 + 2*q1*q3) - OP_l_18_c_17_r_*(2*q0*q1 - 2*q2*q3) + OP_l_3_c_17_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) - (2*q0*q1 - 2*q2*q3)*(OP_l_22_c_18_r_ + OP_l_1_c_18_r_*SH_MAG(2) - OP_l_2_c_18_r_*SH_MAG(3) + OP_l_4_c_18_r_*SH_MAG(1) + OP_l_19_c_18_r_*(SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7)) + OP_l_17_c_18_r_*(2*q0*q2 + 2*q1*q3) - OP_l_18_c_18_r_*(2*q0*q1 - 2*q2*q3) + OP_l_3_c_18_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + (SH_MAG(8) + SH_MAG(9) - 2*magD*q2)*(OP_l_22_c_3_r_ + OP_l_1_c_3_r_*SH_MAG(2) - OP_l_2_c_3_r_*SH_MAG(3) + OP_l_4_c_3_r_*SH_MAG(1) + OP_l_19_c_3_r_*(SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7)) + OP_l_17_c_3_r_*(2*q0*q2 + 2*q1*q3) - OP_l_18_c_3_r_*(2*q0*q1 - 2*q2*q3) + OP_l_3_c_3_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + OP_l_17_c_22_r_*(2*q0*q2 + 2*q1*q3) - OP_l_18_c_22_r_*(2*q0*q1 - 2*q2*q3) + SH_MAG(2)*(OP_l_22_c_1_r_ + OP_l_1_c_1_r_*SH_MAG(2) - OP_l_2_c_1_r_*SH_MAG(3) + OP_l_4_c_1_r_*SH_MAG(1) + OP_l_19_c_1_r_*(SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7)) + OP_l_17_c_1_r_*(2*q0*q2 + 2*q1*q3) - OP_l_18_c_1_r_*(2*q0*q1 - 2*q2*q3) + OP_l_3_c_1_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) - SH_MAG(3)*(OP_l_22_c_2_r_ + OP_l_1_c_2_r_*SH_MAG(2) - OP_l_2_c_2_r_*SH_MAG(3) + OP_l_4_c_2_r_*SH_MAG(1) + OP_l_19_c_2_r_*(SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7)) + OP_l_17_c_2_r_*(2*q0*q2 + 2*q1*q3) - OP_l_18_c_2_r_*(2*q0*q1 - 2*q2*q3) + OP_l_3_c_2_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + SH_MAG(1)*(OP_l_22_c_4_r_ + OP_l_1_c_4_r_*SH_MAG(2) - OP_l_2_c_4_r_*SH_MAG(3) + OP_l_4_c_4_r_*SH_MAG(1) + OP_l_19_c_4_r_*(SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7)) + OP_l_17_c_4_r_*(2*q0*q2 + 2*q1*q3) - OP_l_18_c_4_r_*(2*q0*q1 - 2*q2*q3) + OP_l_3_c_4_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + (SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7))*(OP_l_22_c_19_r_ + OP_l_1_c_19_r_*SH_MAG(2) - OP_l_2_c_19_r_*SH_MAG(3) + OP_l_4_c_19_r_*SH_MAG(1) + OP_l_19_c_19_r_*(SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7)) + OP_l_17_c_19_r_*(2*q0*q2 + 2*q1*q3) - OP_l_18_c_19_r_*(2*q0*q1 - 2*q2*q3) + OP_l_3_c_19_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + OP_l_3_c_22_r_*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)); +SK_MZ(2) = SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7); +SK_MZ(3) = SH_MAG(8) + SH_MAG(9) - 2*magD*q2; SK_MZ(4) = 2*q0*q1 - 2*q2*q3; +SK_MZ(5) = 2*q0*q2 + 2*q1*q3; Kfusion = zeros(24,1); Kfusion = zeros(1,1); -Kfusion(1) = SK_MZ(1)*(OP_l_1_c_22_r_ + OP_l_1_c_19_r_*SH_MAG(3) + OP_l_1_c_17_r_*SH_MAG(6) - OP_l_1_c_1_r_*SK_MZ(2) + OP_l_1_c_2_r_*SK_MZ(3) - OP_l_1_c_18_r_*SK_MZ(4)); -Kfusion(2) = SK_MZ(1)*(OP_l_2_c_22_r_ + OP_l_2_c_19_r_*SH_MAG(3) + OP_l_2_c_17_r_*SH_MAG(6) - OP_l_2_c_1_r_*SK_MZ(2) + OP_l_2_c_2_r_*SK_MZ(3) - OP_l_2_c_18_r_*SK_MZ(4)); -Kfusion(3) = SK_MZ(1)*(OP_l_3_c_22_r_ + OP_l_3_c_19_r_*SH_MAG(3) + OP_l_3_c_17_r_*SH_MAG(6) - OP_l_3_c_1_r_*SK_MZ(2) + OP_l_3_c_2_r_*SK_MZ(3) - OP_l_3_c_18_r_*SK_MZ(4)); -Kfusion(4) = SK_MZ(1)*(OP_l_4_c_22_r_ + OP_l_4_c_19_r_*SH_MAG(3) + OP_l_4_c_17_r_*SH_MAG(6) - OP_l_4_c_1_r_*SK_MZ(2) + OP_l_4_c_2_r_*SK_MZ(3) - OP_l_4_c_18_r_*SK_MZ(4)); -Kfusion(5) = SK_MZ(1)*(OP_l_5_c_22_r_ + OP_l_5_c_19_r_*SH_MAG(3) + OP_l_5_c_17_r_*SH_MAG(6) - OP_l_5_c_1_r_*SK_MZ(2) + OP_l_5_c_2_r_*SK_MZ(3) - OP_l_5_c_18_r_*SK_MZ(4)); -Kfusion(6) = SK_MZ(1)*(OP_l_6_c_22_r_ + OP_l_6_c_19_r_*SH_MAG(3) + OP_l_6_c_17_r_*SH_MAG(6) - OP_l_6_c_1_r_*SK_MZ(2) + OP_l_6_c_2_r_*SK_MZ(3) - OP_l_6_c_18_r_*SK_MZ(4)); -Kfusion(7) = SK_MZ(1)*(OP_l_7_c_22_r_ + OP_l_7_c_19_r_*SH_MAG(3) + OP_l_7_c_17_r_*SH_MAG(6) - OP_l_7_c_1_r_*SK_MZ(2) + OP_l_7_c_2_r_*SK_MZ(3) - OP_l_7_c_18_r_*SK_MZ(4)); -Kfusion(8) = SK_MZ(1)*(OP_l_8_c_22_r_ + OP_l_8_c_19_r_*SH_MAG(3) + OP_l_8_c_17_r_*SH_MAG(6) - OP_l_8_c_1_r_*SK_MZ(2) + OP_l_8_c_2_r_*SK_MZ(3) - OP_l_8_c_18_r_*SK_MZ(4)); -Kfusion(9) = SK_MZ(1)*(OP_l_9_c_22_r_ + OP_l_9_c_19_r_*SH_MAG(3) + OP_l_9_c_17_r_*SH_MAG(6) - OP_l_9_c_1_r_*SK_MZ(2) + OP_l_9_c_2_r_*SK_MZ(3) - OP_l_9_c_18_r_*SK_MZ(4)); -Kfusion(10) = SK_MZ(1)*(OP_l_10_c_22_r_ + OP_l_10_c_19_r_*SH_MAG(3) + OP_l_10_c_17_r_*SH_MAG(6) - OP_l_10_c_1_r_*SK_MZ(2) + OP_l_10_c_2_r_*SK_MZ(3) - OP_l_10_c_18_r_*SK_MZ(4)); -Kfusion(11) = SK_MZ(1)*(OP_l_11_c_22_r_ + OP_l_11_c_19_r_*SH_MAG(3) + OP_l_11_c_17_r_*SH_MAG(6) - OP_l_11_c_1_r_*SK_MZ(2) + OP_l_11_c_2_r_*SK_MZ(3) - OP_l_11_c_18_r_*SK_MZ(4)); -Kfusion(12) = SK_MZ(1)*(OP_l_12_c_22_r_ + OP_l_12_c_19_r_*SH_MAG(3) + OP_l_12_c_17_r_*SH_MAG(6) - OP_l_12_c_1_r_*SK_MZ(2) + OP_l_12_c_2_r_*SK_MZ(3) - OP_l_12_c_18_r_*SK_MZ(4)); -Kfusion(13) = SK_MZ(1)*(OP_l_13_c_22_r_ + OP_l_13_c_19_r_*SH_MAG(3) + OP_l_13_c_17_r_*SH_MAG(6) - OP_l_13_c_1_r_*SK_MZ(2) + OP_l_13_c_2_r_*SK_MZ(3) - OP_l_13_c_18_r_*SK_MZ(4)); -Kfusion(14) = SK_MZ(1)*(OP_l_14_c_22_r_ + OP_l_14_c_19_r_*SH_MAG(3) + OP_l_14_c_17_r_*SH_MAG(6) - OP_l_14_c_1_r_*SK_MZ(2) + OP_l_14_c_2_r_*SK_MZ(3) - OP_l_14_c_18_r_*SK_MZ(4)); -Kfusion(15) = SK_MZ(1)*(OP_l_15_c_22_r_ + OP_l_15_c_19_r_*SH_MAG(3) + OP_l_15_c_17_r_*SH_MAG(6) - OP_l_15_c_1_r_*SK_MZ(2) + OP_l_15_c_2_r_*SK_MZ(3) - OP_l_15_c_18_r_*SK_MZ(4)); -Kfusion(16) = SK_MZ(1)*(OP_l_16_c_22_r_ + OP_l_16_c_19_r_*SH_MAG(3) + OP_l_16_c_17_r_*SH_MAG(6) - OP_l_16_c_1_r_*SK_MZ(2) + OP_l_16_c_2_r_*SK_MZ(3) - OP_l_16_c_18_r_*SK_MZ(4)); -Kfusion(17) = SK_MZ(1)*(OP_l_17_c_22_r_ + OP_l_17_c_19_r_*SH_MAG(3) + OP_l_17_c_17_r_*SH_MAG(6) - OP_l_17_c_1_r_*SK_MZ(2) + OP_l_17_c_2_r_*SK_MZ(3) - OP_l_17_c_18_r_*SK_MZ(4)); -Kfusion(18) = SK_MZ(1)*(OP_l_18_c_22_r_ + OP_l_18_c_19_r_*SH_MAG(3) + OP_l_18_c_17_r_*SH_MAG(6) - OP_l_18_c_1_r_*SK_MZ(2) + OP_l_18_c_2_r_*SK_MZ(3) - OP_l_18_c_18_r_*SK_MZ(4)); -Kfusion(19) = SK_MZ(1)*(OP_l_19_c_22_r_ + OP_l_19_c_19_r_*SH_MAG(3) + OP_l_19_c_17_r_*SH_MAG(6) - OP_l_19_c_1_r_*SK_MZ(2) + OP_l_19_c_2_r_*SK_MZ(3) - OP_l_19_c_18_r_*SK_MZ(4)); -Kfusion(20) = SK_MZ(1)*(OP_l_20_c_22_r_ + OP_l_20_c_19_r_*SH_MAG(3) + OP_l_20_c_17_r_*SH_MAG(6) - OP_l_20_c_1_r_*SK_MZ(2) + OP_l_20_c_2_r_*SK_MZ(3) - OP_l_20_c_18_r_*SK_MZ(4)); -Kfusion(21) = SK_MZ(1)*(OP_l_21_c_22_r_ + OP_l_21_c_19_r_*SH_MAG(3) + OP_l_21_c_17_r_*SH_MAG(6) - OP_l_21_c_1_r_*SK_MZ(2) + OP_l_21_c_2_r_*SK_MZ(3) - OP_l_21_c_18_r_*SK_MZ(4)); -Kfusion(22) = SK_MZ(1)*(OP_l_22_c_22_r_ + OP_l_22_c_19_r_*SH_MAG(3) + OP_l_22_c_17_r_*SH_MAG(6) - OP_l_22_c_1_r_*SK_MZ(2) + OP_l_22_c_2_r_*SK_MZ(3) - OP_l_22_c_18_r_*SK_MZ(4)); -Kfusion(23) = SK_MZ(1)*(OP_l_23_c_22_r_ + OP_l_23_c_19_r_*SH_MAG(3) + OP_l_23_c_17_r_*SH_MAG(6) - OP_l_23_c_1_r_*SK_MZ(2) + OP_l_23_c_2_r_*SK_MZ(3) - OP_l_23_c_18_r_*SK_MZ(4)); -Kfusion(24) = SK_MZ(1)*(OP_l_24_c_22_r_ + OP_l_24_c_19_r_*SH_MAG(3) + OP_l_24_c_17_r_*SH_MAG(6) - OP_l_24_c_1_r_*SK_MZ(2) + OP_l_24_c_2_r_*SK_MZ(3) - OP_l_24_c_18_r_*SK_MZ(4)); +Kfusion(1) = SK_MZ(1)*(OP_l_1_c_22_r_ + OP_l_1_c_1_r_*SH_MAG(2) - OP_l_1_c_2_r_*SH_MAG(3) + OP_l_1_c_4_r_*SH_MAG(1) + OP_l_1_c_3_r_*SK_MZ(3) + OP_l_1_c_19_r_*SK_MZ(2) + OP_l_1_c_17_r_*SK_MZ(5) - OP_l_1_c_18_r_*SK_MZ(4)); +Kfusion(2) = SK_MZ(1)*(OP_l_2_c_22_r_ + OP_l_2_c_1_r_*SH_MAG(2) - OP_l_2_c_2_r_*SH_MAG(3) + OP_l_2_c_4_r_*SH_MAG(1) + OP_l_2_c_3_r_*SK_MZ(3) + OP_l_2_c_19_r_*SK_MZ(2) + OP_l_2_c_17_r_*SK_MZ(5) - OP_l_2_c_18_r_*SK_MZ(4)); +Kfusion(3) = SK_MZ(1)*(OP_l_3_c_22_r_ + OP_l_3_c_1_r_*SH_MAG(2) - OP_l_3_c_2_r_*SH_MAG(3) + OP_l_3_c_4_r_*SH_MAG(1) + OP_l_3_c_3_r_*SK_MZ(3) + OP_l_3_c_19_r_*SK_MZ(2) + OP_l_3_c_17_r_*SK_MZ(5) - OP_l_3_c_18_r_*SK_MZ(4)); +Kfusion(4) = SK_MZ(1)*(OP_l_4_c_22_r_ + OP_l_4_c_1_r_*SH_MAG(2) - OP_l_4_c_2_r_*SH_MAG(3) + OP_l_4_c_4_r_*SH_MAG(1) + OP_l_4_c_3_r_*SK_MZ(3) + OP_l_4_c_19_r_*SK_MZ(2) + OP_l_4_c_17_r_*SK_MZ(5) - OP_l_4_c_18_r_*SK_MZ(4)); +Kfusion(5) = SK_MZ(1)*(OP_l_5_c_22_r_ + OP_l_5_c_1_r_*SH_MAG(2) - OP_l_5_c_2_r_*SH_MAG(3) + OP_l_5_c_4_r_*SH_MAG(1) + OP_l_5_c_3_r_*SK_MZ(3) + OP_l_5_c_19_r_*SK_MZ(2) + OP_l_5_c_17_r_*SK_MZ(5) - OP_l_5_c_18_r_*SK_MZ(4)); +Kfusion(6) = SK_MZ(1)*(OP_l_6_c_22_r_ + OP_l_6_c_1_r_*SH_MAG(2) - OP_l_6_c_2_r_*SH_MAG(3) + OP_l_6_c_4_r_*SH_MAG(1) + OP_l_6_c_3_r_*SK_MZ(3) + OP_l_6_c_19_r_*SK_MZ(2) + OP_l_6_c_17_r_*SK_MZ(5) - OP_l_6_c_18_r_*SK_MZ(4)); +Kfusion(7) = SK_MZ(1)*(OP_l_7_c_22_r_ + OP_l_7_c_1_r_*SH_MAG(2) - OP_l_7_c_2_r_*SH_MAG(3) + OP_l_7_c_4_r_*SH_MAG(1) + OP_l_7_c_3_r_*SK_MZ(3) + OP_l_7_c_19_r_*SK_MZ(2) + OP_l_7_c_17_r_*SK_MZ(5) - OP_l_7_c_18_r_*SK_MZ(4)); +Kfusion(8) = SK_MZ(1)*(OP_l_8_c_22_r_ + OP_l_8_c_1_r_*SH_MAG(2) - OP_l_8_c_2_r_*SH_MAG(3) + OP_l_8_c_4_r_*SH_MAG(1) + OP_l_8_c_3_r_*SK_MZ(3) + OP_l_8_c_19_r_*SK_MZ(2) + OP_l_8_c_17_r_*SK_MZ(5) - OP_l_8_c_18_r_*SK_MZ(4)); +Kfusion(9) = SK_MZ(1)*(OP_l_9_c_22_r_ + OP_l_9_c_1_r_*SH_MAG(2) - OP_l_9_c_2_r_*SH_MAG(3) + OP_l_9_c_4_r_*SH_MAG(1) + OP_l_9_c_3_r_*SK_MZ(3) + OP_l_9_c_19_r_*SK_MZ(2) + OP_l_9_c_17_r_*SK_MZ(5) - OP_l_9_c_18_r_*SK_MZ(4)); +Kfusion(10) = SK_MZ(1)*(OP_l_10_c_22_r_ + OP_l_10_c_1_r_*SH_MAG(2) - OP_l_10_c_2_r_*SH_MAG(3) + OP_l_10_c_4_r_*SH_MAG(1) + OP_l_10_c_3_r_*SK_MZ(3) + OP_l_10_c_19_r_*SK_MZ(2) + OP_l_10_c_17_r_*SK_MZ(5) - OP_l_10_c_18_r_*SK_MZ(4)); +Kfusion(11) = SK_MZ(1)*(OP_l_11_c_22_r_ + OP_l_11_c_1_r_*SH_MAG(2) - OP_l_11_c_2_r_*SH_MAG(3) + OP_l_11_c_4_r_*SH_MAG(1) + OP_l_11_c_3_r_*SK_MZ(3) + OP_l_11_c_19_r_*SK_MZ(2) + OP_l_11_c_17_r_*SK_MZ(5) - OP_l_11_c_18_r_*SK_MZ(4)); +Kfusion(12) = SK_MZ(1)*(OP_l_12_c_22_r_ + OP_l_12_c_1_r_*SH_MAG(2) - OP_l_12_c_2_r_*SH_MAG(3) + OP_l_12_c_4_r_*SH_MAG(1) + OP_l_12_c_3_r_*SK_MZ(3) + OP_l_12_c_19_r_*SK_MZ(2) + OP_l_12_c_17_r_*SK_MZ(5) - OP_l_12_c_18_r_*SK_MZ(4)); +Kfusion(13) = SK_MZ(1)*(OP_l_13_c_22_r_ + OP_l_13_c_1_r_*SH_MAG(2) - OP_l_13_c_2_r_*SH_MAG(3) + OP_l_13_c_4_r_*SH_MAG(1) + OP_l_13_c_3_r_*SK_MZ(3) + OP_l_13_c_19_r_*SK_MZ(2) + OP_l_13_c_17_r_*SK_MZ(5) - OP_l_13_c_18_r_*SK_MZ(4)); +Kfusion(14) = SK_MZ(1)*(OP_l_14_c_22_r_ + OP_l_14_c_1_r_*SH_MAG(2) - OP_l_14_c_2_r_*SH_MAG(3) + OP_l_14_c_4_r_*SH_MAG(1) + OP_l_14_c_3_r_*SK_MZ(3) + OP_l_14_c_19_r_*SK_MZ(2) + OP_l_14_c_17_r_*SK_MZ(5) - OP_l_14_c_18_r_*SK_MZ(4)); +Kfusion(15) = SK_MZ(1)*(OP_l_15_c_22_r_ + OP_l_15_c_1_r_*SH_MAG(2) - OP_l_15_c_2_r_*SH_MAG(3) + OP_l_15_c_4_r_*SH_MAG(1) + OP_l_15_c_3_r_*SK_MZ(3) + OP_l_15_c_19_r_*SK_MZ(2) + OP_l_15_c_17_r_*SK_MZ(5) - OP_l_15_c_18_r_*SK_MZ(4)); +Kfusion(16) = SK_MZ(1)*(OP_l_16_c_22_r_ + OP_l_16_c_1_r_*SH_MAG(2) - OP_l_16_c_2_r_*SH_MAG(3) + OP_l_16_c_4_r_*SH_MAG(1) + OP_l_16_c_3_r_*SK_MZ(3) + OP_l_16_c_19_r_*SK_MZ(2) + OP_l_16_c_17_r_*SK_MZ(5) - OP_l_16_c_18_r_*SK_MZ(4)); +Kfusion(17) = SK_MZ(1)*(OP_l_17_c_22_r_ + OP_l_17_c_1_r_*SH_MAG(2) - OP_l_17_c_2_r_*SH_MAG(3) + OP_l_17_c_4_r_*SH_MAG(1) + OP_l_17_c_3_r_*SK_MZ(3) + OP_l_17_c_19_r_*SK_MZ(2) + OP_l_17_c_17_r_*SK_MZ(5) - OP_l_17_c_18_r_*SK_MZ(4)); +Kfusion(18) = SK_MZ(1)*(OP_l_18_c_22_r_ + OP_l_18_c_1_r_*SH_MAG(2) - OP_l_18_c_2_r_*SH_MAG(3) + OP_l_18_c_4_r_*SH_MAG(1) + OP_l_18_c_3_r_*SK_MZ(3) + OP_l_18_c_19_r_*SK_MZ(2) + OP_l_18_c_17_r_*SK_MZ(5) - OP_l_18_c_18_r_*SK_MZ(4)); +Kfusion(19) = SK_MZ(1)*(OP_l_19_c_22_r_ + OP_l_19_c_1_r_*SH_MAG(2) - OP_l_19_c_2_r_*SH_MAG(3) + OP_l_19_c_4_r_*SH_MAG(1) + OP_l_19_c_3_r_*SK_MZ(3) + OP_l_19_c_19_r_*SK_MZ(2) + OP_l_19_c_17_r_*SK_MZ(5) - OP_l_19_c_18_r_*SK_MZ(4)); +Kfusion(20) = SK_MZ(1)*(OP_l_20_c_22_r_ + OP_l_20_c_1_r_*SH_MAG(2) - OP_l_20_c_2_r_*SH_MAG(3) + OP_l_20_c_4_r_*SH_MAG(1) + OP_l_20_c_3_r_*SK_MZ(3) + OP_l_20_c_19_r_*SK_MZ(2) + OP_l_20_c_17_r_*SK_MZ(5) - OP_l_20_c_18_r_*SK_MZ(4)); +Kfusion(21) = SK_MZ(1)*(OP_l_21_c_22_r_ + OP_l_21_c_1_r_*SH_MAG(2) - OP_l_21_c_2_r_*SH_MAG(3) + OP_l_21_c_4_r_*SH_MAG(1) + OP_l_21_c_3_r_*SK_MZ(3) + OP_l_21_c_19_r_*SK_MZ(2) + OP_l_21_c_17_r_*SK_MZ(5) - OP_l_21_c_18_r_*SK_MZ(4)); +Kfusion(22) = SK_MZ(1)*(OP_l_22_c_22_r_ + OP_l_22_c_1_r_*SH_MAG(2) - OP_l_22_c_2_r_*SH_MAG(3) + OP_l_22_c_4_r_*SH_MAG(1) + OP_l_22_c_3_r_*SK_MZ(3) + OP_l_22_c_19_r_*SK_MZ(2) + OP_l_22_c_17_r_*SK_MZ(5) - OP_l_22_c_18_r_*SK_MZ(4)); +Kfusion(23) = SK_MZ(1)*(OP_l_23_c_22_r_ + OP_l_23_c_1_r_*SH_MAG(2) - OP_l_23_c_2_r_*SH_MAG(3) + OP_l_23_c_4_r_*SH_MAG(1) + OP_l_23_c_3_r_*SK_MZ(3) + OP_l_23_c_19_r_*SK_MZ(2) + OP_l_23_c_17_r_*SK_MZ(5) - OP_l_23_c_18_r_*SK_MZ(4)); +Kfusion(24) = SK_MZ(1)*(OP_l_24_c_22_r_ + OP_l_24_c_1_r_*SH_MAG(2) - OP_l_24_c_2_r_*SH_MAG(3) + OP_l_24_c_4_r_*SH_MAG(1) + OP_l_24_c_3_r_*SK_MZ(3) + OP_l_24_c_19_r_*SK_MZ(2) + OP_l_24_c_17_r_*SK_MZ(5) - OP_l_24_c_18_r_*SK_MZ(4)); -SH_ACCX = zeros(7,1); +SH_ACCX = zeros(4,1); SH_ACCX(1) = q0^2 + q1^2 - q2^2 - q3^2; -SH_ACCX(2) = 2*q0*q3 + 2*q1*q2; -SH_ACCX(3) = vn - vwn; -SH_ACCX(4) = ve - vwe; -SH_ACCX(5) = q3^2; -SH_ACCX(6) = 2*q0*q2; -SH_ACCX(7) = 2*q0*q1; +SH_ACCX(2) = vn - vwn; +SH_ACCX(3) = ve - vwe; +SH_ACCX(4) = 2*q0*q3 + 2*q1*q2; H_ACCX = zeros(1,24); -H_ACCX(1,2) = Kaccx*(SH_ACCX(3)*(SH_ACCX(6) + 2*q1*q3) - SH_ACCX(4)*(SH_ACCX(7) - 2*q2*q3) + vd*(SH_ACCX(5) + q0^2 - q1^2 - q2^2)); -H_ACCX(1,3) = Kaccx*(SH_ACCX(3)*(2*q0*q3 - 2*q1*q2) + SH_ACCX(4)*(SH_ACCX(5) - q0^2 + q1^2 - q2^2) - vd*(SH_ACCX(7) + 2*q2*q3)); -H_ACCX(1,4) = -Kaccx*SH_ACCX(1); -H_ACCX(1,5) = -Kaccx*SH_ACCX(2); -H_ACCX(1,6) = Kaccx*(SH_ACCX(6) - 2*q1*q3); +H_ACCX(1,1) = -Kaccx*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd); +H_ACCX(1,2) = -Kaccx*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd); +H_ACCX(1,3) = Kaccx*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd); +H_ACCX(1,4) = -Kaccx*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd); +H_ACCX(1,5) = -Kaccx*SH_ACCX(1); +H_ACCX(1,6) = -Kaccx*SH_ACCX(4); +H_ACCX(1,7) = Kaccx*(2*q0*q2 - 2*q1*q3); H_ACCX(1,23) = Kaccx*SH_ACCX(1); -H_ACCX(1,24) = Kaccx*SH_ACCX(2); +H_ACCX(1,24) = Kaccx*SH_ACCX(4); -SK_ACCX = zeros(5,1); -SK_ACCX(1) = 1/(R_ACC - Kaccx*SH_ACCX(1)*(Kaccx*OP_l_23_c_4_r_*SH_ACCX(1) - Kaccx*OP_l_5_c_4_r_*SH_ACCX(2) - Kaccx*OP_l_4_c_4_r_*SH_ACCX(1) + Kaccx*OP_l_24_c_4_r_*SH_ACCX(2) + Kaccx*OP_l_3_c_4_r_*(SH_ACCX(3)*(2*q0*q3 - 2*q1*q2) + SH_ACCX(4)*(SH_ACCX(5) - q0^2 + q1^2 - q2^2) - vd*(SH_ACCX(7) + 2*q2*q3)) + Kaccx*OP_l_2_c_4_r_*(SH_ACCX(3)*(SH_ACCX(6) + 2*q1*q3) - SH_ACCX(4)*(SH_ACCX(7) - 2*q2*q3) + vd*(SH_ACCX(5) + q0^2 - q1^2 - q2^2)) + Kaccx*OP_l_6_c_4_r_*(SH_ACCX(6) - 2*q1*q3)) - Kaccx*SH_ACCX(2)*(Kaccx*OP_l_23_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_5_c_5_r_*SH_ACCX(2) - Kaccx*OP_l_4_c_5_r_*SH_ACCX(1) + Kaccx*OP_l_24_c_5_r_*SH_ACCX(2) + Kaccx*OP_l_3_c_5_r_*(SH_ACCX(3)*(2*q0*q3 - 2*q1*q2) + SH_ACCX(4)*(SH_ACCX(5) - q0^2 + q1^2 - q2^2) - vd*(SH_ACCX(7) + 2*q2*q3)) + Kaccx*OP_l_2_c_5_r_*(SH_ACCX(3)*(SH_ACCX(6) + 2*q1*q3) - SH_ACCX(4)*(SH_ACCX(7) - 2*q2*q3) + vd*(SH_ACCX(5) + q0^2 - q1^2 - q2^2)) + Kaccx*OP_l_6_c_5_r_*(SH_ACCX(6) - 2*q1*q3)) + Kaccx*SH_ACCX(1)*(Kaccx*OP_l_23_c_23_r_*SH_ACCX(1) - Kaccx*OP_l_5_c_23_r_*SH_ACCX(2) - Kaccx*OP_l_4_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_24_c_23_r_*SH_ACCX(2) + Kaccx*OP_l_3_c_23_r_*(SH_ACCX(3)*(2*q0*q3 - 2*q1*q2) + SH_ACCX(4)*(SH_ACCX(5) - q0^2 + q1^2 - q2^2) - vd*(SH_ACCX(7) + 2*q2*q3)) + Kaccx*OP_l_2_c_23_r_*(SH_ACCX(3)*(SH_ACCX(6) + 2*q1*q3) - SH_ACCX(4)*(SH_ACCX(7) - 2*q2*q3) + vd*(SH_ACCX(5) + q0^2 - q1^2 - q2^2)) + Kaccx*OP_l_6_c_23_r_*(SH_ACCX(6) - 2*q1*q3)) + Kaccx*SH_ACCX(2)*(Kaccx*OP_l_23_c_24_r_*SH_ACCX(1) - Kaccx*OP_l_5_c_24_r_*SH_ACCX(2) - Kaccx*OP_l_4_c_24_r_*SH_ACCX(1) + Kaccx*OP_l_24_c_24_r_*SH_ACCX(2) + Kaccx*OP_l_3_c_24_r_*(SH_ACCX(3)*(2*q0*q3 - 2*q1*q2) + SH_ACCX(4)*(SH_ACCX(5) - q0^2 + q1^2 - q2^2) - vd*(SH_ACCX(7) + 2*q2*q3)) + Kaccx*OP_l_2_c_24_r_*(SH_ACCX(3)*(SH_ACCX(6) + 2*q1*q3) - SH_ACCX(4)*(SH_ACCX(7) - 2*q2*q3) + vd*(SH_ACCX(5) + q0^2 - q1^2 - q2^2)) + Kaccx*OP_l_6_c_24_r_*(SH_ACCX(6) - 2*q1*q3)) + Kaccx*(SH_ACCX(3)*(2*q0*q3 - 2*q1*q2) + SH_ACCX(4)*(SH_ACCX(5) - q0^2 + q1^2 - q2^2) - vd*(SH_ACCX(7) + 2*q2*q3))*(Kaccx*OP_l_23_c_3_r_*SH_ACCX(1) - Kaccx*OP_l_5_c_3_r_*SH_ACCX(2) - Kaccx*OP_l_4_c_3_r_*SH_ACCX(1) + Kaccx*OP_l_24_c_3_r_*SH_ACCX(2) + Kaccx*OP_l_3_c_3_r_*(SH_ACCX(3)*(2*q0*q3 - 2*q1*q2) + SH_ACCX(4)*(SH_ACCX(5) - q0^2 + q1^2 - q2^2) - vd*(SH_ACCX(7) + 2*q2*q3)) + Kaccx*OP_l_2_c_3_r_*(SH_ACCX(3)*(SH_ACCX(6) + 2*q1*q3) - SH_ACCX(4)*(SH_ACCX(7) - 2*q2*q3) + vd*(SH_ACCX(5) + q0^2 - q1^2 - q2^2)) + Kaccx*OP_l_6_c_3_r_*(SH_ACCX(6) - 2*q1*q3)) + Kaccx*(SH_ACCX(3)*(SH_ACCX(6) + 2*q1*q3) - SH_ACCX(4)*(SH_ACCX(7) - 2*q2*q3) + vd*(SH_ACCX(5) + q0^2 - q1^2 - q2^2))*(Kaccx*OP_l_23_c_2_r_*SH_ACCX(1) - Kaccx*OP_l_5_c_2_r_*SH_ACCX(2) - Kaccx*OP_l_4_c_2_r_*SH_ACCX(1) + Kaccx*OP_l_24_c_2_r_*SH_ACCX(2) + Kaccx*OP_l_3_c_2_r_*(SH_ACCX(3)*(2*q0*q3 - 2*q1*q2) + SH_ACCX(4)*(SH_ACCX(5) - q0^2 + q1^2 - q2^2) - vd*(SH_ACCX(7) + 2*q2*q3)) + Kaccx*OP_l_2_c_2_r_*(SH_ACCX(3)*(SH_ACCX(6) + 2*q1*q3) - SH_ACCX(4)*(SH_ACCX(7) - 2*q2*q3) + vd*(SH_ACCX(5) + q0^2 - q1^2 - q2^2)) + Kaccx*OP_l_6_c_2_r_*(SH_ACCX(6) - 2*q1*q3)) + Kaccx*(SH_ACCX(6) - 2*q1*q3)*(Kaccx*OP_l_23_c_6_r_*SH_ACCX(1) - Kaccx*OP_l_5_c_6_r_*SH_ACCX(2) - Kaccx*OP_l_4_c_6_r_*SH_ACCX(1) + Kaccx*OP_l_24_c_6_r_*SH_ACCX(2) + Kaccx*OP_l_3_c_6_r_*(SH_ACCX(3)*(2*q0*q3 - 2*q1*q2) + SH_ACCX(4)*(SH_ACCX(5) - q0^2 + q1^2 - q2^2) - vd*(SH_ACCX(7) + 2*q2*q3)) + Kaccx*OP_l_2_c_6_r_*(SH_ACCX(3)*(SH_ACCX(6) + 2*q1*q3) - SH_ACCX(4)*(SH_ACCX(7) - 2*q2*q3) + vd*(SH_ACCX(5) + q0^2 - q1^2 - q2^2)) + Kaccx*OP_l_6_c_6_r_*(SH_ACCX(6) - 2*q1*q3))); -SK_ACCX(2) = SH_ACCX(3)*(2*q0*q3 - 2*q1*q2) + SH_ACCX(4)*(SH_ACCX(5) - q0^2 + q1^2 - q2^2) - vd*(SH_ACCX(7) + 2*q2*q3); -SK_ACCX(3) = SH_ACCX(3)*(SH_ACCX(6) + 2*q1*q3) - SH_ACCX(4)*(SH_ACCX(7) - 2*q2*q3) + vd*(SH_ACCX(5) + q0^2 - q1^2 - q2^2); -SK_ACCX(4) = SH_ACCX(6) - 2*q1*q3; -SK_ACCX(5) = SH_ACCX(2); +SK_ACCX = zeros(7,1); +SK_ACCX(1) = 1/(R_ACC + Kaccx*SH_ACCX(1)*(Kaccx*OP_l_5_c_5_r_*SH_ACCX(1) + Kaccx*OP_l_6_c_5_r_*SH_ACCX(4) - Kaccx*OP_l_23_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_24_c_5_r_*SH_ACCX(4) - Kaccx*OP_l_7_c_5_r_*(2*q0*q2 - 2*q1*q3) + Kaccx*OP_l_1_c_5_r_*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd) + Kaccx*OP_l_2_c_5_r_*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd) - Kaccx*OP_l_3_c_5_r_*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd) + Kaccx*OP_l_4_c_5_r_*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd)) + Kaccx*SH_ACCX(4)*(Kaccx*OP_l_5_c_6_r_*SH_ACCX(1) + Kaccx*OP_l_6_c_6_r_*SH_ACCX(4) - Kaccx*OP_l_23_c_6_r_*SH_ACCX(1) - Kaccx*OP_l_24_c_6_r_*SH_ACCX(4) - Kaccx*OP_l_7_c_6_r_*(2*q0*q2 - 2*q1*q3) + Kaccx*OP_l_1_c_6_r_*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd) + Kaccx*OP_l_2_c_6_r_*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd) - Kaccx*OP_l_3_c_6_r_*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd) + Kaccx*OP_l_4_c_6_r_*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd)) - Kaccx*SH_ACCX(1)*(Kaccx*OP_l_5_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_6_c_23_r_*SH_ACCX(4) - Kaccx*OP_l_23_c_23_r_*SH_ACCX(1) - Kaccx*OP_l_24_c_23_r_*SH_ACCX(4) - Kaccx*OP_l_7_c_23_r_*(2*q0*q2 - 2*q1*q3) + Kaccx*OP_l_1_c_23_r_*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd) + Kaccx*OP_l_2_c_23_r_*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd) - Kaccx*OP_l_3_c_23_r_*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd) + Kaccx*OP_l_4_c_23_r_*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd)) - Kaccx*SH_ACCX(4)*(Kaccx*OP_l_5_c_24_r_*SH_ACCX(1) + Kaccx*OP_l_6_c_24_r_*SH_ACCX(4) - Kaccx*OP_l_23_c_24_r_*SH_ACCX(1) - Kaccx*OP_l_24_c_24_r_*SH_ACCX(4) - Kaccx*OP_l_7_c_24_r_*(2*q0*q2 - 2*q1*q3) + Kaccx*OP_l_1_c_24_r_*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd) + Kaccx*OP_l_2_c_24_r_*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd) - Kaccx*OP_l_3_c_24_r_*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd) + Kaccx*OP_l_4_c_24_r_*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd)) - Kaccx*(2*q0*q2 - 2*q1*q3)*(Kaccx*OP_l_5_c_7_r_*SH_ACCX(1) + Kaccx*OP_l_6_c_7_r_*SH_ACCX(4) - Kaccx*OP_l_23_c_7_r_*SH_ACCX(1) - Kaccx*OP_l_24_c_7_r_*SH_ACCX(4) - Kaccx*OP_l_7_c_7_r_*(2*q0*q2 - 2*q1*q3) + Kaccx*OP_l_1_c_7_r_*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd) + Kaccx*OP_l_2_c_7_r_*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd) - Kaccx*OP_l_3_c_7_r_*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd) + Kaccx*OP_l_4_c_7_r_*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd)) + Kaccx*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd)*(Kaccx*OP_l_5_c_1_r_*SH_ACCX(1) + Kaccx*OP_l_6_c_1_r_*SH_ACCX(4) - Kaccx*OP_l_23_c_1_r_*SH_ACCX(1) - Kaccx*OP_l_24_c_1_r_*SH_ACCX(4) - Kaccx*OP_l_7_c_1_r_*(2*q0*q2 - 2*q1*q3) + Kaccx*OP_l_1_c_1_r_*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd) + Kaccx*OP_l_2_c_1_r_*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd) - Kaccx*OP_l_3_c_1_r_*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd) + Kaccx*OP_l_4_c_1_r_*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd)) + Kaccx*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd)*(Kaccx*OP_l_5_c_2_r_*SH_ACCX(1) + Kaccx*OP_l_6_c_2_r_*SH_ACCX(4) - Kaccx*OP_l_23_c_2_r_*SH_ACCX(1) - Kaccx*OP_l_24_c_2_r_*SH_ACCX(4) - Kaccx*OP_l_7_c_2_r_*(2*q0*q2 - 2*q1*q3) + Kaccx*OP_l_1_c_2_r_*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd) + Kaccx*OP_l_2_c_2_r_*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd) - Kaccx*OP_l_3_c_2_r_*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd) + Kaccx*OP_l_4_c_2_r_*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd)) - Kaccx*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd)*(Kaccx*OP_l_5_c_3_r_*SH_ACCX(1) + Kaccx*OP_l_6_c_3_r_*SH_ACCX(4) - Kaccx*OP_l_23_c_3_r_*SH_ACCX(1) - Kaccx*OP_l_24_c_3_r_*SH_ACCX(4) - Kaccx*OP_l_7_c_3_r_*(2*q0*q2 - 2*q1*q3) + Kaccx*OP_l_1_c_3_r_*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd) + Kaccx*OP_l_2_c_3_r_*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd) - Kaccx*OP_l_3_c_3_r_*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd) + Kaccx*OP_l_4_c_3_r_*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd)) + Kaccx*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd)*(Kaccx*OP_l_5_c_4_r_*SH_ACCX(1) + Kaccx*OP_l_6_c_4_r_*SH_ACCX(4) - Kaccx*OP_l_23_c_4_r_*SH_ACCX(1) - Kaccx*OP_l_24_c_4_r_*SH_ACCX(4) - Kaccx*OP_l_7_c_4_r_*(2*q0*q2 - 2*q1*q3) + Kaccx*OP_l_1_c_4_r_*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd) + Kaccx*OP_l_2_c_4_r_*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd) - Kaccx*OP_l_3_c_4_r_*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd) + Kaccx*OP_l_4_c_4_r_*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd))); +SK_ACCX(2) = 2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd; +SK_ACCX(3) = 2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd; +SK_ACCX(4) = 2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd; +SK_ACCX(5) = 2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd; +SK_ACCX(6) = 2*q0*q2 - 2*q1*q3; +SK_ACCX(7) = SH_ACCX(4); Kfusion = zeros(24,1); Kfusion = zeros(1,1); -Kfusion(1) = SK_ACCX(1)*(Kaccx*OP_l_1_c_23_r_*SH_ACCX(1) - Kaccx*OP_l_1_c_4_r_*SH_ACCX(1) + Kaccx*OP_l_1_c_2_r_*SK_ACCX(3) + Kaccx*OP_l_1_c_3_r_*SK_ACCX(2) - Kaccx*OP_l_1_c_5_r_*SK_ACCX(5) + Kaccx*OP_l_1_c_6_r_*SK_ACCX(4) + Kaccx*OP_l_1_c_24_r_*SK_ACCX(5)); -Kfusion(2) = SK_ACCX(1)*(Kaccx*OP_l_2_c_23_r_*SH_ACCX(1) - Kaccx*OP_l_2_c_4_r_*SH_ACCX(1) + Kaccx*OP_l_2_c_2_r_*SK_ACCX(3) + Kaccx*OP_l_2_c_3_r_*SK_ACCX(2) - Kaccx*OP_l_2_c_5_r_*SK_ACCX(5) + Kaccx*OP_l_2_c_6_r_*SK_ACCX(4) + Kaccx*OP_l_2_c_24_r_*SK_ACCX(5)); -Kfusion(3) = SK_ACCX(1)*(Kaccx*OP_l_3_c_23_r_*SH_ACCX(1) - Kaccx*OP_l_3_c_4_r_*SH_ACCX(1) + Kaccx*OP_l_3_c_2_r_*SK_ACCX(3) + Kaccx*OP_l_3_c_3_r_*SK_ACCX(2) - Kaccx*OP_l_3_c_5_r_*SK_ACCX(5) + Kaccx*OP_l_3_c_6_r_*SK_ACCX(4) + Kaccx*OP_l_3_c_24_r_*SK_ACCX(5)); -Kfusion(4) = SK_ACCX(1)*(Kaccx*OP_l_4_c_23_r_*SH_ACCX(1) - Kaccx*OP_l_4_c_4_r_*SH_ACCX(1) + Kaccx*OP_l_4_c_2_r_*SK_ACCX(3) + Kaccx*OP_l_4_c_3_r_*SK_ACCX(2) - Kaccx*OP_l_4_c_5_r_*SK_ACCX(5) + Kaccx*OP_l_4_c_6_r_*SK_ACCX(4) + Kaccx*OP_l_4_c_24_r_*SK_ACCX(5)); -Kfusion(5) = SK_ACCX(1)*(Kaccx*OP_l_5_c_23_r_*SH_ACCX(1) - Kaccx*OP_l_5_c_4_r_*SH_ACCX(1) + Kaccx*OP_l_5_c_2_r_*SK_ACCX(3) + Kaccx*OP_l_5_c_3_r_*SK_ACCX(2) - Kaccx*OP_l_5_c_5_r_*SK_ACCX(5) + Kaccx*OP_l_5_c_6_r_*SK_ACCX(4) + Kaccx*OP_l_5_c_24_r_*SK_ACCX(5)); -Kfusion(6) = SK_ACCX(1)*(Kaccx*OP_l_6_c_23_r_*SH_ACCX(1) - Kaccx*OP_l_6_c_4_r_*SH_ACCX(1) + Kaccx*OP_l_6_c_2_r_*SK_ACCX(3) + Kaccx*OP_l_6_c_3_r_*SK_ACCX(2) - Kaccx*OP_l_6_c_5_r_*SK_ACCX(5) + Kaccx*OP_l_6_c_6_r_*SK_ACCX(4) + Kaccx*OP_l_6_c_24_r_*SK_ACCX(5)); -Kfusion(7) = SK_ACCX(1)*(Kaccx*OP_l_7_c_23_r_*SH_ACCX(1) - Kaccx*OP_l_7_c_4_r_*SH_ACCX(1) + Kaccx*OP_l_7_c_2_r_*SK_ACCX(3) + Kaccx*OP_l_7_c_3_r_*SK_ACCX(2) - Kaccx*OP_l_7_c_5_r_*SK_ACCX(5) + Kaccx*OP_l_7_c_6_r_*SK_ACCX(4) + Kaccx*OP_l_7_c_24_r_*SK_ACCX(5)); -Kfusion(8) = SK_ACCX(1)*(Kaccx*OP_l_8_c_23_r_*SH_ACCX(1) - Kaccx*OP_l_8_c_4_r_*SH_ACCX(1) + Kaccx*OP_l_8_c_2_r_*SK_ACCX(3) + Kaccx*OP_l_8_c_3_r_*SK_ACCX(2) - Kaccx*OP_l_8_c_5_r_*SK_ACCX(5) + Kaccx*OP_l_8_c_6_r_*SK_ACCX(4) + Kaccx*OP_l_8_c_24_r_*SK_ACCX(5)); -Kfusion(9) = SK_ACCX(1)*(Kaccx*OP_l_9_c_23_r_*SH_ACCX(1) - Kaccx*OP_l_9_c_4_r_*SH_ACCX(1) + Kaccx*OP_l_9_c_2_r_*SK_ACCX(3) + Kaccx*OP_l_9_c_3_r_*SK_ACCX(2) - Kaccx*OP_l_9_c_5_r_*SK_ACCX(5) + Kaccx*OP_l_9_c_6_r_*SK_ACCX(4) + Kaccx*OP_l_9_c_24_r_*SK_ACCX(5)); -Kfusion(10) = SK_ACCX(1)*(Kaccx*OP_l_10_c_23_r_*SH_ACCX(1) - Kaccx*OP_l_10_c_4_r_*SH_ACCX(1) + Kaccx*OP_l_10_c_2_r_*SK_ACCX(3) + Kaccx*OP_l_10_c_3_r_*SK_ACCX(2) - Kaccx*OP_l_10_c_5_r_*SK_ACCX(5) + Kaccx*OP_l_10_c_6_r_*SK_ACCX(4) + Kaccx*OP_l_10_c_24_r_*SK_ACCX(5)); -Kfusion(11) = SK_ACCX(1)*(Kaccx*OP_l_11_c_23_r_*SH_ACCX(1) - Kaccx*OP_l_11_c_4_r_*SH_ACCX(1) + Kaccx*OP_l_11_c_2_r_*SK_ACCX(3) + Kaccx*OP_l_11_c_3_r_*SK_ACCX(2) - Kaccx*OP_l_11_c_5_r_*SK_ACCX(5) + Kaccx*OP_l_11_c_6_r_*SK_ACCX(4) + Kaccx*OP_l_11_c_24_r_*SK_ACCX(5)); -Kfusion(12) = SK_ACCX(1)*(Kaccx*OP_l_12_c_23_r_*SH_ACCX(1) - Kaccx*OP_l_12_c_4_r_*SH_ACCX(1) + Kaccx*OP_l_12_c_2_r_*SK_ACCX(3) + Kaccx*OP_l_12_c_3_r_*SK_ACCX(2) - Kaccx*OP_l_12_c_5_r_*SK_ACCX(5) + Kaccx*OP_l_12_c_6_r_*SK_ACCX(4) + Kaccx*OP_l_12_c_24_r_*SK_ACCX(5)); -Kfusion(13) = SK_ACCX(1)*(Kaccx*OP_l_13_c_23_r_*SH_ACCX(1) - Kaccx*OP_l_13_c_4_r_*SH_ACCX(1) + Kaccx*OP_l_13_c_2_r_*SK_ACCX(3) + Kaccx*OP_l_13_c_3_r_*SK_ACCX(2) - Kaccx*OP_l_13_c_5_r_*SK_ACCX(5) + Kaccx*OP_l_13_c_6_r_*SK_ACCX(4) + Kaccx*OP_l_13_c_24_r_*SK_ACCX(5)); -Kfusion(14) = SK_ACCX(1)*(Kaccx*OP_l_14_c_23_r_*SH_ACCX(1) - Kaccx*OP_l_14_c_4_r_*SH_ACCX(1) + Kaccx*OP_l_14_c_2_r_*SK_ACCX(3) + Kaccx*OP_l_14_c_3_r_*SK_ACCX(2) - Kaccx*OP_l_14_c_5_r_*SK_ACCX(5) + Kaccx*OP_l_14_c_6_r_*SK_ACCX(4) + Kaccx*OP_l_14_c_24_r_*SK_ACCX(5)); -Kfusion(15) = SK_ACCX(1)*(Kaccx*OP_l_15_c_23_r_*SH_ACCX(1) - Kaccx*OP_l_15_c_4_r_*SH_ACCX(1) + Kaccx*OP_l_15_c_2_r_*SK_ACCX(3) + Kaccx*OP_l_15_c_3_r_*SK_ACCX(2) - Kaccx*OP_l_15_c_5_r_*SK_ACCX(5) + Kaccx*OP_l_15_c_6_r_*SK_ACCX(4) + Kaccx*OP_l_15_c_24_r_*SK_ACCX(5)); -Kfusion(16) = SK_ACCX(1)*(Kaccx*OP_l_16_c_23_r_*SH_ACCX(1) - Kaccx*OP_l_16_c_4_r_*SH_ACCX(1) + Kaccx*OP_l_16_c_2_r_*SK_ACCX(3) + Kaccx*OP_l_16_c_3_r_*SK_ACCX(2) - Kaccx*OP_l_16_c_5_r_*SK_ACCX(5) + Kaccx*OP_l_16_c_6_r_*SK_ACCX(4) + Kaccx*OP_l_16_c_24_r_*SK_ACCX(5)); -Kfusion(17) = SK_ACCX(1)*(Kaccx*OP_l_17_c_23_r_*SH_ACCX(1) - Kaccx*OP_l_17_c_4_r_*SH_ACCX(1) + Kaccx*OP_l_17_c_2_r_*SK_ACCX(3) + Kaccx*OP_l_17_c_3_r_*SK_ACCX(2) - Kaccx*OP_l_17_c_5_r_*SK_ACCX(5) + Kaccx*OP_l_17_c_6_r_*SK_ACCX(4) + Kaccx*OP_l_17_c_24_r_*SK_ACCX(5)); -Kfusion(18) = SK_ACCX(1)*(Kaccx*OP_l_18_c_23_r_*SH_ACCX(1) - Kaccx*OP_l_18_c_4_r_*SH_ACCX(1) + Kaccx*OP_l_18_c_2_r_*SK_ACCX(3) + Kaccx*OP_l_18_c_3_r_*SK_ACCX(2) - Kaccx*OP_l_18_c_5_r_*SK_ACCX(5) + Kaccx*OP_l_18_c_6_r_*SK_ACCX(4) + Kaccx*OP_l_18_c_24_r_*SK_ACCX(5)); -Kfusion(19) = SK_ACCX(1)*(Kaccx*OP_l_19_c_23_r_*SH_ACCX(1) - Kaccx*OP_l_19_c_4_r_*SH_ACCX(1) + Kaccx*OP_l_19_c_2_r_*SK_ACCX(3) + Kaccx*OP_l_19_c_3_r_*SK_ACCX(2) - Kaccx*OP_l_19_c_5_r_*SK_ACCX(5) + Kaccx*OP_l_19_c_6_r_*SK_ACCX(4) + Kaccx*OP_l_19_c_24_r_*SK_ACCX(5)); -Kfusion(20) = SK_ACCX(1)*(Kaccx*OP_l_20_c_23_r_*SH_ACCX(1) - Kaccx*OP_l_20_c_4_r_*SH_ACCX(1) + Kaccx*OP_l_20_c_2_r_*SK_ACCX(3) + Kaccx*OP_l_20_c_3_r_*SK_ACCX(2) - Kaccx*OP_l_20_c_5_r_*SK_ACCX(5) + Kaccx*OP_l_20_c_6_r_*SK_ACCX(4) + Kaccx*OP_l_20_c_24_r_*SK_ACCX(5)); -Kfusion(21) = SK_ACCX(1)*(Kaccx*OP_l_21_c_23_r_*SH_ACCX(1) - Kaccx*OP_l_21_c_4_r_*SH_ACCX(1) + Kaccx*OP_l_21_c_2_r_*SK_ACCX(3) + Kaccx*OP_l_21_c_3_r_*SK_ACCX(2) - Kaccx*OP_l_21_c_5_r_*SK_ACCX(5) + Kaccx*OP_l_21_c_6_r_*SK_ACCX(4) + Kaccx*OP_l_21_c_24_r_*SK_ACCX(5)); -Kfusion(22) = SK_ACCX(1)*(Kaccx*OP_l_22_c_23_r_*SH_ACCX(1) - Kaccx*OP_l_22_c_4_r_*SH_ACCX(1) + Kaccx*OP_l_22_c_2_r_*SK_ACCX(3) + Kaccx*OP_l_22_c_3_r_*SK_ACCX(2) - Kaccx*OP_l_22_c_5_r_*SK_ACCX(5) + Kaccx*OP_l_22_c_6_r_*SK_ACCX(4) + Kaccx*OP_l_22_c_24_r_*SK_ACCX(5)); -Kfusion(23) = SK_ACCX(1)*(Kaccx*OP_l_23_c_23_r_*SH_ACCX(1) - Kaccx*OP_l_23_c_4_r_*SH_ACCX(1) + Kaccx*OP_l_23_c_2_r_*SK_ACCX(3) + Kaccx*OP_l_23_c_3_r_*SK_ACCX(2) - Kaccx*OP_l_23_c_5_r_*SK_ACCX(5) + Kaccx*OP_l_23_c_6_r_*SK_ACCX(4) + Kaccx*OP_l_23_c_24_r_*SK_ACCX(5)); -Kfusion(24) = SK_ACCX(1)*(Kaccx*OP_l_24_c_23_r_*SH_ACCX(1) - Kaccx*OP_l_24_c_4_r_*SH_ACCX(1) + Kaccx*OP_l_24_c_2_r_*SK_ACCX(3) + Kaccx*OP_l_24_c_3_r_*SK_ACCX(2) - Kaccx*OP_l_24_c_5_r_*SK_ACCX(5) + Kaccx*OP_l_24_c_6_r_*SK_ACCX(4) + Kaccx*OP_l_24_c_24_r_*SK_ACCX(5)); +Kfusion(1) = -SK_ACCX(1)*(Kaccx*OP_l_1_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_1_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_1_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_1_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_1_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_1_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_1_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_1_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_1_c_24_r_*SK_ACCX(7)); +Kfusion(2) = -SK_ACCX(1)*(Kaccx*OP_l_2_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_2_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_2_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_2_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_2_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_2_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_2_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_2_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_2_c_24_r_*SK_ACCX(7)); +Kfusion(3) = -SK_ACCX(1)*(Kaccx*OP_l_3_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_3_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_3_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_3_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_3_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_3_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_3_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_3_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_3_c_24_r_*SK_ACCX(7)); +Kfusion(4) = -SK_ACCX(1)*(Kaccx*OP_l_4_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_4_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_4_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_4_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_4_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_4_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_4_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_4_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_4_c_24_r_*SK_ACCX(7)); +Kfusion(5) = -SK_ACCX(1)*(Kaccx*OP_l_5_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_5_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_5_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_5_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_5_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_5_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_5_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_5_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_5_c_24_r_*SK_ACCX(7)); +Kfusion(6) = -SK_ACCX(1)*(Kaccx*OP_l_6_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_6_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_6_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_6_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_6_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_6_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_6_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_6_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_6_c_24_r_*SK_ACCX(7)); +Kfusion(7) = -SK_ACCX(1)*(Kaccx*OP_l_7_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_7_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_7_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_7_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_7_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_7_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_7_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_7_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_7_c_24_r_*SK_ACCX(7)); +Kfusion(8) = -SK_ACCX(1)*(Kaccx*OP_l_8_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_8_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_8_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_8_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_8_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_8_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_8_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_8_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_8_c_24_r_*SK_ACCX(7)); +Kfusion(9) = -SK_ACCX(1)*(Kaccx*OP_l_9_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_9_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_9_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_9_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_9_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_9_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_9_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_9_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_9_c_24_r_*SK_ACCX(7)); +Kfusion(10) = -SK_ACCX(1)*(Kaccx*OP_l_10_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_10_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_10_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_10_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_10_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_10_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_10_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_10_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_10_c_24_r_*SK_ACCX(7)); +Kfusion(11) = -SK_ACCX(1)*(Kaccx*OP_l_11_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_11_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_11_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_11_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_11_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_11_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_11_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_11_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_11_c_24_r_*SK_ACCX(7)); +Kfusion(12) = -SK_ACCX(1)*(Kaccx*OP_l_12_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_12_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_12_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_12_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_12_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_12_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_12_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_12_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_12_c_24_r_*SK_ACCX(7)); +Kfusion(13) = -SK_ACCX(1)*(Kaccx*OP_l_13_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_13_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_13_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_13_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_13_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_13_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_13_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_13_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_13_c_24_r_*SK_ACCX(7)); +Kfusion(14) = -SK_ACCX(1)*(Kaccx*OP_l_14_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_14_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_14_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_14_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_14_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_14_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_14_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_14_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_14_c_24_r_*SK_ACCX(7)); +Kfusion(15) = -SK_ACCX(1)*(Kaccx*OP_l_15_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_15_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_15_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_15_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_15_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_15_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_15_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_15_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_15_c_24_r_*SK_ACCX(7)); +Kfusion(16) = -SK_ACCX(1)*(Kaccx*OP_l_16_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_16_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_16_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_16_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_16_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_16_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_16_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_16_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_16_c_24_r_*SK_ACCX(7)); +Kfusion(17) = -SK_ACCX(1)*(Kaccx*OP_l_17_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_17_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_17_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_17_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_17_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_17_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_17_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_17_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_17_c_24_r_*SK_ACCX(7)); +Kfusion(18) = -SK_ACCX(1)*(Kaccx*OP_l_18_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_18_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_18_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_18_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_18_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_18_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_18_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_18_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_18_c_24_r_*SK_ACCX(7)); +Kfusion(19) = -SK_ACCX(1)*(Kaccx*OP_l_19_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_19_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_19_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_19_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_19_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_19_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_19_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_19_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_19_c_24_r_*SK_ACCX(7)); +Kfusion(20) = -SK_ACCX(1)*(Kaccx*OP_l_20_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_20_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_20_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_20_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_20_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_20_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_20_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_20_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_20_c_24_r_*SK_ACCX(7)); +Kfusion(21) = -SK_ACCX(1)*(Kaccx*OP_l_21_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_21_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_21_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_21_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_21_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_21_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_21_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_21_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_21_c_24_r_*SK_ACCX(7)); +Kfusion(22) = -SK_ACCX(1)*(Kaccx*OP_l_22_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_22_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_22_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_22_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_22_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_22_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_22_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_22_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_22_c_24_r_*SK_ACCX(7)); +Kfusion(23) = -SK_ACCX(1)*(Kaccx*OP_l_23_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_23_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_23_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_23_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_23_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_23_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_23_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_23_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_23_c_24_r_*SK_ACCX(7)); +Kfusion(24) = -SK_ACCX(1)*(Kaccx*OP_l_24_c_5_r_*SH_ACCX(1) - Kaccx*OP_l_24_c_23_r_*SH_ACCX(1) + Kaccx*OP_l_24_c_1_r_*SK_ACCX(4) - Kaccx*OP_l_24_c_3_r_*SK_ACCX(3) + Kaccx*OP_l_24_c_4_r_*SK_ACCX(2) + Kaccx*OP_l_24_c_2_r_*SK_ACCX(5) + Kaccx*OP_l_24_c_6_r_*SK_ACCX(7) - Kaccx*OP_l_24_c_7_r_*SK_ACCX(6) - Kaccx*OP_l_24_c_24_r_*SK_ACCX(7)); -SH_ACCY = zeros(6,1); +SH_ACCY = zeros(3,1); SH_ACCY(1) = q0^2 - q1^2 + q2^2 - q3^2; -SH_ACCY(2) = ve - vwe; -SH_ACCY(3) = vn - vwn; -SH_ACCY(4) = q1^2; -SH_ACCY(5) = 2*q0*q1; -SH_ACCY(6) = 2*q0*q3; +SH_ACCY(2) = vn - vwn; +SH_ACCY(3) = ve - vwe; H_ACCY = zeros(1,24); -H_ACCY(1,1) = Kaccy*(SH_ACCY(2)*(SH_ACCY(5) - 2*q2*q3) - SH_ACCY(3)*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY(4) - q0^2 + q2^2 - q3^2)); -H_ACCY(1,3) = Kaccy*(SH_ACCY(3)*(SH_ACCY(4) + q0^2 - q2^2 - q3^2) + SH_ACCY(2)*(SH_ACCY(6) + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)); -H_ACCY(1,4) = Kaccy*(SH_ACCY(6) - 2*q1*q2); -H_ACCY(1,5) = -Kaccy*SH_ACCY(1); -H_ACCY(1,6) = -Kaccy*(SH_ACCY(5) + 2*q2*q3); +H_ACCY(1,1) = -Kaccy*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd); +H_ACCY(1,2) = -Kaccy*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd); +H_ACCY(1,3) = -Kaccy*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd); +H_ACCY(1,4) = Kaccy*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd); +H_ACCY(1,5) = Kaccy*(2*q0*q3 - 2*q1*q2); +H_ACCY(1,6) = -Kaccy*SH_ACCY(1); +H_ACCY(1,7) = -Kaccy*(2*q0*q1 + 2*q2*q3); H_ACCY(1,23) = -2*Kaccy*(q0*q3 - q1*q2); H_ACCY(1,24) = Kaccy*SH_ACCY(1); -SK_ACCY = zeros(7,1); -SK_ACCY(1) = 1/(R_ACC - Kaccy*SH_ACCY(1)*(Kaccy*OP_l_24_c_5_r_*SH_ACCY(1) - Kaccy*OP_l_5_c_5_r_*SH_ACCY(1) + Kaccy*OP_l_1_c_5_r_*(SH_ACCY(2)*(SH_ACCY(5) - 2*q2*q3) - SH_ACCY(3)*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY(4) - q0^2 + q2^2 - q3^2)) + Kaccy*OP_l_3_c_5_r_*(SH_ACCY(3)*(SH_ACCY(4) + q0^2 - q2^2 - q3^2) + SH_ACCY(2)*(SH_ACCY(6) + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)) - 2*Kaccy*OP_l_23_c_5_r_*(q0*q3 - q1*q2) + Kaccy*OP_l_4_c_5_r_*(SH_ACCY(6) - 2*q1*q2) - Kaccy*OP_l_6_c_5_r_*(SH_ACCY(5) + 2*q2*q3)) + Kaccy*SH_ACCY(1)*(Kaccy*OP_l_24_c_24_r_*SH_ACCY(1) - Kaccy*OP_l_5_c_24_r_*SH_ACCY(1) + Kaccy*OP_l_1_c_24_r_*(SH_ACCY(2)*(SH_ACCY(5) - 2*q2*q3) - SH_ACCY(3)*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY(4) - q0^2 + q2^2 - q3^2)) + Kaccy*OP_l_3_c_24_r_*(SH_ACCY(3)*(SH_ACCY(4) + q0^2 - q2^2 - q3^2) + SH_ACCY(2)*(SH_ACCY(6) + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)) - 2*Kaccy*OP_l_23_c_24_r_*(q0*q3 - q1*q2) + Kaccy*OP_l_4_c_24_r_*(SH_ACCY(6) - 2*q1*q2) - Kaccy*OP_l_6_c_24_r_*(SH_ACCY(5) + 2*q2*q3)) + Kaccy*(SH_ACCY(2)*(SH_ACCY(5) - 2*q2*q3) - SH_ACCY(3)*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY(4) - q0^2 + q2^2 - q3^2))*(Kaccy*OP_l_24_c_1_r_*SH_ACCY(1) - Kaccy*OP_l_5_c_1_r_*SH_ACCY(1) + Kaccy*OP_l_1_c_1_r_*(SH_ACCY(2)*(SH_ACCY(5) - 2*q2*q3) - SH_ACCY(3)*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY(4) - q0^2 + q2^2 - q3^2)) + Kaccy*OP_l_3_c_1_r_*(SH_ACCY(3)*(SH_ACCY(4) + q0^2 - q2^2 - q3^2) + SH_ACCY(2)*(SH_ACCY(6) + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)) - 2*Kaccy*OP_l_23_c_1_r_*(q0*q3 - q1*q2) + Kaccy*OP_l_4_c_1_r_*(SH_ACCY(6) - 2*q1*q2) - Kaccy*OP_l_6_c_1_r_*(SH_ACCY(5) + 2*q2*q3)) + Kaccy*(SH_ACCY(3)*(SH_ACCY(4) + q0^2 - q2^2 - q3^2) + SH_ACCY(2)*(SH_ACCY(6) + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3))*(Kaccy*OP_l_24_c_3_r_*SH_ACCY(1) - Kaccy*OP_l_5_c_3_r_*SH_ACCY(1) + Kaccy*OP_l_1_c_3_r_*(SH_ACCY(2)*(SH_ACCY(5) - 2*q2*q3) - SH_ACCY(3)*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY(4) - q0^2 + q2^2 - q3^2)) + Kaccy*OP_l_3_c_3_r_*(SH_ACCY(3)*(SH_ACCY(4) + q0^2 - q2^2 - q3^2) + SH_ACCY(2)*(SH_ACCY(6) + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)) - 2*Kaccy*OP_l_23_c_3_r_*(q0*q3 - q1*q2) + Kaccy*OP_l_4_c_3_r_*(SH_ACCY(6) - 2*q1*q2) - Kaccy*OP_l_6_c_3_r_*(SH_ACCY(5) + 2*q2*q3)) - 2*Kaccy*(q0*q3 - q1*q2)*(Kaccy*OP_l_24_c_23_r_*SH_ACCY(1) - Kaccy*OP_l_5_c_23_r_*SH_ACCY(1) + Kaccy*OP_l_1_c_23_r_*(SH_ACCY(2)*(SH_ACCY(5) - 2*q2*q3) - SH_ACCY(3)*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY(4) - q0^2 + q2^2 - q3^2)) + Kaccy*OP_l_3_c_23_r_*(SH_ACCY(3)*(SH_ACCY(4) + q0^2 - q2^2 - q3^2) + SH_ACCY(2)*(SH_ACCY(6) + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)) - 2*Kaccy*OP_l_23_c_23_r_*(q0*q3 - q1*q2) + Kaccy*OP_l_4_c_23_r_*(SH_ACCY(6) - 2*q1*q2) - Kaccy*OP_l_6_c_23_r_*(SH_ACCY(5) + 2*q2*q3)) + Kaccy*(SH_ACCY(6) - 2*q1*q2)*(Kaccy*OP_l_24_c_4_r_*SH_ACCY(1) - Kaccy*OP_l_5_c_4_r_*SH_ACCY(1) + Kaccy*OP_l_1_c_4_r_*(SH_ACCY(2)*(SH_ACCY(5) - 2*q2*q3) - SH_ACCY(3)*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY(4) - q0^2 + q2^2 - q3^2)) + Kaccy*OP_l_3_c_4_r_*(SH_ACCY(3)*(SH_ACCY(4) + q0^2 - q2^2 - q3^2) + SH_ACCY(2)*(SH_ACCY(6) + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)) - 2*Kaccy*OP_l_23_c_4_r_*(q0*q3 - q1*q2) + Kaccy*OP_l_4_c_4_r_*(SH_ACCY(6) - 2*q1*q2) - Kaccy*OP_l_6_c_4_r_*(SH_ACCY(5) + 2*q2*q3)) - Kaccy*(SH_ACCY(5) + 2*q2*q3)*(Kaccy*OP_l_24_c_6_r_*SH_ACCY(1) - Kaccy*OP_l_5_c_6_r_*SH_ACCY(1) + Kaccy*OP_l_1_c_6_r_*(SH_ACCY(2)*(SH_ACCY(5) - 2*q2*q3) - SH_ACCY(3)*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY(4) - q0^2 + q2^2 - q3^2)) + Kaccy*OP_l_3_c_6_r_*(SH_ACCY(3)*(SH_ACCY(4) + q0^2 - q2^2 - q3^2) + SH_ACCY(2)*(SH_ACCY(6) + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3)) - 2*Kaccy*OP_l_23_c_6_r_*(q0*q3 - q1*q2) + Kaccy*OP_l_4_c_6_r_*(SH_ACCY(6) - 2*q1*q2) - Kaccy*OP_l_6_c_6_r_*(SH_ACCY(5) + 2*q2*q3))); -SK_ACCY(2) = SH_ACCY(3)*(SH_ACCY(4) + q0^2 - q2^2 - q3^2) + SH_ACCY(2)*(SH_ACCY(6) + 2*q1*q2) - 2*vd*(q0*q2 - q1*q3); -SK_ACCY(3) = SH_ACCY(2)*(SH_ACCY(5) - 2*q2*q3) - SH_ACCY(3)*(2*q0*q2 + 2*q1*q3) + vd*(SH_ACCY(4) - q0^2 + q2^2 - q3^2); -SK_ACCY(4) = q0*q3 - q1*q2; -SK_ACCY(5) = SH_ACCY(6) - 2*q1*q2; -SK_ACCY(6) = SH_ACCY(5) + 2*q2*q3; -SK_ACCY(7) = SH_ACCY(1); +SK_ACCY = zeros(9,1); +SK_ACCY(1) = 1/(R_ACC + Kaccy*SH_ACCY(1)*(Kaccy*OP_l_6_c_6_r_*SH_ACCY(1) - Kaccy*OP_l_24_c_6_r_*SH_ACCY(1) - Kaccy*OP_l_5_c_6_r_*(2*q0*q3 - 2*q1*q2) + Kaccy*OP_l_7_c_6_r_*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*OP_l_23_c_6_r_*(q0*q3 - q1*q2) + Kaccy*OP_l_1_c_6_r_*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd) + Kaccy*OP_l_2_c_6_r_*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd) + Kaccy*OP_l_3_c_6_r_*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd) - Kaccy*OP_l_4_c_6_r_*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd)) - Kaccy*SH_ACCY(1)*(Kaccy*OP_l_6_c_24_r_*SH_ACCY(1) - Kaccy*OP_l_24_c_24_r_*SH_ACCY(1) - Kaccy*OP_l_5_c_24_r_*(2*q0*q3 - 2*q1*q2) + Kaccy*OP_l_7_c_24_r_*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*OP_l_23_c_24_r_*(q0*q3 - q1*q2) + Kaccy*OP_l_1_c_24_r_*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd) + Kaccy*OP_l_2_c_24_r_*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd) + Kaccy*OP_l_3_c_24_r_*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd) - Kaccy*OP_l_4_c_24_r_*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd)) - Kaccy*(2*q0*q3 - 2*q1*q2)*(Kaccy*OP_l_6_c_5_r_*SH_ACCY(1) - Kaccy*OP_l_24_c_5_r_*SH_ACCY(1) - Kaccy*OP_l_5_c_5_r_*(2*q0*q3 - 2*q1*q2) + Kaccy*OP_l_7_c_5_r_*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*OP_l_23_c_5_r_*(q0*q3 - q1*q2) + Kaccy*OP_l_1_c_5_r_*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd) + Kaccy*OP_l_2_c_5_r_*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd) + Kaccy*OP_l_3_c_5_r_*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd) - Kaccy*OP_l_4_c_5_r_*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd)) + Kaccy*(2*q0*q1 + 2*q2*q3)*(Kaccy*OP_l_6_c_7_r_*SH_ACCY(1) - Kaccy*OP_l_24_c_7_r_*SH_ACCY(1) - Kaccy*OP_l_5_c_7_r_*(2*q0*q3 - 2*q1*q2) + Kaccy*OP_l_7_c_7_r_*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*OP_l_23_c_7_r_*(q0*q3 - q1*q2) + Kaccy*OP_l_1_c_7_r_*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd) + Kaccy*OP_l_2_c_7_r_*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd) + Kaccy*OP_l_3_c_7_r_*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd) - Kaccy*OP_l_4_c_7_r_*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd)) + 2*Kaccy*(q0*q3 - q1*q2)*(Kaccy*OP_l_6_c_23_r_*SH_ACCY(1) - Kaccy*OP_l_24_c_23_r_*SH_ACCY(1) - Kaccy*OP_l_5_c_23_r_*(2*q0*q3 - 2*q1*q2) + Kaccy*OP_l_7_c_23_r_*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*OP_l_23_c_23_r_*(q0*q3 - q1*q2) + Kaccy*OP_l_1_c_23_r_*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd) + Kaccy*OP_l_2_c_23_r_*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd) + Kaccy*OP_l_3_c_23_r_*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd) - Kaccy*OP_l_4_c_23_r_*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd)) + Kaccy*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd)*(Kaccy*OP_l_6_c_1_r_*SH_ACCY(1) - Kaccy*OP_l_24_c_1_r_*SH_ACCY(1) - Kaccy*OP_l_5_c_1_r_*(2*q0*q3 - 2*q1*q2) + Kaccy*OP_l_7_c_1_r_*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*OP_l_23_c_1_r_*(q0*q3 - q1*q2) + Kaccy*OP_l_1_c_1_r_*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd) + Kaccy*OP_l_2_c_1_r_*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd) + Kaccy*OP_l_3_c_1_r_*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd) - Kaccy*OP_l_4_c_1_r_*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd)) + Kaccy*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd)*(Kaccy*OP_l_6_c_2_r_*SH_ACCY(1) - Kaccy*OP_l_24_c_2_r_*SH_ACCY(1) - Kaccy*OP_l_5_c_2_r_*(2*q0*q3 - 2*q1*q2) + Kaccy*OP_l_7_c_2_r_*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*OP_l_23_c_2_r_*(q0*q3 - q1*q2) + Kaccy*OP_l_1_c_2_r_*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd) + Kaccy*OP_l_2_c_2_r_*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd) + Kaccy*OP_l_3_c_2_r_*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd) - Kaccy*OP_l_4_c_2_r_*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd)) + Kaccy*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd)*(Kaccy*OP_l_6_c_3_r_*SH_ACCY(1) - Kaccy*OP_l_24_c_3_r_*SH_ACCY(1) - Kaccy*OP_l_5_c_3_r_*(2*q0*q3 - 2*q1*q2) + Kaccy*OP_l_7_c_3_r_*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*OP_l_23_c_3_r_*(q0*q3 - q1*q2) + Kaccy*OP_l_1_c_3_r_*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd) + Kaccy*OP_l_2_c_3_r_*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd) + Kaccy*OP_l_3_c_3_r_*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd) - Kaccy*OP_l_4_c_3_r_*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd)) - Kaccy*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd)*(Kaccy*OP_l_6_c_4_r_*SH_ACCY(1) - Kaccy*OP_l_24_c_4_r_*SH_ACCY(1) - Kaccy*OP_l_5_c_4_r_*(2*q0*q3 - 2*q1*q2) + Kaccy*OP_l_7_c_4_r_*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*OP_l_23_c_4_r_*(q0*q3 - q1*q2) + Kaccy*OP_l_1_c_4_r_*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd) + Kaccy*OP_l_2_c_4_r_*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd) + Kaccy*OP_l_3_c_4_r_*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd) - Kaccy*OP_l_4_c_4_r_*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd))); +SK_ACCY(2) = 2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd; +SK_ACCY(3) = 2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd; +SK_ACCY(4) = 2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd; +SK_ACCY(5) = 2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd; +SK_ACCY(6) = 2*q0*q3 - 2*q1*q2; +SK_ACCY(7) = q0*q3 - q1*q2; +SK_ACCY(8) = 2*q0*q1 + 2*q2*q3; +SK_ACCY(9) = SH_ACCY(1); Kfusion = zeros(24,1); Kfusion = zeros(1,1); -Kfusion(1) = SK_ACCY(1)*(Kaccy*OP_l_1_c_1_r_*SK_ACCY(3) + Kaccy*OP_l_1_c_3_r_*SK_ACCY(2) + Kaccy*OP_l_1_c_4_r_*SK_ACCY(5) - Kaccy*OP_l_1_c_5_r_*SK_ACCY(7) - Kaccy*OP_l_1_c_6_r_*SK_ACCY(6) - 2*Kaccy*OP_l_1_c_23_r_*SK_ACCY(4) + Kaccy*OP_l_1_c_24_r_*SK_ACCY(7)); -Kfusion(2) = SK_ACCY(1)*(Kaccy*OP_l_2_c_1_r_*SK_ACCY(3) + Kaccy*OP_l_2_c_3_r_*SK_ACCY(2) + Kaccy*OP_l_2_c_4_r_*SK_ACCY(5) - Kaccy*OP_l_2_c_5_r_*SK_ACCY(7) - Kaccy*OP_l_2_c_6_r_*SK_ACCY(6) - 2*Kaccy*OP_l_2_c_23_r_*SK_ACCY(4) + Kaccy*OP_l_2_c_24_r_*SK_ACCY(7)); -Kfusion(3) = SK_ACCY(1)*(Kaccy*OP_l_3_c_1_r_*SK_ACCY(3) + Kaccy*OP_l_3_c_3_r_*SK_ACCY(2) + Kaccy*OP_l_3_c_4_r_*SK_ACCY(5) - Kaccy*OP_l_3_c_5_r_*SK_ACCY(7) - Kaccy*OP_l_3_c_6_r_*SK_ACCY(6) - 2*Kaccy*OP_l_3_c_23_r_*SK_ACCY(4) + Kaccy*OP_l_3_c_24_r_*SK_ACCY(7)); -Kfusion(4) = SK_ACCY(1)*(Kaccy*OP_l_4_c_1_r_*SK_ACCY(3) + Kaccy*OP_l_4_c_3_r_*SK_ACCY(2) + Kaccy*OP_l_4_c_4_r_*SK_ACCY(5) - Kaccy*OP_l_4_c_5_r_*SK_ACCY(7) - Kaccy*OP_l_4_c_6_r_*SK_ACCY(6) - 2*Kaccy*OP_l_4_c_23_r_*SK_ACCY(4) + Kaccy*OP_l_4_c_24_r_*SK_ACCY(7)); -Kfusion(5) = SK_ACCY(1)*(Kaccy*OP_l_5_c_1_r_*SK_ACCY(3) + Kaccy*OP_l_5_c_3_r_*SK_ACCY(2) + Kaccy*OP_l_5_c_4_r_*SK_ACCY(5) - Kaccy*OP_l_5_c_5_r_*SK_ACCY(7) - Kaccy*OP_l_5_c_6_r_*SK_ACCY(6) - 2*Kaccy*OP_l_5_c_23_r_*SK_ACCY(4) + Kaccy*OP_l_5_c_24_r_*SK_ACCY(7)); -Kfusion(6) = SK_ACCY(1)*(Kaccy*OP_l_6_c_1_r_*SK_ACCY(3) + Kaccy*OP_l_6_c_3_r_*SK_ACCY(2) + Kaccy*OP_l_6_c_4_r_*SK_ACCY(5) - Kaccy*OP_l_6_c_5_r_*SK_ACCY(7) - Kaccy*OP_l_6_c_6_r_*SK_ACCY(6) - 2*Kaccy*OP_l_6_c_23_r_*SK_ACCY(4) + Kaccy*OP_l_6_c_24_r_*SK_ACCY(7)); -Kfusion(7) = SK_ACCY(1)*(Kaccy*OP_l_7_c_1_r_*SK_ACCY(3) + Kaccy*OP_l_7_c_3_r_*SK_ACCY(2) + Kaccy*OP_l_7_c_4_r_*SK_ACCY(5) - Kaccy*OP_l_7_c_5_r_*SK_ACCY(7) - Kaccy*OP_l_7_c_6_r_*SK_ACCY(6) - 2*Kaccy*OP_l_7_c_23_r_*SK_ACCY(4) + Kaccy*OP_l_7_c_24_r_*SK_ACCY(7)); -Kfusion(8) = SK_ACCY(1)*(Kaccy*OP_l_8_c_1_r_*SK_ACCY(3) + Kaccy*OP_l_8_c_3_r_*SK_ACCY(2) + Kaccy*OP_l_8_c_4_r_*SK_ACCY(5) - Kaccy*OP_l_8_c_5_r_*SK_ACCY(7) - Kaccy*OP_l_8_c_6_r_*SK_ACCY(6) - 2*Kaccy*OP_l_8_c_23_r_*SK_ACCY(4) + Kaccy*OP_l_8_c_24_r_*SK_ACCY(7)); -Kfusion(9) = SK_ACCY(1)*(Kaccy*OP_l_9_c_1_r_*SK_ACCY(3) + Kaccy*OP_l_9_c_3_r_*SK_ACCY(2) + Kaccy*OP_l_9_c_4_r_*SK_ACCY(5) - Kaccy*OP_l_9_c_5_r_*SK_ACCY(7) - Kaccy*OP_l_9_c_6_r_*SK_ACCY(6) - 2*Kaccy*OP_l_9_c_23_r_*SK_ACCY(4) + Kaccy*OP_l_9_c_24_r_*SK_ACCY(7)); -Kfusion(10) = SK_ACCY(1)*(Kaccy*OP_l_10_c_1_r_*SK_ACCY(3) + Kaccy*OP_l_10_c_3_r_*SK_ACCY(2) + Kaccy*OP_l_10_c_4_r_*SK_ACCY(5) - Kaccy*OP_l_10_c_5_r_*SK_ACCY(7) - Kaccy*OP_l_10_c_6_r_*SK_ACCY(6) - 2*Kaccy*OP_l_10_c_23_r_*SK_ACCY(4) + Kaccy*OP_l_10_c_24_r_*SK_ACCY(7)); -Kfusion(11) = SK_ACCY(1)*(Kaccy*OP_l_11_c_1_r_*SK_ACCY(3) + Kaccy*OP_l_11_c_3_r_*SK_ACCY(2) + Kaccy*OP_l_11_c_4_r_*SK_ACCY(5) - Kaccy*OP_l_11_c_5_r_*SK_ACCY(7) - Kaccy*OP_l_11_c_6_r_*SK_ACCY(6) - 2*Kaccy*OP_l_11_c_23_r_*SK_ACCY(4) + Kaccy*OP_l_11_c_24_r_*SK_ACCY(7)); -Kfusion(12) = SK_ACCY(1)*(Kaccy*OP_l_12_c_1_r_*SK_ACCY(3) + Kaccy*OP_l_12_c_3_r_*SK_ACCY(2) + Kaccy*OP_l_12_c_4_r_*SK_ACCY(5) - Kaccy*OP_l_12_c_5_r_*SK_ACCY(7) - Kaccy*OP_l_12_c_6_r_*SK_ACCY(6) - 2*Kaccy*OP_l_12_c_23_r_*SK_ACCY(4) + Kaccy*OP_l_12_c_24_r_*SK_ACCY(7)); -Kfusion(13) = SK_ACCY(1)*(Kaccy*OP_l_13_c_1_r_*SK_ACCY(3) + Kaccy*OP_l_13_c_3_r_*SK_ACCY(2) + Kaccy*OP_l_13_c_4_r_*SK_ACCY(5) - Kaccy*OP_l_13_c_5_r_*SK_ACCY(7) - Kaccy*OP_l_13_c_6_r_*SK_ACCY(6) - 2*Kaccy*OP_l_13_c_23_r_*SK_ACCY(4) + Kaccy*OP_l_13_c_24_r_*SK_ACCY(7)); -Kfusion(14) = SK_ACCY(1)*(Kaccy*OP_l_14_c_1_r_*SK_ACCY(3) + Kaccy*OP_l_14_c_3_r_*SK_ACCY(2) + Kaccy*OP_l_14_c_4_r_*SK_ACCY(5) - Kaccy*OP_l_14_c_5_r_*SK_ACCY(7) - Kaccy*OP_l_14_c_6_r_*SK_ACCY(6) - 2*Kaccy*OP_l_14_c_23_r_*SK_ACCY(4) + Kaccy*OP_l_14_c_24_r_*SK_ACCY(7)); -Kfusion(15) = SK_ACCY(1)*(Kaccy*OP_l_15_c_1_r_*SK_ACCY(3) + Kaccy*OP_l_15_c_3_r_*SK_ACCY(2) + Kaccy*OP_l_15_c_4_r_*SK_ACCY(5) - Kaccy*OP_l_15_c_5_r_*SK_ACCY(7) - Kaccy*OP_l_15_c_6_r_*SK_ACCY(6) - 2*Kaccy*OP_l_15_c_23_r_*SK_ACCY(4) + Kaccy*OP_l_15_c_24_r_*SK_ACCY(7)); -Kfusion(16) = SK_ACCY(1)*(Kaccy*OP_l_16_c_1_r_*SK_ACCY(3) + Kaccy*OP_l_16_c_3_r_*SK_ACCY(2) + Kaccy*OP_l_16_c_4_r_*SK_ACCY(5) - Kaccy*OP_l_16_c_5_r_*SK_ACCY(7) - Kaccy*OP_l_16_c_6_r_*SK_ACCY(6) - 2*Kaccy*OP_l_16_c_23_r_*SK_ACCY(4) + Kaccy*OP_l_16_c_24_r_*SK_ACCY(7)); -Kfusion(17) = SK_ACCY(1)*(Kaccy*OP_l_17_c_1_r_*SK_ACCY(3) + Kaccy*OP_l_17_c_3_r_*SK_ACCY(2) + Kaccy*OP_l_17_c_4_r_*SK_ACCY(5) - Kaccy*OP_l_17_c_5_r_*SK_ACCY(7) - Kaccy*OP_l_17_c_6_r_*SK_ACCY(6) - 2*Kaccy*OP_l_17_c_23_r_*SK_ACCY(4) + Kaccy*OP_l_17_c_24_r_*SK_ACCY(7)); -Kfusion(18) = SK_ACCY(1)*(Kaccy*OP_l_18_c_1_r_*SK_ACCY(3) + Kaccy*OP_l_18_c_3_r_*SK_ACCY(2) + Kaccy*OP_l_18_c_4_r_*SK_ACCY(5) - Kaccy*OP_l_18_c_5_r_*SK_ACCY(7) - Kaccy*OP_l_18_c_6_r_*SK_ACCY(6) - 2*Kaccy*OP_l_18_c_23_r_*SK_ACCY(4) + Kaccy*OP_l_18_c_24_r_*SK_ACCY(7)); -Kfusion(19) = SK_ACCY(1)*(Kaccy*OP_l_19_c_1_r_*SK_ACCY(3) + Kaccy*OP_l_19_c_3_r_*SK_ACCY(2) + Kaccy*OP_l_19_c_4_r_*SK_ACCY(5) - Kaccy*OP_l_19_c_5_r_*SK_ACCY(7) - Kaccy*OP_l_19_c_6_r_*SK_ACCY(6) - 2*Kaccy*OP_l_19_c_23_r_*SK_ACCY(4) + Kaccy*OP_l_19_c_24_r_*SK_ACCY(7)); -Kfusion(20) = SK_ACCY(1)*(Kaccy*OP_l_20_c_1_r_*SK_ACCY(3) + Kaccy*OP_l_20_c_3_r_*SK_ACCY(2) + Kaccy*OP_l_20_c_4_r_*SK_ACCY(5) - Kaccy*OP_l_20_c_5_r_*SK_ACCY(7) - Kaccy*OP_l_20_c_6_r_*SK_ACCY(6) - 2*Kaccy*OP_l_20_c_23_r_*SK_ACCY(4) + Kaccy*OP_l_20_c_24_r_*SK_ACCY(7)); -Kfusion(21) = SK_ACCY(1)*(Kaccy*OP_l_21_c_1_r_*SK_ACCY(3) + Kaccy*OP_l_21_c_3_r_*SK_ACCY(2) + Kaccy*OP_l_21_c_4_r_*SK_ACCY(5) - Kaccy*OP_l_21_c_5_r_*SK_ACCY(7) - Kaccy*OP_l_21_c_6_r_*SK_ACCY(6) - 2*Kaccy*OP_l_21_c_23_r_*SK_ACCY(4) + Kaccy*OP_l_21_c_24_r_*SK_ACCY(7)); -Kfusion(22) = SK_ACCY(1)*(Kaccy*OP_l_22_c_1_r_*SK_ACCY(3) + Kaccy*OP_l_22_c_3_r_*SK_ACCY(2) + Kaccy*OP_l_22_c_4_r_*SK_ACCY(5) - Kaccy*OP_l_22_c_5_r_*SK_ACCY(7) - Kaccy*OP_l_22_c_6_r_*SK_ACCY(6) - 2*Kaccy*OP_l_22_c_23_r_*SK_ACCY(4) + Kaccy*OP_l_22_c_24_r_*SK_ACCY(7)); -Kfusion(23) = SK_ACCY(1)*(Kaccy*OP_l_23_c_1_r_*SK_ACCY(3) + Kaccy*OP_l_23_c_3_r_*SK_ACCY(2) + Kaccy*OP_l_23_c_4_r_*SK_ACCY(5) - Kaccy*OP_l_23_c_5_r_*SK_ACCY(7) - Kaccy*OP_l_23_c_6_r_*SK_ACCY(6) - 2*Kaccy*OP_l_23_c_23_r_*SK_ACCY(4) + Kaccy*OP_l_23_c_24_r_*SK_ACCY(7)); -Kfusion(24) = SK_ACCY(1)*(Kaccy*OP_l_24_c_1_r_*SK_ACCY(3) + Kaccy*OP_l_24_c_3_r_*SK_ACCY(2) + Kaccy*OP_l_24_c_4_r_*SK_ACCY(5) - Kaccy*OP_l_24_c_5_r_*SK_ACCY(7) - Kaccy*OP_l_24_c_6_r_*SK_ACCY(6) - 2*Kaccy*OP_l_24_c_23_r_*SK_ACCY(4) + Kaccy*OP_l_24_c_24_r_*SK_ACCY(7)); +Kfusion(1) = -SK_ACCY(1)*(Kaccy*OP_l_1_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_1_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_1_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_1_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_1_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_1_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_1_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_1_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_1_c_24_r_*SK_ACCY(9)); +Kfusion(2) = -SK_ACCY(1)*(Kaccy*OP_l_2_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_2_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_2_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_2_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_2_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_2_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_2_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_2_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_2_c_24_r_*SK_ACCY(9)); +Kfusion(3) = -SK_ACCY(1)*(Kaccy*OP_l_3_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_3_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_3_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_3_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_3_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_3_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_3_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_3_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_3_c_24_r_*SK_ACCY(9)); +Kfusion(4) = -SK_ACCY(1)*(Kaccy*OP_l_4_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_4_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_4_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_4_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_4_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_4_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_4_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_4_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_4_c_24_r_*SK_ACCY(9)); +Kfusion(5) = -SK_ACCY(1)*(Kaccy*OP_l_5_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_5_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_5_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_5_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_5_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_5_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_5_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_5_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_5_c_24_r_*SK_ACCY(9)); +Kfusion(6) = -SK_ACCY(1)*(Kaccy*OP_l_6_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_6_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_6_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_6_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_6_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_6_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_6_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_6_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_6_c_24_r_*SK_ACCY(9)); +Kfusion(7) = -SK_ACCY(1)*(Kaccy*OP_l_7_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_7_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_7_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_7_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_7_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_7_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_7_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_7_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_7_c_24_r_*SK_ACCY(9)); +Kfusion(8) = -SK_ACCY(1)*(Kaccy*OP_l_8_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_8_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_8_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_8_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_8_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_8_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_8_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_8_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_8_c_24_r_*SK_ACCY(9)); +Kfusion(9) = -SK_ACCY(1)*(Kaccy*OP_l_9_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_9_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_9_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_9_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_9_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_9_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_9_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_9_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_9_c_24_r_*SK_ACCY(9)); +Kfusion(10) = -SK_ACCY(1)*(Kaccy*OP_l_10_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_10_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_10_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_10_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_10_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_10_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_10_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_10_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_10_c_24_r_*SK_ACCY(9)); +Kfusion(11) = -SK_ACCY(1)*(Kaccy*OP_l_11_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_11_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_11_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_11_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_11_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_11_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_11_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_11_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_11_c_24_r_*SK_ACCY(9)); +Kfusion(12) = -SK_ACCY(1)*(Kaccy*OP_l_12_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_12_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_12_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_12_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_12_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_12_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_12_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_12_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_12_c_24_r_*SK_ACCY(9)); +Kfusion(13) = -SK_ACCY(1)*(Kaccy*OP_l_13_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_13_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_13_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_13_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_13_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_13_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_13_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_13_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_13_c_24_r_*SK_ACCY(9)); +Kfusion(14) = -SK_ACCY(1)*(Kaccy*OP_l_14_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_14_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_14_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_14_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_14_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_14_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_14_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_14_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_14_c_24_r_*SK_ACCY(9)); +Kfusion(15) = -SK_ACCY(1)*(Kaccy*OP_l_15_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_15_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_15_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_15_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_15_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_15_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_15_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_15_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_15_c_24_r_*SK_ACCY(9)); +Kfusion(16) = -SK_ACCY(1)*(Kaccy*OP_l_16_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_16_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_16_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_16_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_16_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_16_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_16_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_16_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_16_c_24_r_*SK_ACCY(9)); +Kfusion(17) = -SK_ACCY(1)*(Kaccy*OP_l_17_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_17_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_17_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_17_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_17_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_17_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_17_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_17_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_17_c_24_r_*SK_ACCY(9)); +Kfusion(18) = -SK_ACCY(1)*(Kaccy*OP_l_18_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_18_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_18_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_18_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_18_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_18_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_18_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_18_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_18_c_24_r_*SK_ACCY(9)); +Kfusion(19) = -SK_ACCY(1)*(Kaccy*OP_l_19_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_19_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_19_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_19_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_19_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_19_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_19_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_19_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_19_c_24_r_*SK_ACCY(9)); +Kfusion(20) = -SK_ACCY(1)*(Kaccy*OP_l_20_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_20_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_20_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_20_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_20_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_20_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_20_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_20_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_20_c_24_r_*SK_ACCY(9)); +Kfusion(21) = -SK_ACCY(1)*(Kaccy*OP_l_21_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_21_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_21_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_21_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_21_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_21_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_21_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_21_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_21_c_24_r_*SK_ACCY(9)); +Kfusion(22) = -SK_ACCY(1)*(Kaccy*OP_l_22_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_22_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_22_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_22_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_22_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_22_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_22_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_22_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_22_c_24_r_*SK_ACCY(9)); +Kfusion(23) = -SK_ACCY(1)*(Kaccy*OP_l_23_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_23_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_23_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_23_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_23_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_23_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_23_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_23_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_23_c_24_r_*SK_ACCY(9)); +Kfusion(24) = -SK_ACCY(1)*(Kaccy*OP_l_24_c_1_r_*SK_ACCY(4) + Kaccy*OP_l_24_c_2_r_*SK_ACCY(3) - Kaccy*OP_l_24_c_4_r_*SK_ACCY(2) + Kaccy*OP_l_24_c_3_r_*SK_ACCY(5) - Kaccy*OP_l_24_c_5_r_*SK_ACCY(6) + Kaccy*OP_l_24_c_6_r_*SK_ACCY(9) + Kaccy*OP_l_24_c_7_r_*SK_ACCY(8) + 2*Kaccy*OP_l_24_c_23_r_*SK_ACCY(7) - Kaccy*OP_l_24_c_24_r_*SK_ACCY(9)); diff --git a/matlab/scripts/Inertial Nav EKF/calcH_YAW312.c b/matlab/scripts/Inertial Nav EKF/calcH_YAW312.c index 19868e1a17..399e544299 100644 --- a/matlab/scripts/Inertial Nav EKF/calcH_YAW312.c +++ b/matlab/scripts/Inertial Nav EKF/calcH_YAW312.c @@ -1,16 +1,17 @@ -t2 = q0*q0; -t3 = q1*q1; -t4 = q2*q2; -t5 = q3*q3; -t6 = t2-t3+t4-t5; -t7 = q0*q3*2.0; -t10 = q1*q2*2.0; -t8 = t7-t10; -t9 = 1.0/(t6*t6); -t11 = t8*t8; -t12 = t9*t11; +t9 = q0*q3; +t10 = q1*q2; +t2 = t9-t10; +t3 = q0*q0; +t4 = q1*q1; +t5 = q2*q2; +t6 = q3*q3; +t7 = t3-t4+t5-t6; +t8 = 1.0/(t7*t7); +t11 = t2*t2; +t12 = t8*t11*4.0; t13 = t12+1.0; t14 = 1.0/t13; -t15 = 1.0/t6; -A0[0][0] = -t14*(t15*(q0*q2*2.0+q1*q3*2.0)-t8*t9*(q0*q1*2.0-q2*q3*2.0)); -A0[0][2] = t14*(t15*(t2+t3-t4-t5)+t8*t9*(t7+t10)); +A0[0][0] = t8*t14*(q3*t3+q3*t4-q3*t5+q3*t6-q0*q1*q2*2.0)*-2.0; +A0[0][1] = t8*t14*(q2*t3+q2*t4+q2*t5-q2*t6-q0*q1*q3*2.0)*-2.0; +A0[0][2] = t8*t14*(-q1*t3+q1*t4+q1*t5+q1*t6-q0*q2*q3*2.0)*2.0; +A0[0][3] = t8*t14*(q0*t3-q0*t4+q0*t5+q0*t6-q1*q2*q3*2.0)*2.0; diff --git a/matlab/scripts/Inertial Nav EKF/calcH_YAW321.c b/matlab/scripts/Inertial Nav EKF/calcH_YAW321.c index 1e303866cb..90ad9098c2 100644 --- a/matlab/scripts/Inertial Nav EKF/calcH_YAW321.c +++ b/matlab/scripts/Inertial Nav EKF/calcH_YAW321.c @@ -1,16 +1,17 @@ -t2 = q0*q0; -t3 = q1*q1; -t4 = q2*q2; -t5 = q3*q3; -t6 = t2+t3-t4-t5; -t7 = q0*q3*2.0; -t8 = q1*q2*2.0; -t9 = t7+t8; -t10 = 1.0/(t6*t6); -t11 = t9*t9; -t12 = t10*t11; +t9 = q0*q3; +t10 = q1*q2; +t2 = t9+t10; +t3 = q0*q0; +t4 = q1*q1; +t5 = q2*q2; +t6 = q3*q3; +t7 = t3+t4-t5-t6; +t8 = 1.0/(t7*t7); +t11 = t2*t2; +t12 = t8*t11*4.0; t13 = t12+1.0; t14 = 1.0/t13; -t15 = 1.0/t6; -A0[0][1] = t14*(t15*(q0*q1*2.0-q2*q3*2.0)+t9*t10*(q0*q2*2.0+q1*q3*2.0)); -A0[0][2] = t14*(t15*(t2-t3+t4-t5)+t9*t10*(t7-t8)); +A0[0][0] = t8*t14*(q3*t3-q3*t4+q3*t5+q3*t6+q0*q1*q2*2.0)*-2.0; +A0[0][1] = t8*t14*(-q2*t3+q2*t4+q2*t5+q2*t6+q0*q1*q3*2.0)*-2.0; +A0[0][2] = t8*t14*(q1*t3+q1*t4+q1*t5-q1*t6+q0*q2*q3*2.0)*2.0; +A0[0][3] = t8*t14*(q0*t3+q0*t4-q0*t5+q0*t6+q1*q2*q3*2.0)*2.0; diff --git a/matlab/scripts/Inertial Nav EKF/temp1.mat b/matlab/scripts/Inertial Nav EKF/temp1.mat index 804be63a42..28375e5022 100644 Binary files a/matlab/scripts/Inertial Nav EKF/temp1.mat and b/matlab/scripts/Inertial Nav EKF/temp1.mat differ diff --git a/matlab/scripts/Inertial Nav EKF/temp2.mat b/matlab/scripts/Inertial Nav EKF/temp2.mat index bc29af7ab5..698457b1fd 100644 Binary files a/matlab/scripts/Inertial Nav EKF/temp2.mat and b/matlab/scripts/Inertial Nav EKF/temp2.mat differ diff --git a/matlab/scripts/Inertial Nav EKF/temp3.mat b/matlab/scripts/Inertial Nav EKF/temp3.mat index d50def9d24..24e0cc9cf4 100644 Binary files a/matlab/scripts/Inertial Nav EKF/temp3.mat and b/matlab/scripts/Inertial Nav EKF/temp3.mat differ