Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ardupilot/libraries/AP_NavEKF/Models/GimbalEstimatorExample/calcP.txt

153 lines
22 KiB
Plaintext
Raw Blame History

SF = zeros(25,1);
SF(1) = daz/2 - daz_b/2;
SF(2) = day/2 - day_b/2;
SF(3) = dax/2 - dax_b/2;
SF(4) = (q0*SF(2))/2 - q2/2 + (q1*SF(1))/2 + (q3*SF(3))/2;
SF(5) = q3/2 + (q0*SF(1))/2 - (q1*SF(2))/2 + (q2*SF(3))/2;
SF(6) = q0/2 + (q1*SF(3))/2 + (q2*SF(2))/2 - (q3*SF(1))/2;
SF(7) = q1/2 - (q0*SF(3))/2 + (q2*SF(1))/2 + (q3*SF(2))/2;
SF(8) = q3/2 - (q0*SF(1))/2 + (q1*SF(2))/2 + (q2*SF(3))/2;
SF(9) = q2/2 + (q0*SF(2))/2 + (q1*SF(1))/2 - (q3*SF(3))/2;
SF(10) = (q0*SF(3))/2 - q1/2 + (q2*SF(1))/2 + (q3*SF(2))/2;
SF(11) = q0/2 + (q1*SF(3))/2 - (q2*SF(2))/2 + (q3*SF(1))/2;
SF(12) = q0/2 - (q1*SF(3))/2 + (q2*SF(2))/2 + (q3*SF(1))/2;
SF(13) = q1/2 + (q0*SF(3))/2 - (q2*SF(1))/2 + (q3*SF(2))/2;
SF(14) = q2/2 - (q0*SF(2))/2 + (q1*SF(1))/2 + (q3*SF(3))/2;
SF(15) = (q0*SF(1))/2 - q3/2 + (q1*SF(2))/2 + (q2*SF(3))/2;
SF(16) = - q0^2 - q1^2 - q2^2 - q3^2;
SF(17) = q0^2 - q1^2 - q2^2 + q3^2;
SF(18) = q0^2 - q1^2 + q2^2 - q3^2;
SF(19) = q0^2 + q1^2 - q2^2 - q3^2;
SF(20) = 2*q0*q2 - 2*q1*q3;
SF(21) = 2*q0*q1 - 2*q2*q3;
SF(22) = 2*q0*q3 - 2*q1*q2;
SF(23) = 2*q0*q1 + 2*q2*q3;
SF(24) = 2*q0*q3 + 2*q1*q2;
SF(25) = 2*q0*q2 + 2*q1*q3;
SG = zeros(5,1);
SG(1) = - q0^2 - q1^2 - q2^2 - q3^2;
SG(2) = q3^2;
SG(3) = q2^2;
SG(4) = q1^2;
SG(5) = q0^2;
SQ = zeros(8,1);
SQ(1) = dvyNoise*(2*q0*q1 + 2*q2*q3)*(q0^2 - q1^2 + q2^2 - q3^2) - dvzNoise*(2*q0*q1 - 2*q2*q3)*(q0^2 - q1^2 - q2^2 + q3^2) - dvxNoise*(2*q0*q2 - 2*q1*q3)*(2*q0*q3 + 2*q1*q2);
SQ(2) = dvzNoise*(2*q0*q2 + 2*q1*q3)*(q0^2 - q1^2 - q2^2 + q3^2) - dvxNoise*(2*q0*q2 - 2*q1*q3)*(q0^2 + q1^2 - q2^2 - q3^2) - dvyNoise*(2*q0*q1 + 2*q2*q3)*(2*q0*q3 - 2*q1*q2);
SQ(3) = dvxNoise*(2*q0*q3 + 2*q1*q2)*(q0^2 + q1^2 - q2^2 - q3^2) - dvyNoise*(2*q0*q3 - 2*q1*q2)*(q0^2 - q1^2 + q2^2 - q3^2) - dvzNoise*(2*q0*q1 - 2*q2*q3)*(2*q0*q2 + 2*q1*q3);
SQ(4) = (q0^2 + q1^2 + q2^2 + q3^2)^2;
SQ(5) = q3^2;
SQ(6) = q2^2;
SQ(7) = q1^2;
SQ(8) = q0^2;
SPP = zeros(19,1);
SPP(1) = 2*q2*SF(12) - 2*q0*SF(14) + 2*q1*SF(15) + 2*q3*SF(13);
SPP(2) = 2*q0*SF(10) + 2*q2*SF(8) + 2*q1*SF(11) - 2*q3*SF(9);
SPP(3) = 2*q0*SF(15) + 2*q1*SF(14) - 2*q2*SF(13) + 2*q3*SF(12);
SPP(4) = 2*q0*SF(11) - 2*q1*SF(10) + 2*q2*SF(9) + 2*q3*SF(8);
SPP(5) = 2*q0*SF(12) + 2*q1*SF(13) + 2*q2*SF(14) - 2*q3*SF(15);
SPP(6) = 2*q1*SF(9) - 2*q0*SF(8) + 2*q2*SF(10) + 2*q3*SF(11);
SPP(7) = 2*q0*SF(6) - 2*q2*SF(4) + 2*q1*SF(7) + 2*q3*SF(5);
SPP(8) = 2*q1*SF(6) - 2*q0*SF(7) + 2*q2*SF(5) + 2*q3*SF(4);
SPP(9) = 2*q0*SF(4) - 2*q1*SF(5) + 2*q2*SF(6) + 2*q3*SF(7);
SPP(10) = dvy*SF(17) - dvz*SF(23);
SPP(11) = dvx*SF(18) - dvy*SF(24);
SPP(12) = dvx*SF(25) - dvz*SF(19);
SPP(13) = dvx*SF(17) + dvz*SF(20);
SPP(14) = dvx*SF(23) + dvy*SF(20);
SPP(15) = dvy*SF(21) + dvz*SF(18);
SPP(16) = dvx*SF(21) + dvz*SF(24);
SPP(17) = dvy*SF(25) + dvz*SF(22);
SPP(18) = dvx*SF(22) + dvy*SF(19);
SPP(19) = SF(16);
nextP = zeros(9,9);
nextP(1,1) = daxNoise*SQ(4) + SPP(5)*(OP_l_1_c_1_r_*SPP(5) + OP_l_2_c_1_r_*SPP(6) - OP_l_3_c_1_r_*SPP(9) + OP_l_7_c_1_r_*SPP(19)) + SPP(6)*(OP_l_1_c_2_r_*SPP(5) + OP_l_2_c_2_r_*SPP(6) - OP_l_3_c_2_r_*SPP(9) + OP_l_7_c_2_r_*SPP(19)) - SPP(9)*(OP_l_1_c_3_r_*SPP(5) + OP_l_2_c_3_r_*SPP(6) - OP_l_3_c_3_r_*SPP(9) + OP_l_7_c_3_r_*SPP(19)) + SPP(19)*(OP_l_1_c_7_r_*SPP(5) + OP_l_2_c_7_r_*SPP(6) - OP_l_3_c_7_r_*SPP(9) + OP_l_7_c_7_r_*SPP(19));
nextP(1,2) = SPP(4)*(OP_l_1_c_2_r_*SPP(5) + OP_l_2_c_2_r_*SPP(6) - OP_l_3_c_2_r_*SPP(9) + OP_l_7_c_2_r_*SPP(19)) - SPP(3)*(OP_l_1_c_1_r_*SPP(5) + OP_l_2_c_1_r_*SPP(6) - OP_l_3_c_1_r_*SPP(9) + OP_l_7_c_1_r_*SPP(19)) + SPP(8)*(OP_l_1_c_3_r_*SPP(5) + OP_l_2_c_3_r_*SPP(6) - OP_l_3_c_3_r_*SPP(9) + OP_l_7_c_3_r_*SPP(19)) + SPP(19)*(OP_l_1_c_8_r_*SPP(5) + OP_l_2_c_8_r_*SPP(6) - OP_l_3_c_8_r_*SPP(9) + OP_l_7_c_8_r_*SPP(19));
nextP(1,3) = SPP(1)*(OP_l_1_c_1_r_*SPP(5) + OP_l_2_c_1_r_*SPP(6) - OP_l_3_c_1_r_*SPP(9) + OP_l_7_c_1_r_*SPP(19)) - SPP(2)*(OP_l_1_c_2_r_*SPP(5) + OP_l_2_c_2_r_*SPP(6) - OP_l_3_c_2_r_*SPP(9) + OP_l_7_c_2_r_*SPP(19)) + SPP(7)*(OP_l_1_c_3_r_*SPP(5) + OP_l_2_c_3_r_*SPP(6) - OP_l_3_c_3_r_*SPP(9) + OP_l_7_c_3_r_*SPP(19)) + SPP(19)*(OP_l_1_c_9_r_*SPP(5) + OP_l_2_c_9_r_*SPP(6) - OP_l_3_c_9_r_*SPP(9) + OP_l_7_c_9_r_*SPP(19));
nextP(1,4) = OP_l_1_c_4_r_*SPP(5) + OP_l_2_c_4_r_*SPP(6) - OP_l_3_c_4_r_*SPP(9) + OP_l_7_c_4_r_*SPP(19) - SPP(12)*(OP_l_1_c_2_r_*SPP(5) + OP_l_2_c_2_r_*SPP(6) - OP_l_3_c_2_r_*SPP(9) + OP_l_7_c_2_r_*SPP(19)) + SPP(17)*(OP_l_1_c_1_r_*SPP(5) + OP_l_2_c_1_r_*SPP(6) - OP_l_3_c_1_r_*SPP(9) + OP_l_7_c_1_r_*SPP(19)) - SPP(18)*(OP_l_1_c_3_r_*SPP(5) + OP_l_2_c_3_r_*SPP(6) - OP_l_3_c_3_r_*SPP(9) + OP_l_7_c_3_r_*SPP(19));
nextP(1,5) = OP_l_1_c_5_r_*SPP(5) + OP_l_2_c_5_r_*SPP(6) - OP_l_3_c_5_r_*SPP(9) + OP_l_7_c_5_r_*SPP(19) - SPP(15)*(OP_l_1_c_1_r_*SPP(5) + OP_l_2_c_1_r_*SPP(6) - OP_l_3_c_1_r_*SPP(9) + OP_l_7_c_1_r_*SPP(19)) + SPP(11)*(OP_l_1_c_3_r_*SPP(5) + OP_l_2_c_3_r_*SPP(6) - OP_l_3_c_3_r_*SPP(9) + OP_l_7_c_3_r_*SPP(19)) + SPP(16)*(OP_l_1_c_2_r_*SPP(5) + OP_l_2_c_2_r_*SPP(6) - OP_l_3_c_2_r_*SPP(9) + OP_l_7_c_2_r_*SPP(19));
nextP(1,6) = OP_l_1_c_6_r_*SPP(5) + OP_l_2_c_6_r_*SPP(6) - OP_l_3_c_6_r_*SPP(9) + OP_l_7_c_6_r_*SPP(19) + SPP(10)*(OP_l_1_c_1_r_*SPP(5) + OP_l_2_c_1_r_*SPP(6) - OP_l_3_c_1_r_*SPP(9) + OP_l_7_c_1_r_*SPP(19)) - SPP(13)*(OP_l_1_c_2_r_*SPP(5) + OP_l_2_c_2_r_*SPP(6) - OP_l_3_c_2_r_*SPP(9) + OP_l_7_c_2_r_*SPP(19)) + SPP(14)*(OP_l_1_c_3_r_*SPP(5) + OP_l_2_c_3_r_*SPP(6) - OP_l_3_c_3_r_*SPP(9) + OP_l_7_c_3_r_*SPP(19));
nextP(1,7) = OP_l_1_c_7_r_*SPP(5) + OP_l_2_c_7_r_*SPP(6) - OP_l_3_c_7_r_*SPP(9) + OP_l_7_c_7_r_*SPP(19);
nextP(1,8) = OP_l_1_c_8_r_*SPP(5) + OP_l_2_c_8_r_*SPP(6) - OP_l_3_c_8_r_*SPP(9) + OP_l_7_c_8_r_*SPP(19);
nextP(1,9) = OP_l_1_c_9_r_*SPP(5) + OP_l_2_c_9_r_*SPP(6) - OP_l_3_c_9_r_*SPP(9) + OP_l_7_c_9_r_*SPP(19);
nextP(2,1) = SPP(5)*(OP_l_2_c_1_r_*SPP(4) - OP_l_1_c_1_r_*SPP(3) + OP_l_3_c_1_r_*SPP(8) + OP_l_8_c_1_r_*SPP(19)) + SPP(6)*(OP_l_2_c_2_r_*SPP(4) - OP_l_1_c_2_r_*SPP(3) + OP_l_3_c_2_r_*SPP(8) + OP_l_8_c_2_r_*SPP(19)) - SPP(9)*(OP_l_2_c_3_r_*SPP(4) - OP_l_1_c_3_r_*SPP(3) + OP_l_3_c_3_r_*SPP(8) + OP_l_8_c_3_r_*SPP(19)) + SPP(19)*(OP_l_2_c_7_r_*SPP(4) - OP_l_1_c_7_r_*SPP(3) + OP_l_3_c_7_r_*SPP(8) + OP_l_8_c_7_r_*SPP(19));
nextP(2,2) = dayNoise*SQ(4) - SPP(3)*(OP_l_2_c_1_r_*SPP(4) - OP_l_1_c_1_r_*SPP(3) + OP_l_3_c_1_r_*SPP(8) + OP_l_8_c_1_r_*SPP(19)) + SPP(4)*(OP_l_2_c_2_r_*SPP(4) - OP_l_1_c_2_r_*SPP(3) + OP_l_3_c_2_r_*SPP(8) + OP_l_8_c_2_r_*SPP(19)) + SPP(8)*(OP_l_2_c_3_r_*SPP(4) - OP_l_1_c_3_r_*SPP(3) + OP_l_3_c_3_r_*SPP(8) + OP_l_8_c_3_r_*SPP(19)) + SPP(19)*(OP_l_2_c_8_r_*SPP(4) - OP_l_1_c_8_r_*SPP(3) + OP_l_3_c_8_r_*SPP(8) + OP_l_8_c_8_r_*SPP(19));
nextP(2,3) = SPP(1)*(OP_l_2_c_1_r_*SPP(4) - OP_l_1_c_1_r_*SPP(3) + OP_l_3_c_1_r_*SPP(8) + OP_l_8_c_1_r_*SPP(19)) - SPP(2)*(OP_l_2_c_2_r_*SPP(4) - OP_l_1_c_2_r_*SPP(3) + OP_l_3_c_2_r_*SPP(8) + OP_l_8_c_2_r_*SPP(19)) + SPP(7)*(OP_l_2_c_3_r_*SPP(4) - OP_l_1_c_3_r_*SPP(3) + OP_l_3_c_3_r_*SPP(8) + OP_l_8_c_3_r_*SPP(19)) + SPP(19)*(OP_l_2_c_9_r_*SPP(4) - OP_l_1_c_9_r_*SPP(3) + OP_l_3_c_9_r_*SPP(8) + OP_l_8_c_9_r_*SPP(19));
nextP(2,4) = OP_l_2_c_4_r_*SPP(4) - OP_l_1_c_4_r_*SPP(3) + OP_l_3_c_4_r_*SPP(8) + OP_l_8_c_4_r_*SPP(19) - SPP(12)*(OP_l_2_c_2_r_*SPP(4) - OP_l_1_c_2_r_*SPP(3) + OP_l_3_c_2_r_*SPP(8) + OP_l_8_c_2_r_*SPP(19)) + SPP(17)*(OP_l_2_c_1_r_*SPP(4) - OP_l_1_c_1_r_*SPP(3) + OP_l_3_c_1_r_*SPP(8) + OP_l_8_c_1_r_*SPP(19)) - SPP(18)*(OP_l_2_c_3_r_*SPP(4) - OP_l_1_c_3_r_*SPP(3) + OP_l_3_c_3_r_*SPP(8) + OP_l_8_c_3_r_*SPP(19));
nextP(2,5) = OP_l_2_c_5_r_*SPP(4) - OP_l_1_c_5_r_*SPP(3) + OP_l_3_c_5_r_*SPP(8) + OP_l_8_c_5_r_*SPP(19) - SPP(15)*(OP_l_2_c_1_r_*SPP(4) - OP_l_1_c_1_r_*SPP(3) + OP_l_3_c_1_r_*SPP(8) + OP_l_8_c_1_r_*SPP(19)) + SPP(11)*(OP_l_2_c_3_r_*SPP(4) - OP_l_1_c_3_r_*SPP(3) + OP_l_3_c_3_r_*SPP(8) + OP_l_8_c_3_r_*SPP(19)) + SPP(16)*(OP_l_2_c_2_r_*SPP(4) - OP_l_1_c_2_r_*SPP(3) + OP_l_3_c_2_r_*SPP(8) + OP_l_8_c_2_r_*SPP(19));
nextP(2,6) = OP_l_2_c_6_r_*SPP(4) - OP_l_1_c_6_r_*SPP(3) + OP_l_3_c_6_r_*SPP(8) + OP_l_8_c_6_r_*SPP(19) + SPP(10)*(OP_l_2_c_1_r_*SPP(4) - OP_l_1_c_1_r_*SPP(3) + OP_l_3_c_1_r_*SPP(8) + OP_l_8_c_1_r_*SPP(19)) - SPP(13)*(OP_l_2_c_2_r_*SPP(4) - OP_l_1_c_2_r_*SPP(3) + OP_l_3_c_2_r_*SPP(8) + OP_l_8_c_2_r_*SPP(19)) + SPP(14)*(OP_l_2_c_3_r_*SPP(4) - OP_l_1_c_3_r_*SPP(3) + OP_l_3_c_3_r_*SPP(8) + OP_l_8_c_3_r_*SPP(19));
nextP(2,7) = OP_l_2_c_7_r_*SPP(4) - OP_l_1_c_7_r_*SPP(3) + OP_l_3_c_7_r_*SPP(8) + OP_l_8_c_7_r_*SPP(19);
nextP(2,8) = OP_l_2_c_8_r_*SPP(4) - OP_l_1_c_8_r_*SPP(3) + OP_l_3_c_8_r_*SPP(8) + OP_l_8_c_8_r_*SPP(19);
nextP(2,9) = OP_l_2_c_9_r_*SPP(4) - OP_l_1_c_9_r_*SPP(3) + OP_l_3_c_9_r_*SPP(8) + OP_l_8_c_9_r_*SPP(19);
nextP(3,1) = SPP(5)*(OP_l_1_c_1_r_*SPP(1) - OP_l_2_c_1_r_*SPP(2) + OP_l_3_c_1_r_*SPP(7) + OP_l_9_c_1_r_*SPP(19)) + SPP(6)*(OP_l_1_c_2_r_*SPP(1) - OP_l_2_c_2_r_*SPP(2) + OP_l_3_c_2_r_*SPP(7) + OP_l_9_c_2_r_*SPP(19)) - SPP(9)*(OP_l_1_c_3_r_*SPP(1) - OP_l_2_c_3_r_*SPP(2) + OP_l_3_c_3_r_*SPP(7) + OP_l_9_c_3_r_*SPP(19)) + SPP(19)*(OP_l_1_c_7_r_*SPP(1) - OP_l_2_c_7_r_*SPP(2) + OP_l_3_c_7_r_*SPP(7) + OP_l_9_c_7_r_*SPP(19));
nextP(3,2) = SPP(4)*(OP_l_1_c_2_r_*SPP(1) - OP_l_2_c_2_r_*SPP(2) + OP_l_3_c_2_r_*SPP(7) + OP_l_9_c_2_r_*SPP(19)) - SPP(3)*(OP_l_1_c_1_r_*SPP(1) - OP_l_2_c_1_r_*SPP(2) + OP_l_3_c_1_r_*SPP(7) + OP_l_9_c_1_r_*SPP(19)) + SPP(8)*(OP_l_1_c_3_r_*SPP(1) - OP_l_2_c_3_r_*SPP(2) + OP_l_3_c_3_r_*SPP(7) + OP_l_9_c_3_r_*SPP(19)) + SPP(19)*(OP_l_1_c_8_r_*SPP(1) - OP_l_2_c_8_r_*SPP(2) + OP_l_3_c_8_r_*SPP(7) + OP_l_9_c_8_r_*SPP(19));
nextP(3,3) = dazNoise*SQ(4) + SPP(1)*(OP_l_1_c_1_r_*SPP(1) - OP_l_2_c_1_r_*SPP(2) + OP_l_3_c_1_r_*SPP(7) + OP_l_9_c_1_r_*SPP(19)) - SPP(2)*(OP_l_1_c_2_r_*SPP(1) - OP_l_2_c_2_r_*SPP(2) + OP_l_3_c_2_r_*SPP(7) + OP_l_9_c_2_r_*SPP(19)) + SPP(7)*(OP_l_1_c_3_r_*SPP(1) - OP_l_2_c_3_r_*SPP(2) + OP_l_3_c_3_r_*SPP(7) + OP_l_9_c_3_r_*SPP(19)) + SPP(19)*(OP_l_1_c_9_r_*SPP(1) - OP_l_2_c_9_r_*SPP(2) + OP_l_3_c_9_r_*SPP(7) + OP_l_9_c_9_r_*SPP(19));
nextP(3,4) = OP_l_1_c_4_r_*SPP(1) - OP_l_2_c_4_r_*SPP(2) + OP_l_3_c_4_r_*SPP(7) + OP_l_9_c_4_r_*SPP(19) - SPP(12)*(OP_l_1_c_2_r_*SPP(1) - OP_l_2_c_2_r_*SPP(2) + OP_l_3_c_2_r_*SPP(7) + OP_l_9_c_2_r_*SPP(19)) + SPP(17)*(OP_l_1_c_1_r_*SPP(1) - OP_l_2_c_1_r_*SPP(2) + OP_l_3_c_1_r_*SPP(7) + OP_l_9_c_1_r_*SPP(19)) - SPP(18)*(OP_l_1_c_3_r_*SPP(1) - OP_l_2_c_3_r_*SPP(2) + OP_l_3_c_3_r_*SPP(7) + OP_l_9_c_3_r_*SPP(19));
nextP(3,5) = OP_l_1_c_5_r_*SPP(1) - OP_l_2_c_5_r_*SPP(2) + OP_l_3_c_5_r_*SPP(7) + OP_l_9_c_5_r_*SPP(19) - SPP(15)*(OP_l_1_c_1_r_*SPP(1) - OP_l_2_c_1_r_*SPP(2) + OP_l_3_c_1_r_*SPP(7) + OP_l_9_c_1_r_*SPP(19)) + SPP(11)*(OP_l_1_c_3_r_*SPP(1) - OP_l_2_c_3_r_*SPP(2) + OP_l_3_c_3_r_*SPP(7) + OP_l_9_c_3_r_*SPP(19)) + SPP(16)*(OP_l_1_c_2_r_*SPP(1) - OP_l_2_c_2_r_*SPP(2) + OP_l_3_c_2_r_*SPP(7) + OP_l_9_c_2_r_*SPP(19));
nextP(3,6) = OP_l_1_c_6_r_*SPP(1) - OP_l_2_c_6_r_*SPP(2) + OP_l_3_c_6_r_*SPP(7) + OP_l_9_c_6_r_*SPP(19) + SPP(10)*(OP_l_1_c_1_r_*SPP(1) - OP_l_2_c_1_r_*SPP(2) + OP_l_3_c_1_r_*SPP(7) + OP_l_9_c_1_r_*SPP(19)) - SPP(13)*(OP_l_1_c_2_r_*SPP(1) - OP_l_2_c_2_r_*SPP(2) + OP_l_3_c_2_r_*SPP(7) + OP_l_9_c_2_r_*SPP(19)) + SPP(14)*(OP_l_1_c_3_r_*SPP(1) - OP_l_2_c_3_r_*SPP(2) + OP_l_3_c_3_r_*SPP(7) + OP_l_9_c_3_r_*SPP(19));
nextP(3,7) = OP_l_1_c_7_r_*SPP(1) - OP_l_2_c_7_r_*SPP(2) + OP_l_3_c_7_r_*SPP(7) + OP_l_9_c_7_r_*SPP(19);
nextP(3,8) = OP_l_1_c_8_r_*SPP(1) - OP_l_2_c_8_r_*SPP(2) + OP_l_3_c_8_r_*SPP(7) + OP_l_9_c_8_r_*SPP(19);
nextP(3,9) = OP_l_1_c_9_r_*SPP(1) - OP_l_2_c_9_r_*SPP(2) + OP_l_3_c_9_r_*SPP(7) + OP_l_9_c_9_r_*SPP(19);
nextP(4,1) = SPP(5)*(OP_l_4_c_1_r_ + OP_l_1_c_1_r_*SPP(17) - OP_l_2_c_1_r_*SPP(12) - OP_l_3_c_1_r_*SPP(18)) + SPP(6)*(OP_l_4_c_2_r_ + OP_l_1_c_2_r_*SPP(17) - OP_l_2_c_2_r_*SPP(12) - OP_l_3_c_2_r_*SPP(18)) - SPP(9)*(OP_l_4_c_3_r_ + OP_l_1_c_3_r_*SPP(17) - OP_l_2_c_3_r_*SPP(12) - OP_l_3_c_3_r_*SPP(18)) + SPP(19)*(OP_l_4_c_7_r_ + OP_l_1_c_7_r_*SPP(17) - OP_l_2_c_7_r_*SPP(12) - OP_l_3_c_7_r_*SPP(18));
nextP(4,2) = SPP(4)*(OP_l_4_c_2_r_ + OP_l_1_c_2_r_*SPP(17) - OP_l_2_c_2_r_*SPP(12) - OP_l_3_c_2_r_*SPP(18)) - SPP(3)*(OP_l_4_c_1_r_ + OP_l_1_c_1_r_*SPP(17) - OP_l_2_c_1_r_*SPP(12) - OP_l_3_c_1_r_*SPP(18)) + SPP(8)*(OP_l_4_c_3_r_ + OP_l_1_c_3_r_*SPP(17) - OP_l_2_c_3_r_*SPP(12) - OP_l_3_c_3_r_*SPP(18)) + SPP(19)*(OP_l_4_c_8_r_ + OP_l_1_c_8_r_*SPP(17) - OP_l_2_c_8_r_*SPP(12) - OP_l_3_c_8_r_*SPP(18));
nextP(4,3) = SPP(1)*(OP_l_4_c_1_r_ + OP_l_1_c_1_r_*SPP(17) - OP_l_2_c_1_r_*SPP(12) - OP_l_3_c_1_r_*SPP(18)) - SPP(2)*(OP_l_4_c_2_r_ + OP_l_1_c_2_r_*SPP(17) - OP_l_2_c_2_r_*SPP(12) - OP_l_3_c_2_r_*SPP(18)) + SPP(7)*(OP_l_4_c_3_r_ + OP_l_1_c_3_r_*SPP(17) - OP_l_2_c_3_r_*SPP(12) - OP_l_3_c_3_r_*SPP(18)) + SPP(19)*(OP_l_4_c_9_r_ + OP_l_1_c_9_r_*SPP(17) - OP_l_2_c_9_r_*SPP(12) - OP_l_3_c_9_r_*SPP(18));
nextP(4,4) = OP_l_4_c_4_r_ + OP_l_1_c_4_r_*SPP(17) - OP_l_2_c_4_r_*SPP(12) - OP_l_3_c_4_r_*SPP(18) + dvyNoise*(2*q0*q3 - 2*q1*q2)^2 + dvzNoise*(2*q0*q2 + 2*q1*q3)^2 - SPP(12)*(OP_l_4_c_2_r_ + OP_l_1_c_2_r_*SPP(17) - OP_l_2_c_2_r_*SPP(12) - OP_l_3_c_2_r_*SPP(18)) + SPP(17)*(OP_l_4_c_1_r_ + OP_l_1_c_1_r_*SPP(17) - OP_l_2_c_1_r_*SPP(12) - OP_l_3_c_1_r_*SPP(18)) - SPP(18)*(OP_l_4_c_3_r_ + OP_l_1_c_3_r_*SPP(17) - OP_l_2_c_3_r_*SPP(12) - OP_l_3_c_3_r_*SPP(18)) + dvxNoise*(SQ(5) + SQ(6) - SQ(7) - SQ(8))^2;
nextP(4,5) = OP_l_4_c_5_r_ + SQ(3) + OP_l_1_c_5_r_*SPP(17) - OP_l_2_c_5_r_*SPP(12) - OP_l_3_c_5_r_*SPP(18) - SPP(15)*(OP_l_4_c_1_r_ + OP_l_1_c_1_r_*SPP(17) - OP_l_2_c_1_r_*SPP(12) - OP_l_3_c_1_r_*SPP(18)) + SPP(11)*(OP_l_4_c_3_r_ + OP_l_1_c_3_r_*SPP(17) - OP_l_2_c_3_r_*SPP(12) - OP_l_3_c_3_r_*SPP(18)) + SPP(16)*(OP_l_4_c_2_r_ + OP_l_1_c_2_r_*SPP(17) - OP_l_2_c_2_r_*SPP(12) - OP_l_3_c_2_r_*SPP(18));
nextP(4,6) = OP_l_4_c_6_r_ + SQ(2) + OP_l_1_c_6_r_*SPP(17) - OP_l_2_c_6_r_*SPP(12) - OP_l_3_c_6_r_*SPP(18) + SPP(10)*(OP_l_4_c_1_r_ + OP_l_1_c_1_r_*SPP(17) - OP_l_2_c_1_r_*SPP(12) - OP_l_3_c_1_r_*SPP(18)) - SPP(13)*(OP_l_4_c_2_r_ + OP_l_1_c_2_r_*SPP(17) - OP_l_2_c_2_r_*SPP(12) - OP_l_3_c_2_r_*SPP(18)) + SPP(14)*(OP_l_4_c_3_r_ + OP_l_1_c_3_r_*SPP(17) - OP_l_2_c_3_r_*SPP(12) - OP_l_3_c_3_r_*SPP(18));
nextP(4,7) = OP_l_4_c_7_r_ + OP_l_1_c_7_r_*SPP(17) - OP_l_2_c_7_r_*SPP(12) - OP_l_3_c_7_r_*SPP(18);
nextP(4,8) = OP_l_4_c_8_r_ + OP_l_1_c_8_r_*SPP(17) - OP_l_2_c_8_r_*SPP(12) - OP_l_3_c_8_r_*SPP(18);
nextP(4,9) = OP_l_4_c_9_r_ + OP_l_1_c_9_r_*SPP(17) - OP_l_2_c_9_r_*SPP(12) - OP_l_3_c_9_r_*SPP(18);
nextP(5,1) = SPP(5)*(OP_l_5_c_1_r_ - OP_l_1_c_1_r_*SPP(15) + OP_l_2_c_1_r_*SPP(16) + OP_l_3_c_1_r_*SPP(11)) + SPP(6)*(OP_l_5_c_2_r_ - OP_l_1_c_2_r_*SPP(15) + OP_l_2_c_2_r_*SPP(16) + OP_l_3_c_2_r_*SPP(11)) - SPP(9)*(OP_l_5_c_3_r_ - OP_l_1_c_3_r_*SPP(15) + OP_l_2_c_3_r_*SPP(16) + OP_l_3_c_3_r_*SPP(11)) + SPP(19)*(OP_l_5_c_7_r_ - OP_l_1_c_7_r_*SPP(15) + OP_l_2_c_7_r_*SPP(16) + OP_l_3_c_7_r_*SPP(11));
nextP(5,2) = SPP(4)*(OP_l_5_c_2_r_ - OP_l_1_c_2_r_*SPP(15) + OP_l_2_c_2_r_*SPP(16) + OP_l_3_c_2_r_*SPP(11)) - SPP(3)*(OP_l_5_c_1_r_ - OP_l_1_c_1_r_*SPP(15) + OP_l_2_c_1_r_*SPP(16) + OP_l_3_c_1_r_*SPP(11)) + SPP(8)*(OP_l_5_c_3_r_ - OP_l_1_c_3_r_*SPP(15) + OP_l_2_c_3_r_*SPP(16) + OP_l_3_c_3_r_*SPP(11)) + SPP(19)*(OP_l_5_c_8_r_ - OP_l_1_c_8_r_*SPP(15) + OP_l_2_c_8_r_*SPP(16) + OP_l_3_c_8_r_*SPP(11));
nextP(5,3) = SPP(1)*(OP_l_5_c_1_r_ - OP_l_1_c_1_r_*SPP(15) + OP_l_2_c_1_r_*SPP(16) + OP_l_3_c_1_r_*SPP(11)) - SPP(2)*(OP_l_5_c_2_r_ - OP_l_1_c_2_r_*SPP(15) + OP_l_2_c_2_r_*SPP(16) + OP_l_3_c_2_r_*SPP(11)) + SPP(7)*(OP_l_5_c_3_r_ - OP_l_1_c_3_r_*SPP(15) + OP_l_2_c_3_r_*SPP(16) + OP_l_3_c_3_r_*SPP(11)) + SPP(19)*(OP_l_5_c_9_r_ - OP_l_1_c_9_r_*SPP(15) + OP_l_2_c_9_r_*SPP(16) + OP_l_3_c_9_r_*SPP(11));
nextP(5,4) = OP_l_5_c_4_r_ + SQ(3) - OP_l_1_c_4_r_*SPP(15) + OP_l_2_c_4_r_*SPP(16) + OP_l_3_c_4_r_*SPP(11) - SPP(12)*(OP_l_5_c_2_r_ - OP_l_1_c_2_r_*SPP(15) + OP_l_2_c_2_r_*SPP(16) + OP_l_3_c_2_r_*SPP(11)) + SPP(17)*(OP_l_5_c_1_r_ - OP_l_1_c_1_r_*SPP(15) + OP_l_2_c_1_r_*SPP(16) + OP_l_3_c_1_r_*SPP(11)) - SPP(18)*(OP_l_5_c_3_r_ - OP_l_1_c_3_r_*SPP(15) + OP_l_2_c_3_r_*SPP(16) + OP_l_3_c_3_r_*SPP(11));
nextP(5,5) = OP_l_5_c_5_r_ - OP_l_1_c_5_r_*SPP(15) + OP_l_2_c_5_r_*SPP(16) + OP_l_3_c_5_r_*SPP(11) + dvxNoise*(2*q0*q3 + 2*q1*q2)^2 + dvzNoise*(2*q0*q1 - 2*q2*q3)^2 - SPP(15)*(OP_l_5_c_1_r_ - OP_l_1_c_1_r_*SPP(15) + OP_l_2_c_1_r_*SPP(16) + OP_l_3_c_1_r_*SPP(11)) + SPP(11)*(OP_l_5_c_3_r_ - OP_l_1_c_3_r_*SPP(15) + OP_l_2_c_3_r_*SPP(16) + OP_l_3_c_3_r_*SPP(11)) + SPP(16)*(OP_l_5_c_2_r_ - OP_l_1_c_2_r_*SPP(15) + OP_l_2_c_2_r_*SPP(16) + OP_l_3_c_2_r_*SPP(11)) + dvyNoise*(SQ(5) - SQ(6) + SQ(7) - SQ(8))^2;
nextP(5,6) = OP_l_5_c_6_r_ + SQ(1) - OP_l_1_c_6_r_*SPP(15) + OP_l_2_c_6_r_*SPP(16) + OP_l_3_c_6_r_*SPP(11) + SPP(10)*(OP_l_5_c_1_r_ - OP_l_1_c_1_r_*SPP(15) + OP_l_2_c_1_r_*SPP(16) + OP_l_3_c_1_r_*SPP(11)) - SPP(13)*(OP_l_5_c_2_r_ - OP_l_1_c_2_r_*SPP(15) + OP_l_2_c_2_r_*SPP(16) + OP_l_3_c_2_r_*SPP(11)) + SPP(14)*(OP_l_5_c_3_r_ - OP_l_1_c_3_r_*SPP(15) + OP_l_2_c_3_r_*SPP(16) + OP_l_3_c_3_r_*SPP(11));
nextP(5,7) = OP_l_5_c_7_r_ - OP_l_1_c_7_r_*SPP(15) + OP_l_2_c_7_r_*SPP(16) + OP_l_3_c_7_r_*SPP(11);
nextP(5,8) = OP_l_5_c_8_r_ - OP_l_1_c_8_r_*SPP(15) + OP_l_2_c_8_r_*SPP(16) + OP_l_3_c_8_r_*SPP(11);
nextP(5,9) = OP_l_5_c_9_r_ - OP_l_1_c_9_r_*SPP(15) + OP_l_2_c_9_r_*SPP(16) + OP_l_3_c_9_r_*SPP(11);
nextP(6,1) = SPP(5)*(OP_l_6_c_1_r_ + OP_l_1_c_1_r_*SPP(10) - OP_l_2_c_1_r_*SPP(13) + OP_l_3_c_1_r_*SPP(14)) + SPP(6)*(OP_l_6_c_2_r_ + OP_l_1_c_2_r_*SPP(10) - OP_l_2_c_2_r_*SPP(13) + OP_l_3_c_2_r_*SPP(14)) - SPP(9)*(OP_l_6_c_3_r_ + OP_l_1_c_3_r_*SPP(10) - OP_l_2_c_3_r_*SPP(13) + OP_l_3_c_3_r_*SPP(14)) + SPP(19)*(OP_l_6_c_7_r_ + OP_l_1_c_7_r_*SPP(10) - OP_l_2_c_7_r_*SPP(13) + OP_l_3_c_7_r_*SPP(14));
nextP(6,2) = SPP(4)*(OP_l_6_c_2_r_ + OP_l_1_c_2_r_*SPP(10) - OP_l_2_c_2_r_*SPP(13) + OP_l_3_c_2_r_*SPP(14)) - SPP(3)*(OP_l_6_c_1_r_ + OP_l_1_c_1_r_*SPP(10) - OP_l_2_c_1_r_*SPP(13) + OP_l_3_c_1_r_*SPP(14)) + SPP(8)*(OP_l_6_c_3_r_ + OP_l_1_c_3_r_*SPP(10) - OP_l_2_c_3_r_*SPP(13) + OP_l_3_c_3_r_*SPP(14)) + SPP(19)*(OP_l_6_c_8_r_ + OP_l_1_c_8_r_*SPP(10) - OP_l_2_c_8_r_*SPP(13) + OP_l_3_c_8_r_*SPP(14));
nextP(6,3) = SPP(1)*(OP_l_6_c_1_r_ + OP_l_1_c_1_r_*SPP(10) - OP_l_2_c_1_r_*SPP(13) + OP_l_3_c_1_r_*SPP(14)) - SPP(2)*(OP_l_6_c_2_r_ + OP_l_1_c_2_r_*SPP(10) - OP_l_2_c_2_r_*SPP(13) + OP_l_3_c_2_r_*SPP(14)) + SPP(7)*(OP_l_6_c_3_r_ + OP_l_1_c_3_r_*SPP(10) - OP_l_2_c_3_r_*SPP(13) + OP_l_3_c_3_r_*SPP(14)) + SPP(19)*(OP_l_6_c_9_r_ + OP_l_1_c_9_r_*SPP(10) - OP_l_2_c_9_r_*SPP(13) + OP_l_3_c_9_r_*SPP(14));
nextP(6,4) = OP_l_6_c_4_r_ + SQ(2) + OP_l_1_c_4_r_*SPP(10) - OP_l_2_c_4_r_*SPP(13) + OP_l_3_c_4_r_*SPP(14) - SPP(12)*(OP_l_6_c_2_r_ + OP_l_1_c_2_r_*SPP(10) - OP_l_2_c_2_r_*SPP(13) + OP_l_3_c_2_r_*SPP(14)) + SPP(17)*(OP_l_6_c_1_r_ + OP_l_1_c_1_r_*SPP(10) - OP_l_2_c_1_r_*SPP(13) + OP_l_3_c_1_r_*SPP(14)) - SPP(18)*(OP_l_6_c_3_r_ + OP_l_1_c_3_r_*SPP(10) - OP_l_2_c_3_r_*SPP(13) + OP_l_3_c_3_r_*SPP(14));
nextP(6,5) = OP_l_6_c_5_r_ + SQ(1) + OP_l_1_c_5_r_*SPP(10) - OP_l_2_c_5_r_*SPP(13) + OP_l_3_c_5_r_*SPP(14) - SPP(15)*(OP_l_6_c_1_r_ + OP_l_1_c_1_r_*SPP(10) - OP_l_2_c_1_r_*SPP(13) + OP_l_3_c_1_r_*SPP(14)) + SPP(11)*(OP_l_6_c_3_r_ + OP_l_1_c_3_r_*SPP(10) - OP_l_2_c_3_r_*SPP(13) + OP_l_3_c_3_r_*SPP(14)) + SPP(16)*(OP_l_6_c_2_r_ + OP_l_1_c_2_r_*SPP(10) - OP_l_2_c_2_r_*SPP(13) + OP_l_3_c_2_r_*SPP(14));
nextP(6,6) = OP_l_6_c_6_r_ + OP_l_1_c_6_r_*SPP(10) - OP_l_2_c_6_r_*SPP(13) + OP_l_3_c_6_r_*SPP(14) + dvxNoise*(2*q0*q2 - 2*q1*q3)^2 + dvyNoise*(2*q0*q1 + 2*q2*q3)^2 + SPP(10)*(OP_l_6_c_1_r_ + OP_l_1_c_1_r_*SPP(10) - OP_l_2_c_1_r_*SPP(13) + OP_l_3_c_1_r_*SPP(14)) - SPP(13)*(OP_l_6_c_2_r_ + OP_l_1_c_2_r_*SPP(10) - OP_l_2_c_2_r_*SPP(13) + OP_l_3_c_2_r_*SPP(14)) + SPP(14)*(OP_l_6_c_3_r_ + OP_l_1_c_3_r_*SPP(10) - OP_l_2_c_3_r_*SPP(13) + OP_l_3_c_3_r_*SPP(14)) + dvzNoise*(SQ(5) - SQ(6) - SQ(7) + SQ(8))^2;
nextP(6,7) = OP_l_6_c_7_r_ + OP_l_1_c_7_r_*SPP(10) - OP_l_2_c_7_r_*SPP(13) + OP_l_3_c_7_r_*SPP(14);
nextP(6,8) = OP_l_6_c_8_r_ + OP_l_1_c_8_r_*SPP(10) - OP_l_2_c_8_r_*SPP(13) + OP_l_3_c_8_r_*SPP(14);
nextP(6,9) = OP_l_6_c_9_r_ + OP_l_1_c_9_r_*SPP(10) - OP_l_2_c_9_r_*SPP(13) + OP_l_3_c_9_r_*SPP(14);
nextP(7,1) = OP_l_7_c_1_r_*SPP(5) + OP_l_7_c_2_r_*SPP(6) - OP_l_7_c_3_r_*SPP(9) + OP_l_7_c_7_r_*SPP(19);
nextP(7,2) = OP_l_7_c_2_r_*SPP(4) - OP_l_7_c_1_r_*SPP(3) + OP_l_7_c_3_r_*SPP(8) + OP_l_7_c_8_r_*SPP(19);
nextP(7,3) = OP_l_7_c_1_r_*SPP(1) - OP_l_7_c_2_r_*SPP(2) + OP_l_7_c_3_r_*SPP(7) + OP_l_7_c_9_r_*SPP(19);
nextP(7,4) = OP_l_7_c_4_r_ - OP_l_7_c_2_r_*SPP(12) + OP_l_7_c_1_r_*SPP(17) - OP_l_7_c_3_r_*SPP(18);
nextP(7,5) = OP_l_7_c_5_r_ + OP_l_7_c_3_r_*SPP(11) - OP_l_7_c_1_r_*SPP(15) + OP_l_7_c_2_r_*SPP(16);
nextP(7,6) = OP_l_7_c_6_r_ + OP_l_7_c_1_r_*SPP(10) - OP_l_7_c_2_r_*SPP(13) + OP_l_7_c_3_r_*SPP(14);
nextP(7,7) = OP_l_7_c_7_r_;
nextP(7,8) = OP_l_7_c_8_r_;
nextP(7,9) = OP_l_7_c_9_r_;
nextP(8,1) = OP_l_8_c_1_r_*SPP(5) + OP_l_8_c_2_r_*SPP(6) - OP_l_8_c_3_r_*SPP(9) + OP_l_8_c_7_r_*SPP(19);
nextP(8,2) = OP_l_8_c_2_r_*SPP(4) - OP_l_8_c_1_r_*SPP(3) + OP_l_8_c_3_r_*SPP(8) + OP_l_8_c_8_r_*SPP(19);
nextP(8,3) = OP_l_8_c_1_r_*SPP(1) - OP_l_8_c_2_r_*SPP(2) + OP_l_8_c_3_r_*SPP(7) + OP_l_8_c_9_r_*SPP(19);
nextP(8,4) = OP_l_8_c_4_r_ - OP_l_8_c_2_r_*SPP(12) + OP_l_8_c_1_r_*SPP(17) - OP_l_8_c_3_r_*SPP(18);
nextP(8,5) = OP_l_8_c_5_r_ + OP_l_8_c_3_r_*SPP(11) - OP_l_8_c_1_r_*SPP(15) + OP_l_8_c_2_r_*SPP(16);
nextP(8,6) = OP_l_8_c_6_r_ + OP_l_8_c_1_r_*SPP(10) - OP_l_8_c_2_r_*SPP(13) + OP_l_8_c_3_r_*SPP(14);
nextP(8,7) = OP_l_8_c_7_r_;
nextP(8,8) = OP_l_8_c_8_r_;
nextP(8,9) = OP_l_8_c_9_r_;
nextP(9,1) = OP_l_9_c_1_r_*SPP(5) + OP_l_9_c_2_r_*SPP(6) - OP_l_9_c_3_r_*SPP(9) + OP_l_9_c_7_r_*SPP(19);
nextP(9,2) = OP_l_9_c_2_r_*SPP(4) - OP_l_9_c_1_r_*SPP(3) + OP_l_9_c_3_r_*SPP(8) + OP_l_9_c_8_r_*SPP(19);
nextP(9,3) = OP_l_9_c_1_r_*SPP(1) - OP_l_9_c_2_r_*SPP(2) + OP_l_9_c_3_r_*SPP(7) + OP_l_9_c_9_r_*SPP(19);
nextP(9,4) = OP_l_9_c_4_r_ - OP_l_9_c_2_r_*SPP(12) + OP_l_9_c_1_r_*SPP(17) - OP_l_9_c_3_r_*SPP(18);
nextP(9,5) = OP_l_9_c_5_r_ + OP_l_9_c_3_r_*SPP(11) - OP_l_9_c_1_r_*SPP(15) + OP_l_9_c_2_r_*SPP(16);
nextP(9,6) = OP_l_9_c_6_r_ + OP_l_9_c_1_r_*SPP(10) - OP_l_9_c_2_r_*SPP(13) + OP_l_9_c_3_r_*SPP(14);
nextP(9,7) = OP_l_9_c_7_r_;
nextP(9,8) = OP_l_9_c_8_r_;
nextP(9,9) = OP_l_9_c_9_r_;
Reference in New Issue View Git Blame Copy Permalink