diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index ececbb4cb3..527de849f3 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -252,64 +252,52 @@ void Ekf::predictCovariance() SG[3] = sq(q1); SG[4] = sq(q0); - float SQ[8] = {}; - SQ[0] = - dvyNoise * (2 * q0 * q1 + 2 * q2 * q3) * (SG[1] - SG[2] + SG[3] - SG[4]) - dvzNoise * - (2 * q0 * q1 - 2 * q2 * q3) * (SG[1] - SG[2] - SG[3] + SG[4]) - dvxNoise * (2 * q0 * q2 - 2 * q1 * q3) * - (2 * q0 * q3 + 2 * q1 * q2); - SQ[1] = dvxNoise * (2 * q0 * q2 - 2 * q1 * q3) * (SG[1] + SG[2] - SG[3] - SG[4]) + dvzNoise * - (2 * q0 * q2 + 2 * q1 * q3) * (SG[1] - SG[2] - SG[3] + SG[4]) - dvyNoise * (2 * q0 * q1 + 2 * q2 * q3) * - (2 * q0 * q3 - 2 * q1 * q2); - SQ[2] = dvyNoise * (2 * q0 * q3 - 2 * q1 * q2) * (SG[1] - SG[2] + SG[3] - SG[4]) - dvxNoise * - (2 * q0 * q3 + 2 * q1 * q2) * (SG[1] + SG[2] - SG[3] - SG[4]) - dvzNoise * (2 * q0 * q1 - 2 * q2 * q3) * - (2 * q0 * q2 + 2 * q1 * q3); + 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] = 2 * q2 * q3; - SQ[5] = 2 * q1 * q3; - SQ[6] = 2 * q1 * q2; - SQ[7] = SG[4]; + SQ[4] = sq(dvyNoise); + SQ[5] = sq(dvzNoise); + SQ[6] = sq(dvxNoise); + SQ[7] = 2*q2*q3; + SQ[8] = 2*q1*q3; + SQ[9] = 2*q1*q2; float SPP[23] = {}; - 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[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]; // covariance update float nextP[24][24] = {}; - nextP[0][0] = daxNoise * SQ[3] + 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]); + 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] = dayNoise * SQ[3] - 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[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[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]); + 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]) + @@ -320,12 +308,7 @@ void Ekf::predictCovariance() 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] = dazNoise * SQ[3] - 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[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[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]); + 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] * @@ -341,12 +324,7 @@ void Ekf::predictCovariance() 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] + dvyNoise * sq( - SQ[6] - 2 * q0 * q3) + dvzNoise * sq(SQ[5] + 2 * q0 * q2) + 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]) + - dvxNoise * sq(SG[1] + SG[2] - SG[3] - SQ[7]); + 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]) + @@ -367,12 +345,7 @@ void Ekf::predictCovariance() (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] + dvxNoise * sq( - SQ[6] + 2 * q0 * q3) + dvzNoise * sq(SQ[4] - 2 * q0 * q1) + 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]) + - dvyNoise * sq(SG[1] - SG[2] + SG[3] - SQ[7]); + 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] * @@ -398,12 +371,7 @@ void Ekf::predictCovariance() (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] + dvxNoise * sq( - SQ[5] - 2 * q0 * q2) + dvyNoise * sq(SQ[4] + 2 * q0 * q1) + 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]) + dvzNoise * sq( - SG[1] - SG[2] - SG[3] + SQ[7]); + 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 *