fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-26 04:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: test new derivation

Browse Source 
Use direct attitude parameterisation
Discard scale factors
Add accel bias
This commit is contained in:
Paul Riseborough
2016-05-05 21:23:03 +10:00
parent 22d18d638c
commit fe9f88a8b4
12 changed files with 928 additions and 1498 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/covariance.cpp
+309 -414
View File
@@ -55,34 +55,31 @@ 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];
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.035f * 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 +102,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,25 +134,22 @@ 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_vel_dt;
float dt = _imu_sample_delayed.delta_ang_dt;
// compute process noise
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 d_ang_bias_sig = dt * math::constrain(_params.gyro_bias_p_noise, 0.0f, 1.0f);
float d_vel_bias_sig = dt * math::constrain(_params.accel_bias_p_noise, 0.0f, 1.0f);
float mag_I_sig, mag_B_sig;
// 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
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;
@@ -163,7 +157,7 @@ void Ekf::predictCovariance()
// 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
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,20 +173,18 @@ void Ekf::predictCovariance()
wind_vel_sig = 0.0f;
}
for (unsigned i = 0; i < 9; i++) {
for (unsigned i = 0; i <= 9; i++) {
process_noise[i] = 0.0f;
}
for (unsigned i = 9; i < 12; i++) {
for (unsigned i = 10; 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);
for (unsigned i = 13; i <= 15; i++) {
process_noise[i] = sq(d_vel_bias_sig);
}
process_noise[15] = sq(d_vel_bias_sig);
for (unsigned i = 16; i < 19; i++) {
process_noise[i] = sq(mag_I_sig);
}
@@ -206,299 +198,210 @@ void Ekf::predictCovariance()
}
// 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
// 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.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];
// 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];
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] + 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] + 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] + 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)*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] + 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)*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] + 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[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] + 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[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[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] + 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[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[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] + P[6][15]*dt;
nextP[10][15] = P[10][15];
nextP[11][15] = P[11][15];
nextP[12][15] = P[12][15];
@@ -515,16 +418,16 @@ 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];
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] + P[6][16]*dt;
nextP[10][16] = P[10][16];
nextP[11][16] = P[11][16];
nextP[12][16] = P[12][16];
@@ -532,17 +435,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)*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] + P[6][17]*dt;
nextP[10][17] = P[10][17];
nextP[11][17] = P[11][17];
nextP[12][17] = P[12][17];
@@ -551,17 +453,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)*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] + P[6][18]*dt;
nextP[10][18] = P[10][18];
nextP[11][18] = P[11][18];
nextP[12][18] = P[12][18];
@@ -571,17 +472,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)*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] + P[6][19]*dt;
nextP[10][19] = P[10][19];
nextP[11][19] = P[11][19];
nextP[12][19] = P[12][19];
@@ -592,17 +492,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)*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] + P[6][20]*dt;
nextP[10][20] = P[10][20];
nextP[11][20] = P[11][20];
nextP[12][20] = P[12][20];
@@ -614,17 +513,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)*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] + P[6][21]*dt;
nextP[10][21] = P[10][21];
nextP[11][21] = P[11][21];
nextP[12][21] = P[12][21];
@@ -637,6 +535,7 @@ void Ekf::predictCovariance()
nextP[19][21] = P[19][21];
nextP[20][21] = P[20][21];
nextP[21][21] = P[21][21];
}
// Don't do covariance prediction on wind states unless we are using them
@@ -686,16 +585,16 @@ 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];
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] + P[6][22]*dt;
nextP[10][22] = P[10][22];
nextP[11][22] = P[11][22];
nextP[12][22] = P[12][22];
@@ -709,17 +608,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)*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] + P[6][23]*dt;
nextP[10][23] = P[10][23];
nextP[11][23] = P[11][23];
nextP[12][23] = P[12][23];
@@ -744,8 +642,8 @@ void Ekf::predictCovariance()
// 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];
@@ -783,57 +681,54 @@ void Ekf::limitCov()
{
// 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
float P_lim[8] = {};
P_lim[0] = 1.0f; // quaternion 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
P_lim[4] = 0.1f; // delta velocity z bias max var
P_lim[5] = 0.1f; // earth mag field max var
P_lim[6] = 0.1f; // body mag field max var
P_lim[7] = 1000.0f; // wind max var
for (int i = 0; i < 3; i++) {
for (int i = 0; i <= 3; i++) {
math::constrain(P[i][i], 0.0f, P_lim[0]);
}
for (int i = 3; i < 6; i++) {
for (int i = 4; i <= 6; i++) {
math::constrain(P[i][i], 0.0f, P_lim[1]);
}
for (int i = 6; i < 9; i++) {
for (int i = 7; i <= 9; i++) {
math::constrain(P[i][i], 0.0f, P_lim[2]);
}
for (int i = 9; i < 12; i++) {
for (int i = 10; i <= 12; i++) {
math::constrain(P[i][i], 0.0f, P_lim[3]);
}
for (int i = 12; i < 15; i++) {
for (int i = 13; i <= 15; i++) {
math::constrain(P[i][i], 0.0f, P_lim[4]);
}
math::constrain(P[15][15], 0.0f, P_lim[5]);
for (int i = 16; i <= 18; i++) {
math::constrain(P[i][i], 0.0f, P_lim[5]);
}
for (int i = 16; i < 19; i++) {
for (int i = 19; i <= 21; i++) {
math::constrain(P[i][i], 0.0f, P_lim[6]);
}
for (int i = 19; i < 22; i++) {
for (int i = 22; i <= 23; 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]);
}
}