dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity
px4-firmware/EKF/matlab/scripts/Inertial Nav EKF/M_code24.txt

687 lines
114 KiB
Plaintext
Raw Normal View History

EKF: update derivation files
2016-05-01 02:06:17 -03:00
SF = zeros(21,1);
SF(1) = dvz - dvz_b;
SF(2) = dvy - dvy_b;
SF(3) = dvx - dvx_b;
SF(4) = 2*q1*SF(3) + 2*q2*SF(2) + 2*q3*SF(1);
SF(5) = 2*q0*SF(2) - 2*q1*SF(1) + 2*q3*SF(3);
SF(6) = 2*q0*SF(3) + 2*q2*SF(1) - 2*q3*SF(2);
SF(7) = day/2 - day_b/2;
SF(8) = daz/2 - daz_b/2;
SF(9) = dax/2 - dax_b/2;
SF(10) = dax_b/2 - dax/2;
SF(11) = daz_b/2 - daz/2;
SF(12) = day_b/2 - day/2;
SF(13) = 2*q1*SF(2);
SF(14) = 2*q0*SF(1);
SF(15) = q1/2;
SF(16) = q2/2;
SF(17) = q3/2;
SF(18) = q3^2;
SF(19) = q2^2;
SF(20) = q1^2;
SF(21) = q0^2;
SG = zeros(8,1);
SG(1) = q0/2;
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
SG(2) = q3^2;
SG(3) = q2^2;
SG(4) = q1^2;
SG(5) = q0^2;
EKF: update derivation files
2016-05-01 02:06:17 -03:00
SG(6) = 2*q2*q3;
SG(7) = 2*q1*q3;
SG(8) = 2*q1*q2;
SQ = zeros(11,1);
SQ(1) = dvzVar*(SG(6) - 2*q0*q1)*(SG(2) - SG(3) - SG(4) + SG(5)) - dvyVar*(SG(6) + 2*q0*q1)*(SG(2) - SG(3) + SG(4) - SG(5)) + dvxVar*(SG(7) - 2*q0*q2)*(SG(8) + 2*q0*q3);
SQ(2) = dvzVar*(SG(7) + 2*q0*q2)*(SG(2) - SG(3) - SG(4) + SG(5)) - dvxVar*(SG(7) - 2*q0*q2)*(SG(2) + SG(3) - SG(4) - SG(5)) + dvyVar*(SG(6) + 2*q0*q1)*(SG(8) - 2*q0*q3);
SQ(3) = dvzVar*(SG(6) - 2*q0*q1)*(SG(7) + 2*q0*q2) - dvyVar*(SG(8) - 2*q0*q3)*(SG(2) - SG(3) + SG(4) - SG(5)) - dvxVar*(SG(8) + 2*q0*q3)*(SG(2) + SG(3) - SG(4) - SG(5));
SQ(4) = (dayVar*q1*SG(1))/2 - (dazVar*q1*SG(1))/2 - (daxVar*q2*q3)/4;
SQ(5) = (dazVar*q2*SG(1))/2 - (daxVar*q2*SG(1))/2 - (dayVar*q1*q3)/4;
SQ(6) = (daxVar*q3*SG(1))/2 - (dayVar*q3*SG(1))/2 - (dazVar*q1*q2)/4;
SQ(7) = (daxVar*q1*q2)/4 - (dazVar*q3*SG(1))/2 - (dayVar*q1*q2)/4;
SQ(8) = (dazVar*q1*q3)/4 - (daxVar*q1*q3)/4 - (dayVar*q2*SG(1))/2;
SQ(9) = (dayVar*q2*q3)/4 - (daxVar*q1*SG(1))/2 - (dazVar*q2*q3)/4;
SQ(10) = SG(1)^2;
SQ(11) = q1^2;
SPP = zeros(11,1);
SPP(1) = SF(13) + SF(14) - 2*q2*SF(3);
SPP(2) = SF(18) - SF(19) - SF(20) + SF(21);
SPP(3) = SF(18) - SF(19) + SF(20) - SF(21);
SPP(4) = SF(18) + SF(19) - SF(20) - SF(21);
SPP(5) = 2*q0*q2 - 2*q1*q3;
SPP(6) = 2*q0*q1 - 2*q2*q3;
SPP(7) = 2*q0*q3 - 2*q1*q2;
SPP(8) = 2*q0*q1 + 2*q2*q3;
SPP(9) = 2*q0*q3 + 2*q1*q2;
SPP(10) = 2*q0*q2 + 2*q1*q3;
SPP(11) = SF(17);
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
nextP = zeros(24,24);
EKF: update derivation files
2016-05-01 02:06:17 -03:00
nextP(1,1) = OP(1,1) + OP(2,1)*SF(10) + OP(3,1)*SF(12) + OP(4,1)*SF(11) + OP(11,1)*SF(15) + OP(12,1)*SF(16) + OP(13,1)*SPP(11) + (daxVar*SQ(11))/4 + SF(10)*(OP(1,2) + OP(2,2)*SF(10) + OP(3,2)*SF(12) + OP(4,2)*SF(11) + OP(11,2)*SF(15) + OP(12,2)*SF(16) + OP(13,2)*SPP(11)) + SF(12)*(OP(1,3) + OP(2,3)*SF(10) + OP(3,3)*SF(12) + OP(4,3)*SF(11) + OP(11,3)*SF(15) + OP(12,3)*SF(16) + OP(13,3)*SPP(11)) + SF(11)*(OP(1,4) + OP(2,4)*SF(10) + OP(3,4)*SF(12) + OP(4,4)*SF(11) + OP(11,4)*SF(15) + OP(12,4)*SF(16) + OP(13,4)*SPP(11)) + SF(15)*(OP(1,11) + OP(2,11)*SF(10) + OP(3,11)*SF(12) + OP(4,11)*SF(11) + OP(11,11)*SF(15) + OP(12,11)*SF(16) + OP(13,11)*SPP(11)) + SF(16)*(OP(1,12) + OP(2,12)*SF(10) + OP(3,12)*SF(12) + OP(4,12)*SF(11) + OP(11,12)*SF(15) + OP(12,12)*SF(16) + OP(13,12)*SPP(11)) + SPP(11)*(OP(1,13) + OP(2,13)*SF(10) + OP(3,13)*SF(12) + OP(4,13)*SF(11) + OP(11,13)*SF(15) + OP(12,13)*SF(16) + OP(13,13)*SPP(11)) + (dayVar*q2^2)/4 + (dazVar*q3^2)/4;
nextP(1,2) = OP(1,2) + SQ(9) + OP(2,2)*SF(10) + OP(3,2)*SF(12) + OP(4,2)*SF(11) + OP(11,2)*SF(15) + OP(12,2)*SF(16) + OP(13,2)*SPP(11) + SF(9)*(OP(1,1) + OP(2,1)*SF(10) + OP(3,1)*SF(12) + OP(4,1)*SF(11) + OP(11,1)*SF(15) + OP(12,1)*SF(16) + OP(13,1)*SPP(11)) + SF(8)*(OP(1,3) + OP(2,3)*SF(10) + OP(3,3)*SF(12) + OP(4,3)*SF(11) + OP(11,3)*SF(15) + OP(12,3)*SF(16) + OP(13,3)*SPP(11)) + SF(12)*(OP(1,4) + OP(2,4)*SF(10) + OP(3,4)*SF(12) + OP(4,4)*SF(11) + OP(11,4)*SF(15) + OP(12,4)*SF(16) + OP(13,4)*SPP(11)) - SF(16)*(OP(1,13) + OP(2,13)*SF(10) + OP(3,13)*SF(12) + OP(4,13)*SF(11) + OP(11,13)*SF(15) + OP(12,13)*SF(16) + OP(13,13)*SPP(11)) + SPP(11)*(OP(1,12) + OP(2,12)*SF(10) + OP(3,12)*SF(12) + OP(4,12)*SF(11) + OP(11,12)*SF(15) + OP(12,12)*SF(16) + OP(13,12)*SPP(11)) - (q0*(OP(1,11) + OP(2,11)*SF(10) + OP(3,11)*SF(12) + OP(4,11)*SF(11) + OP(11,11)*SF(15) + OP(12,11)*SF(16) + OP(13,11)*SPP(11)))/2;
nextP(2,2) = OP(2,2) + OP(1,2)*SF(9) + OP(3,2)*SF(8) + OP(4,2)*SF(12) - OP(13,2)*SF(16) + OP(12,2)*SPP(11) + daxVar*SQ(10) - (OP(11,2)*q0)/2 + SF(9)*(OP(2,1) + OP(1,1)*SF(9) + OP(3,1)*SF(8) + OP(4,1)*SF(12) - OP(13,1)*SF(16) + OP(12,1)*SPP(11) - (OP(11,1)*q0)/2) + SF(8)*(OP(2,3) + OP(1,3)*SF(9) + OP(3,3)*SF(8) + OP(4,3)*SF(12) - OP(13,3)*SF(16) + OP(12,3)*SPP(11) - (OP(11,3)*q0)/2) + SF(12)*(OP(2,4) + OP(1,4)*SF(9) + OP(3,4)*SF(8) + OP(4,4)*SF(12) - OP(13,4)*SF(16) + OP(12,4)*SPP(11) - (OP(11,4)*q0)/2) - SF(16)*(OP(2,13) + OP(1,13)*SF(9) + OP(3,13)*SF(8) + OP(4,13)*SF(12) - OP(13,13)*SF(16) + OP(12,13)*SPP(11) - (OP(11,13)*q0)/2) + SPP(11)*(OP(2,12) + OP(1,12)*SF(9) + OP(3,12)*SF(8) + OP(4,12)*SF(12) - OP(13,12)*SF(16) + OP(12,12)*SPP(11) - (OP(11,12)*q0)/2) + (dayVar*q3^2)/4 + (dazVar*q2^2)/4 - (q0*(OP(2,11) + OP(1,11)*SF(9) + OP(3,11)*SF(8) + OP(4,11)*SF(12) - OP(13,11)*SF(16) + OP(12,11)*SPP(11) - (OP(11,11)*q0)/2))/2;
nextP(1,3) = OP(1,3) + SQ(8) + OP(2,3)*SF(10) + OP(3,3)*SF(12) + OP(4,3)*SF(11) + OP(11,3)*SF(15) + OP(12,3)*SF(16) + OP(13,3)*SPP(11) + SF(7)*(OP(1,1) + OP(2,1)*SF(10) + OP(3,1)*SF(12) + OP(4,1)*SF(11) + OP(11,1)*SF(15) + OP(12,1)*SF(16) + OP(13,1)*SPP(11)) + SF(11)*(OP(1,2) + OP(2,2)*SF(10) + OP(3,2)*SF(12) + OP(4,2)*SF(11) + OP(11,2)*SF(15) + OP(12,2)*SF(16) + OP(13,2)*SPP(11)) + SF(9)*(OP(1,4) + OP(2,4)*SF(10) + OP(3,4)*SF(12) + OP(4,4)*SF(11) + OP(11,4)*SF(15) + OP(12,4)*SF(16) + OP(13,4)*SPP(11)) + SF(15)*(OP(1,13) + OP(2,13)*SF(10) + OP(3,13)*SF(12) + OP(4,13)*SF(11) + OP(11,13)*SF(15) + OP(12,13)*SF(16) + OP(13,13)*SPP(11)) - SPP(11)*(OP(1,11) + OP(2,11)*SF(10) + OP(3,11)*SF(12) + OP(4,11)*SF(11) + OP(11,11)*SF(15) + OP(12,11)*SF(16) + OP(13,11)*SPP(11)) - (q0*(OP(1,12) + OP(2,12)*SF(10) + OP(3,12)*SF(12) + OP(4,12)*SF(11) + OP(11,12)*SF(15) + OP(12,12)*SF(16) + OP(13,12)*SPP(11)))/2;
nextP(2,3) = OP(2,3) + SQ(6) + OP(1,3)*SF(9) + OP(3,3)*SF(8) + OP(4,3)*SF(12) - OP(13,3)*SF(16) + OP(12,3)*SPP(11) - (OP(11,3)*q0)/2 + SF(7)*(OP(2,1) + OP(1,1)*SF(9) + OP(3,1)*SF(8) + OP(4,1)*SF(12) - OP(13,1)*SF(16) + OP(12,1)*SPP(11) - (OP(11,1)*q0)/2) + SF(11)*(OP(2,2) + OP(1,2)*SF(9) + OP(3,2)*SF(8) + OP(4,2)*SF(12) - OP(13,2)*SF(16) + OP(12,2)*SPP(11) - (OP(11,2)*q0)/2) + SF(9)*(OP(2,4) + OP(1,4)*SF(9) + OP(3,4)*SF(8) + OP(4,4)*SF(12) - OP(13,4)*SF(16) + OP(12,4)*SPP(11) - (OP(11,4)*q0)/2) + SF(15)*(OP(2,13) + OP(1,13)*SF(9) + OP(3,13)*SF(8) + OP(4,13)*SF(12) - OP(13,13)*SF(16) + OP(12,13)*SPP(11) - (OP(11,13)*q0)/2) - SPP(11)*(OP(2,11) + OP(1,11)*SF(9) + OP(3,11)*SF(8) + OP(4,11)*SF(12) - OP(13,11)*SF(16) + OP(12,11)*SPP(11) - (OP(11,11)*q0)/2) - (q0*(OP(2,12) + OP(1,12)*SF(9) + OP(3,12)*SF(8) + OP(4,12)*SF(12) - OP(13,12)*SF(16) + OP(12,12)*SPP(11) - (OP(11,12)*q0)/2))/2;
nextP(3,3) = OP(3,3) + OP(1,3)*SF(7) + OP(2,3)*SF(11) + OP(4,3)*SF(9) + OP(13,3)*SF(15) - OP(11,3)*SPP(11) + dayVar*SQ(10) + (dazVar*SQ(11))/4 - (OP(12,3)*q0)/2 + SF(7)*(OP(3,1) + OP(1,1)*SF(7) + OP(2,1)*SF(11) + OP(4,1)*SF(9) + OP(13,1)*SF(15) - OP(11,1)*SPP(11) - (OP(12,1)*q0)/2) + SF(11)*(OP(3,2) + OP(1,2)*SF(7) + OP(2,2)*SF(11) + OP(4,2)*SF(9) + OP(13,2)*SF(15) - OP(11,2)*SPP(11) - (OP(12,2)*q0)/2) + SF(9)*(OP(3,4) + OP(1,4)*SF(7) + OP(2,4)*SF(11) + OP(4,4)*SF(9) + OP(13,4)*SF(15) - OP(11,4)*SPP(11) - (OP(12,4)*q0)/2) + SF(15)*(OP(3,13) + OP(1,13)*SF(7) + OP(2,13)*SF(11) + OP(4,13)*SF(9) + OP(13,13)*SF(15) - OP(11,13)*SPP(11) - (OP(12,13)*q0)/2) - SPP(11)*(OP(3,11) + OP(1,11)*SF(7) + OP(2,11)*SF(11) + OP(4,11)*SF(9) + OP(13,11)*SF(15) - OP(11,11)*SPP(11) - (OP(12,11)*q0)/2) + (daxVar*q3^2)/4 - (q0*(OP(3,12) + OP(1,12)*SF(7) + OP(2,12)*SF(11) + OP(4,12)*SF(9) + OP(13,12)*SF(15) - OP(11,12)*SPP(11) - (OP(12,12)*q0)/2))/2;
nextP(1,4) = OP(1,4) + SQ(7) + OP(2,4)*SF(10) + OP(3,4)*SF(12) + OP(4,4)*SF(11) + OP(11,4)*SF(15) + OP(12,4)*SF(16) + OP(13,4)*SPP(11) + SF(8)*(OP(1,1) + OP(2,1)*SF(10) + OP(3,1)*SF(12) + OP(4,1)*SF(11) + OP(11,1)*SF(15) + OP(12,1)*SF(16) + OP(13,1)*SPP(11)) + SF(7)*(OP(1,2) + OP(2,2)*SF(10) + OP(3,2)*SF(12) + OP(4,2)*SF(11) + OP(11,2)*SF(15) + OP(12,2)*SF(16) + OP(13,2)*SPP(11)) + SF(10)*(OP(1,3) + OP(2,3)*SF(10) + OP(3,3)*SF(12) + OP(4,3)*SF(11) + OP(11,3)*SF(15) + OP(12,3)*SF(16) + OP(13,3)*SPP(11)) + SF(16)*(OP(1,11) + OP(2,11)*SF(10) + OP(3,11)*SF(12) + OP(4,11)*SF(11) + OP(11,11)*SF(15) + OP(12,11)*SF(16) + OP(13,11)*SPP(11)) - SF(15)*(OP(1,12) + OP(2,12)*SF(10) + OP(3,12)*SF(12) + OP(4,12)*SF(11) + OP(11,12)*SF(15) + OP(12,12)*SF(16) + OP(13,12)*SPP(11)) - (q0*(OP(1,13) + OP(2,13)*SF(10) + OP(3,13)*SF(12) + OP(4,13)*SF(11) + OP(11,13)*SF(15) + OP(12,13)*SF(16) + OP(13,13)*SPP(11)))/2;
nextP(2,4) = OP(2,4) + SQ(5) + OP(1,4)*SF(9) + OP(3,4)*SF(8) + OP(4,4)*SF(12) - OP(13,4)*SF(16) + OP(12,4)*SPP(11) - (OP(11,4)*q0)/2 + SF(8)*(OP(2,1) + OP(1,1)*SF(9) + OP(3,1)*SF(8) + OP(4,1)*SF(12) - OP(13,1)*SF(16) + OP(12,1)*SPP(11) - (OP(11,1)*q0)/2) + SF(7)*(OP(2,2) + OP(1,2)*SF(9) + OP(3,2)*SF(8) + OP(4,2)*SF(12) - OP(13,2)*SF(16) + OP(12,2)*SPP(11) - (OP(11,2)*q0)/2) + SF(10)*(OP(2,3) + OP(1,3)*SF(9) + OP(3,3)*SF(8) + OP(4,3)*SF(12) - OP(13,3)*SF(16) + OP(12,3)*SPP(11) - (OP(11,3)*q0)/2) + SF(16)*(OP(2,11) + OP(1,11)*SF(9) + OP(3,11)*SF(8) + OP(4,11)*SF(12) - OP(13,11)*SF(16) + OP(12,11)*SPP(11) - (OP(11,11)*q0)/2) - SF(15)*(OP(2,12) + OP(1,12)*SF(9) + OP(3,12)*SF(8) + OP(4,12)*SF(12) - OP(13,12)*SF(16) + OP(12,12)*SPP(11) - (OP(11,12)*q0)/2) - (q0*(OP(2,13) + OP(1,13)*SF(9) + OP(3,13)*SF(8) + OP(4,13)*SF(12) - OP(13,13)*SF(16) + OP(12,13)*SPP(11) - (OP(11,13)*q0)/2))/2;
nextP(3,4) = OP(3,4) + SQ(4) + OP(1,4)*SF(7) + OP(2,4)*SF(11) + OP(4,4)*SF(9) + OP(13,4)*SF(15) - OP(11,4)*SPP(11) - (OP(12,4)*q0)/2 + SF(8)*(OP(3,1) + OP(1,1)*SF(7) + OP(2,1)*SF(11) + OP(4,1)*SF(9) + OP(13,1)*SF(15) - OP(11,1)*SPP(11) - (OP(12,1)*q0)/2) + SF(7)*(OP(3,2) + OP(1,2)*SF(7) + OP(2,2)*SF(11) + OP(4,2)*SF(9) + OP(13,2)*SF(15) - OP(11,2)*SPP(11) - (OP(12,2)*q0)/2) + SF(10)*(OP(3,3) + OP(1,3)*SF(7) + OP(2,3)*SF(11) + OP(4,3)*SF(9) + OP(13,3)*SF(15) - OP(11,3)*SPP(11) - (OP(12,3)*q0)/2) + SF(16)*(OP(3,11) + OP(1,11)*SF(7) + OP(2,11)*SF(11) + OP(4,11)*SF(9) + OP(13,11)*SF(15) - OP(11,11)*SPP(11) - (OP(12,11)*q0)/2) - SF(15)*(OP(3,12) + OP(1,12)*SF(7) + OP(2,12)*SF(11) + OP(4,12)*SF(9) + OP(13,12)*SF(15) - OP(11,12)*SPP(11) - (OP(12,12)*q0)/2) - (q0*(OP(3,13) + OP(1,13)*SF(7) + OP(2,13)*SF(11) + OP(4,13)*SF(9) + OP(13,13)*SF(15) - OP(11,13)*SPP(11) - (OP(12,13)*q0)/2))/2;
nextP(4,4) = OP(4,4) + OP(1,4)*SF(8) + OP(2,4)*SF(7) + OP(3,4)*SF(10) + OP(11,4)*SF(16) - OP(12,4)*SF(15) + (dayVar*SQ(11))/4 + dazVar*SQ(10) - (OP(13,4)*q0)/2 + SF(8)*(OP(4,1) + OP(1,1)*SF(8) + OP(2,1)*SF(7) + OP(3,1)*SF(10) + OP(11,1)*SF(16) - OP(12,1)*SF(15) - (OP(13,1)*q0)/2) + SF(7)*(OP(4,2) + OP(1,2)*SF(8) + OP(2,2)*SF(7) + OP(3,2)*SF(10) + OP(11,2)*SF(16) - OP(12,2)*SF(15) - (OP(13,2)*q0)/2) + SF(10)*(OP(4,3) + OP(1,3)*SF(8) + OP(2,3)*SF(7) + OP(3,3)*SF(10) + OP(11,3)*SF(16) - OP(12,3)*SF(15) - (OP(13,3)*q0)/2) + SF(16)*(OP(4,11) + OP(1,11)*SF(8) + OP(2,11)*SF(7) + OP(3,11)*SF(10) + OP(11,11)*SF(16) - OP(12,11)*SF(15) - (OP(13,11)*q0)/2) - SF(15)*(OP(4,12) + OP(1,12)*SF(8) + OP(2,12)*SF(7) + OP(3,12)*SF(10) + OP(11,12)*SF(16) - OP(12,12)*SF(15) - (OP(13,12)*q0)/2) + (daxVar*q2^2)/4 - (q0*(OP(4,13) + OP(1,13)*SF(8) + OP(2,13)*SF(7) + OP(3,13)*SF(10) + OP(11,13)*SF(16) - OP(12,13)*SF(15) - (OP(13,13)*q0)/2))/2;
nextP(1,5) = OP(1,5) + OP(2,5)*SF(10) + OP(3,5)*SF(12) + OP(4,5)*SF(11) + OP(11,5)*SF(15) + OP(12,5)*SF(16) + OP(13,5)*SPP(11) + SF(6)*(OP(1,1) + OP(2,1)*SF(10) + OP(3,1)*SF(12) + OP(4,1)*SF(11) + OP(11,1)*SF(15) + OP(12,1)*SF(16) + OP(13,1)*SPP(11)) + SF(4)*(OP(1,2) + OP(2,2)*SF(10) + OP(3,2)*SF(12) + OP(4,2)*SF(11) + OP(11,2)*SF(15) + OP(12,2)*SF(16) + OP(13,2)*SPP(11)) - SF(5)*(OP(1,4) + OP(2,4)*SF(10) + OP(3,4)*SF(12) + OP(4,4)*SF(11) + OP(11,4)*SF(15) + OP(12,4)*SF(16) + OP(13,4)*SPP(11)) + SPP(1)*(OP(1,3) + OP(2,3)*SF(10) + OP(3,3)*SF(12) + OP(4,3)*SF(11) + OP(11,3)*SF(15) + OP(12,3)*SF(16) + OP(13,3)*SPP(11)) + SPP(4)*(OP(1,14) + OP(2,14)*SF(10) + OP(3,14)*SF(12) + OP(4,14)*SF(11) + OP(11,14)*SF(15) + OP(12,14)*SF(16) + OP(13,14)*SPP(11)) + SPP(7)*(OP(1,15) + OP(2,15)*SF(10) + OP(3,15)*SF(12) + OP(4,15)*SF(11) + OP(11,15)*SF(15) + OP(12,15)*SF(16) + OP(13,15)*SPP(11)) - SPP(10)*(OP(1,16) + OP(2,16)*SF(10) + OP(3,16)*SF(12) + OP(4,16)*SF(11) + OP(11,16)*SF(15) + OP(12,16)*SF(16) + OP(13,16)*SPP(11));
nextP(2,5) = OP(2,5) + OP(1,5)*SF(9) + OP(3,5)*SF(8) + OP(4,5)*SF(12) - OP(13,5)*SF(16) + OP(12,5)*SPP(11) - (OP(11,5)*q0)/2 + SF(6)*(OP(2,1) + OP(1,1)*SF(9) + OP(3,1)*SF(8) + OP(4,1)*SF(12) - OP(13,1)*SF(16) + OP(12,1)*SPP(11) - (OP(11,1)*q0)/2) + SF(4)*(OP(2,2) + OP(1,2)*SF(9) + OP(3,2)*SF(8) + OP(4,2)*SF(12) - OP(13,2)*SF(16) + OP(12,2)*SPP(11) - (OP(11,2)*q0)/2) - SF(5)*(OP(2,4) + OP(1,4)*SF(9) + OP(3,4)*SF(8) + OP(4,4)*SF(12) - OP(13,4)*SF(16) + OP(12,4)*SPP(11) - (OP(11,4)*q0)/2) + SPP(1)*(OP(2,3) + OP(1,3)*SF(9) + OP(3,3)*SF(8) + OP(4,3)*SF(12) - OP(13,3)*SF(16) + OP(12,3)*SPP(11) - (OP(11,3)*q0)/2) + SPP(4)*(OP(2,14) + OP(1,14)*SF(9) + OP(3,14)*SF(8) + OP(4,14)*SF(12) - OP(13,14)*SF(16) + OP(12,14)*SPP(11) - (OP(11,14)*q0)/2) + SPP(7)*(OP(2,15) + OP(1,15)*SF(9) + OP(3,15)*SF(8) + OP(4,15)*SF(12) - OP(13,15)*SF(16) + OP(12,15)*SPP(11) - (OP(11,15)*q0)/2) - SPP(10)*(OP(2,16) + OP(1,16)*SF(9) + OP(3,16)*SF(8) + OP(4,16)*SF(12) - OP(13,16)*SF(16) + OP(12,16)*SPP(11) - (OP(11,16)*q0)/2);
nextP(3,5) = OP(3,5) + OP(1,5)*SF(7) + OP(2,5)*SF(11) + OP(4,5)*SF(9) + OP(13,5)*SF(15) - OP(11,5)*SPP(11) - (OP(12,5)*q0)/2 + SF(6)*(OP(3,1) + OP(1,1)*SF(7) + OP(2,1)*SF(11) + OP(4,1)*SF(9) + OP(13,1)*SF(15) - OP(11,1)*SPP(11) - (OP(12,1)*q0)/2) + SF(4)*(OP(3,2) + OP(1,2)*SF(7) + OP(2,2)*SF(11) + OP(4,2)*SF(9) + OP(13,2)*SF(15) - OP(11,2)*SPP(11) - (OP(12,2)*q0)/2) - SF(5)*(OP(3,4) + OP(1,4)*SF(7) + OP(2,4)*SF(11) + OP(4,4)*SF(9) + OP(13,4)*SF(15) - OP(11,4)*SPP(11) - (OP(12,4)*q0)/2) + SPP(1)*(OP(3,3) + OP(1,3)*SF(7) + OP(2,3)*SF(11) + OP(4,3)*SF(9) + OP(13,3)*SF(15) - OP(11,3)*SPP(11) - (OP(12,3)*q0)/2) + SPP(4)*(OP(3,14) + OP(1,14)*SF(7) + OP(2,14)*SF(11) + OP(4,14)*SF(9) + OP(13,14)*SF(15) - OP(11,14)*SPP(11) - (OP(12,14)*q0)/2) + SPP(7)*(OP(3,15) + OP(1,15)*SF(7) + OP(2,15)*SF(11) + OP(4,15)*SF(9) + OP(13,15)*SF(15) - OP(11,15)*SPP(11) - (OP(12,15)*q0)/2) - SPP(10)*(OP(3,16) + OP(1,16)*SF(7) + OP(2,16)*SF(11) + OP(4,16)*SF(9) + OP(13,16)*SF(15) - OP(11,16)*SPP(11) - (OP(12,16)*q0)/2);
nextP(4,5) = OP(4,5) + OP(1,5)*SF(8) + OP(2,5)*SF(7) + OP(3,5)*SF(10) + OP(11,5)*SF(16) - OP(12,5)*SF(15) - (OP(13,5)*q0)/2 + SF(6)*(OP(4,1) + OP(1,1)*SF(8) + OP(2,1)*SF(7) + OP(3,1)*SF(10) + OP(11,1)*SF(16) - OP(12,1)*SF(15) - (OP(13,1)*q0)/2) + SF(4)*(OP(4,2) + OP(1,2)*SF(8) + OP(2,2)*SF(7) + OP(3,2)*SF(10) + OP(11,2)*SF(16) - OP(12,2)*SF(15) - (OP(13,2)*q0)/2) - SF(5)*(OP(4,4) + OP(1,4)*SF(8) + OP(2,4)*SF(7) + OP(3,4)*SF(10) + OP(11,4)*SF(16) - OP(12,4)*SF(15) - (OP(13,4)*q0)/2) + SPP(1)*(OP(4,3) + OP(1,3)*SF(8) + OP(2,3)*SF(7) + OP(3,3)*SF(10) + OP(11,3)*SF(16) - OP(12,3)*SF(15) - (OP(13,3)*q0)/2) + SPP(4)*(OP(4,14) + OP(1,14)*SF(8) + OP(2,14)*SF(7) + OP(3,14)*SF(10) + OP(11,14)*SF(16) - OP(12,14)*SF(15) - (OP(13,14)*q0)/2) + SPP(7)*(OP(4,15) + OP(1,15)*SF(8) + OP(2,15)*SF(7) + OP(3,15)*SF(10) + OP(11,15)*SF(16) - OP(12,15)*SF(15) - (OP(13,15)*q0)/2) - SPP(10)*(OP(4,16) + OP(1,16)*SF(8) + OP(2,16)*SF(7) + OP(3,16)*SF(10) + OP(11,16)*SF(16) - OP(12,16)*SF(15) - (OP(13,16)*q0)/2);
nextP(5,5) = OP(5,5) + OP(1,5)*SF(6) + OP(2,5)*SF(4) - OP(4,5)*SF(5) + OP(3,5)*SPP(1) + OP(14,5)*SPP(4) + OP(15,5)*SPP(7) - OP(16,5)*SPP(10) + dvyVar*(SG(8) - 2*q0*q3)^2 + dvzVar*(SG(7) + 2*q0*q2)^2 + SF(6)*(OP(5,1) + OP(1,1)*SF(6) + OP(2,1)*SF(4) - OP(4,1)*SF(5) + OP(3,1)*SPP(1) + OP(14,1)*SPP(4) + OP(15,1)*SPP(7) - OP(16,1)*SPP(10)) + SF(4)*(OP(5,2) + OP(1,2)*SF(6) + OP(2,2)*SF(4) - OP(4,2)*SF(5) + OP(3,2)*SPP(1) + OP(14,2)*SPP(4) + OP(15,2)*SPP(7) - OP(16,2)*SPP(10)) - SF(5)*(OP(5,4) + OP(1,4)*SF(6) + OP(2,4)*SF(4) - OP(4,4)*SF(5) + OP(3,4)*SPP(1) + OP(14,4)*SPP(4) + OP(15,4)*SPP(7) - OP(16,4)*SPP(10)) + SPP(1)*(OP(5,3) + OP(1,3)*SF(6) + OP(2,3)*SF(4) - OP(4,3)*SF(5) + OP(3,3)*SPP(1) + OP(14,3)*SPP(4) + OP(15,3)*SPP(7) - OP(16,3)*SPP(10)) + SPP(4)*(OP(5,14) + OP(1,14)*SF(6) + OP(2,14)*SF(4) - OP(4,14)*SF(5) + OP(3,14)*SPP(1) + OP(14,14)*SPP(4) + OP(15,14)*SPP(7) - OP(16,14)*SPP(10)) + SPP(7)*(OP(5,15) + OP(1,15)*SF(6) + OP(2,15)*SF(4) - OP(4,15)*SF(5) + OP(3,15)*SPP(1) + OP(14,15)*SPP(4) + OP(15,15)*SPP(7) - OP(16,15)*SPP(10)) - SPP(10)*(OP(5,16) + OP(1,16)*SF(6) + OP(2,16)*SF(4) - OP(4,16)*SF(5) + OP(3,16)*SPP(1) + OP(14,16)*SPP(4) + OP(15,16)*SPP(7) - OP(16,16)*SPP(10)) + dvxVar*(SG(2) + SG(3) - SG(4) - SG(5))^2;
nextP(1,6) = OP(1,6) + OP(2,6)*SF(10) + OP(3,6)*SF(12) + OP(4,6)*SF(11) + OP(11,6)*SF(15) + OP(12,6)*SF(16) + OP(13,6)*SPP(11) + SF(5)*(OP(1,1) + OP(2,1)*SF(10) + OP(3,1)*SF(12) + OP(4,1)*SF(11) + OP(11,1)*SF(15) + OP(12,1)*SF(16) + OP(13,1)*SPP(11)) + SF(4)*(OP(1,3) + OP(2,3)*SF(10) + OP(3,3)*SF(12) + OP(4,3)*SF(11) + OP(11,3)*SF(15) + OP(12,3)*SF(16) + OP(13,3)*SPP(11)) + SF(6)*(OP(1,4) + OP(2,4)*SF(10) + OP(3,4)*SF(12) + OP(4,4)*SF(11) + OP(11,4)*SF(15) + OP(12,4)*SF(16) + OP(13,4)*SPP(11)) - SPP(1)*(OP(1,2) + OP(2,2)*SF(10) + OP(3,2)*SF(12) + OP(4,2)*SF(11) + OP(11,2)*SF(15) + OP(12,2)*SF(16) + OP(13,2)*SPP(11)) - SPP(9)*(OP(1,14) + OP(2,14)*SF(10) + OP(3,14)*SF(12) + OP(4,14)*SF(11) + OP(11,14)*SF(15) + OP(12,14)*SF(16) + OP(13,14)*SPP(11)) + SPP(3)*(OP(1,15) + OP(2,15)*SF(10) + OP(3,15)*SF(12) + OP(4,15)*SF(11) + OP(11,15)*SF(15) + OP(12,15)*SF(16) + OP(13,15)*SPP(11)) + SPP(6)*(OP(1,16) + OP(2,16)*SF(10) + OP(3,16)*SF(12) + OP(4,16)*SF(11) + OP(11,16)*SF(15) + OP(12,16)*SF(16) + OP(13,16)*SPP(11));
nextP(2,6) = OP(2,6) + OP(1,6)*SF(9) + OP(3,6)*SF(8) + OP(4,6)*SF(12) - OP(13,6)*SF(16) + OP(12,6)*SPP(11) - (OP(11,6)*q0)/2 + SF(5)*(OP(2,1) + OP(1,1)*SF(9) + OP(3,1)*SF(8) + OP(4,1)*SF(12) - OP(13,1)*SF(16) + OP(12,1)*SPP(11) - (OP(11,1)*q0)/2) + SF(4)*(OP(2,3) + OP(1,3)*SF(9) + OP(3,3)*SF(8) + OP(4,3)*SF(12) - OP(13,3)*SF(16) + OP(12,3)*SPP(11) - (OP(11,3)*q0)/2) + SF(6)*(OP(2,4) + OP(1,4)*SF(9) + OP(3,4)*SF(8) + OP(4,4)*SF(12) - OP(13,4)*SF(16) + OP(12,4)*SPP(11) - (OP(11,4)*q0)/2) - SPP(1)*(OP(2,2) + OP(1,2)*SF(9) + OP(3,2)*SF(8) + OP(4,2)*SF(12) - OP(13,2)*SF(16) + OP(12,2)*SPP(11) - (OP(11,2)*q0)/2) - SPP(9)*(OP(2,14) + OP(1,14)*SF(9) + OP(3,14)*SF(8) + OP(4,14)*SF(12) - OP(13,14)*SF(16) + OP(12,14)*SPP(11) - (OP(11,14)*q0)/2) + SPP(3)*(OP(2,15) + OP(1,15)*SF(9) + OP(3,15)*SF(8) + OP(4,15)*SF(12) - OP(13,15)*SF(16) + OP(12,15)*SPP(11) - (OP(11,15)*q0)/2) + SPP(6)*(OP(2,16) + OP(1,16)*SF(9) + OP(3,16)*SF(8) + OP(4,16)*SF(12) - OP(13,16)*SF(16) + OP(12,16)*SPP(11) - (OP(11,16)*q0)/2);
nextP(3,6) = OP(3,6) + OP(1,6)*SF(7) + OP(2,6)*SF(11) + OP(4,6)*SF(9) + OP(13,6)*SF(15) - OP(11,6)*SPP(11) - (OP(12,6)*q0)/2 + SF(5)*(OP(3,1) + OP(1,1)*SF(7) + OP(2,1)*SF(11) + OP(4,1)*SF(9) + OP(13,1)*SF(15) - OP(11,1)*SPP(11) - (OP(12,1)*q0)/2) + SF(4)*(OP(3,3) + OP(1,3)*SF(7) + OP(2,3)*SF(11) + OP(4,3)*SF(9) + OP(13,3)*SF(15) - OP(11,3)*SPP(11) - (OP(12,3)*q0)/2) + SF(6)*(OP(3,4) + OP(1,4)*SF(7) + OP(2,4)*SF(11) + OP(4,4)*SF(9) + OP(13,4)*SF(15) - OP(11,4)*SPP(11) - (OP(12,4)*q0)/2) - SPP(1)*(OP(3,2) + OP(1,2)*SF(7) + OP(2,2)*SF(11) + OP(4,2)*SF(9) + OP(13,2)*SF(15) - OP(11,2)*SPP(11) - (OP(12,2)*q0)/2) - SPP(9)*(OP(3,14) + OP(1,14)*SF(7) + OP(2,14)*SF(11) + OP(4,14)*SF(9) + OP(13,14)*SF(15) - OP(11,14)*SPP(11) - (OP(12,14)*q0)/2) + SPP(3)*(OP(3,15) + OP(1,15)*SF(7) + OP(2,15)*SF(11) + OP(4,15)*SF(9) + OP(13,15)*SF(15) - OP(11,15)*SPP(11) - (OP(12,15)*q0)/2) + SPP(6)*(OP(3,16) + OP(1,16)*SF(7) + OP(2,16)*SF(11) + OP(4,16)*SF(9) + OP(13,16)*SF(15) - OP(11,16)*SPP(11) - (OP(12,16)*q0)/2);
nextP(4,6) = OP(4,6) + OP(1,6)*SF(8) + OP(2,6)*SF(7) + OP(3,6)*SF(10) + OP(11,6)*SF(16) - OP(12,6)*SF(15) - (OP(13,6)*q0)/2 + SF(5)*(OP(4,1) + OP(1,1)*SF(8) + OP(2,1)*SF(7) + OP(3,1)*SF(10) + OP(11,1)*SF(16) - OP(12,1)*SF(15) - (OP(13,1)*q0)/2) + SF(4)*(OP(4,3) + OP(1,3)*SF(8) + OP(2,3)*SF(7) + OP(3,3)*SF(10) + OP(11,3)*SF(16) - OP(12,3)*SF(15) - (OP(13,3)*q0)/2) + SF(6)*(OP(4,4) + OP(1,4)*SF(8) + OP(2,4)*SF(7) + OP(3,4)*SF(10) + OP(11,4)*SF(16) - OP(12,4)*SF(15) - (OP(13,4)*q0)/2) - SPP(1)*(OP(4,2) + OP(1,2)*SF(8) + OP(2,2)*SF(7) + OP(3,2)*SF(10) + OP(11,2)*SF(16) - OP(12,2)*SF(15) - (OP(13,2)*q0)/2) - SPP(9)*(OP(4,14) + OP(1,14)*SF(8) + OP(2,14)*SF(7) + OP(3,14)*SF(10) + OP(11,14)*SF(16) - OP(12,14)*SF(15) - (OP(13,14)*q0)/2) + SPP(3)*(OP(4,15) + OP(1,15)*SF(8) + OP(2,15)*SF(7) + OP(3,15)*SF(10) + OP(11,15)*SF(16) - OP(12,15)*SF(15) - (OP(13,15)*q0)/2) + SPP(6)*(OP(4,16) + OP(1,16)*SF(8) + OP(2,16)*SF(7) + OP(3,16)*SF(10) + OP(11,16)*SF(16) - OP(12,16)*SF(15) - (OP(13,16)*q0)/2);
nextP(5,6) = OP(5,6) + SQ(3) + OP(1,6)*SF(6) + OP(2,6)*SF(4) - OP(4,6)*SF(5) + OP(3,6)*SPP(1) + OP(14,6)*SPP(4) + OP(15,6)*SPP(7) - OP(16,6)*SPP(10) + SF(5)*(OP(5,1) + OP(1,1)*SF(6) + OP(2,1)*SF(4) - OP(4,1)*SF(5) + OP(3,1)*SPP(1) + OP(14,1)*SPP(4) + OP(15,1)*SPP(7) - OP(16,1)*SPP(10)) + SF(4)*(OP(5,3) + OP(1,3)*SF(6) + OP(2,3)*SF(4) - OP(4,3)*SF(5) + OP(3,3)*SPP(1) + OP(14,3)*SPP(4) + OP(15,3)*SPP(7) - OP(16,3)*SPP(10)) + SF(6)*(OP(5,4) + OP(1,4)*SF(6) + OP(2,4)*SF(4) - OP(4,4)*SF(5) + OP(3,4)*SPP(1) + OP(14,4)*SPP(4) + OP(15,4)*SPP(7) - OP(16,4)*SPP(10)) - SPP(1)*(OP(5,2) + OP(1,2)*SF(6) + OP(2,2)*SF(4) - OP(4,2)*SF(5) + OP(3,2)*SPP(1) + OP(14,2)*SPP(4) + OP(15,2)*SPP(7) - OP(16,2)*SPP(10)) - SPP(9)*(OP(5,14) + OP(1,14)*SF(6) + OP(2,14)*SF(4) - OP(4,14)*SF(5) + OP(3,14)*SPP(1) + OP(14,14)*SPP(4) + OP(15,14)*SPP(7) - OP(16,14)*SPP(10)) + SPP(3)*(OP(5,15) + OP(1,15)*SF(6) + OP(2,15)*SF(4) - OP(4,15)*SF(5) + OP(3,15)*SPP(1) + OP(14,15)*SPP(4) + OP(15,15)*SPP(7) - OP(16,15)*SPP(10)) + SPP(6)*(OP(5,16) + OP(1,16)*SF(6) + OP(2,16)*SF(4) - OP(4,16)*SF(5) + OP(3,16)*SPP(1) + OP(14,16)*SPP(4) + OP(15,16)*SPP(7) - OP(16,16)*SPP(10));
nextP(6,6) = OP(6,6) + OP(1,6)*SF(5) + OP(3,6)*SF(4) + OP(4,6)*SF(6) - OP(2,6)*SPP(1) - OP(14,6)*SPP(9) + OP(15,6)*SPP(3) + OP(16,6)*SPP(6) + dvxVar*(SG(8) + 2*q0*q3)^2 + dvzVar*(SG(6) - 2*q0*q1)^2 + SF(5)*(OP(6,1) + OP(1,1)*SF(5) + OP(3,1)*SF(4) + OP(4,1)*SF(6) - OP(2,1)*SPP(1) - OP(14,1)*SPP(9) + OP(15,1)*SPP(3) + OP(16,1)*SPP(6)) + SF(4)*(OP(6,3) + OP(1,3)*SF(5) + OP(3,3)*SF(4) + OP(4,3)*SF(6) - OP(2,3)*SPP(1) - OP(14,3)*SPP(9) + OP(15,3)*SPP(3) + OP(16,3)*SPP(6)) + SF(6)*(OP(6,4) + OP(1,4)*SF(5) + OP(3,4)*SF(4) + OP(4,4)*SF(6) - OP(2,4)*SPP(1) - OP(14,4)*SPP(9) + OP(15,4)*SPP(3) + OP(16,4)*SPP(6)) - SPP(1)*(OP(6,2) + OP(1,2)*SF(5) + OP(3,2)*SF(4) + OP(4,2)*SF(6) - OP(2,2)*SPP(1) - OP(14,2)*SPP(9) + OP(15,2)*SPP(3) + OP(16,2)*SPP(6)) - SPP(9)*(OP(6,14) + OP(1,14)*SF(5) + OP(3,14)*SF(4) + OP(4,14)*SF(6) - OP(2,14)*SPP(1) - OP(14,14)*SPP(9) + OP(15,14)*SPP(3) + OP(16,14)*SPP(6)) + SPP(3)*(OP(6,15) + OP(1,15)*SF(5) + OP(3,15)*SF(4) + OP(4,15)*SF(6) - OP(2,15)*SPP(1) - OP(14,15)*SPP(9) + OP(15,15)*SPP(3) + OP(16,15)*SPP(6)) + SPP(6)*(OP(6,16) + OP(1,16)*SF(5) + OP(3,16)*SF(4) + OP(4,16)*SF(6) - OP(2,16)*SPP(1) - OP(14,16)*SPP(9) + OP(15,16)*SPP(3) + OP(16,16)*SPP(6)) + dvyVar*(SG(2) - SG(3) + SG(4) - SG(5))^2;
nextP(1,7) = OP(1,7) + OP(2,7)*SF(10) + OP(3,7)*SF(12) + OP(4,7)*SF(11) + OP(11,7)*SF(15) + OP(12,7)*SF(16) + OP(13,7)*SPP(11) + SF(5)*(OP(1,2) + OP(2,2)*SF(10) + OP(3,2)*SF(12) + OP(4,2)*SF(11) + OP(11,2)*SF(15) + OP(12,2)*SF(16) + OP(13,2)*SPP(11)) - SF(6)*(OP(1,3) + OP(2,3)*SF(10) + OP(3,3)*SF(12) + OP(4,3)*SF(11) + OP(11,3)*SF(15) + OP(12,3)*SF(16) + OP(13,3)*SPP(11)) + SF(4)*(OP(1,4) + OP(2,4)*SF(10) + OP(3,4)*SF(12) + OP(4,4)*SF(11) + OP(11,4)*SF(15) + OP(12,4)*SF(16) + OP(13,4)*SPP(11)) + SPP(1)*(OP(1,1) + OP(2,1)*SF(10) + OP(3,1)*SF(12) + OP(4,1)*SF(11) + OP(11,1)*SF(15) + OP(12,1)*SF(16) + OP(13,1)*SPP(11)) + SPP(5)*(OP(1,14) + OP(2,14)*SF(10) + OP(3,14)*SF(12) + OP(4,14)*SF(11) + OP(11,14)*SF(15) + OP(12,14)*SF(16) + OP(13,14)*SPP(11)) - SPP(8)*(OP(1,15) + OP(2,15)*SF(10) + OP(3,15)*SF(12) + OP(4,15)*SF(11) + OP(11,15)*SF(15) + OP(12,15)*SF(16) + OP(13,15)*SPP(11)) - SPP(2)*(OP(1,16) + OP(2,16)*SF(10) + OP(3,16)*SF(12) + OP(4,16)*SF(11) + OP(11,16)*SF(15) + OP(12,16)*SF(16) + OP(13,16)*SPP(11));
nextP(2,7) = OP(2,7) + OP(1,7)*SF(9) + OP(3,7)*SF(8) + OP(4,7)*SF(12) - OP(13,7)*SF(16) + OP(12,7)*SPP(11) - (OP(11,7)*q0)/2 + SF(5)*(OP(2,2) + OP(1,2)*SF(9) + OP(3,2)*SF(8) + OP(4,2)*SF(12) - OP(13,2)*SF(16) + OP(12,2)*SPP(11) - (OP(11,2)*q0)/2) - SF(6)*(OP(2,3) + OP(1,3)*SF(9) + OP(3,3)*SF(8) + OP(4,3)*SF(12) - OP(13,3)*SF(16) + OP(12,3)*SPP(11) - (OP(11,3)*q0)/2) + SF(4)*(OP(2,4) + OP(1,4)*SF(9) + OP(3,4)*SF(8) + OP(4,4)*SF(12) - OP(13,4)*SF(16) + OP(12,4)*SPP(11) - (OP(11,4)*q0)/2) + SPP(1)*(OP(2,1) + OP(1,1)*SF(9) + OP(3,1)*SF(8) + OP(4,1)*SF(12) - OP(13,1)*SF(16) + OP(12,1)*SPP(11) - (OP(11,1)*q0)/2) + SPP(5)*(OP(2,14) + OP(1,14)*SF(9) + OP(3,14)*SF(8) + OP(4,14)*SF(12) - OP(13,14)*SF(16) + OP(12,14)*SPP(11) - (OP(11,14)*q0)/2) - SPP(8)*(OP(2,15) + OP(1,15)*SF(9) + OP(3,15)*SF(8) + OP(4,15)*SF(12) - OP(13,15)*SF(16) + OP(12,15)*SPP(11) - (OP(11,15)*q0)/2) - SPP(2)*(OP(2,16) + OP(1,16)*SF(9) + OP(3,16)*SF(8) + OP(4,16)*SF(12) - OP(13,16)*SF(16) + OP(12,16)*SPP(11) - (OP(11,16)*q0)/2);
nextP(3,7) = OP(3,7) + OP(1,7)*SF(7) + OP(2,7)*SF(11) + OP(4,7)*SF(9) + OP(13,7)*SF(15) - OP(11,7)*SPP(11) - (OP(12,7)*q0)/2 + SF(5)*(OP(3,2) + OP(1,2)*SF(7) + OP(2,2)*SF(11) + OP(4,2)*SF(9) + OP(13,2)*SF(15) - OP(11,2)*SPP(11) - (OP(12,2)*q0)/2) - SF(6)*(OP(3,3) + OP(1,3)*SF(7) + OP(2,3)*SF(11) + OP(4,3)*SF(9) + OP(13,3)*SF(15) - OP(11,3)*SPP(11) - (OP(12,3)*q0)/2) + SF(4)*(OP(3,4) + OP(1,4)*SF(7) + OP(2,4)*SF(11) + OP(4,4)*SF(9) + OP(13,4)*SF(15) - OP(11,4)*SPP(11) - (OP(12,4)*q0)/2) + SPP(1)*(OP(3,1) + OP(1,1)*SF(7) + OP(2,1)*SF(11) + OP(4,1)*SF(9) + OP(13,1)*SF(15) - OP(11,1)*SPP(11) - (OP(12,1)*q0)/2) + SPP(5)*(OP(3,14) + OP(1,14)*SF(7) + OP(2,14)*SF(11) + OP(4,14)*SF(9) + OP(13,14)*SF(15) - OP(11,14)*SPP(11) - (OP(12,14)*q0)/2) - SPP(8)*(OP(3,15) + OP(1,15)*SF(7) + OP(2,15)*SF(11) + OP(4,15)*SF(9) + OP(13,15)*SF(15) - OP(11,15)*SPP(11) - (OP(12,15)*q0)/2) - SPP(2)*(OP(3,16) + OP(1,16)*SF(7) + OP(2,16)*SF(11) + OP(4,16)*SF(9) + OP(13,16)*SF(15) - OP(11,16)*SPP(11) - (OP(12,16)*q0)/2);
nextP(4,7) = OP(4,7) + OP(1,7)*SF(8) + OP(2,7)*SF(7) + OP(3,7)*SF(10) + OP(11,7)*SF(16) - OP(12,7)*SF(15) - (OP(13,7)*q0)/2 + SF(5)*(OP(4,2) + OP(1,2)*SF(8) + OP(2,2)*SF(7) + OP(3,2)*SF(10) + OP(11,2)*SF(16) - OP(12,2)*SF(15) - (OP(13,2)*q0)/2) - SF(6)*(OP(4,3) + OP(1,3)*SF(8) + OP(2,3)*SF(7) + OP(3,3)*SF(10) + OP(11,3)*SF(16) - OP(12,3)*SF(15) - (OP(13,3)*q0)/2) + SF(4)*(OP(4,4) + OP(1,4)*SF(8) + OP(2,4)*SF(7) + OP(3,4)*SF(10) + OP(11,4)*SF(16) - OP(12,4)*SF(15) - (OP(13,4)*q0)/2) + SPP(1)*(OP(4,1) + OP(1,1)*SF(8) + OP(2,1)*SF(7) + OP(3,1)*SF(10) + OP(11,1)*SF(16) - OP(12,1)*SF(15) - (OP(13,1)*q0)/2) + SPP(5)*(OP(4,14) + OP(1,14)*SF(8) + OP(2,14)*SF(7) + OP(3,14)*SF(10) + OP(11,14)*SF(16) - OP(12,14)*SF(15) - (OP(13,14)*q0)/2) - SPP(8)*(OP(4,15) + OP(1,15)*SF(8) + OP(2,15)*SF(7) + OP(3,15)*SF(10) + OP(11,15)*SF(16) - OP(12,15)*SF(15) - (OP(13,15)*q0)/2) - SPP(2)*(OP(4,16) + OP(1,16)*SF(8) + OP(2,16)*SF(7) + OP(3,16)*SF(10) + OP(11,16)*SF(16) - OP(12,16)*SF(15) - (OP(13,16)*q0)/2);
nextP(5,7) = OP(5,7) + SQ(2) + OP(1,7)*SF(6) + OP(2,7)*SF(4) - OP(4,7)*SF(5) + OP(3,7)*SPP(1) + OP(14,7)*SPP(4) + OP(15,7)*SPP(7) - OP(16,7)*SPP(10) + SF(5)*(OP(5,2) + OP(1,2)*SF(6) + OP(2,2)*SF(4) - OP(4,2)*SF(5) + OP(3,2)*SPP(1) + OP(14,2)*SPP(4) + OP(15,2)*SPP(7) - OP(16,2)*SPP(10)) - SF(6)*(OP(5,3) + OP(1,3)*SF(6) + OP(2,3)*SF(4) - OP(4,3)*SF(5) + OP(3,3)*SPP(1) + OP(14,3)*SPP(4) + OP(15,3)*SPP(7) - OP(16,3)*SPP(10)) + SF(4)*(OP(5,4) + OP(1,4)*SF(6) + OP(2,4)*SF(4) - OP(4,4)*SF(5) + OP(3,4)*SPP(1) + OP(14,4)*SPP(4) + OP(15,4)*SPP(7) - OP(16,4)*SPP(10)) + SPP(1)*(OP(5,1) + OP(1,1)*SF(6) + OP(2,1)*SF(4) - OP(4,1)*SF(5) + OP(3,1)*SPP(1) + OP(14,1)*SPP(4) + OP(15,1)*SPP(7) - OP(16,1)*SPP(10)) + SPP(5)*(OP(5,14) + OP(1,14)*SF(6) + OP(2,14)*SF(4) - OP(4,14)*SF(5) + OP(3,14)*SPP(1) + OP(14,14)*SPP(4) + OP(15,14)*SPP(7) - OP(16,14)*SPP(10)) - SPP(8)*(OP(5,15) + OP(1,15)*SF(6) + OP(2,15)*SF(4) - OP(4,15)*SF(5) + OP(3,15)*SPP(1) + OP(14,15)*SPP(4) + OP(15,15)*SPP(7) - OP(16,15)*SPP(10)) - SPP(2)*(OP(5,16) + OP(1,16)*SF(6) + OP(2,16)*SF(4) - OP(4,16)*SF(5) + OP(3,16)*SPP(1) + OP(14,16)*SPP(4) + OP(15,16)*SPP(7) - OP(16,16)*SPP(10));
nextP(6,7) = OP(6,7) + SQ(1) + OP(1,7)*SF(5) + OP(3,7)*SF(4) + OP(4,7)*SF(6) - OP(2,7)*SPP(1) - OP(14,7)*SPP(9) + OP(15,7)*SPP(3) + OP(16,7)*SPP(6) + SF(5)*(OP(6,2) + OP(1,2)*SF(5) + OP(3,2)*SF(4) + OP(4,2)*SF(6) - OP(2,2)*SPP(1) - OP(14,2)*SPP(9) + OP(15,2)*SPP(3) + OP(16,2)*SPP(6)) - SF(6)*(OP(6,3) + OP(1,3)*SF(5) + OP(3,3)*SF(4) + OP(4,3)*SF(6) - OP(2,3)*SPP(1) - OP(14,3)*SPP(9) + OP(15,3)*SPP(3) + OP(16,3)*SPP(6)) + SF(4)*(OP(6,4) + OP(1,4)*SF(5) + OP(3,4)*SF(4) + OP(4,4)*SF(6) - OP(2,4)*SPP(1) - OP(14,4)*SPP(9) + OP(15,4)*SPP(3) + OP(16,4)*SPP(6)) + SPP(1)*(OP(6,1) + OP(1,1)*SF(5) + OP(3,1)*SF(4) + OP(4,1)*SF(6) - OP(2,1)*SPP(1) - OP(14,1)*SPP(9) + OP(15,1)*SPP(3) + OP(16,1)*SPP(6)) + SPP(5)*(OP(6,14) + OP(1,14)*SF(5) + OP(3,14)*SF(4) + OP(4,14)*SF(6) - OP(2,14)*SPP(1) - OP(14,14)*SPP(9) + OP(15,14)*SPP(3) + OP(16,14)*SPP(6)) - SPP(8)*(OP(6,15) + OP(1,15)*SF(5) + OP(3,15)*SF(4) + OP(4,15)*SF(6) - OP(2,15)*SPP(1) - OP(14,15)*SPP(9) + OP(15,15)*SPP(3) + OP(16,15)*SPP(6)) - SPP(2)*(OP(6,16) + OP(1,16)*SF(5) + OP(3,16)*SF(4) + OP(4,16)*SF(6) - OP(2,16)*SPP(1) - OP(14,16)*SPP(9) + OP(15,16)*SPP(3) + OP(16,16)*SPP(6));
nextP(7,7) = OP(7,7) + OP(2,7)*SF(5) - OP(3,7)*SF(6) + OP(4,7)*SF(4) + OP(1,7)*SPP(1) + OP(14,7)*SPP(5) - OP(15,7)*SPP(8) - OP(16,7)*SPP(2) + dvxVar*(SG(7) - 2*q0*q2)^2 + dvyVar*(SG(6) + 2*q0*q1)^2 + SF(5)*(OP(7,2) + OP(2,2)*SF(5) - OP(3,2)*SF(6) + OP(4,2)*SF(4) + OP(1,2)*SPP(1) + OP(14,2)*SPP(5) - OP(15,2)*SPP(8) - OP(16,2)*SPP(2)) - SF(6)*(OP(7,3) + OP(2,3)*SF(5) - OP(3,3)*SF(6) + OP(4,3)*SF(4) + OP(1,3)*SPP(1) + OP(14,3)*SPP(5) - OP(15,3)*SPP(8) - OP(16,3)*SPP(2)) + SF(4)*(OP(7,4) + OP(2,4)*SF(5) - OP(3,4)*SF(6) + OP(4,4)*SF(4) + OP(1,4)*SPP(1) + OP(14,4)*SPP(5) - OP(15,4)*SPP(8) - OP(16,4)*SPP(2)) + SPP(1)*(OP(7,1) + OP(2,1)*SF(5) - OP(3,1)*SF(6) + OP(4,1)*SF(4) + OP(1,1)*SPP(1) + OP(14,1)*SPP(5) - OP(15,1)*SPP(8) - OP(16,1)*SPP(2)) + SPP(5)*(OP(7,14) + OP(2,14)*SF(5) - OP(3,14)*SF(6) + OP(4,14)*SF(4) + OP(1,14)*SPP(1) + OP(14,14)*SPP(5) - OP(15,14)*SPP(8) - OP(16,14)*SPP(2)) - SPP(8)*(OP(7,15) + OP(2,15)*SF(5) - OP(3,15)*SF(6) + OP(4,15)*SF(4) + OP(1,15)*SPP(1) + OP(14,15)*SPP(5) - OP(15,15)*SPP(8) - OP(16,15)*SPP(2)) - SPP(2)*(OP(7,16) + OP(2,16)*SF(5) - OP(3,16)*SF(6) + OP(4,16)*SF(4) + OP(1,16)*SPP(1) + OP(14,16)*SPP(5) - OP(15,16)*SPP(8) - OP(16,16)*SPP(2)) + dvzVar*(SG(2) - SG(3) - SG(4) + SG(5))^2;
nextP(1,8) = OP(1,8) + OP(2,8)*SF(10) + OP(3,8)*SF(12) + OP(4,8)*SF(11) + OP(11,8)*SF(15) + OP(12,8)*SF(16) + OP(13,8)*SPP(11) + dt*(OP(1,5) + OP(2,5)*SF(10) + OP(3,5)*SF(12) + OP(4,5)*SF(11) + OP(11,5)*SF(15) + OP(12,5)*SF(16) + OP(13,5)*SPP(11));
nextP(2,8) = OP(2,8) + OP(1,8)*SF(9) + OP(3,8)*SF(8) + OP(4,8)*SF(12) - OP(13,8)*SF(16) + OP(12,8)*SPP(11) - (OP(11,8)*q0)/2 + dt*(OP(2,5) + OP(1,5)*SF(9) + OP(3,5)*SF(8) + OP(4,5)*SF(12) - OP(13,5)*SF(16) + OP(12,5)*SPP(11) - (OP(11,5)*q0)/2);
nextP(3,8) = OP(3,8) + OP(1,8)*SF(7) + OP(2,8)*SF(11) + OP(4,8)*SF(9) + OP(13,8)*SF(15) - OP(11,8)*SPP(11) - (OP(12,8)*q0)/2 + dt*(OP(3,5) + OP(1,5)*SF(7) + OP(2,5)*SF(11) + OP(4,5)*SF(9) + OP(13,5)*SF(15) - OP(11,5)*SPP(11) - (OP(12,5)*q0)/2);
nextP(4,8) = OP(4,8) + OP(1,8)*SF(8) + OP(2,8)*SF(7) + OP(3,8)*SF(10) + OP(11,8)*SF(16) - OP(12,8)*SF(15) - (OP(13,8)*q0)/2 + dt*(OP(4,5) + OP(1,5)*SF(8) + OP(2,5)*SF(7) + OP(3,5)*SF(10) + OP(11,5)*SF(16) - OP(12,5)*SF(15) - (OP(13,5)*q0)/2);
nextP(5,8) = OP(5,8) + OP(1,8)*SF(6) + OP(2,8)*SF(4) - OP(4,8)*SF(5) + OP(3,8)*SPP(1) + OP(14,8)*SPP(4) + OP(15,8)*SPP(7) - OP(16,8)*SPP(10) + dt*(OP(5,5) + OP(1,5)*SF(6) + OP(2,5)*SF(4) - OP(4,5)*SF(5) + OP(3,5)*SPP(1) + OP(14,5)*SPP(4) + OP(15,5)*SPP(7) - OP(16,5)*SPP(10));
nextP(6,8) = OP(6,8) + OP(1,8)*SF(5) + OP(3,8)*SF(4) + OP(4,8)*SF(6) - OP(2,8)*SPP(1) - OP(14,8)*SPP(9) + OP(15,8)*SPP(3) + OP(16,8)*SPP(6) + dt*(OP(6,5) + OP(1,5)*SF(5) + OP(3,5)*SF(4) + OP(4,5)*SF(6) - OP(2,5)*SPP(1) - OP(14,5)*SPP(9) + OP(15,5)*SPP(3) + OP(16,5)*SPP(6));
nextP(7,8) = OP(7,8) + OP(2,8)*SF(5) - OP(3,8)*SF(6) + OP(4,8)*SF(4) + OP(1,8)*SPP(1) + OP(14,8)*SPP(5) - OP(15,8)*SPP(8) - OP(16,8)*SPP(2) + dt*(OP(7,5) + OP(2,5)*SF(5) - OP(3,5)*SF(6) + OP(4,5)*SF(4) + OP(1,5)*SPP(1) + OP(14,5)*SPP(5) - OP(15,5)*SPP(8) - OP(16,5)*SPP(2));
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
nextP(8,8) = OP(8,8) + OP(5,8)*dt + dt*(OP(8,5) + OP(5,5)*dt);
EKF: update derivation files
2016-05-01 02:06:17 -03:00
nextP(1,9) = OP(1,9) + OP(2,9)*SF(10) + OP(3,9)*SF(12) + OP(4,9)*SF(11) + OP(11,9)*SF(15) + OP(12,9)*SF(16) + OP(13,9)*SPP(11) + dt*(OP(1,6) + OP(2,6)*SF(10) + OP(3,6)*SF(12) + OP(4,6)*SF(11) + OP(11,6)*SF(15) + OP(12,6)*SF(16) + OP(13,6)*SPP(11));
nextP(2,9) = OP(2,9) + OP(1,9)*SF(9) + OP(3,9)*SF(8) + OP(4,9)*SF(12) - OP(13,9)*SF(16) + OP(12,9)*SPP(11) - (OP(11,9)*q0)/2 + dt*(OP(2,6) + OP(1,6)*SF(9) + OP(3,6)*SF(8) + OP(4,6)*SF(12) - OP(13,6)*SF(16) + OP(12,6)*SPP(11) - (OP(11,6)*q0)/2);
nextP(3,9) = OP(3,9) + OP(1,9)*SF(7) + OP(2,9)*SF(11) + OP(4,9)*SF(9) + OP(13,9)*SF(15) - OP(11,9)*SPP(11) - (OP(12,9)*q0)/2 + dt*(OP(3,6) + OP(1,6)*SF(7) + OP(2,6)*SF(11) + OP(4,6)*SF(9) + OP(13,6)*SF(15) - OP(11,6)*SPP(11) - (OP(12,6)*q0)/2);
nextP(4,9) = OP(4,9) + OP(1,9)*SF(8) + OP(2,9)*SF(7) + OP(3,9)*SF(10) + OP(11,9)*SF(16) - OP(12,9)*SF(15) - (OP(13,9)*q0)/2 + dt*(OP(4,6) + OP(1,6)*SF(8) + OP(2,6)*SF(7) + OP(3,6)*SF(10) + OP(11,6)*SF(16) - OP(12,6)*SF(15) - (OP(13,6)*q0)/2);
nextP(5,9) = OP(5,9) + OP(1,9)*SF(6) + OP(2,9)*SF(4) - OP(4,9)*SF(5) + OP(3,9)*SPP(1) + OP(14,9)*SPP(4) + OP(15,9)*SPP(7) - OP(16,9)*SPP(10) + dt*(OP(5,6) + OP(1,6)*SF(6) + OP(2,6)*SF(4) - OP(4,6)*SF(5) + OP(3,6)*SPP(1) + OP(14,6)*SPP(4) + OP(15,6)*SPP(7) - OP(16,6)*SPP(10));
nextP(6,9) = OP(6,9) + OP(1,9)*SF(5) + OP(3,9)*SF(4) + OP(4,9)*SF(6) - OP(2,9)*SPP(1) - OP(14,9)*SPP(9) + OP(15,9)*SPP(3) + OP(16,9)*SPP(6) + dt*(OP(6,6) + OP(1,6)*SF(5) + OP(3,6)*SF(4) + OP(4,6)*SF(6) - OP(2,6)*SPP(1) - OP(14,6)*SPP(9) + OP(15,6)*SPP(3) + OP(16,6)*SPP(6));
nextP(7,9) = OP(7,9) + OP(2,9)*SF(5) - OP(3,9)*SF(6) + OP(4,9)*SF(4) + OP(1,9)*SPP(1) + OP(14,9)*SPP(5) - OP(15,9)*SPP(8) - OP(16,9)*SPP(2) + dt*(OP(7,6) + OP(2,6)*SF(5) - OP(3,6)*SF(6) + OP(4,6)*SF(4) + OP(1,6)*SPP(1) + OP(14,6)*SPP(5) - OP(15,6)*SPP(8) - OP(16,6)*SPP(2));
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
nextP(8,9) = OP(8,9) + OP(5,9)*dt + dt*(OP(8,6) + OP(5,6)*dt);
nextP(9,9) = OP(9,9) + OP(6,9)*dt + dt*(OP(9,6) + OP(6,6)*dt);
EKF: update derivation files
2016-05-01 02:06:17 -03:00
nextP(1,10) = OP(1,10) + OP(2,10)*SF(10) + OP(3,10)*SF(12) + OP(4,10)*SF(11) + OP(11,10)*SF(15) + OP(12,10)*SF(16) + OP(13,10)*SPP(11) + dt*(OP(1,7) + OP(2,7)*SF(10) + OP(3,7)*SF(12) + OP(4,7)*SF(11) + OP(11,7)*SF(15) + OP(12,7)*SF(16) + OP(13,7)*SPP(11));
nextP(2,10) = OP(2,10) + OP(1,10)*SF(9) + OP(3,10)*SF(8) + OP(4,10)*SF(12) - OP(13,10)*SF(16) + OP(12,10)*SPP(11) - (OP(11,10)*q0)/2 + dt*(OP(2,7) + OP(1,7)*SF(9) + OP(3,7)*SF(8) + OP(4,7)*SF(12) - OP(13,7)*SF(16) + OP(12,7)*SPP(11) - (OP(11,7)*q0)/2);
nextP(3,10) = OP(3,10) + OP(1,10)*SF(7) + OP(2,10)*SF(11) + OP(4,10)*SF(9) + OP(13,10)*SF(15) - OP(11,10)*SPP(11) - (OP(12,10)*q0)/2 + dt*(OP(3,7) + OP(1,7)*SF(7) + OP(2,7)*SF(11) + OP(4,7)*SF(9) + OP(13,7)*SF(15) - OP(11,7)*SPP(11) - (OP(12,7)*q0)/2);
nextP(4,10) = OP(4,10) + OP(1,10)*SF(8) + OP(2,10)*SF(7) + OP(3,10)*SF(10) + OP(11,10)*SF(16) - OP(12,10)*SF(15) - (OP(13,10)*q0)/2 + dt*(OP(4,7) + OP(1,7)*SF(8) + OP(2,7)*SF(7) + OP(3,7)*SF(10) + OP(11,7)*SF(16) - OP(12,7)*SF(15) - (OP(13,7)*q0)/2);
nextP(5,10) = OP(5,10) + OP(1,10)*SF(6) + OP(2,10)*SF(4) - OP(4,10)*SF(5) + OP(3,10)*SPP(1) + OP(14,10)*SPP(4) + OP(15,10)*SPP(7) - OP(16,10)*SPP(10) + dt*(OP(5,7) + OP(1,7)*SF(6) + OP(2,7)*SF(4) - OP(4,7)*SF(5) + OP(3,7)*SPP(1) + OP(14,7)*SPP(4) + OP(15,7)*SPP(7) - OP(16,7)*SPP(10));
nextP(6,10) = OP(6,10) + OP(1,10)*SF(5) + OP(3,10)*SF(4) + OP(4,10)*SF(6) - OP(2,10)*SPP(1) - OP(14,10)*SPP(9) + OP(15,10)*SPP(3) + OP(16,10)*SPP(6) + dt*(OP(6,7) + OP(1,7)*SF(5) + OP(3,7)*SF(4) + OP(4,7)*SF(6) - OP(2,7)*SPP(1) - OP(14,7)*SPP(9) + OP(15,7)*SPP(3) + OP(16,7)*SPP(6));
nextP(7,10) = OP(7,10) + OP(2,10)*SF(5) - OP(3,10)*SF(6) + OP(4,10)*SF(4) + OP(1,10)*SPP(1) + OP(14,10)*SPP(5) - OP(15,10)*SPP(8) - OP(16,10)*SPP(2) + dt*(OP(7,7) + OP(2,7)*SF(5) - OP(3,7)*SF(6) + OP(4,7)*SF(4) + OP(1,7)*SPP(1) + OP(14,7)*SPP(5) - OP(15,7)*SPP(8) - OP(16,7)*SPP(2));
nextP(8,10) = OP(8,10) + OP(5,10)*dt + dt*(OP(8,7) + OP(5,7)*dt);
nextP(9,10) = OP(9,10) + OP(6,10)*dt + dt*(OP(9,7) + OP(6,7)*dt);
nextP(10,10) = OP(10,10) + OP(7,10)*dt + dt*(OP(10,7) + OP(7,7)*dt);
nextP(1,11) = OP(1,11) + OP(2,11)*SF(10) + OP(3,11)*SF(12) + OP(4,11)*SF(11) + OP(11,11)*SF(15) + OP(12,11)*SF(16) + OP(13,11)*SPP(11);
nextP(2,11) = OP(2,11) + OP(1,11)*SF(9) + OP(3,11)*SF(8) + OP(4,11)*SF(12) - OP(13,11)*SF(16) + OP(12,11)*SPP(11) - (OP(11,11)*q0)/2;
nextP(3,11) = OP(3,11) + OP(1,11)*SF(7) + OP(2,11)*SF(11) + OP(4,11)*SF(9) + OP(13,11)*SF(15) - OP(11,11)*SPP(11) - (OP(12,11)*q0)/2;
nextP(4,11) = OP(4,11) + OP(1,11)*SF(8) + OP(2,11)*SF(7) + OP(3,11)*SF(10) + OP(11,11)*SF(16) - OP(12,11)*SF(15) - (OP(13,11)*q0)/2;
nextP(5,11) = OP(5,11) + OP(1,11)*SF(6) + OP(2,11)*SF(4) - OP(4,11)*SF(5) + OP(3,11)*SPP(1) + OP(14,11)*SPP(4) + OP(15,11)*SPP(7) - OP(16,11)*SPP(10);
nextP(6,11) = OP(6,11) + OP(1,11)*SF(5) + OP(3,11)*SF(4) + OP(4,11)*SF(6) - OP(2,11)*SPP(1) - OP(14,11)*SPP(9) + OP(15,11)*SPP(3) + OP(16,11)*SPP(6);
nextP(7,11) = OP(7,11) + OP(2,11)*SF(5) - OP(3,11)*SF(6) + OP(4,11)*SF(4) + OP(1,11)*SPP(1) + OP(14,11)*SPP(5) - OP(15,11)*SPP(8) - OP(16,11)*SPP(2);
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
nextP(8,11) = OP(8,11) + OP(5,11)*dt;
nextP(9,11) = OP(9,11) + OP(6,11)*dt;
EKF: update derivation files
2016-05-01 02:06:17 -03:00
nextP(10,11) = OP(10,11) + OP(7,11)*dt;
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
nextP(11,11) = OP(11,11);
EKF: update derivation files
2016-05-01 02:06:17 -03:00
nextP(1,12) = OP(1,12) + OP(2,12)*SF(10) + OP(3,12)*SF(12) + OP(4,12)*SF(11) + OP(11,12)*SF(15) + OP(12,12)*SF(16) + OP(13,12)*SPP(11);
nextP(2,12) = OP(2,12) + OP(1,12)*SF(9) + OP(3,12)*SF(8) + OP(4,12)*SF(12) - OP(13,12)*SF(16) + OP(12,12)*SPP(11) - (OP(11,12)*q0)/2;
nextP(3,12) = OP(3,12) + OP(1,12)*SF(7) + OP(2,12)*SF(11) + OP(4,12)*SF(9) + OP(13,12)*SF(15) - OP(11,12)*SPP(11) - (OP(12,12)*q0)/2;
nextP(4,12) = OP(4,12) + OP(1,12)*SF(8) + OP(2,12)*SF(7) + OP(3,12)*SF(10) + OP(11,12)*SF(16) - OP(12,12)*SF(15) - (OP(13,12)*q0)/2;
nextP(5,12) = OP(5,12) + OP(1,12)*SF(6) + OP(2,12)*SF(4) - OP(4,12)*SF(5) + OP(3,12)*SPP(1) + OP(14,12)*SPP(4) + OP(15,12)*SPP(7) - OP(16,12)*SPP(10);
nextP(6,12) = OP(6,12) + OP(1,12)*SF(5) + OP(3,12)*SF(4) + OP(4,12)*SF(6) - OP(2,12)*SPP(1) - OP(14,12)*SPP(9) + OP(15,12)*SPP(3) + OP(16,12)*SPP(6);
nextP(7,12) = OP(7,12) + OP(2,12)*SF(5) - OP(3,12)*SF(6) + OP(4,12)*SF(4) + OP(1,12)*SPP(1) + OP(14,12)*SPP(5) - OP(15,12)*SPP(8) - OP(16,12)*SPP(2);
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
nextP(8,12) = OP(8,12) + OP(5,12)*dt;
nextP(9,12) = OP(9,12) + OP(6,12)*dt;
EKF: update derivation files
2016-05-01 02:06:17 -03:00
nextP(10,12) = OP(10,12) + OP(7,12)*dt;
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
nextP(11,12) = OP(11,12);
nextP(12,12) = OP(12,12);
EKF: update derivation files
2016-05-01 02:06:17 -03:00
nextP(1,13) = OP(1,13) + OP(2,13)*SF(10) + OP(3,13)*SF(12) + OP(4,13)*SF(11) + OP(11,13)*SF(15) + OP(12,13)*SF(16) + OP(13,13)*SPP(11);
nextP(2,13) = OP(2,13) + OP(1,13)*SF(9) + OP(3,13)*SF(8) + OP(4,13)*SF(12) - OP(13,13)*SF(16) + OP(12,13)*SPP(11) - (OP(11,13)*q0)/2;
nextP(3,13) = OP(3,13) + OP(1,13)*SF(7) + OP(2,13)*SF(11) + OP(4,13)*SF(9) + OP(13,13)*SF(15) - OP(11,13)*SPP(11) - (OP(12,13)*q0)/2;
nextP(4,13) = OP(4,13) + OP(1,13)*SF(8) + OP(2,13)*SF(7) + OP(3,13)*SF(10) + OP(11,13)*SF(16) - OP(12,13)*SF(15) - (OP(13,13)*q0)/2;
nextP(5,13) = OP(5,13) + OP(1,13)*SF(6) + OP(2,13)*SF(4) - OP(4,13)*SF(5) + OP(3,13)*SPP(1) + OP(14,13)*SPP(4) + OP(15,13)*SPP(7) - OP(16,13)*SPP(10);
nextP(6,13) = OP(6,13) + OP(1,13)*SF(5) + OP(3,13)*SF(4) + OP(4,13)*SF(6) - OP(2,13)*SPP(1) - OP(14,13)*SPP(9) + OP(15,13)*SPP(3) + OP(16,13)*SPP(6);
nextP(7,13) = OP(7,13) + OP(2,13)*SF(5) - OP(3,13)*SF(6) + OP(4,13)*SF(4) + OP(1,13)*SPP(1) + OP(14,13)*SPP(5) - OP(15,13)*SPP(8) - OP(16,13)*SPP(2);
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
nextP(8,13) = OP(8,13) + OP(5,13)*dt;
nextP(9,13) = OP(9,13) + OP(6,13)*dt;
EKF: update derivation files
2016-05-01 02:06:17 -03:00
nextP(10,13) = OP(10,13) + OP(7,13)*dt;
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
nextP(11,13) = OP(11,13);
nextP(12,13) = OP(12,13);
nextP(13,13) = OP(13,13);
EKF: update derivation files
2016-05-01 02:06:17 -03:00
nextP(1,14) = OP(1,14) + OP(2,14)*SF(10) + OP(3,14)*SF(12) + OP(4,14)*SF(11) + OP(11,14)*SF(15) + OP(12,14)*SF(16) + OP(13,14)*SPP(11);
nextP(2,14) = OP(2,14) + OP(1,14)*SF(9) + OP(3,14)*SF(8) + OP(4,14)*SF(12) - OP(13,14)*SF(16) + OP(12,14)*SPP(11) - (OP(11,14)*q0)/2;
nextP(3,14) = OP(3,14) + OP(1,14)*SF(7) + OP(2,14)*SF(11) + OP(4,14)*SF(9) + OP(13,14)*SF(15) - OP(11,14)*SPP(11) - (OP(12,14)*q0)/2;
nextP(4,14) = OP(4,14) + OP(1,14)*SF(8) + OP(2,14)*SF(7) + OP(3,14)*SF(10) + OP(11,14)*SF(16) - OP(12,14)*SF(15) - (OP(13,14)*q0)/2;
nextP(5,14) = OP(5,14) + OP(1,14)*SF(6) + OP(2,14)*SF(4) - OP(4,14)*SF(5) + OP(3,14)*SPP(1) + OP(14,14)*SPP(4) + OP(15,14)*SPP(7) - OP(16,14)*SPP(10);
nextP(6,14) = OP(6,14) + OP(1,14)*SF(5) + OP(3,14)*SF(4) + OP(4,14)*SF(6) - OP(2,14)*SPP(1) - OP(14,14)*SPP(9) + OP(15,14)*SPP(3) + OP(16,14)*SPP(6);
nextP(7,14) = OP(7,14) + OP(2,14)*SF(5) - OP(3,14)*SF(6) + OP(4,14)*SF(4) + OP(1,14)*SPP(1) + OP(14,14)*SPP(5) - OP(15,14)*SPP(8) - OP(16,14)*SPP(2);
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
nextP(8,14) = OP(8,14) + OP(5,14)*dt;
nextP(9,14) = OP(9,14) + OP(6,14)*dt;
EKF: update derivation files
2016-05-01 02:06:17 -03:00
nextP(10,14) = OP(10,14) + OP(7,14)*dt;
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
nextP(11,14) = OP(11,14);
nextP(12,14) = OP(12,14);
nextP(13,14) = OP(13,14);
nextP(14,14) = OP(14,14);
EKF: update derivation files
2016-05-01 02:06:17 -03:00
nextP(1,15) = OP(1,15) + OP(2,15)*SF(10) + OP(3,15)*SF(12) + OP(4,15)*SF(11) + OP(11,15)*SF(15) + OP(12,15)*SF(16) + OP(13,15)*SPP(11);
nextP(2,15) = OP(2,15) + OP(1,15)*SF(9) + OP(3,15)*SF(8) + OP(4,15)*SF(12) - OP(13,15)*SF(16) + OP(12,15)*SPP(11) - (OP(11,15)*q0)/2;
nextP(3,15) = OP(3,15) + OP(1,15)*SF(7) + OP(2,15)*SF(11) + OP(4,15)*SF(9) + OP(13,15)*SF(15) - OP(11,15)*SPP(11) - (OP(12,15)*q0)/2;
nextP(4,15) = OP(4,15) + OP(1,15)*SF(8) + OP(2,15)*SF(7) + OP(3,15)*SF(10) + OP(11,15)*SF(16) - OP(12,15)*SF(15) - (OP(13,15)*q0)/2;
nextP(5,15) = OP(5,15) + OP(1,15)*SF(6) + OP(2,15)*SF(4) - OP(4,15)*SF(5) + OP(3,15)*SPP(1) + OP(14,15)*SPP(4) + OP(15,15)*SPP(7) - OP(16,15)*SPP(10);
nextP(6,15) = OP(6,15) + OP(1,15)*SF(5) + OP(3,15)*SF(4) + OP(4,15)*SF(6) - OP(2,15)*SPP(1) - OP(14,15)*SPP(9) + OP(15,15)*SPP(3) + OP(16,15)*SPP(6);
nextP(7,15) = OP(7,15) + OP(2,15)*SF(5) - OP(3,15)*SF(6) + OP(4,15)*SF(4) + OP(1,15)*SPP(1) + OP(14,15)*SPP(5) - OP(15,15)*SPP(8) - OP(16,15)*SPP(2);
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
nextP(8,15) = OP(8,15) + OP(5,15)*dt;
nextP(9,15) = OP(9,15) + OP(6,15)*dt;
EKF: update derivation files
2016-05-01 02:06:17 -03:00
nextP(10,15) = OP(10,15) + OP(7,15)*dt;
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
nextP(11,15) = OP(11,15);
nextP(12,15) = OP(12,15);
nextP(13,15) = OP(13,15);
nextP(14,15) = OP(14,15);
nextP(15,15) = OP(15,15);
EKF: update derivation files
2016-05-01 02:06:17 -03:00
nextP(1,16) = OP(1,16) + OP(2,16)*SF(10) + OP(3,16)*SF(12) + OP(4,16)*SF(11) + OP(11,16)*SF(15) + OP(12,16)*SF(16) + OP(13,16)*SPP(11);
nextP(2,16) = OP(2,16) + OP(1,16)*SF(9) + OP(3,16)*SF(8) + OP(4,16)*SF(12) - OP(13,16)*SF(16) + OP(12,16)*SPP(11) - (OP(11,16)*q0)/2;
nextP(3,16) = OP(3,16) + OP(1,16)*SF(7) + OP(2,16)*SF(11) + OP(4,16)*SF(9) + OP(13,16)*SF(15) - OP(11,16)*SPP(11) - (OP(12,16)*q0)/2;
nextP(4,16) = OP(4,16) + OP(1,16)*SF(8) + OP(2,16)*SF(7) + OP(3,16)*SF(10) + OP(11,16)*SF(16) - OP(12,16)*SF(15) - (OP(13,16)*q0)/2;
nextP(5,16) = OP(5,16) + OP(1,16)*SF(6) + OP(2,16)*SF(4) - OP(4,16)*SF(5) + OP(3,16)*SPP(1) + OP(14,16)*SPP(4) + OP(15,16)*SPP(7) - OP(16,16)*SPP(10);
nextP(6,16) = OP(6,16) + OP(1,16)*SF(5) + OP(3,16)*SF(4) + OP(4,16)*SF(6) - OP(2,16)*SPP(1) - OP(14,16)*SPP(9) + OP(15,16)*SPP(3) + OP(16,16)*SPP(6);
nextP(7,16) = OP(7,16) + OP(2,16)*SF(5) - OP(3,16)*SF(6) + OP(4,16)*SF(4) + OP(1,16)*SPP(1) + OP(14,16)*SPP(5) - OP(15,16)*SPP(8) - OP(16,16)*SPP(2);
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
nextP(8,16) = OP(8,16) + OP(5,16)*dt;
nextP(9,16) = OP(9,16) + OP(6,16)*dt;
EKF: update derivation files
2016-05-01 02:06:17 -03:00
nextP(10,16) = OP(10,16) + OP(7,16)*dt;
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
nextP(11,16) = OP(11,16);
nextP(12,16) = OP(12,16);
nextP(13,16) = OP(13,16);
nextP(14,16) = OP(14,16);
nextP(15,16) = OP(15,16);
nextP(16,16) = OP(16,16);
EKF: update derivation files
2016-05-01 02:06:17 -03:00
nextP(1,17) = OP(1,17) + OP(2,17)*SF(10) + OP(3,17)*SF(12) + OP(4,17)*SF(11) + OP(11,17)*SF(15) + OP(12,17)*SF(16) + OP(13,17)*SPP(11);
nextP(2,17) = OP(2,17) + OP(1,17)*SF(9) + OP(3,17)*SF(8) + OP(4,17)*SF(12) - OP(13,17)*SF(16) + OP(12,17)*SPP(11) - (OP(11,17)*q0)/2;
nextP(3,17) = OP(3,17) + OP(1,17)*SF(7) + OP(2,17)*SF(11) + OP(4,17)*SF(9) + OP(13,17)*SF(15) - OP(11,17)*SPP(11) - (OP(12,17)*q0)/2;
nextP(4,17) = OP(4,17) + OP(1,17)*SF(8) + OP(2,17)*SF(7) + OP(3,17)*SF(10) + OP(11,17)*SF(16) - OP(12,17)*SF(15) - (OP(13,17)*q0)/2;
nextP(5,17) = OP(5,17) + OP(1,17)*SF(6) + OP(2,17)*SF(4) - OP(4,17)*SF(5) + OP(3,17)*SPP(1) + OP(14,17)*SPP(4) + OP(15,17)*SPP(7) - OP(16,17)*SPP(10);
nextP(6,17) = OP(6,17) + OP(1,17)*SF(5) + OP(3,17)*SF(4) + OP(4,17)*SF(6) - OP(2,17)*SPP(1) - OP(14,17)*SPP(9) + OP(15,17)*SPP(3) + OP(16,17)*SPP(6);
nextP(7,17) = OP(7,17) + OP(2,17)*SF(5) - OP(3,17)*SF(6) + OP(4,17)*SF(4) + OP(1,17)*SPP(1) + OP(14,17)*SPP(5) - OP(15,17)*SPP(8) - OP(16,17)*SPP(2);
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
nextP(8,17) = OP(8,17) + OP(5,17)*dt;
nextP(9,17) = OP(9,17) + OP(6,17)*dt;
EKF: update derivation files
2016-05-01 02:06:17 -03:00
nextP(10,17) = OP(10,17) + OP(7,17)*dt;
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
nextP(11,17) = OP(11,17);
nextP(12,17) = OP(12,17);
nextP(13,17) = OP(13,17);
nextP(14,17) = OP(14,17);
nextP(15,17) = OP(15,17);
nextP(16,17) = OP(16,17);
nextP(17,17) = OP(17,17);
EKF: update derivation files
2016-05-01 02:06:17 -03:00
nextP(1,18) = OP(1,18) + OP(2,18)*SF(10) + OP(3,18)*SF(12) + OP(4,18)*SF(11) + OP(11,18)*SF(15) + OP(12,18)*SF(16) + OP(13,18)*SPP(11);
nextP(2,18) = OP(2,18) + OP(1,18)*SF(9) + OP(3,18)*SF(8) + OP(4,18)*SF(12) - OP(13,18)*SF(16) + OP(12,18)*SPP(11) - (OP(11,18)*q0)/2;
nextP(3,18) = OP(3,18) + OP(1,18)*SF(7) + OP(2,18)*SF(11) + OP(4,18)*SF(9) + OP(13,18)*SF(15) - OP(11,18)*SPP(11) - (OP(12,18)*q0)/2;
nextP(4,18) = OP(4,18) + OP(1,18)*SF(8) + OP(2,18)*SF(7) + OP(3,18)*SF(10) + OP(11,18)*SF(16) - OP(12,18)*SF(15) - (OP(13,18)*q0)/2;
nextP(5,18) = OP(5,18) + OP(1,18)*SF(6) + OP(2,18)*SF(4) - OP(4,18)*SF(5) + OP(3,18)*SPP(1) + OP(14,18)*SPP(4) + OP(15,18)*SPP(7) - OP(16,18)*SPP(10);
nextP(6,18) = OP(6,18) + OP(1,18)*SF(5) + OP(3,18)*SF(4) + OP(4,18)*SF(6) - OP(2,18)*SPP(1) - OP(14,18)*SPP(9) + OP(15,18)*SPP(3) + OP(16,18)*SPP(6);
nextP(7,18) = OP(7,18) + OP(2,18)*SF(5) - OP(3,18)*SF(6) + OP(4,18)*SF(4) + OP(1,18)*SPP(1) + OP(14,18)*SPP(5) - OP(15,18)*SPP(8) - OP(16,18)*SPP(2);
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
nextP(8,18) = OP(8,18) + OP(5,18)*dt;
nextP(9,18) = OP(9,18) + OP(6,18)*dt;
EKF: update derivation files
2016-05-01 02:06:17 -03:00
nextP(10,18) = OP(10,18) + OP(7,18)*dt;
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
nextP(11,18) = OP(11,18);
nextP(12,18) = OP(12,18);
nextP(13,18) = OP(13,18);
nextP(14,18) = OP(14,18);
nextP(15,18) = OP(15,18);
nextP(16,18) = OP(16,18);
nextP(17,18) = OP(17,18);
nextP(18,18) = OP(18,18);
EKF: update derivation files
2016-05-01 02:06:17 -03:00
nextP(1,19) = OP(1,19) + OP(2,19)*SF(10) + OP(3,19)*SF(12) + OP(4,19)*SF(11) + OP(11,19)*SF(15) + OP(12,19)*SF(16) + OP(13,19)*SPP(11);
nextP(2,19) = OP(2,19) + OP(1,19)*SF(9) + OP(3,19)*SF(8) + OP(4,19)*SF(12) - OP(13,19)*SF(16) + OP(12,19)*SPP(11) - (OP(11,19)*q0)/2;
nextP(3,19) = OP(3,19) + OP(1,19)*SF(7) + OP(2,19)*SF(11) + OP(4,19)*SF(9) + OP(13,19)*SF(15) - OP(11,19)*SPP(11) - (OP(12,19)*q0)/2;
nextP(4,19) = OP(4,19) + OP(1,19)*SF(8) + OP(2,19)*SF(7) + OP(3,19)*SF(10) + OP(11,19)*SF(16) - OP(12,19)*SF(15) - (OP(13,19)*q0)/2;
nextP(5,19) = OP(5,19) + OP(1,19)*SF(6) + OP(2,19)*SF(4) - OP(4,19)*SF(5) + OP(3,19)*SPP(1) + OP(14,19)*SPP(4) + OP(15,19)*SPP(7) - OP(16,19)*SPP(10);
nextP(6,19) = OP(6,19) + OP(1,19)*SF(5) + OP(3,19)*SF(4) + OP(4,19)*SF(6) - OP(2,19)*SPP(1) - OP(14,19)*SPP(9) + OP(15,19)*SPP(3) + OP(16,19)*SPP(6);
nextP(7,19) = OP(7,19) + OP(2,19)*SF(5) - OP(3,19)*SF(6) + OP(4,19)*SF(4) + OP(1,19)*SPP(1) + OP(14,19)*SPP(5) - OP(15,19)*SPP(8) - OP(16,19)*SPP(2);
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
nextP(8,19) = OP(8,19) + OP(5,19)*dt;
nextP(9,19) = OP(9,19) + OP(6,19)*dt;
EKF: update derivation files
2016-05-01 02:06:17 -03:00
nextP(10,19) = OP(10,19) + OP(7,19)*dt;
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
nextP(11,19) = OP(11,19);
nextP(12,19) = OP(12,19);
nextP(13,19) = OP(13,19);
nextP(14,19) = OP(14,19);
nextP(15,19) = OP(15,19);
nextP(16,19) = OP(16,19);
nextP(17,19) = OP(17,19);
nextP(18,19) = OP(18,19);
nextP(19,19) = OP(19,19);
EKF: update derivation files
2016-05-01 02:06:17 -03:00
nextP(1,20) = OP(1,20) + OP(2,20)*SF(10) + OP(3,20)*SF(12) + OP(4,20)*SF(11) + OP(11,20)*SF(15) + OP(12,20)*SF(16) + OP(13,20)*SPP(11);
nextP(2,20) = OP(2,20) + OP(1,20)*SF(9) + OP(3,20)*SF(8) + OP(4,20)*SF(12) - OP(13,20)*SF(16) + OP(12,20)*SPP(11) - (OP(11,20)*q0)/2;
nextP(3,20) = OP(3,20) + OP(1,20)*SF(7) + OP(2,20)*SF(11) + OP(4,20)*SF(9) + OP(13,20)*SF(15) - OP(11,20)*SPP(11) - (OP(12,20)*q0)/2;
nextP(4,20) = OP(4,20) + OP(1,20)*SF(8) + OP(2,20)*SF(7) + OP(3,20)*SF(10) + OP(11,20)*SF(16) - OP(12,20)*SF(15) - (OP(13,20)*q0)/2;
nextP(5,20) = OP(5,20) + OP(1,20)*SF(6) + OP(2,20)*SF(4) - OP(4,20)*SF(5) + OP(3,20)*SPP(1) + OP(14,20)*SPP(4) + OP(15,20)*SPP(7) - OP(16,20)*SPP(10);
nextP(6,20) = OP(6,20) + OP(1,20)*SF(5) + OP(3,20)*SF(4) + OP(4,20)*SF(6) - OP(2,20)*SPP(1) - OP(14,20)*SPP(9) + OP(15,20)*SPP(3) + OP(16,20)*SPP(6);
nextP(7,20) = OP(7,20) + OP(2,20)*SF(5) - OP(3,20)*SF(6) + OP(4,20)*SF(4) + OP(1,20)*SPP(1) + OP(14,20)*SPP(5) - OP(15,20)*SPP(8) - OP(16,20)*SPP(2);
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
nextP(8,20) = OP(8,20) + OP(5,20)*dt;
nextP(9,20) = OP(9,20) + OP(6,20)*dt;
EKF: update derivation files
2016-05-01 02:06:17 -03:00
nextP(10,20) = OP(10,20) + OP(7,20)*dt;
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
nextP(11,20) = OP(11,20);
nextP(12,20) = OP(12,20);
nextP(13,20) = OP(13,20);
nextP(14,20) = OP(14,20);
nextP(15,20) = OP(15,20);
nextP(16,20) = OP(16,20);
nextP(17,20) = OP(17,20);
nextP(18,20) = OP(18,20);
nextP(19,20) = OP(19,20);
nextP(20,20) = OP(20,20);
EKF: update derivation files
2016-05-01 02:06:17 -03:00
nextP(1,21) = OP(1,21) + OP(2,21)*SF(10) + OP(3,21)*SF(12) + OP(4,21)*SF(11) + OP(11,21)*SF(15) + OP(12,21)*SF(16) + OP(13,21)*SPP(11);
nextP(2,21) = OP(2,21) + OP(1,21)*SF(9) + OP(3,21)*SF(8) + OP(4,21)*SF(12) - OP(13,21)*SF(16) + OP(12,21)*SPP(11) - (OP(11,21)*q0)/2;
nextP(3,21) = OP(3,21) + OP(1,21)*SF(7) + OP(2,21)*SF(11) + OP(4,21)*SF(9) + OP(13,21)*SF(15) - OP(11,21)*SPP(11) - (OP(12,21)*q0)/2;
nextP(4,21) = OP(4,21) + OP(1,21)*SF(8) + OP(2,21)*SF(7) + OP(3,21)*SF(10) + OP(11,21)*SF(16) - OP(12,21)*SF(15) - (OP(13,21)*q0)/2;
nextP(5,21) = OP(5,21) + OP(1,21)*SF(6) + OP(2,21)*SF(4) - OP(4,21)*SF(5) + OP(3,21)*SPP(1) + OP(14,21)*SPP(4) + OP(15,21)*SPP(7) - OP(16,21)*SPP(10);
nextP(6,21) = OP(6,21) + OP(1,21)*SF(5) + OP(3,21)*SF(4) + OP(4,21)*SF(6) - OP(2,21)*SPP(1) - OP(14,21)*SPP(9) + OP(15,21)*SPP(3) + OP(16,21)*SPP(6);
nextP(7,21) = OP(7,21) + OP(2,21)*SF(5) - OP(3,21)*SF(6) + OP(4,21)*SF(4) + OP(1,21)*SPP(1) + OP(14,21)*SPP(5) - OP(15,21)*SPP(8) - OP(16,21)*SPP(2);
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
nextP(8,21) = OP(8,21) + OP(5,21)*dt;
nextP(9,21) = OP(9,21) + OP(6,21)*dt;
EKF: update derivation files
2016-05-01 02:06:17 -03:00
nextP(10,21) = OP(10,21) + OP(7,21)*dt;
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
nextP(11,21) = OP(11,21);
nextP(12,21) = OP(12,21);
nextP(13,21) = OP(13,21);
nextP(14,21) = OP(14,21);
nextP(15,21) = OP(15,21);
nextP(16,21) = OP(16,21);
nextP(17,21) = OP(17,21);
nextP(18,21) = OP(18,21);
nextP(19,21) = OP(19,21);
nextP(20,21) = OP(20,21);
nextP(21,21) = OP(21,21);
EKF: update derivation files
2016-05-01 02:06:17 -03:00
nextP(1,22) = OP(1,22) + OP(2,22)*SF(10) + OP(3,22)*SF(12) + OP(4,22)*SF(11) + OP(11,22)*SF(15) + OP(12,22)*SF(16) + OP(13,22)*SPP(11);
nextP(2,22) = OP(2,22) + OP(1,22)*SF(9) + OP(3,22)*SF(8) + OP(4,22)*SF(12) - OP(13,22)*SF(16) + OP(12,22)*SPP(11) - (OP(11,22)*q0)/2;
nextP(3,22) = OP(3,22) + OP(1,22)*SF(7) + OP(2,22)*SF(11) + OP(4,22)*SF(9) + OP(13,22)*SF(15) - OP(11,22)*SPP(11) - (OP(12,22)*q0)/2;
nextP(4,22) = OP(4,22) + OP(1,22)*SF(8) + OP(2,22)*SF(7) + OP(3,22)*SF(10) + OP(11,22)*SF(16) - OP(12,22)*SF(15) - (OP(13,22)*q0)/2;
nextP(5,22) = OP(5,22) + OP(1,22)*SF(6) + OP(2,22)*SF(4) - OP(4,22)*SF(5) + OP(3,22)*SPP(1) + OP(14,22)*SPP(4) + OP(15,22)*SPP(7) - OP(16,22)*SPP(10);
nextP(6,22) = OP(6,22) + OP(1,22)*SF(5) + OP(3,22)*SF(4) + OP(4,22)*SF(6) - OP(2,22)*SPP(1) - OP(14,22)*SPP(9) + OP(15,22)*SPP(3) + OP(16,22)*SPP(6);
nextP(7,22) = OP(7,22) + OP(2,22)*SF(5) - OP(3,22)*SF(6) + OP(4,22)*SF(4) + OP(1,22)*SPP(1) + OP(14,22)*SPP(5) - OP(15,22)*SPP(8) - OP(16,22)*SPP(2);
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
nextP(8,22) = OP(8,22) + OP(5,22)*dt;
nextP(9,22) = OP(9,22) + OP(6,22)*dt;
EKF: update derivation files
2016-05-01 02:06:17 -03:00
nextP(10,22) = OP(10,22) + OP(7,22)*dt;
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
nextP(11,22) = OP(11,22);
nextP(12,22) = OP(12,22);
nextP(13,22) = OP(13,22);
nextP(14,22) = OP(14,22);
nextP(15,22) = OP(15,22);
nextP(16,22) = OP(16,22);
nextP(17,22) = OP(17,22);
nextP(18,22) = OP(18,22);
nextP(19,22) = OP(19,22);
nextP(20,22) = OP(20,22);
nextP(21,22) = OP(21,22);
nextP(22,22) = OP(22,22);
EKF: update derivation files
2016-05-01 02:06:17 -03:00
nextP(1,23) = OP(1,23) + OP(2,23)*SF(10) + OP(3,23)*SF(12) + OP(4,23)*SF(11) + OP(11,23)*SF(15) + OP(12,23)*SF(16) + OP(13,23)*SPP(11);
nextP(2,23) = OP(2,23) + OP(1,23)*SF(9) + OP(3,23)*SF(8) + OP(4,23)*SF(12) - OP(13,23)*SF(16) + OP(12,23)*SPP(11) - (OP(11,23)*q0)/2;
nextP(3,23) = OP(3,23) + OP(1,23)*SF(7) + OP(2,23)*SF(11) + OP(4,23)*SF(9) + OP(13,23)*SF(15) - OP(11,23)*SPP(11) - (OP(12,23)*q0)/2;
nextP(4,23) = OP(4,23) + OP(1,23)*SF(8) + OP(2,23)*SF(7) + OP(3,23)*SF(10) + OP(11,23)*SF(16) - OP(12,23)*SF(15) - (OP(13,23)*q0)/2;
nextP(5,23) = OP(5,23) + OP(1,23)*SF(6) + OP(2,23)*SF(4) - OP(4,23)*SF(5) + OP(3,23)*SPP(1) + OP(14,23)*SPP(4) + OP(15,23)*SPP(7) - OP(16,23)*SPP(10);
nextP(6,23) = OP(6,23) + OP(1,23)*SF(5) + OP(3,23)*SF(4) + OP(4,23)*SF(6) - OP(2,23)*SPP(1) - OP(14,23)*SPP(9) + OP(15,23)*SPP(3) + OP(16,23)*SPP(6);
nextP(7,23) = OP(7,23) + OP(2,23)*SF(5) - OP(3,23)*SF(6) + OP(4,23)*SF(4) + OP(1,23)*SPP(1) + OP(14,23)*SPP(5) - OP(15,23)*SPP(8) - OP(16,23)*SPP(2);
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
nextP(8,23) = OP(8,23) + OP(5,23)*dt;
nextP(9,23) = OP(9,23) + OP(6,23)*dt;
EKF: update derivation files
2016-05-01 02:06:17 -03:00
nextP(10,23) = OP(10,23) + OP(7,23)*dt;
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
nextP(11,23) = OP(11,23);
nextP(12,23) = OP(12,23);
nextP(13,23) = OP(13,23);
nextP(14,23) = OP(14,23);
nextP(15,23) = OP(15,23);
nextP(16,23) = OP(16,23);
nextP(17,23) = OP(17,23);
nextP(18,23) = OP(18,23);
nextP(19,23) = OP(19,23);
nextP(20,23) = OP(20,23);
nextP(21,23) = OP(21,23);
nextP(22,23) = OP(22,23);
nextP(23,23) = OP(23,23);
EKF: update derivation files
2016-05-01 02:06:17 -03:00
nextP(1,24) = OP(1,24) + OP(2,24)*SF(10) + OP(3,24)*SF(12) + OP(4,24)*SF(11) + OP(11,24)*SF(15) + OP(12,24)*SF(16) + OP(13,24)*SPP(11);
nextP(2,24) = OP(2,24) + OP(1,24)*SF(9) + OP(3,24)*SF(8) + OP(4,24)*SF(12) - OP(13,24)*SF(16) + OP(12,24)*SPP(11) - (OP(11,24)*q0)/2;
nextP(3,24) = OP(3,24) + OP(1,24)*SF(7) + OP(2,24)*SF(11) + OP(4,24)*SF(9) + OP(13,24)*SF(15) - OP(11,24)*SPP(11) - (OP(12,24)*q0)/2;
nextP(4,24) = OP(4,24) + OP(1,24)*SF(8) + OP(2,24)*SF(7) + OP(3,24)*SF(10) + OP(11,24)*SF(16) - OP(12,24)*SF(15) - (OP(13,24)*q0)/2;
nextP(5,24) = OP(5,24) + OP(1,24)*SF(6) + OP(2,24)*SF(4) - OP(4,24)*SF(5) + OP(3,24)*SPP(1) + OP(14,24)*SPP(4) + OP(15,24)*SPP(7) - OP(16,24)*SPP(10);
nextP(6,24) = OP(6,24) + OP(1,24)*SF(5) + OP(3,24)*SF(4) + OP(4,24)*SF(6) - OP(2,24)*SPP(1) - OP(14,24)*SPP(9) + OP(15,24)*SPP(3) + OP(16,24)*SPP(6);
nextP(7,24) = OP(7,24) + OP(2,24)*SF(5) - OP(3,24)*SF(6) + OP(4,24)*SF(4) + OP(1,24)*SPP(1) + OP(14,24)*SPP(5) - OP(15,24)*SPP(8) - OP(16,24)*SPP(2);
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
nextP(8,24) = OP(8,24) + OP(5,24)*dt;
nextP(9,24) = OP(9,24) + OP(6,24)*dt;
EKF: update derivation files
2016-05-01 02:06:17 -03:00
nextP(10,24) = OP(10,24) + OP(7,24)*dt;
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
nextP(11,24) = OP(11,24);
nextP(12,24) = OP(12,24);
nextP(13,24) = OP(13,24);
nextP(14,24) = OP(14,24);
nextP(15,24) = OP(15,24);
nextP(16,24) = OP(16,24);
nextP(17,24) = OP(17,24);
nextP(18,24) = OP(18,24);
nextP(19,24) = OP(19,24);
nextP(20,24) = OP(20,24);
nextP(21,24) = OP(21,24);
nextP(22,24) = OP(22,24);
nextP(23,24) = OP(23,24);
nextP(24,24) = OP(24,24);
SH_TAS = zeros(3,1);
SH_TAS(1) = 1/((ve - vwe)^2 + (vn - vwn)^2 + vd^2)^(1/2);
SH_TAS(2) = (SH_TAS(1)*(2*ve - 2*vwe))/2;
SH_TAS(3) = (SH_TAS(1)*(2*vn - 2*vwn))/2;
H_TAS = zeros(1,24);
EKF: update derivation files
2016-05-01 02:06:17 -03:00
H_TAS(1,5) = SH_TAS(3);
H_TAS(1,6) = SH_TAS(2);
H_TAS(1,7) = vd*SH_TAS(1);
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
H_TAS(1,23) = -SH_TAS(3);
H_TAS(1,24) = -SH_TAS(2);
SK_TAS = zeros(2,1);
EKF: update derivation files
2016-05-01 02:06:17 -03:00
SK_TAS(1) = 1/(R_TAS + SH_TAS(3)*(OP(5,5)*SH_TAS(3) + OP(6,5)*SH_TAS(2) - OP(23,5)*SH_TAS(3) - OP(24,5)*SH_TAS(2) + OP(7,5)*vd*SH_TAS(1)) + SH_TAS(2)*(OP(5,6)*SH_TAS(3) + OP(6,6)*SH_TAS(2) - OP(23,6)*SH_TAS(3) - OP(24,6)*SH_TAS(2) + OP(7,6)*vd*SH_TAS(1)) - SH_TAS(3)*(OP(5,23)*SH_TAS(3) + OP(6,23)*SH_TAS(2) - OP(23,23)*SH_TAS(3) - OP(24,23)*SH_TAS(2) + OP(7,23)*vd*SH_TAS(1)) - SH_TAS(2)*(OP(5,24)*SH_TAS(3) + OP(6,24)*SH_TAS(2) - OP(23,24)*SH_TAS(3) - OP(24,24)*SH_TAS(2) + OP(7,24)*vd*SH_TAS(1)) + vd*SH_TAS(1)*(OP(5,7)*SH_TAS(3) + OP(6,7)*SH_TAS(2) - OP(23,7)*SH_TAS(3) - OP(24,7)*SH_TAS(2) + OP(7,7)*vd*SH_TAS(1)));
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
SK_TAS(2) = SH_TAS(2);
Kfusion = zeros(24,1);
Kfusion = zeros(1,1);
EKF: update derivation files
2016-05-01 02:06:17 -03:00
Kfusion(1) = SK_TAS(1)*(OP(1,5)*SH_TAS(3) - OP(1,23)*SH_TAS(3) + OP(1,6)*SK_TAS(2) - OP(1,24)*SK_TAS(2) + OP(1,7)*vd*SH_TAS(1));
Kfusion(2) = SK_TAS(1)*(OP(2,5)*SH_TAS(3) - OP(2,23)*SH_TAS(3) + OP(2,6)*SK_TAS(2) - OP(2,24)*SK_TAS(2) + OP(2,7)*vd*SH_TAS(1));
Kfusion(3) = SK_TAS(1)*(OP(3,5)*SH_TAS(3) - OP(3,23)*SH_TAS(3) + OP(3,6)*SK_TAS(2) - OP(3,24)*SK_TAS(2) + OP(3,7)*vd*SH_TAS(1));
Kfusion(4) = SK_TAS(1)*(OP(4,5)*SH_TAS(3) - OP(4,23)*SH_TAS(3) + OP(4,6)*SK_TAS(2) - OP(4,24)*SK_TAS(2) + OP(4,7)*vd*SH_TAS(1));
Kfusion(5) = SK_TAS(1)*(OP(5,5)*SH_TAS(3) - OP(5,23)*SH_TAS(3) + OP(5,6)*SK_TAS(2) - OP(5,24)*SK_TAS(2) + OP(5,7)*vd*SH_TAS(1));
Kfusion(6) = SK_TAS(1)*(OP(6,5)*SH_TAS(3) - OP(6,23)*SH_TAS(3) + OP(6,6)*SK_TAS(2) - OP(6,24)*SK_TAS(2) + OP(6,7)*vd*SH_TAS(1));
Kfusion(7) = SK_TAS(1)*(OP(7,5)*SH_TAS(3) - OP(7,23)*SH_TAS(3) + OP(7,6)*SK_TAS(2) - OP(7,24)*SK_TAS(2) + OP(7,7)*vd*SH_TAS(1));
Kfusion(8) = SK_TAS(1)*(OP(8,5)*SH_TAS(3) - OP(8,23)*SH_TAS(3) + OP(8,6)*SK_TAS(2) - OP(8,24)*SK_TAS(2) + OP(8,7)*vd*SH_TAS(1));
Kfusion(9) = SK_TAS(1)*(OP(9,5)*SH_TAS(3) - OP(9,23)*SH_TAS(3) + OP(9,6)*SK_TAS(2) - OP(9,24)*SK_TAS(2) + OP(9,7)*vd*SH_TAS(1));
Kfusion(10) = SK_TAS(1)*(OP(10,5)*SH_TAS(3) - OP(10,23)*SH_TAS(3) + OP(10,6)*SK_TAS(2) - OP(10,24)*SK_TAS(2) + OP(10,7)*vd*SH_TAS(1));
Kfusion(11) = SK_TAS(1)*(OP(11,5)*SH_TAS(3) - OP(11,23)*SH_TAS(3) + OP(11,6)*SK_TAS(2) - OP(11,24)*SK_TAS(2) + OP(11,7)*vd*SH_TAS(1));
Kfusion(12) = SK_TAS(1)*(OP(12,5)*SH_TAS(3) - OP(12,23)*SH_TAS(3) + OP(12,6)*SK_TAS(2) - OP(12,24)*SK_TAS(2) + OP(12,7)*vd*SH_TAS(1));
Kfusion(13) = SK_TAS(1)*(OP(13,5)*SH_TAS(3) - OP(13,23)*SH_TAS(3) + OP(13,6)*SK_TAS(2) - OP(13,24)*SK_TAS(2) + OP(13,7)*vd*SH_TAS(1));
Kfusion(14) = SK_TAS(1)*(OP(14,5)*SH_TAS(3) - OP(14,23)*SH_TAS(3) + OP(14,6)*SK_TAS(2) - OP(14,24)*SK_TAS(2) + OP(14,7)*vd*SH_TAS(1));
Kfusion(15) = SK_TAS(1)*(OP(15,5)*SH_TAS(3) - OP(15,23)*SH_TAS(3) + OP(15,6)*SK_TAS(2) - OP(15,24)*SK_TAS(2) + OP(15,7)*vd*SH_TAS(1));
Kfusion(16) = SK_TAS(1)*(OP(16,5)*SH_TAS(3) - OP(16,23)*SH_TAS(3) + OP(16,6)*SK_TAS(2) - OP(16,24)*SK_TAS(2) + OP(16,7)*vd*SH_TAS(1));
Kfusion(17) = SK_TAS(1)*(OP(17,5)*SH_TAS(3) - OP(17,23)*SH_TAS(3) + OP(17,6)*SK_TAS(2) - OP(17,24)*SK_TAS(2) + OP(17,7)*vd*SH_TAS(1));
Kfusion(18) = SK_TAS(1)*(OP(18,5)*SH_TAS(3) - OP(18,23)*SH_TAS(3) + OP(18,6)*SK_TAS(2) - OP(18,24)*SK_TAS(2) + OP(18,7)*vd*SH_TAS(1));
Kfusion(19) = SK_TAS(1)*(OP(19,5)*SH_TAS(3) - OP(19,23)*SH_TAS(3) + OP(19,6)*SK_TAS(2) - OP(19,24)*SK_TAS(2) + OP(19,7)*vd*SH_TAS(1));
Kfusion(20) = SK_TAS(1)*(OP(20,5)*SH_TAS(3) - OP(20,23)*SH_TAS(3) + OP(20,6)*SK_TAS(2) - OP(20,24)*SK_TAS(2) + OP(20,7)*vd*SH_TAS(1));
Kfusion(21) = SK_TAS(1)*(OP(21,5)*SH_TAS(3) - OP(21,23)*SH_TAS(3) + OP(21,6)*SK_TAS(2) - OP(21,24)*SK_TAS(2) + OP(21,7)*vd*SH_TAS(1));
Kfusion(22) = SK_TAS(1)*(OP(22,5)*SH_TAS(3) - OP(22,23)*SH_TAS(3) + OP(22,6)*SK_TAS(2) - OP(22,24)*SK_TAS(2) + OP(22,7)*vd*SH_TAS(1));
Kfusion(23) = SK_TAS(1)*(OP(23,5)*SH_TAS(3) - OP(23,23)*SH_TAS(3) + OP(23,6)*SK_TAS(2) - OP(23,24)*SK_TAS(2) + OP(23,7)*vd*SH_TAS(1));
Kfusion(24) = SK_TAS(1)*(OP(24,5)*SH_TAS(3) - OP(24,23)*SH_TAS(3) + OP(24,6)*SK_TAS(2) - OP(24,24)*SK_TAS(2) + OP(24,7)*vd*SH_TAS(1));
SH_BETA = zeros(13,1);
SH_BETA(1) = (vn - vwn)*(q0^2 + q1^2 - q2^2 - q3^2) - vd*(2*q0*q2 - 2*q1*q3) + (ve - vwe)*(2*q0*q3 + 2*q1*q2);
SH_BETA(2) = (ve - vwe)*(q0^2 - q1^2 + q2^2 - q3^2) + vd*(2*q0*q1 + 2*q2*q3) - (vn - vwn)*(2*q0*q3 - 2*q1*q2);
SH_BETA(3) = vn - vwn;
SH_BETA(4) = ve - vwe;
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
SH_BETA(5) = 1/SH_BETA(1)^2;
EKF: update derivation files
2016-05-01 02:06:17 -03:00
SH_BETA(6) = 1/SH_BETA(1);
SH_BETA(7) = SH_BETA(6)*(q0^2 - q1^2 + q2^2 - q3^2);
SH_BETA(8) = q0^2 + q1^2 - q2^2 - q3^2;
SH_BETA(9) = 2*q0*SH_BETA(4) - 2*q3*SH_BETA(3) + 2*q1*vd;
SH_BETA(10) = 2*q0*SH_BETA(3) + 2*q3*SH_BETA(4) - 2*q2*vd;
SH_BETA(11) = 2*q2*SH_BETA(3) - 2*q1*SH_BETA(4) + 2*q0*vd;
SH_BETA(12) = 2*q1*SH_BETA(3) + 2*q2*SH_BETA(4) + 2*q3*vd;
SH_BETA(13) = 2*q0*q3;
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
H_BETA = zeros(1,24);
EKF: update derivation files
2016-05-01 02:06:17 -03:00
H_BETA(1,1) = SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10);
H_BETA(1,2) = SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12);
H_BETA(1,3) = SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11);
H_BETA(1,4) = - SH_BETA(6)*SH_BETA(10) - SH_BETA(2)*SH_BETA(5)*SH_BETA(9);
H_BETA(1,5) = - SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) - SH_BETA(2)*SH_BETA(5)*SH_BETA(8);
H_BETA(1,6) = SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2);
H_BETA(1,7) = SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3);
H_BETA(1,23) = SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8);
H_BETA(1,24) = SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2) - SH_BETA(7);
SK_BETA = zeros(8,1);
SK_BETA(1) = 1/(R_BETA - (SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8))*(OP(23,5)*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) - OP(5,5)*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) + OP(6,5)*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) - OP(24,5)*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) + OP(1,5)*(SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10)) + OP(2,5)*(SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12)) + OP(3,5)*(SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11)) - OP(4,5)*(SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP(7,5)*(SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3))) + (SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8))*(OP(23,23)*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) - OP(5,23)*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) + OP(6,23)*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) - OP(24,23)*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) + OP(1,23)*(SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10)) + OP(2,23)*(SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12)) + OP(3,23)*(SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11)) - OP(4,23)*(SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP(7,23)*(SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3))) + (SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2))*(OP(23,6)*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) - OP(5,6)*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) + OP(6,6)*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) - OP(24,6)*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) + OP(1,6)*(SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10)) + OP(2,6)*(SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12)) + OP(3,6)*(SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11)) - OP(4,6)*(SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP(7,6)*(SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3))) - (SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2))*(OP(23,24)*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) - OP(5,24)*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) + OP(6,24)*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) - OP(24,24)*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) + OP(1,24)*(SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10)) + OP(2,24)*(SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12)) + OP(3,24)*(SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11)) - OP(4,24)*(SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP(7,24)*(SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3))) + (SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10))*(OP(23,1)*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) - OP(5,1)*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) + OP(6,1)*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) - OP(24,1)*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) + OP(1,1)*(SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10)) + OP(2,1)*(SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12)) + OP(3,1)*(SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11)) - OP(4,1)*(SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9)) + OP(7,1)*(SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3))) + (SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12))*(OP(23,2)*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) - OP(5,2)*(SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8)) + OP(6,2)*(SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2)) - OP(24,2)*(SH_BETA(7) - SH_BETA(2)
SK_BETA(2) = SH_BETA(6)*(SH_BETA(13) - 2*q1*q2) + SH_BETA(2)*SH_BETA(5)*SH_BETA(8);
SK_BETA(3) = SH_BETA(7) - SH_BETA(2)*SH_BETA(5)*(SH_BETA(13) + 2*q1*q2);
SK_BETA(4) = SH_BETA(6)*(2*q0*q1 + 2*q2*q3) + SH_BETA(2)*SH_BETA(5)*(2*q0*q2 - 2*q1*q3);
SK_BETA(5) = SH_BETA(6)*SH_BETA(11) - SH_BETA(2)*SH_BETA(5)*SH_BETA(12);
SK_BETA(6) = SH_BETA(6)*SH_BETA(9) - SH_BETA(2)*SH_BETA(5)*SH_BETA(10);
SK_BETA(7) = SH_BETA(6)*SH_BETA(12) + SH_BETA(2)*SH_BETA(5)*SH_BETA(11);
SK_BETA(8) = SH_BETA(6)*SH_BETA(10) + SH_BETA(2)*SH_BETA(5)*SH_BETA(9);
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
Kfusion = zeros(24,1);
Kfusion = zeros(1,1);
EKF: update derivation files
2016-05-01 02:06:17 -03:00
Kfusion(1) = SK_BETA(1)*(OP(1,1)*SK_BETA(6) + OP(1,2)*SK_BETA(5) - OP(1,5)*SK_BETA(2) + OP(1,6)*SK_BETA(3) + OP(1,3)*SK_BETA(7) + OP(1,7)*SK_BETA(4) - OP(1,4)*SK_BETA(8) + OP(1,23)*SK_BETA(2) - OP(1,24)*SK_BETA(3));
Kfusion(2) = SK_BETA(1)*(OP(2,1)*SK_BETA(6) + OP(2,2)*SK_BETA(5) - OP(2,5)*SK_BETA(2) + OP(2,6)*SK_BETA(3) + OP(2,3)*SK_BETA(7) + OP(2,7)*SK_BETA(4) - OP(2,4)*SK_BETA(8) + OP(2,23)*SK_BETA(2) - OP(2,24)*SK_BETA(3));
Kfusion(3) = SK_BETA(1)*(OP(3,1)*SK_BETA(6) + OP(3,2)*SK_BETA(5) - OP(3,5)*SK_BETA(2) + OP(3,6)*SK_BETA(3) + OP(3,3)*SK_BETA(7) + OP(3,7)*SK_BETA(4) - OP(3,4)*SK_BETA(8) + OP(3,23)*SK_BETA(2) - OP(3,24)*SK_BETA(3));
Kfusion(4) = SK_BETA(1)*(OP(4,1)*SK_BETA(6) + OP(4,2)*SK_BETA(5) - OP(4,5)*SK_BETA(2) + OP(4,6)*SK_BETA(3) + OP(4,3)*SK_BETA(7) + OP(4,7)*SK_BETA(4) - OP(4,4)*SK_BETA(8) + OP(4,23)*SK_BETA(2) - OP(4,24)*SK_BETA(3));
Kfusion(5) = SK_BETA(1)*(OP(5,1)*SK_BETA(6) + OP(5,2)*SK_BETA(5) - OP(5,5)*SK_BETA(2) + OP(5,6)*SK_BETA(3) + OP(5,3)*SK_BETA(7) + OP(5,7)*SK_BETA(4) - OP(5,4)*SK_BETA(8) + OP(5,23)*SK_BETA(2) - OP(5,24)*SK_BETA(3));
Kfusion(6) = SK_BETA(1)*(OP(6,1)*SK_BETA(6) + OP(6,2)*SK_BETA(5) - OP(6,5)*SK_BETA(2) + OP(6,6)*SK_BETA(3) + OP(6,3)*SK_BETA(7) + OP(6,7)*SK_BETA(4) - OP(6,4)*SK_BETA(8) + OP(6,23)*SK_BETA(2) - OP(6,24)*SK_BETA(3));
Kfusion(7) = SK_BETA(1)*(OP(7,1)*SK_BETA(6) + OP(7,2)*SK_BETA(5) - OP(7,5)*SK_BETA(2) + OP(7,6)*SK_BETA(3) + OP(7,3)*SK_BETA(7) + OP(7,7)*SK_BETA(4) - OP(7,4)*SK_BETA(8) + OP(7,23)*SK_BETA(2) - OP(7,24)*SK_BETA(3));
Kfusion(8) = SK_BETA(1)*(OP(8,1)*SK_BETA(6) + OP(8,2)*SK_BETA(5) - OP(8,5)*SK_BETA(2) + OP(8,6)*SK_BETA(3) + OP(8,3)*SK_BETA(7) + OP(8,7)*SK_BETA(4) - OP(8,4)*SK_BETA(8) + OP(8,23)*SK_BETA(2) - OP(8,24)*SK_BETA(3));
Kfusion(9) = SK_BETA(1)*(OP(9,1)*SK_BETA(6) + OP(9,2)*SK_BETA(5) - OP(9,5)*SK_BETA(2) + OP(9,6)*SK_BETA(3) + OP(9,3)*SK_BETA(7) + OP(9,7)*SK_BETA(4) - OP(9,4)*SK_BETA(8) + OP(9,23)*SK_BETA(2) - OP(9,24)*SK_BETA(3));
Kfusion(10) = SK_BETA(1)*(OP(10,1)*SK_BETA(6) + OP(10,2)*SK_BETA(5) - OP(10,5)*SK_BETA(2) + OP(10,6)*SK_BETA(3) + OP(10,3)*SK_BETA(7) + OP(10,7)*SK_BETA(4) - OP(10,4)*SK_BETA(8) + OP(10,23)*SK_BETA(2) - OP(10,24)*SK_BETA(3));
Kfusion(11) = SK_BETA(1)*(OP(11,1)*SK_BETA(6) + OP(11,2)*SK_BETA(5) - OP(11,5)*SK_BETA(2) + OP(11,6)*SK_BETA(3) + OP(11,3)*SK_BETA(7) + OP(11,7)*SK_BETA(4) - OP(11,4)*SK_BETA(8) + OP(11,23)*SK_BETA(2) - OP(11,24)*SK_BETA(3));
Kfusion(12) = SK_BETA(1)*(OP(12,1)*SK_BETA(6) + OP(12,2)*SK_BETA(5) - OP(12,5)*SK_BETA(2) + OP(12,6)*SK_BETA(3) + OP(12,3)*SK_BETA(7) + OP(12,7)*SK_BETA(4) - OP(12,4)*SK_BETA(8) + OP(12,23)*SK_BETA(2) - OP(12,24)*SK_BETA(3));
Kfusion(13) = SK_BETA(1)*(OP(13,1)*SK_BETA(6) + OP(13,2)*SK_BETA(5) - OP(13,5)*SK_BETA(2) + OP(13,6)*SK_BETA(3) + OP(13,3)*SK_BETA(7) + OP(13,7)*SK_BETA(4) - OP(13,4)*SK_BETA(8) + OP(13,23)*SK_BETA(2) - OP(13,24)*SK_BETA(3));
Kfusion(14) = SK_BETA(1)*(OP(14,1)*SK_BETA(6) + OP(14,2)*SK_BETA(5) - OP(14,5)*SK_BETA(2) + OP(14,6)*SK_BETA(3) + OP(14,3)*SK_BETA(7) + OP(14,7)*SK_BETA(4) - OP(14,4)*SK_BETA(8) + OP(14,23)*SK_BETA(2) - OP(14,24)*SK_BETA(3));
Kfusion(15) = SK_BETA(1)*(OP(15,1)*SK_BETA(6) + OP(15,2)*SK_BETA(5) - OP(15,5)*SK_BETA(2) + OP(15,6)*SK_BETA(3) + OP(15,3)*SK_BETA(7) + OP(15,7)*SK_BETA(4) - OP(15,4)*SK_BETA(8) + OP(15,23)*SK_BETA(2) - OP(15,24)*SK_BETA(3));
Kfusion(16) = SK_BETA(1)*(OP(16,1)*SK_BETA(6) + OP(16,2)*SK_BETA(5) - OP(16,5)*SK_BETA(2) + OP(16,6)*SK_BETA(3) + OP(16,3)*SK_BETA(7) + OP(16,7)*SK_BETA(4) - OP(16,4)*SK_BETA(8) + OP(16,23)*SK_BETA(2) - OP(16,24)*SK_BETA(3));
Kfusion(17) = SK_BETA(1)*(OP(17,1)*SK_BETA(6) + OP(17,2)*SK_BETA(5) - OP(17,5)*SK_BETA(2) + OP(17,6)*SK_BETA(3) + OP(17,3)*SK_BETA(7) + OP(17,7)*SK_BETA(4) - OP(17,4)*SK_BETA(8) + OP(17,23)*SK_BETA(2) - OP(17,24)*SK_BETA(3));
Kfusion(18) = SK_BETA(1)*(OP(18,1)*SK_BETA(6) + OP(18,2)*SK_BETA(5) - OP(18,5)*SK_BETA(2) + OP(18,6)*SK_BETA(3) + OP(18,3)*SK_BETA(7) + OP(18,7)*SK_BETA(4) - OP(18,4)*SK_BETA(8) + OP(18,23)*SK_BETA(2) - OP(18,24)*SK_BETA(3));
Kfusion(19) = SK_BETA(1)*(OP(19,1)*SK_BETA(6) + OP(19,2)*SK_BETA(5) - OP(19,5)*SK_BETA(2) + OP(19,6)*SK_BETA(3) + OP(19,3)*SK_BETA(7) + OP(19,7)*SK_BETA(4) - OP(19,4)*SK_BETA(8) + OP(19,23)*SK_BETA(2) - OP(19,24)*SK_BETA(3));
Kfusion(20) = SK_BETA(1)*(OP(20,1)*SK_BETA(6) + OP(20,2)*SK_BETA(5) - OP(20,5)*SK_BETA(2) + OP(20,6)*SK_BETA(3) + OP(20,3)*SK_BETA(7) + OP(20,7)*SK_BETA(4) - OP(20,4)*SK_BETA(8) + OP(20,23)*SK_BETA(2) - OP(20,24)*SK_BETA(3));
Kfusion(21) = SK_BETA(1)*(OP(21,1)*SK_BETA(6) + OP(21,2)*SK_BETA(5) - OP(21,5)*SK_BETA(2) + OP(21,6)*SK_BETA(3) + OP(21,3)*SK_BETA(7) + OP(21,7)*SK_BETA(4) - OP(21,4)*SK_BETA(8) + OP(21,23)*SK_BETA(2) - OP(21,24)*SK_BETA(3));
Kfusion(22) = SK_BETA(1)*(OP(22,1)*SK_BETA(6) + OP(22,2)*SK_BETA(5) - OP(22,5)*SK_BETA(2) + OP(22,6)*SK_BETA(3) + OP(22,3)*SK_BETA(7) + OP(22,7)*SK_BETA(4) - OP(22,4)*SK_BETA(8) + OP(22,23)*SK_BETA(2) - OP(22,24)*SK_BETA(3));
Kfusion(23) = SK_BETA(1)*(OP(23,1)*SK_BETA(6) + OP(23,2)*SK_BETA(5) - OP(23,5)*SK_BETA(2) + OP(23,6)*SK_BETA(3) + OP(23,3)*SK_BETA(7) + OP(23,7)*SK_BETA(4) - OP(23,4)*SK_BETA(8) + OP(23,23)*SK_BETA(2) - OP(23,24)*SK_BETA(3));
Kfusion(24) = SK_BETA(1)*(OP(24,1)*SK_BETA(6) + OP(24,2)*SK_BETA(5) - OP(24,5)*SK_BETA(2) + OP(24,6)*SK_BETA(3) + OP(24,3)*SK_BETA(7) + OP(24,7)*SK_BETA(4) - OP(24,4)*SK_BETA(8) + OP(24,23)*SK_BETA(2) - OP(24,24)*SK_BETA(3));
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
SH_MAG = zeros(9,1);
EKF: update derivation files
2016-05-01 02:06:17 -03:00
SH_MAG(1) = 2*magD*q3 + 2*magE*q2 + 2*magN*q1;
SH_MAG(2) = 2*magD*q0 - 2*magE*q1 + 2*magN*q2;
SH_MAG(3) = 2*magD*q1 + 2*magE*q0 - 2*magN*q3;
SH_MAG(4) = q3^2;
SH_MAG(5) = q2^2;
SH_MAG(6) = q1^2;
SH_MAG(7) = q0^2;
SH_MAG(8) = 2*magN*q0;
SH_MAG(9) = 2*magE*q3;
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
H_MAG = zeros(1,24);
EKF: update derivation files
2016-05-01 02:06:17 -03:00
H_MAG(1) = SH_MAG(8) + SH_MAG(9) - 2*magD*q2;
H_MAG(2) = SH_MAG(1);
H_MAG(3) = -SH_MAG(2);
H_MAG(4) = SH_MAG(3);
H_MAG(17) = SH_MAG(6) - SH_MAG(5) - SH_MAG(4) + SH_MAG(7);
H_MAG(18) = 2*q0*q3 + 2*q1*q2;
H_MAG(19) = 2*q1*q3 - 2*q0*q2;
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
H_MAG(20) = 1;
EKF: update derivation files
2016-05-01 02:06:17 -03:00
SK_MX = zeros(5,1);
SK_MX(1) = 1/(OP(20,20) + R_MAG + OP(2,20)*SH_MAG(1) - OP(3,20)*SH_MAG(2) + OP(4,20)*SH_MAG(3) - OP(17,20)*(SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7)) + (2*q0*q3 + 2*q1*q2)*(OP(20,18) + OP(2,18)*SH_MAG(1) - OP(3,18)*SH_MAG(2) + OP(4,18)*SH_MAG(3) - OP(17,18)*(SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7)) + OP(18,18)*(2*q0*q3 + 2*q1*q2) - OP(19,18)*(2*q0*q2 - 2*q1*q3) + OP(1,18)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) - (2*q0*q2 - 2*q1*q3)*(OP(20,19) + OP(2,19)*SH_MAG(1) - OP(3,19)*SH_MAG(2) + OP(4,19)*SH_MAG(3) - OP(17,19)*(SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7)) + OP(18,19)*(2*q0*q3 + 2*q1*q2) - OP(19,19)*(2*q0*q2 - 2*q1*q3) + OP(1,19)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + (SH_MAG(8) + SH_MAG(9) - 2*magD*q2)*(OP(20,1) + OP(2,1)*SH_MAG(1) - OP(3,1)*SH_MAG(2) + OP(4,1)*SH_MAG(3) - OP(17,1)*(SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7)) + OP(18,1)*(2*q0*q3 + 2*q1*q2) - OP(19,1)*(2*q0*q2 - 2*q1*q3) + OP(1,1)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + OP(18,20)*(2*q0*q3 + 2*q1*q2) - OP(19,20)*(2*q0*q2 - 2*q1*q3) + SH_MAG(1)*(OP(20,2) + OP(2,2)*SH_MAG(1) - OP(3,2)*SH_MAG(2) + OP(4,2)*SH_MAG(3) - OP(17,2)*(SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7)) + OP(18,2)*(2*q0*q3 + 2*q1*q2) - OP(19,2)*(2*q0*q2 - 2*q1*q3) + OP(1,2)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) - SH_MAG(2)*(OP(20,3) + OP(2,3)*SH_MAG(1) - OP(3,3)*SH_MAG(2) + OP(4,3)*SH_MAG(3) - OP(17,3)*(SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7)) + OP(18,3)*(2*q0*q3 + 2*q1*q2) - OP(19,3)*(2*q0*q2 - 2*q1*q3) + OP(1,3)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + SH_MAG(3)*(OP(20,4) + OP(2,4)*SH_MAG(1) - OP(3,4)*SH_MAG(2) + OP(4,4)*SH_MAG(3) - OP(17,4)*(SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7)) + OP(18,4)*(2*q0*q3 + 2*q1*q2) - OP(19,4)*(2*q0*q2 - 2*q1*q3) + OP(1,4)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) - (SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7))*(OP(20,17) + OP(2,17)*SH_MAG(1) - OP(3,17)*SH_MAG(2) + OP(4,17)*SH_MAG(3) - OP(17,17)*(SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7)) + OP(18,17)*(2*q0*q3 + 2*q1*q2) - OP(19,17)*(2*q0*q2 - 2*q1*q3) + OP(1,17)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + OP(1,20)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2));
SK_MX(2) = SH_MAG(4) + SH_MAG(5) - SH_MAG(6) - SH_MAG(7);
SK_MX(3) = SH_MAG(8) + SH_MAG(9) - 2*magD*q2;
SK_MX(4) = 2*q0*q2 - 2*q1*q3;
SK_MX(5) = 2*q0*q3 + 2*q1*q2;
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
Kfusion = zeros(24,1);
Kfusion = zeros(1,1);
EKF: update derivation files
2016-05-01 02:06:17 -03:00
Kfusion(1) = SK_MX(1)*(OP(1,20) + OP(1,2)*SH_MAG(1) - OP(1,3)*SH_MAG(2) + OP(1,4)*SH_MAG(3) + OP(1,1)*SK_MX(3) - OP(1,17)*SK_MX(2) + OP(1,18)*SK_MX(5) - OP(1,19)*SK_MX(4));
Kfusion(2) = SK_MX(1)*(OP(2,20) + OP(2,2)*SH_MAG(1) - OP(2,3)*SH_MAG(2) + OP(2,4)*SH_MAG(3) + OP(2,1)*SK_MX(3) - OP(2,17)*SK_MX(2) + OP(2,18)*SK_MX(5) - OP(2,19)*SK_MX(4));
Kfusion(3) = SK_MX(1)*(OP(3,20) + OP(3,2)*SH_MAG(1) - OP(3,3)*SH_MAG(2) + OP(3,4)*SH_MAG(3) + OP(3,1)*SK_MX(3) - OP(3,17)*SK_MX(2) + OP(3,18)*SK_MX(5) - OP(3,19)*SK_MX(4));
Kfusion(4) = SK_MX(1)*(OP(4,20) + OP(4,2)*SH_MAG(1) - OP(4,3)*SH_MAG(2) + OP(4,4)*SH_MAG(3) + OP(4,1)*SK_MX(3) - OP(4,17)*SK_MX(2) + OP(4,18)*SK_MX(5) - OP(4,19)*SK_MX(4));
Kfusion(5) = SK_MX(1)*(OP(5,20) + OP(5,2)*SH_MAG(1) - OP(5,3)*SH_MAG(2) + OP(5,4)*SH_MAG(3) + OP(5,1)*SK_MX(3) - OP(5,17)*SK_MX(2) + OP(5,18)*SK_MX(5) - OP(5,19)*SK_MX(4));
Kfusion(6) = SK_MX(1)*(OP(6,20) + OP(6,2)*SH_MAG(1) - OP(6,3)*SH_MAG(2) + OP(6,4)*SH_MAG(3) + OP(6,1)*SK_MX(3) - OP(6,17)*SK_MX(2) + OP(6,18)*SK_MX(5) - OP(6,19)*SK_MX(4));
Kfusion(7) = SK_MX(1)*(OP(7,20) + OP(7,2)*SH_MAG(1) - OP(7,3)*SH_MAG(2) + OP(7,4)*SH_MAG(3) + OP(7,1)*SK_MX(3) - OP(7,17)*SK_MX(2) + OP(7,18)*SK_MX(5) - OP(7,19)*SK_MX(4));
Kfusion(8) = SK_MX(1)*(OP(8,20) + OP(8,2)*SH_MAG(1) - OP(8,3)*SH_MAG(2) + OP(8,4)*SH_MAG(3) + OP(8,1)*SK_MX(3) - OP(8,17)*SK_MX(2) + OP(8,18)*SK_MX(5) - OP(8,19)*SK_MX(4));
Kfusion(9) = SK_MX(1)*(OP(9,20) + OP(9,2)*SH_MAG(1) - OP(9,3)*SH_MAG(2) + OP(9,4)*SH_MAG(3) + OP(9,1)*SK_MX(3) - OP(9,17)*SK_MX(2) + OP(9,18)*SK_MX(5) - OP(9,19)*SK_MX(4));
Kfusion(10) = SK_MX(1)*(OP(10,20) + OP(10,2)*SH_MAG(1) - OP(10,3)*SH_MAG(2) + OP(10,4)*SH_MAG(3) + OP(10,1)*SK_MX(3) - OP(10,17)*SK_MX(2) + OP(10,18)*SK_MX(5) - OP(10,19)*SK_MX(4));
Kfusion(11) = SK_MX(1)*(OP(11,20) + OP(11,2)*SH_MAG(1) - OP(11,3)*SH_MAG(2) + OP(11,4)*SH_MAG(3) + OP(11,1)*SK_MX(3) - OP(11,17)*SK_MX(2) + OP(11,18)*SK_MX(5) - OP(11,19)*SK_MX(4));
Kfusion(12) = SK_MX(1)*(OP(12,20) + OP(12,2)*SH_MAG(1) - OP(12,3)*SH_MAG(2) + OP(12,4)*SH_MAG(3) + OP(12,1)*SK_MX(3) - OP(12,17)*SK_MX(2) + OP(12,18)*SK_MX(5) - OP(12,19)*SK_MX(4));
Kfusion(13) = SK_MX(1)*(OP(13,20) + OP(13,2)*SH_MAG(1) - OP(13,3)*SH_MAG(2) + OP(13,4)*SH_MAG(3) + OP(13,1)*SK_MX(3) - OP(13,17)*SK_MX(2) + OP(13,18)*SK_MX(5) - OP(13,19)*SK_MX(4));
Kfusion(14) = SK_MX(1)*(OP(14,20) + OP(14,2)*SH_MAG(1) - OP(14,3)*SH_MAG(2) + OP(14,4)*SH_MAG(3) + OP(14,1)*SK_MX(3) - OP(14,17)*SK_MX(2) + OP(14,18)*SK_MX(5) - OP(14,19)*SK_MX(4));
Kfusion(15) = SK_MX(1)*(OP(15,20) + OP(15,2)*SH_MAG(1) - OP(15,3)*SH_MAG(2) + OP(15,4)*SH_MAG(3) + OP(15,1)*SK_MX(3) - OP(15,17)*SK_MX(2) + OP(15,18)*SK_MX(5) - OP(15,19)*SK_MX(4));
Kfusion(16) = SK_MX(1)*(OP(16,20) + OP(16,2)*SH_MAG(1) - OP(16,3)*SH_MAG(2) + OP(16,4)*SH_MAG(3) + OP(16,1)*SK_MX(3) - OP(16,17)*SK_MX(2) + OP(16,18)*SK_MX(5) - OP(16,19)*SK_MX(4));
Kfusion(17) = SK_MX(1)*(OP(17,20) + OP(17,2)*SH_MAG(1) - OP(17,3)*SH_MAG(2) + OP(17,4)*SH_MAG(3) + OP(17,1)*SK_MX(3) - OP(17,17)*SK_MX(2) + OP(17,18)*SK_MX(5) - OP(17,19)*SK_MX(4));
Kfusion(18) = SK_MX(1)*(OP(18,20) + OP(18,2)*SH_MAG(1) - OP(18,3)*SH_MAG(2) + OP(18,4)*SH_MAG(3) + OP(18,1)*SK_MX(3) - OP(18,17)*SK_MX(2) + OP(18,18)*SK_MX(5) - OP(18,19)*SK_MX(4));
Kfusion(19) = SK_MX(1)*(OP(19,20) + OP(19,2)*SH_MAG(1) - OP(19,3)*SH_MAG(2) + OP(19,4)*SH_MAG(3) + OP(19,1)*SK_MX(3) - OP(19,17)*SK_MX(2) + OP(19,18)*SK_MX(5) - OP(19,19)*SK_MX(4));
Kfusion(20) = SK_MX(1)*(OP(20,20) + OP(20,2)*SH_MAG(1) - OP(20,3)*SH_MAG(2) + OP(20,4)*SH_MAG(3) + OP(20,1)*SK_MX(3) - OP(20,17)*SK_MX(2) + OP(20,18)*SK_MX(5) - OP(20,19)*SK_MX(4));
Kfusion(21) = SK_MX(1)*(OP(21,20) + OP(21,2)*SH_MAG(1) - OP(21,3)*SH_MAG(2) + OP(21,4)*SH_MAG(3) + OP(21,1)*SK_MX(3) - OP(21,17)*SK_MX(2) + OP(21,18)*SK_MX(5) - OP(21,19)*SK_MX(4));
Kfusion(22) = SK_MX(1)*(OP(22,20) + OP(22,2)*SH_MAG(1) - OP(22,3)*SH_MAG(2) + OP(22,4)*SH_MAG(3) + OP(22,1)*SK_MX(3) - OP(22,17)*SK_MX(2) + OP(22,18)*SK_MX(5) - OP(22,19)*SK_MX(4));
Kfusion(23) = SK_MX(1)*(OP(23,20) + OP(23,2)*SH_MAG(1) - OP(23,3)*SH_MAG(2) + OP(23,4)*SH_MAG(3) + OP(23,1)*SK_MX(3) - OP(23,17)*SK_MX(2) + OP(23,18)*SK_MX(5) - OP(23,19)*SK_MX(4));
Kfusion(24) = SK_MX(1)*(OP(24,20) + OP(24,2)*SH_MAG(1) - OP(24,3)*SH_MAG(2) + OP(24,4)*SH_MAG(3) + OP(24,1)*SK_MX(3) - OP(24,17)*SK_MX(2) + OP(24,18)*SK_MX(5) - OP(24,19)*SK_MX(4));
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
H_MAG = zeros(1,24);
EKF: update derivation files
2016-05-01 02:06:17 -03:00
H_MAG(1) = SH_MAG(3);
H_MAG(2) = SH_MAG(2);
H_MAG(3) = SH_MAG(1);
H_MAG(4) = 2*magD*q2 - SH_MAG(9) - SH_MAG(8);
H_MAG(17) = 2*q1*q2 - 2*q0*q3;
H_MAG(18) = SH_MAG(5) - SH_MAG(4) - SH_MAG(6) + SH_MAG(7);
H_MAG(19) = 2*q0*q1 + 2*q2*q3;
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
H_MAG(21) = 1;
EKF: update derivation files
2016-05-01 02:06:17 -03:00
SK_MY = zeros(5,1);
SK_MY(1) = 1/(OP(21,21) + R_MAG + OP(1,21)*SH_MAG(3) + OP(2,21)*SH_MAG(2) + OP(3,21)*SH_MAG(1) - OP(18,21)*(SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7)) - (2*q0*q3 - 2*q1*q2)*(OP(21,17) + OP(1,17)*SH_MAG(3) + OP(2,17)*SH_MAG(2) + OP(3,17)*SH_MAG(1) - OP(18,17)*(SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7)) - OP(17,17)*(2*q0*q3 - 2*q1*q2) + OP(19,17)*(2*q0*q1 + 2*q2*q3) - OP(4,17)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + (2*q0*q1 + 2*q2*q3)*(OP(21,19) + OP(1,19)*SH_MAG(3) + OP(2,19)*SH_MAG(2) + OP(3,19)*SH_MAG(1) - OP(18,19)*(SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7)) - OP(17,19)*(2*q0*q3 - 2*q1*q2) + OP(19,19)*(2*q0*q1 + 2*q2*q3) - OP(4,19)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) - (SH_MAG(8) + SH_MAG(9) - 2*magD*q2)*(OP(21,4) + OP(1,4)*SH_MAG(3) + OP(2,4)*SH_MAG(2) + OP(3,4)*SH_MAG(1) - OP(18,4)*(SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7)) - OP(17,4)*(2*q0*q3 - 2*q1*q2) + OP(19,4)*(2*q0*q1 + 2*q2*q3) - OP(4,4)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) - OP(17,21)*(2*q0*q3 - 2*q1*q2) + OP(19,21)*(2*q0*q1 + 2*q2*q3) + SH_MAG(3)*(OP(21,1) + OP(1,1)*SH_MAG(3) + OP(2,1)*SH_MAG(2) + OP(3,1)*SH_MAG(1) - OP(18,1)*(SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7)) - OP(17,1)*(2*q0*q3 - 2*q1*q2) + OP(19,1)*(2*q0*q1 + 2*q2*q3) - OP(4,1)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + SH_MAG(2)*(OP(21,2) + OP(1,2)*SH_MAG(3) + OP(2,2)*SH_MAG(2) + OP(3,2)*SH_MAG(1) - OP(18,2)*(SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7)) - OP(17,2)*(2*q0*q3 - 2*q1*q2) + OP(19,2)*(2*q0*q1 + 2*q2*q3) - OP(4,2)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + SH_MAG(1)*(OP(21,3) + OP(1,3)*SH_MAG(3) + OP(2,3)*SH_MAG(2) + OP(3,3)*SH_MAG(1) - OP(18,3)*(SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7)) - OP(17,3)*(2*q0*q3 - 2*q1*q2) + OP(19,3)*(2*q0*q1 + 2*q2*q3) - OP(4,3)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) - (SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7))*(OP(21,18) + OP(1,18)*SH_MAG(3) + OP(2,18)*SH_MAG(2) + OP(3,18)*SH_MAG(1) - OP(18,18)*(SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7)) - OP(17,18)*(2*q0*q3 - 2*q1*q2) + OP(19,18)*(2*q0*q1 + 2*q2*q3) - OP(4,18)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) - OP(4,21)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2));
SK_MY(2) = SH_MAG(4) - SH_MAG(5) + SH_MAG(6) - SH_MAG(7);
SK_MY(3) = SH_MAG(8) + SH_MAG(9) - 2*magD*q2;
SK_MY(4) = 2*q0*q3 - 2*q1*q2;
SK_MY(5) = 2*q0*q1 + 2*q2*q3;
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
Kfusion = zeros(24,1);
Kfusion = zeros(1,1);
EKF: update derivation files
2016-05-01 02:06:17 -03:00
Kfusion(1) = SK_MY(1)*(OP(1,21) + OP(1,1)*SH_MAG(3) + OP(1,2)*SH_MAG(2) + OP(1,3)*SH_MAG(1) - OP(1,4)*SK_MY(3) - OP(1,18)*SK_MY(2) - OP(1,17)*SK_MY(4) + OP(1,19)*SK_MY(5));
Kfusion(2) = SK_MY(1)*(OP(2,21) + OP(2,1)*SH_MAG(3) + OP(2,2)*SH_MAG(2) + OP(2,3)*SH_MAG(1) - OP(2,4)*SK_MY(3) - OP(2,18)*SK_MY(2) - OP(2,17)*SK_MY(4) + OP(2,19)*SK_MY(5));
Kfusion(3) = SK_MY(1)*(OP(3,21) + OP(3,1)*SH_MAG(3) + OP(3,2)*SH_MAG(2) + OP(3,3)*SH_MAG(1) - OP(3,4)*SK_MY(3) - OP(3,18)*SK_MY(2) - OP(3,17)*SK_MY(4) + OP(3,19)*SK_MY(5));
Kfusion(4) = SK_MY(1)*(OP(4,21) + OP(4,1)*SH_MAG(3) + OP(4,2)*SH_MAG(2) + OP(4,3)*SH_MAG(1) - OP(4,4)*SK_MY(3) - OP(4,18)*SK_MY(2) - OP(4,17)*SK_MY(4) + OP(4,19)*SK_MY(5));
Kfusion(5) = SK_MY(1)*(OP(5,21) + OP(5,1)*SH_MAG(3) + OP(5,2)*SH_MAG(2) + OP(5,3)*SH_MAG(1) - OP(5,4)*SK_MY(3) - OP(5,18)*SK_MY(2) - OP(5,17)*SK_MY(4) + OP(5,19)*SK_MY(5));
Kfusion(6) = SK_MY(1)*(OP(6,21) + OP(6,1)*SH_MAG(3) + OP(6,2)*SH_MAG(2) + OP(6,3)*SH_MAG(1) - OP(6,4)*SK_MY(3) - OP(6,18)*SK_MY(2) - OP(6,17)*SK_MY(4) + OP(6,19)*SK_MY(5));
Kfusion(7) = SK_MY(1)*(OP(7,21) + OP(7,1)*SH_MAG(3) + OP(7,2)*SH_MAG(2) + OP(7,3)*SH_MAG(1) - OP(7,4)*SK_MY(3) - OP(7,18)*SK_MY(2) - OP(7,17)*SK_MY(4) + OP(7,19)*SK_MY(5));
Kfusion(8) = SK_MY(1)*(OP(8,21) + OP(8,1)*SH_MAG(3) + OP(8,2)*SH_MAG(2) + OP(8,3)*SH_MAG(1) - OP(8,4)*SK_MY(3) - OP(8,18)*SK_MY(2) - OP(8,17)*SK_MY(4) + OP(8,19)*SK_MY(5));
Kfusion(9) = SK_MY(1)*(OP(9,21) + OP(9,1)*SH_MAG(3) + OP(9,2)*SH_MAG(2) + OP(9,3)*SH_MAG(1) - OP(9,4)*SK_MY(3) - OP(9,18)*SK_MY(2) - OP(9,17)*SK_MY(4) + OP(9,19)*SK_MY(5));
Kfusion(10) = SK_MY(1)*(OP(10,21) + OP(10,1)*SH_MAG(3) + OP(10,2)*SH_MAG(2) + OP(10,3)*SH_MAG(1) - OP(10,4)*SK_MY(3) - OP(10,18)*SK_MY(2) - OP(10,17)*SK_MY(4) + OP(10,19)*SK_MY(5));
Kfusion(11) = SK_MY(1)*(OP(11,21) + OP(11,1)*SH_MAG(3) + OP(11,2)*SH_MAG(2) + OP(11,3)*SH_MAG(1) - OP(11,4)*SK_MY(3) - OP(11,18)*SK_MY(2) - OP(11,17)*SK_MY(4) + OP(11,19)*SK_MY(5));
Kfusion(12) = SK_MY(1)*(OP(12,21) + OP(12,1)*SH_MAG(3) + OP(12,2)*SH_MAG(2) + OP(12,3)*SH_MAG(1) - OP(12,4)*SK_MY(3) - OP(12,18)*SK_MY(2) - OP(12,17)*SK_MY(4) + OP(12,19)*SK_MY(5));
Kfusion(13) = SK_MY(1)*(OP(13,21) + OP(13,1)*SH_MAG(3) + OP(13,2)*SH_MAG(2) + OP(13,3)*SH_MAG(1) - OP(13,4)*SK_MY(3) - OP(13,18)*SK_MY(2) - OP(13,17)*SK_MY(4) + OP(13,19)*SK_MY(5));
Kfusion(14) = SK_MY(1)*(OP(14,21) + OP(14,1)*SH_MAG(3) + OP(14,2)*SH_MAG(2) + OP(14,3)*SH_MAG(1) - OP(14,4)*SK_MY(3) - OP(14,18)*SK_MY(2) - OP(14,17)*SK_MY(4) + OP(14,19)*SK_MY(5));
Kfusion(15) = SK_MY(1)*(OP(15,21) + OP(15,1)*SH_MAG(3) + OP(15,2)*SH_MAG(2) + OP(15,3)*SH_MAG(1) - OP(15,4)*SK_MY(3) - OP(15,18)*SK_MY(2) - OP(15,17)*SK_MY(4) + OP(15,19)*SK_MY(5));
Kfusion(16) = SK_MY(1)*(OP(16,21) + OP(16,1)*SH_MAG(3) + OP(16,2)*SH_MAG(2) + OP(16,3)*SH_MAG(1) - OP(16,4)*SK_MY(3) - OP(16,18)*SK_MY(2) - OP(16,17)*SK_MY(4) + OP(16,19)*SK_MY(5));
Kfusion(17) = SK_MY(1)*(OP(17,21) + OP(17,1)*SH_MAG(3) + OP(17,2)*SH_MAG(2) + OP(17,3)*SH_MAG(1) - OP(17,4)*SK_MY(3) - OP(17,18)*SK_MY(2) - OP(17,17)*SK_MY(4) + OP(17,19)*SK_MY(5));
Kfusion(18) = SK_MY(1)*(OP(18,21) + OP(18,1)*SH_MAG(3) + OP(18,2)*SH_MAG(2) + OP(18,3)*SH_MAG(1) - OP(18,4)*SK_MY(3) - OP(18,18)*SK_MY(2) - OP(18,17)*SK_MY(4) + OP(18,19)*SK_MY(5));
Kfusion(19) = SK_MY(1)*(OP(19,21) + OP(19,1)*SH_MAG(3) + OP(19,2)*SH_MAG(2) + OP(19,3)*SH_MAG(1) - OP(19,4)*SK_MY(3) - OP(19,18)*SK_MY(2) - OP(19,17)*SK_MY(4) + OP(19,19)*SK_MY(5));
Kfusion(20) = SK_MY(1)*(OP(20,21) + OP(20,1)*SH_MAG(3) + OP(20,2)*SH_MAG(2) + OP(20,3)*SH_MAG(1) - OP(20,4)*SK_MY(3) - OP(20,18)*SK_MY(2) - OP(20,17)*SK_MY(4) + OP(20,19)*SK_MY(5));
Kfusion(21) = SK_MY(1)*(OP(21,21) + OP(21,1)*SH_MAG(3) + OP(21,2)*SH_MAG(2) + OP(21,3)*SH_MAG(1) - OP(21,4)*SK_MY(3) - OP(21,18)*SK_MY(2) - OP(21,17)*SK_MY(4) + OP(21,19)*SK_MY(5));
Kfusion(22) = SK_MY(1)*(OP(22,21) + OP(22,1)*SH_MAG(3) + OP(22,2)*SH_MAG(2) + OP(22,3)*SH_MAG(1) - OP(22,4)*SK_MY(3) - OP(22,18)*SK_MY(2) - OP(22,17)*SK_MY(4) + OP(22,19)*SK_MY(5));
Kfusion(23) = SK_MY(1)*(OP(23,21) + OP(23,1)*SH_MAG(3) + OP(23,2)*SH_MAG(2) + OP(23,3)*SH_MAG(1) - OP(23,4)*SK_MY(3) - OP(23,18)*SK_MY(2) - OP(23,17)*SK_MY(4) + OP(23,19)*SK_MY(5));
Kfusion(24) = SK_MY(1)*(OP(24,21) + OP(24,1)*SH_MAG(3) + OP(24,2)*SH_MAG(2) + OP(24,3)*SH_MAG(1) - OP(24,4)*SK_MY(3) - OP(24,18)*SK_MY(2) - OP(24,17)*SK_MY(4) + OP(24,19)*SK_MY(5));
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
H_MAG = zeros(1,24);
EKF: update derivation files
2016-05-01 02:06:17 -03:00
H_MAG(1) = SH_MAG(2);
H_MAG(2) = -SH_MAG(3);
H_MAG(3) = SH_MAG(8) + SH_MAG(9) - 2*magD*q2;
H_MAG(4) = SH_MAG(1);
H_MAG(17) = 2*q0*q2 + 2*q1*q3;
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
H_MAG(18) = 2*q2*q3 - 2*q0*q1;
EKF: update derivation files
2016-05-01 02:06:17 -03:00
H_MAG(19) = SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7);
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
H_MAG(22) = 1;
EKF: update derivation files
2016-05-01 02:06:17 -03:00
SK_MZ = zeros(5,1);
SK_MZ(1) = 1/(OP(22,22) + R_MAG + OP(1,22)*SH_MAG(2) - OP(2,22)*SH_MAG(3) + OP(4,22)*SH_MAG(1) + OP(19,22)*(SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7)) + (2*q0*q2 + 2*q1*q3)*(OP(22,17) + OP(1,17)*SH_MAG(2) - OP(2,17)*SH_MAG(3) + OP(4,17)*SH_MAG(1) + OP(19,17)*(SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7)) + OP(17,17)*(2*q0*q2 + 2*q1*q3) - OP(18,17)*(2*q0*q1 - 2*q2*q3) + OP(3,17)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) - (2*q0*q1 - 2*q2*q3)*(OP(22,18) + OP(1,18)*SH_MAG(2) - OP(2,18)*SH_MAG(3) + OP(4,18)*SH_MAG(1) + OP(19,18)*(SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7)) + OP(17,18)*(2*q0*q2 + 2*q1*q3) - OP(18,18)*(2*q0*q1 - 2*q2*q3) + OP(3,18)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + (SH_MAG(8) + SH_MAG(9) - 2*magD*q2)*(OP(22,3) + OP(1,3)*SH_MAG(2) - OP(2,3)*SH_MAG(3) + OP(4,3)*SH_MAG(1) + OP(19,3)*(SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7)) + OP(17,3)*(2*q0*q2 + 2*q1*q3) - OP(18,3)*(2*q0*q1 - 2*q2*q3) + OP(3,3)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + OP(17,22)*(2*q0*q2 + 2*q1*q3) - OP(18,22)*(2*q0*q1 - 2*q2*q3) + SH_MAG(2)*(OP(22,1) + OP(1,1)*SH_MAG(2) - OP(2,1)*SH_MAG(3) + OP(4,1)*SH_MAG(1) + OP(19,1)*(SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7)) + OP(17,1)*(2*q0*q2 + 2*q1*q3) - OP(18,1)*(2*q0*q1 - 2*q2*q3) + OP(3,1)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) - SH_MAG(3)*(OP(22,2) + OP(1,2)*SH_MAG(2) - OP(2,2)*SH_MAG(3) + OP(4,2)*SH_MAG(1) + OP(19,2)*(SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7)) + OP(17,2)*(2*q0*q2 + 2*q1*q3) - OP(18,2)*(2*q0*q1 - 2*q2*q3) + OP(3,2)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + SH_MAG(1)*(OP(22,4) + OP(1,4)*SH_MAG(2) - OP(2,4)*SH_MAG(3) + OP(4,4)*SH_MAG(1) + OP(19,4)*(SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7)) + OP(17,4)*(2*q0*q2 + 2*q1*q3) - OP(18,4)*(2*q0*q1 - 2*q2*q3) + OP(3,4)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + (SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7))*(OP(22,19) + OP(1,19)*SH_MAG(2) - OP(2,19)*SH_MAG(3) + OP(4,19)*SH_MAG(1) + OP(19,19)*(SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7)) + OP(17,19)*(2*q0*q2 + 2*q1*q3) - OP(18,19)*(2*q0*q1 - 2*q2*q3) + OP(3,19)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2)) + OP(3,22)*(SH_MAG(8) + SH_MAG(9) - 2*magD*q2));
SK_MZ(2) = SH_MAG(4) - SH_MAG(5) - SH_MAG(6) + SH_MAG(7);
SK_MZ(3) = SH_MAG(8) + SH_MAG(9) - 2*magD*q2;
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
SK_MZ(4) = 2*q0*q1 - 2*q2*q3;
EKF: update derivation files
2016-05-01 02:06:17 -03:00
SK_MZ(5) = 2*q0*q2 + 2*q1*q3;
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
Kfusion = zeros(24,1);
Kfusion = zeros(1,1);
EKF: update derivation files
2016-05-01 02:06:17 -03:00
Kfusion(1) = SK_MZ(1)*(OP(1,22) + OP(1,1)*SH_MAG(2) - OP(1,2)*SH_MAG(3) + OP(1,4)*SH_MAG(1) + OP(1,3)*SK_MZ(3) + OP(1,19)*SK_MZ(2) + OP(1,17)*SK_MZ(5) - OP(1,18)*SK_MZ(4));
Kfusion(2) = SK_MZ(1)*(OP(2,22) + OP(2,1)*SH_MAG(2) - OP(2,2)*SH_MAG(3) + OP(2,4)*SH_MAG(1) + OP(2,3)*SK_MZ(3) + OP(2,19)*SK_MZ(2) + OP(2,17)*SK_MZ(5) - OP(2,18)*SK_MZ(4));
Kfusion(3) = SK_MZ(1)*(OP(3,22) + OP(3,1)*SH_MAG(2) - OP(3,2)*SH_MAG(3) + OP(3,4)*SH_MAG(1) + OP(3,3)*SK_MZ(3) + OP(3,19)*SK_MZ(2) + OP(3,17)*SK_MZ(5) - OP(3,18)*SK_MZ(4));
Kfusion(4) = SK_MZ(1)*(OP(4,22) + OP(4,1)*SH_MAG(2) - OP(4,2)*SH_MAG(3) + OP(4,4)*SH_MAG(1) + OP(4,3)*SK_MZ(3) + OP(4,19)*SK_MZ(2) + OP(4,17)*SK_MZ(5) - OP(4,18)*SK_MZ(4));
Kfusion(5) = SK_MZ(1)*(OP(5,22) + OP(5,1)*SH_MAG(2) - OP(5,2)*SH_MAG(3) + OP(5,4)*SH_MAG(1) + OP(5,3)*SK_MZ(3) + OP(5,19)*SK_MZ(2) + OP(5,17)*SK_MZ(5) - OP(5,18)*SK_MZ(4));
Kfusion(6) = SK_MZ(1)*(OP(6,22) + OP(6,1)*SH_MAG(2) - OP(6,2)*SH_MAG(3) + OP(6,4)*SH_MAG(1) + OP(6,3)*SK_MZ(3) + OP(6,19)*SK_MZ(2) + OP(6,17)*SK_MZ(5) - OP(6,18)*SK_MZ(4));
Kfusion(7) = SK_MZ(1)*(OP(7,22) + OP(7,1)*SH_MAG(2) - OP(7,2)*SH_MAG(3) + OP(7,4)*SH_MAG(1) + OP(7,3)*SK_MZ(3) + OP(7,19)*SK_MZ(2) + OP(7,17)*SK_MZ(5) - OP(7,18)*SK_MZ(4));
Kfusion(8) = SK_MZ(1)*(OP(8,22) + OP(8,1)*SH_MAG(2) - OP(8,2)*SH_MAG(3) + OP(8,4)*SH_MAG(1) + OP(8,3)*SK_MZ(3) + OP(8,19)*SK_MZ(2) + OP(8,17)*SK_MZ(5) - OP(8,18)*SK_MZ(4));
Kfusion(9) = SK_MZ(1)*(OP(9,22) + OP(9,1)*SH_MAG(2) - OP(9,2)*SH_MAG(3) + OP(9,4)*SH_MAG(1) + OP(9,3)*SK_MZ(3) + OP(9,19)*SK_MZ(2) + OP(9,17)*SK_MZ(5) - OP(9,18)*SK_MZ(4));
Kfusion(10) = SK_MZ(1)*(OP(10,22) + OP(10,1)*SH_MAG(2) - OP(10,2)*SH_MAG(3) + OP(10,4)*SH_MAG(1) + OP(10,3)*SK_MZ(3) + OP(10,19)*SK_MZ(2) + OP(10,17)*SK_MZ(5) - OP(10,18)*SK_MZ(4));
Kfusion(11) = SK_MZ(1)*(OP(11,22) + OP(11,1)*SH_MAG(2) - OP(11,2)*SH_MAG(3) + OP(11,4)*SH_MAG(1) + OP(11,3)*SK_MZ(3) + OP(11,19)*SK_MZ(2) + OP(11,17)*SK_MZ(5) - OP(11,18)*SK_MZ(4));
Kfusion(12) = SK_MZ(1)*(OP(12,22) + OP(12,1)*SH_MAG(2) - OP(12,2)*SH_MAG(3) + OP(12,4)*SH_MAG(1) + OP(12,3)*SK_MZ(3) + OP(12,19)*SK_MZ(2) + OP(12,17)*SK_MZ(5) - OP(12,18)*SK_MZ(4));
Kfusion(13) = SK_MZ(1)*(OP(13,22) + OP(13,1)*SH_MAG(2) - OP(13,2)*SH_MAG(3) + OP(13,4)*SH_MAG(1) + OP(13,3)*SK_MZ(3) + OP(13,19)*SK_MZ(2) + OP(13,17)*SK_MZ(5) - OP(13,18)*SK_MZ(4));
Kfusion(14) = SK_MZ(1)*(OP(14,22) + OP(14,1)*SH_MAG(2) - OP(14,2)*SH_MAG(3) + OP(14,4)*SH_MAG(1) + OP(14,3)*SK_MZ(3) + OP(14,19)*SK_MZ(2) + OP(14,17)*SK_MZ(5) - OP(14,18)*SK_MZ(4));
Kfusion(15) = SK_MZ(1)*(OP(15,22) + OP(15,1)*SH_MAG(2) - OP(15,2)*SH_MAG(3) + OP(15,4)*SH_MAG(1) + OP(15,3)*SK_MZ(3) + OP(15,19)*SK_MZ(2) + OP(15,17)*SK_MZ(5) - OP(15,18)*SK_MZ(4));
Kfusion(16) = SK_MZ(1)*(OP(16,22) + OP(16,1)*SH_MAG(2) - OP(16,2)*SH_MAG(3) + OP(16,4)*SH_MAG(1) + OP(16,3)*SK_MZ(3) + OP(16,19)*SK_MZ(2) + OP(16,17)*SK_MZ(5) - OP(16,18)*SK_MZ(4));
Kfusion(17) = SK_MZ(1)*(OP(17,22) + OP(17,1)*SH_MAG(2) - OP(17,2)*SH_MAG(3) + OP(17,4)*SH_MAG(1) + OP(17,3)*SK_MZ(3) + OP(17,19)*SK_MZ(2) + OP(17,17)*SK_MZ(5) - OP(17,18)*SK_MZ(4));
Kfusion(18) = SK_MZ(1)*(OP(18,22) + OP(18,1)*SH_MAG(2) - OP(18,2)*SH_MAG(3) + OP(18,4)*SH_MAG(1) + OP(18,3)*SK_MZ(3) + OP(18,19)*SK_MZ(2) + OP(18,17)*SK_MZ(5) - OP(18,18)*SK_MZ(4));
Kfusion(19) = SK_MZ(1)*(OP(19,22) + OP(19,1)*SH_MAG(2) - OP(19,2)*SH_MAG(3) + OP(19,4)*SH_MAG(1) + OP(19,3)*SK_MZ(3) + OP(19,19)*SK_MZ(2) + OP(19,17)*SK_MZ(5) - OP(19,18)*SK_MZ(4));
Kfusion(20) = SK_MZ(1)*(OP(20,22) + OP(20,1)*SH_MAG(2) - OP(20,2)*SH_MAG(3) + OP(20,4)*SH_MAG(1) + OP(20,3)*SK_MZ(3) + OP(20,19)*SK_MZ(2) + OP(20,17)*SK_MZ(5) - OP(20,18)*SK_MZ(4));
Kfusion(21) = SK_MZ(1)*(OP(21,22) + OP(21,1)*SH_MAG(2) - OP(21,2)*SH_MAG(3) + OP(21,4)*SH_MAG(1) + OP(21,3)*SK_MZ(3) + OP(21,19)*SK_MZ(2) + OP(21,17)*SK_MZ(5) - OP(21,18)*SK_MZ(4));
Kfusion(22) = SK_MZ(1)*(OP(22,22) + OP(22,1)*SH_MAG(2) - OP(22,2)*SH_MAG(3) + OP(22,4)*SH_MAG(1) + OP(22,3)*SK_MZ(3) + OP(22,19)*SK_MZ(2) + OP(22,17)*SK_MZ(5) - OP(22,18)*SK_MZ(4));
Kfusion(23) = SK_MZ(1)*(OP(23,22) + OP(23,1)*SH_MAG(2) - OP(23,2)*SH_MAG(3) + OP(23,4)*SH_MAG(1) + OP(23,3)*SK_MZ(3) + OP(23,19)*SK_MZ(2) + OP(23,17)*SK_MZ(5) - OP(23,18)*SK_MZ(4));
Kfusion(24) = SK_MZ(1)*(OP(24,22) + OP(24,1)*SH_MAG(2) - OP(24,2)*SH_MAG(3) + OP(24,4)*SH_MAG(1) + OP(24,3)*SK_MZ(3) + OP(24,19)*SK_MZ(2) + OP(24,17)*SK_MZ(5) - OP(24,18)*SK_MZ(4));
SH_ACCX = zeros(4,1);
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
SH_ACCX(1) = q0^2 + q1^2 - q2^2 - q3^2;
EKF: update derivation files
2016-05-01 02:06:17 -03:00
SH_ACCX(2) = vn - vwn;
SH_ACCX(3) = ve - vwe;
SH_ACCX(4) = 2*q0*q3 + 2*q1*q2;
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
H_ACCX = zeros(1,24);
EKF: update derivation files
2016-05-01 02:06:17 -03:00
H_ACCX(1,1) = -Kaccx*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd);
H_ACCX(1,2) = -Kaccx*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd);
H_ACCX(1,3) = Kaccx*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd);
H_ACCX(1,4) = -Kaccx*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd);
H_ACCX(1,5) = -Kaccx*SH_ACCX(1);
H_ACCX(1,6) = -Kaccx*SH_ACCX(4);
H_ACCX(1,7) = Kaccx*(2*q0*q2 - 2*q1*q3);
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
H_ACCX(1,23) = Kaccx*SH_ACCX(1);
EKF: update derivation files
2016-05-01 02:06:17 -03:00
H_ACCX(1,24) = Kaccx*SH_ACCX(4);
SK_ACCX = zeros(7,1);
SK_ACCX(1) = 1/(R_ACC + Kaccx*SH_ACCX(1)*(Kaccx*OP(5,5)*SH_ACCX(1) + Kaccx*OP(6,5)*SH_ACCX(4) - Kaccx*OP(23,5)*SH_ACCX(1) - Kaccx*OP(24,5)*SH_ACCX(4) - Kaccx*OP(7,5)*(2*q0*q2 - 2*q1*q3) + Kaccx*OP(1,5)*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd) + Kaccx*OP(2,5)*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd) - Kaccx*OP(3,5)*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd) + Kaccx*OP(4,5)*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd)) + Kaccx*SH_ACCX(4)*(Kaccx*OP(5,6)*SH_ACCX(1) + Kaccx*OP(6,6)*SH_ACCX(4) - Kaccx*OP(23,6)*SH_ACCX(1) - Kaccx*OP(24,6)*SH_ACCX(4) - Kaccx*OP(7,6)*(2*q0*q2 - 2*q1*q3) + Kaccx*OP(1,6)*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd) + Kaccx*OP(2,6)*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd) - Kaccx*OP(3,6)*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd) + Kaccx*OP(4,6)*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd)) - Kaccx*SH_ACCX(1)*(Kaccx*OP(5,23)*SH_ACCX(1) + Kaccx*OP(6,23)*SH_ACCX(4) - Kaccx*OP(23,23)*SH_ACCX(1) - Kaccx*OP(24,23)*SH_ACCX(4) - Kaccx*OP(7,23)*(2*q0*q2 - 2*q1*q3) + Kaccx*OP(1,23)*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd) + Kaccx*OP(2,23)*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd) - Kaccx*OP(3,23)*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd) + Kaccx*OP(4,23)*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd)) - Kaccx*SH_ACCX(4)*(Kaccx*OP(5,24)*SH_ACCX(1) + Kaccx*OP(6,24)*SH_ACCX(4) - Kaccx*OP(23,24)*SH_ACCX(1) - Kaccx*OP(24,24)*SH_ACCX(4) - Kaccx*OP(7,24)*(2*q0*q2 - 2*q1*q3) + Kaccx*OP(1,24)*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd) + Kaccx*OP(2,24)*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd) - Kaccx*OP(3,24)*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd) + Kaccx*OP(4,24)*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd)) - Kaccx*(2*q0*q2 - 2*q1*q3)*(Kaccx*OP(5,7)*SH_ACCX(1) + Kaccx*OP(6,7)*SH_ACCX(4) - Kaccx*OP(23,7)*SH_ACCX(1) - Kaccx*OP(24,7)*SH_ACCX(4) - Kaccx*OP(7,7)*(2*q0*q2 - 2*q1*q3) + Kaccx*OP(1,7)*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd) + Kaccx*OP(2,7)*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd) - Kaccx*OP(3,7)*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd) + Kaccx*OP(4,7)*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd)) + Kaccx*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd)*(Kaccx*OP(5,1)*SH_ACCX(1) + Kaccx*OP(6,1)*SH_ACCX(4) - Kaccx*OP(23,1)*SH_ACCX(1) - Kaccx*OP(24,1)*SH_ACCX(4) - Kaccx*OP(7,1)*(2*q0*q2 - 2*q1*q3) + Kaccx*OP(1,1)*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd) + Kaccx*OP(2,1)*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd) - Kaccx*OP(3,1)*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd) + Kaccx*OP(4,1)*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd)) + Kaccx*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd)*(Kaccx*OP(5,2)*SH_ACCX(1) + Kaccx*OP(6,2)*SH_ACCX(4) - Kaccx*OP(23,2)*SH_ACCX(1) - Kaccx*OP(24,2)*SH_ACCX(4) - Kaccx*OP(7,2)*(2*q0*q2 - 2*q1*q3) + Kaccx*OP(1,2)*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd) + Kaccx*OP(2,2)*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd) - Kaccx*OP(3,2)*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd) + Kaccx*OP(4,2)*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd)) - Kaccx*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd)*(Kaccx*OP(5,3)*SH_ACCX(1) + Kaccx*OP(6,3)*SH_ACCX(4) - Kaccx*OP(23,3)*SH_ACCX(1) - Kaccx*OP(24,3)*SH_ACCX(4) - Kaccx*OP(7,3)*(2*q0*q2 - 2*q1*q3) + Kaccx*OP(1,3)*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd) + Kaccx*OP(2,3)*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd) - Kaccx*OP(3,3)*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd) + Kaccx*OP(4,3)*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd)) + Kaccx*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd)*(Kaccx*OP(5,4)*SH_ACCX(1) + Kaccx*OP(6,4)*SH_ACCX(4) - Kaccx*OP(23,4)*SH_ACCX(1) - Kaccx*OP(24,4)*SH_ACCX(4) - Kaccx*OP(7,4)*(2*q0*q2 - 2*q1*q3) + Kaccx*OP(1,4)*(2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd) + Kaccx*OP(2,4)*(2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd) - Kaccx*OP(3,4)*(2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd) + Kaccx*OP(4,4)*(2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd)));
SK_ACCX(2) = 2*q0*SH_ACCX(3) - 2*q3*SH_ACCX(2) + 2*q1*vd;
SK_ACCX(3) = 2*q2*SH_ACCX(2) - 2*q1*SH_ACCX(3) + 2*q0*vd;
SK_ACCX(4) = 2*q0*SH_ACCX(2) + 2*q3*SH_ACCX(3) - 2*q2*vd;
SK_ACCX(5) = 2*q1*SH_ACCX(2) + 2*q2*SH_ACCX(3) + 2*q3*vd;
SK_ACCX(6) = 2*q0*q2 - 2*q1*q3;
SK_ACCX(7) = SH_ACCX(4);
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
Kfusion = zeros(24,1);
Kfusion = zeros(1,1);
EKF: update derivation files
2016-05-01 02:06:17 -03:00
Kfusion(1) = -SK_ACCX(1)*(Kaccx*OP(1,5)*SH_ACCX(1) - Kaccx*OP(1,23)*SH_ACCX(1) + Kaccx*OP(1,1)*SK_ACCX(4) - Kaccx*OP(1,3)*SK_ACCX(3) + Kaccx*OP(1,4)*SK_ACCX(2) + Kaccx*OP(1,2)*SK_ACCX(5) + Kaccx*OP(1,6)*SK_ACCX(7) - Kaccx*OP(1,7)*SK_ACCX(6) - Kaccx*OP(1,24)*SK_ACCX(7));
Kfusion(2) = -SK_ACCX(1)*(Kaccx*OP(2,5)*SH_ACCX(1) - Kaccx*OP(2,23)*SH_ACCX(1) + Kaccx*OP(2,1)*SK_ACCX(4) - Kaccx*OP(2,3)*SK_ACCX(3) + Kaccx*OP(2,4)*SK_ACCX(2) + Kaccx*OP(2,2)*SK_ACCX(5) + Kaccx*OP(2,6)*SK_ACCX(7) - Kaccx*OP(2,7)*SK_ACCX(6) - Kaccx*OP(2,24)*SK_ACCX(7));
Kfusion(3) = -SK_ACCX(1)*(Kaccx*OP(3,5)*SH_ACCX(1) - Kaccx*OP(3,23)*SH_ACCX(1) + Kaccx*OP(3,1)*SK_ACCX(4) - Kaccx*OP(3,3)*SK_ACCX(3) + Kaccx*OP(3,4)*SK_ACCX(2) + Kaccx*OP(3,2)*SK_ACCX(5) + Kaccx*OP(3,6)*SK_ACCX(7) - Kaccx*OP(3,7)*SK_ACCX(6) - Kaccx*OP(3,24)*SK_ACCX(7));
Kfusion(4) = -SK_ACCX(1)*(Kaccx*OP(4,5)*SH_ACCX(1) - Kaccx*OP(4,23)*SH_ACCX(1) + Kaccx*OP(4,1)*SK_ACCX(4) - Kaccx*OP(4,3)*SK_ACCX(3) + Kaccx*OP(4,4)*SK_ACCX(2) + Kaccx*OP(4,2)*SK_ACCX(5) + Kaccx*OP(4,6)*SK_ACCX(7) - Kaccx*OP(4,7)*SK_ACCX(6) - Kaccx*OP(4,24)*SK_ACCX(7));
Kfusion(5) = -SK_ACCX(1)*(Kaccx*OP(5,5)*SH_ACCX(1) - Kaccx*OP(5,23)*SH_ACCX(1) + Kaccx*OP(5,1)*SK_ACCX(4) - Kaccx*OP(5,3)*SK_ACCX(3) + Kaccx*OP(5,4)*SK_ACCX(2) + Kaccx*OP(5,2)*SK_ACCX(5) + Kaccx*OP(5,6)*SK_ACCX(7) - Kaccx*OP(5,7)*SK_ACCX(6) - Kaccx*OP(5,24)*SK_ACCX(7));
Kfusion(6) = -SK_ACCX(1)*(Kaccx*OP(6,5)*SH_ACCX(1) - Kaccx*OP(6,23)*SH_ACCX(1) + Kaccx*OP(6,1)*SK_ACCX(4) - Kaccx*OP(6,3)*SK_ACCX(3) + Kaccx*OP(6,4)*SK_ACCX(2) + Kaccx*OP(6,2)*SK_ACCX(5) + Kaccx*OP(6,6)*SK_ACCX(7) - Kaccx*OP(6,7)*SK_ACCX(6) - Kaccx*OP(6,24)*SK_ACCX(7));
Kfusion(7) = -SK_ACCX(1)*(Kaccx*OP(7,5)*SH_ACCX(1) - Kaccx*OP(7,23)*SH_ACCX(1) + Kaccx*OP(7,1)*SK_ACCX(4) - Kaccx*OP(7,3)*SK_ACCX(3) + Kaccx*OP(7,4)*SK_ACCX(2) + Kaccx*OP(7,2)*SK_ACCX(5) + Kaccx*OP(7,6)*SK_ACCX(7) - Kaccx*OP(7,7)*SK_ACCX(6) - Kaccx*OP(7,24)*SK_ACCX(7));
Kfusion(8) = -SK_ACCX(1)*(Kaccx*OP(8,5)*SH_ACCX(1) - Kaccx*OP(8,23)*SH_ACCX(1) + Kaccx*OP(8,1)*SK_ACCX(4) - Kaccx*OP(8,3)*SK_ACCX(3) + Kaccx*OP(8,4)*SK_ACCX(2) + Kaccx*OP(8,2)*SK_ACCX(5) + Kaccx*OP(8,6)*SK_ACCX(7) - Kaccx*OP(8,7)*SK_ACCX(6) - Kaccx*OP(8,24)*SK_ACCX(7));
Kfusion(9) = -SK_ACCX(1)*(Kaccx*OP(9,5)*SH_ACCX(1) - Kaccx*OP(9,23)*SH_ACCX(1) + Kaccx*OP(9,1)*SK_ACCX(4) - Kaccx*OP(9,3)*SK_ACCX(3) + Kaccx*OP(9,4)*SK_ACCX(2) + Kaccx*OP(9,2)*SK_ACCX(5) + Kaccx*OP(9,6)*SK_ACCX(7) - Kaccx*OP(9,7)*SK_ACCX(6) - Kaccx*OP(9,24)*SK_ACCX(7));
Kfusion(10) = -SK_ACCX(1)*(Kaccx*OP(10,5)*SH_ACCX(1) - Kaccx*OP(10,23)*SH_ACCX(1) + Kaccx*OP(10,1)*SK_ACCX(4) - Kaccx*OP(10,3)*SK_ACCX(3) + Kaccx*OP(10,4)*SK_ACCX(2) + Kaccx*OP(10,2)*SK_ACCX(5) + Kaccx*OP(10,6)*SK_ACCX(7) - Kaccx*OP(10,7)*SK_ACCX(6) - Kaccx*OP(10,24)*SK_ACCX(7));
Kfusion(11) = -SK_ACCX(1)*(Kaccx*OP(11,5)*SH_ACCX(1) - Kaccx*OP(11,23)*SH_ACCX(1) + Kaccx*OP(11,1)*SK_ACCX(4) - Kaccx*OP(11,3)*SK_ACCX(3) + Kaccx*OP(11,4)*SK_ACCX(2) + Kaccx*OP(11,2)*SK_ACCX(5) + Kaccx*OP(11,6)*SK_ACCX(7) - Kaccx*OP(11,7)*SK_ACCX(6) - Kaccx*OP(11,24)*SK_ACCX(7));
Kfusion(12) = -SK_ACCX(1)*(Kaccx*OP(12,5)*SH_ACCX(1) - Kaccx*OP(12,23)*SH_ACCX(1) + Kaccx*OP(12,1)*SK_ACCX(4) - Kaccx*OP(12,3)*SK_ACCX(3) + Kaccx*OP(12,4)*SK_ACCX(2) + Kaccx*OP(12,2)*SK_ACCX(5) + Kaccx*OP(12,6)*SK_ACCX(7) - Kaccx*OP(12,7)*SK_ACCX(6) - Kaccx*OP(12,24)*SK_ACCX(7));
Kfusion(13) = -SK_ACCX(1)*(Kaccx*OP(13,5)*SH_ACCX(1) - Kaccx*OP(13,23)*SH_ACCX(1) + Kaccx*OP(13,1)*SK_ACCX(4) - Kaccx*OP(13,3)*SK_ACCX(3) + Kaccx*OP(13,4)*SK_ACCX(2) + Kaccx*OP(13,2)*SK_ACCX(5) + Kaccx*OP(13,6)*SK_ACCX(7) - Kaccx*OP(13,7)*SK_ACCX(6) - Kaccx*OP(13,24)*SK_ACCX(7));
Kfusion(14) = -SK_ACCX(1)*(Kaccx*OP(14,5)*SH_ACCX(1) - Kaccx*OP(14,23)*SH_ACCX(1) + Kaccx*OP(14,1)*SK_ACCX(4) - Kaccx*OP(14,3)*SK_ACCX(3) + Kaccx*OP(14,4)*SK_ACCX(2) + Kaccx*OP(14,2)*SK_ACCX(5) + Kaccx*OP(14,6)*SK_ACCX(7) - Kaccx*OP(14,7)*SK_ACCX(6) - Kaccx*OP(14,24)*SK_ACCX(7));
Kfusion(15) = -SK_ACCX(1)*(Kaccx*OP(15,5)*SH_ACCX(1) - Kaccx*OP(15,23)*SH_ACCX(1) + Kaccx*OP(15,1)*SK_ACCX(4) - Kaccx*OP(15,3)*SK_ACCX(3) + Kaccx*OP(15,4)*SK_ACCX(2) + Kaccx*OP(15,2)*SK_ACCX(5) + Kaccx*OP(15,6)*SK_ACCX(7) - Kaccx*OP(15,7)*SK_ACCX(6) - Kaccx*OP(15,24)*SK_ACCX(7));
Kfusion(16) = -SK_ACCX(1)*(Kaccx*OP(16,5)*SH_ACCX(1) - Kaccx*OP(16,23)*SH_ACCX(1) + Kaccx*OP(16,1)*SK_ACCX(4) - Kaccx*OP(16,3)*SK_ACCX(3) + Kaccx*OP(16,4)*SK_ACCX(2) + Kaccx*OP(16,2)*SK_ACCX(5) + Kaccx*OP(16,6)*SK_ACCX(7) - Kaccx*OP(16,7)*SK_ACCX(6) - Kaccx*OP(16,24)*SK_ACCX(7));
Kfusion(17) = -SK_ACCX(1)*(Kaccx*OP(17,5)*SH_ACCX(1) - Kaccx*OP(17,23)*SH_ACCX(1) + Kaccx*OP(17,1)*SK_ACCX(4) - Kaccx*OP(17,3)*SK_ACCX(3) + Kaccx*OP(17,4)*SK_ACCX(2) + Kaccx*OP(17,2)*SK_ACCX(5) + Kaccx*OP(17,6)*SK_ACCX(7) - Kaccx*OP(17,7)*SK_ACCX(6) - Kaccx*OP(17,24)*SK_ACCX(7));
Kfusion(18) = -SK_ACCX(1)*(Kaccx*OP(18,5)*SH_ACCX(1) - Kaccx*OP(18,23)*SH_ACCX(1) + Kaccx*OP(18,1)*SK_ACCX(4) - Kaccx*OP(18,3)*SK_ACCX(3) + Kaccx*OP(18,4)*SK_ACCX(2) + Kaccx*OP(18,2)*SK_ACCX(5) + Kaccx*OP(18,6)*SK_ACCX(7) - Kaccx*OP(18,7)*SK_ACCX(6) - Kaccx*OP(18,24)*SK_ACCX(7));
Kfusion(19) = -SK_ACCX(1)*(Kaccx*OP(19,5)*SH_ACCX(1) - Kaccx*OP(19,23)*SH_ACCX(1) + Kaccx*OP(19,1)*SK_ACCX(4) - Kaccx*OP(19,3)*SK_ACCX(3) + Kaccx*OP(19,4)*SK_ACCX(2) + Kaccx*OP(19,2)*SK_ACCX(5) + Kaccx*OP(19,6)*SK_ACCX(7) - Kaccx*OP(19,7)*SK_ACCX(6) - Kaccx*OP(19,24)*SK_ACCX(7));
Kfusion(20) = -SK_ACCX(1)*(Kaccx*OP(20,5)*SH_ACCX(1) - Kaccx*OP(20,23)*SH_ACCX(1) + Kaccx*OP(20,1)*SK_ACCX(4) - Kaccx*OP(20,3)*SK_ACCX(3) + Kaccx*OP(20,4)*SK_ACCX(2) + Kaccx*OP(20,2)*SK_ACCX(5) + Kaccx*OP(20,6)*SK_ACCX(7) - Kaccx*OP(20,7)*SK_ACCX(6) - Kaccx*OP(20,24)*SK_ACCX(7));
Kfusion(21) = -SK_ACCX(1)*(Kaccx*OP(21,5)*SH_ACCX(1) - Kaccx*OP(21,23)*SH_ACCX(1) + Kaccx*OP(21,1)*SK_ACCX(4) - Kaccx*OP(21,3)*SK_ACCX(3) + Kaccx*OP(21,4)*SK_ACCX(2) + Kaccx*OP(21,2)*SK_ACCX(5) + Kaccx*OP(21,6)*SK_ACCX(7) - Kaccx*OP(21,7)*SK_ACCX(6) - Kaccx*OP(21,24)*SK_ACCX(7));
Kfusion(22) = -SK_ACCX(1)*(Kaccx*OP(22,5)*SH_ACCX(1) - Kaccx*OP(22,23)*SH_ACCX(1) + Kaccx*OP(22,1)*SK_ACCX(4) - Kaccx*OP(22,3)*SK_ACCX(3) + Kaccx*OP(22,4)*SK_ACCX(2) + Kaccx*OP(22,2)*SK_ACCX(5) + Kaccx*OP(22,6)*SK_ACCX(7) - Kaccx*OP(22,7)*SK_ACCX(6) - Kaccx*OP(22,24)*SK_ACCX(7));
Kfusion(23) = -SK_ACCX(1)*(Kaccx*OP(23,5)*SH_ACCX(1) - Kaccx*OP(23,23)*SH_ACCX(1) + Kaccx*OP(23,1)*SK_ACCX(4) - Kaccx*OP(23,3)*SK_ACCX(3) + Kaccx*OP(23,4)*SK_ACCX(2) + Kaccx*OP(23,2)*SK_ACCX(5) + Kaccx*OP(23,6)*SK_ACCX(7) - Kaccx*OP(23,7)*SK_ACCX(6) - Kaccx*OP(23,24)*SK_ACCX(7));
Kfusion(24) = -SK_ACCX(1)*(Kaccx*OP(24,5)*SH_ACCX(1) - Kaccx*OP(24,23)*SH_ACCX(1) + Kaccx*OP(24,1)*SK_ACCX(4) - Kaccx*OP(24,3)*SK_ACCX(3) + Kaccx*OP(24,4)*SK_ACCX(2) + Kaccx*OP(24,2)*SK_ACCX(5) + Kaccx*OP(24,6)*SK_ACCX(7) - Kaccx*OP(24,7)*SK_ACCX(6) - Kaccx*OP(24,24)*SK_ACCX(7));
SH_ACCY = zeros(3,1);
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
SH_ACCY(1) = q0^2 - q1^2 + q2^2 - q3^2;
EKF: update derivation files
2016-05-01 02:06:17 -03:00
SH_ACCY(2) = vn - vwn;
SH_ACCY(3) = ve - vwe;
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
H_ACCY = zeros(1,24);
EKF: update derivation files
2016-05-01 02:06:17 -03:00
H_ACCY(1,1) = -Kaccy*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd);
H_ACCY(1,2) = -Kaccy*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd);
H_ACCY(1,3) = -Kaccy*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd);
H_ACCY(1,4) = Kaccy*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd);
H_ACCY(1,5) = Kaccy*(2*q0*q3 - 2*q1*q2);
H_ACCY(1,6) = -Kaccy*SH_ACCY(1);
H_ACCY(1,7) = -Kaccy*(2*q0*q1 + 2*q2*q3);
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
H_ACCY(1,23) = -2*Kaccy*(q0*q3 - q1*q2);
H_ACCY(1,24) = Kaccy*SH_ACCY(1);
EKF: update derivation files
2016-05-01 02:06:17 -03:00
SK_ACCY = zeros(9,1);
SK_ACCY(1) = 1/(R_ACC + Kaccy*SH_ACCY(1)*(Kaccy*OP(6,6)*SH_ACCY(1) - Kaccy*OP(24,6)*SH_ACCY(1) - Kaccy*OP(5,6)*(2*q0*q3 - 2*q1*q2) + Kaccy*OP(7,6)*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*OP(23,6)*(q0*q3 - q1*q2) + Kaccy*OP(1,6)*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd) + Kaccy*OP(2,6)*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd) + Kaccy*OP(3,6)*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd) - Kaccy*OP(4,6)*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd)) - Kaccy*SH_ACCY(1)*(Kaccy*OP(6,24)*SH_ACCY(1) - Kaccy*OP(24,24)*SH_ACCY(1) - Kaccy*OP(5,24)*(2*q0*q3 - 2*q1*q2) + Kaccy*OP(7,24)*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*OP(23,24)*(q0*q3 - q1*q2) + Kaccy*OP(1,24)*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd) + Kaccy*OP(2,24)*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd) + Kaccy*OP(3,24)*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd) - Kaccy*OP(4,24)*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd)) - Kaccy*(2*q0*q3 - 2*q1*q2)*(Kaccy*OP(6,5)*SH_ACCY(1) - Kaccy*OP(24,5)*SH_ACCY(1) - Kaccy*OP(5,5)*(2*q0*q3 - 2*q1*q2) + Kaccy*OP(7,5)*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*OP(23,5)*(q0*q3 - q1*q2) + Kaccy*OP(1,5)*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd) + Kaccy*OP(2,5)*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd) + Kaccy*OP(3,5)*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd) - Kaccy*OP(4,5)*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd)) + Kaccy*(2*q0*q1 + 2*q2*q3)*(Kaccy*OP(6,7)*SH_ACCY(1) - Kaccy*OP(24,7)*SH_ACCY(1) - Kaccy*OP(5,7)*(2*q0*q3 - 2*q1*q2) + Kaccy*OP(7,7)*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*OP(23,7)*(q0*q3 - q1*q2) + Kaccy*OP(1,7)*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd) + Kaccy*OP(2,7)*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd) + Kaccy*OP(3,7)*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd) - Kaccy*OP(4,7)*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd)) + 2*Kaccy*(q0*q3 - q1*q2)*(Kaccy*OP(6,23)*SH_ACCY(1) - Kaccy*OP(24,23)*SH_ACCY(1) - Kaccy*OP(5,23)*(2*q0*q3 - 2*q1*q2) + Kaccy*OP(7,23)*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*OP(23,23)*(q0*q3 - q1*q2) + Kaccy*OP(1,23)*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd) + Kaccy*OP(2,23)*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd) + Kaccy*OP(3,23)*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd) - Kaccy*OP(4,23)*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd)) + Kaccy*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd)*(Kaccy*OP(6,1)*SH_ACCY(1) - Kaccy*OP(24,1)*SH_ACCY(1) - Kaccy*OP(5,1)*(2*q0*q3 - 2*q1*q2) + Kaccy*OP(7,1)*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*OP(23,1)*(q0*q3 - q1*q2) + Kaccy*OP(1,1)*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd) + Kaccy*OP(2,1)*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd) + Kaccy*OP(3,1)*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd) - Kaccy*OP(4,1)*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd)) + Kaccy*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd)*(Kaccy*OP(6,2)*SH_ACCY(1) - Kaccy*OP(24,2)*SH_ACCY(1) - Kaccy*OP(5,2)*(2*q0*q3 - 2*q1*q2) + Kaccy*OP(7,2)*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*OP(23,2)*(q0*q3 - q1*q2) + Kaccy*OP(1,2)*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd) + Kaccy*OP(2,2)*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd) + Kaccy*OP(3,2)*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd) - Kaccy*OP(4,2)*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd)) + Kaccy*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd)*(Kaccy*OP(6,3)*SH_ACCY(1) - Kaccy*OP(24,3)*SH_ACCY(1) - Kaccy*OP(5,3)*(2*q0*q3 - 2*q1*q2) + Kaccy*OP(7,3)*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*OP(23,3)*(q0*q3 - q1*q2) + Kaccy*OP(1,3)*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd) + Kaccy*OP(2,3)*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd) + Kaccy*OP(3,3)*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd) - Kaccy*OP(4,3)*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd)) - Kaccy*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd)*(Kaccy*OP(6,4)*SH_ACCY(1) - Kaccy*OP(24,4)*SH_ACCY(1) - Kaccy*OP(5,4)*(2*q0*q3 - 2*q1*q2) + Kaccy*OP(7,4)*(2*q0*q1 + 2*q2*q3) + 2*Kaccy*OP(23,4)*(q0*q3 - q1*q2) + Kaccy*OP(1,4)*(2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd) + Kaccy*OP(2,4)*(2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd) + Kaccy*OP(3,4)*(2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd) - Kaccy*OP(4,4)*(2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd)));
SK_ACCY(2) = 2*q0*SH_ACCY(2) + 2*q3*SH_ACCY(3) - 2*q2*vd;
SK_ACCY(3) = 2*q2*SH_ACCY(2) - 2*q1*SH_ACCY(3) + 2*q0*vd;
SK_ACCY(4) = 2*q0*SH_ACCY(3) - 2*q3*SH_ACCY(2) + 2*q1*vd;
SK_ACCY(5) = 2*q1*SH_ACCY(2) + 2*q2*SH_ACCY(3) + 2*q3*vd;
SK_ACCY(6) = 2*q0*q3 - 2*q1*q2;
SK_ACCY(7) = q0*q3 - q1*q2;
SK_ACCY(8) = 2*q0*q1 + 2*q2*q3;
SK_ACCY(9) = SH_ACCY(1);
EKF: Include all output files generated by derivation script
2016-04-04 14:51:28 -03:00
Kfusion = zeros(24,1);
Kfusion = zeros(1,1);
EKF: update derivation files
2016-05-01 02:06:17 -03:00
Kfusion(1) = -SK_ACCY(1)*(Kaccy*OP(1,1)*SK_ACCY(4) + Kaccy*OP(1,2)*SK_ACCY(3) - Kaccy*OP(1,4)*SK_ACCY(2) + Kaccy*OP(1,3)*SK_ACCY(5) - Kaccy*OP(1,5)*SK_ACCY(6) + Kaccy*OP(1,6)*SK_ACCY(9) + Kaccy*OP(1,7)*SK_ACCY(8) + 2*Kaccy*OP(1,23)*SK_ACCY(7) - Kaccy*OP(1,24)*SK_ACCY(9));
Kfusion(2) = -SK_ACCY(1)*(Kaccy*OP(2,1)*SK_ACCY(4) + Kaccy*OP(2,2)*SK_ACCY(3) - Kaccy*OP(2,4)*SK_ACCY(2) + Kaccy*OP(2,3)*SK_ACCY(5) - Kaccy*OP(2,5)*SK_ACCY(6) + Kaccy*OP(2,6)*SK_ACCY(9) + Kaccy*OP(2,7)*SK_ACCY(8) + 2*Kaccy*OP(2,23)*SK_ACCY(7) - Kaccy*OP(2,24)*SK_ACCY(9));
Kfusion(3) = -SK_ACCY(1)*(Kaccy*OP(3,1)*SK_ACCY(4) + Kaccy*OP(3,2)*SK_ACCY(3) - Kaccy*OP(3,4)*SK_ACCY(2) + Kaccy*OP(3,3)*SK_ACCY(5) - Kaccy*OP(3,5)*SK_ACCY(6) + Kaccy*OP(3,6)*SK_ACCY(9) + Kaccy*OP(3,7)*SK_ACCY(8) + 2*Kaccy*OP(3,23)*SK_ACCY(7) - Kaccy*OP(3,24)*SK_ACCY(9));
Kfusion(4) = -SK_ACCY(1)*(Kaccy*OP(4,1)*SK_ACCY(4) + Kaccy*OP(4,2)*SK_ACCY(3) - Kaccy*OP(4,4)*SK_ACCY(2) + Kaccy*OP(4,3)*SK_ACCY(5) - Kaccy*OP(4,5)*SK_ACCY(6) + Kaccy*OP(4,6)*SK_ACCY(9) + Kaccy*OP(4,7)*SK_ACCY(8) + 2*Kaccy*OP(4,23)*SK_ACCY(7) - Kaccy*OP(4,24)*SK_ACCY(9));
Kfusion(5) = -SK_ACCY(1)*(Kaccy*OP(5,1)*SK_ACCY(4) + Kaccy*OP(5,2)*SK_ACCY(3) - Kaccy*OP(5,4)*SK_ACCY(2) + Kaccy*OP(5,3)*SK_ACCY(5) - Kaccy*OP(5,5)*SK_ACCY(6) + Kaccy*OP(5,6)*SK_ACCY(9) + Kaccy*OP(5,7)*SK_ACCY(8) + 2*Kaccy*OP(5,23)*SK_ACCY(7) - Kaccy*OP(5,24)*SK_ACCY(9));
Kfusion(6) = -SK_ACCY(1)*(Kaccy*OP(6,1)*SK_ACCY(4) + Kaccy*OP(6,2)*SK_ACCY(3) - Kaccy*OP(6,4)*SK_ACCY(2) + Kaccy*OP(6,3)*SK_ACCY(5) - Kaccy*OP(6,5)*SK_ACCY(6) + Kaccy*OP(6,6)*SK_ACCY(9) + Kaccy*OP(6,7)*SK_ACCY(8) + 2*Kaccy*OP(6,23)*SK_ACCY(7) - Kaccy*OP(6,24)*SK_ACCY(9));
Kfusion(7) = -SK_ACCY(1)*(Kaccy*OP(7,1)*SK_ACCY(4) + Kaccy*OP(7,2)*SK_ACCY(3) - Kaccy*OP(7,4)*SK_ACCY(2) + Kaccy*OP(7,3)*SK_ACCY(5) - Kaccy*OP(7,5)*SK_ACCY(6) + Kaccy*OP(7,6)*SK_ACCY(9) + Kaccy*OP(7,7)*SK_ACCY(8) + 2*Kaccy*OP(7,23)*SK_ACCY(7) - Kaccy*OP(7,24)*SK_ACCY(9));
Kfusion(8) = -SK_ACCY(1)*(Kaccy*OP(8,1)*SK_ACCY(4) + Kaccy*OP(8,2)*SK_ACCY(3) - Kaccy*OP(8,4)*SK_ACCY(2) + Kaccy*OP(8,3)*SK_ACCY(5) - Kaccy*OP(8,5)*SK_ACCY(6) + Kaccy*OP(8,6)*SK_ACCY(9) + Kaccy*OP(8,7)*SK_ACCY(8) + 2*Kaccy*OP(8,23)*SK_ACCY(7) - Kaccy*OP(8,24)*SK_ACCY(9));
Kfusion(9) = -SK_ACCY(1)*(Kaccy*OP(9,1)*SK_ACCY(4) + Kaccy*OP(9,2)*SK_ACCY(3) - Kaccy*OP(9,4)*SK_ACCY(2) + Kaccy*OP(9,3)*SK_ACCY(5) - Kaccy*OP(9,5)*SK_ACCY(6) + Kaccy*OP(9,6)*SK_ACCY(9) + Kaccy*OP(9,7)*SK_ACCY(8) + 2*Kaccy*OP(9,23)*SK_ACCY(7) - Kaccy*OP(9,24)*SK_ACCY(9));
Kfusion(10) = -SK_ACCY(1)*(Kaccy*OP(10,1)*SK_ACCY(4) + Kaccy*OP(10,2)*SK_ACCY(3) - Kaccy*OP(10,4)*SK_ACCY(2) + Kaccy*OP(10,3)*SK_ACCY(5) - Kaccy*OP(10,5)*SK_ACCY(6) + Kaccy*OP(10,6)*SK_ACCY(9) + Kaccy*OP(10,7)*SK_ACCY(8) + 2*Kaccy*OP(10,23)*SK_ACCY(7) - Kaccy*OP(10,24)*SK_ACCY(9));
Kfusion(11) = -SK_ACCY(1)*(Kaccy*OP(11,1)*SK_ACCY(4) + Kaccy*OP(11,2)*SK_ACCY(3) - Kaccy*OP(11,4)*SK_ACCY(2) + Kaccy*OP(11,3)*SK_ACCY(5) - Kaccy*OP(11,5)*SK_ACCY(6) + Kaccy*OP(11,6)*SK_ACCY(9) + Kaccy*OP(11,7)*SK_ACCY(8) + 2*Kaccy*OP(11,23)*SK_ACCY(7) - Kaccy*OP(11,24)*SK_ACCY(9));
Kfusion(12) = -SK_ACCY(1)*(Kaccy*OP(12,1)*SK_ACCY(4) + Kaccy*OP(12,2)*SK_ACCY(3) - Kaccy*OP(12,4)*SK_ACCY(2) + Kaccy*OP(12,3)*SK_ACCY(5) - Kaccy*OP(12,5)*SK_ACCY(6) + Kaccy*OP(12,6)*SK_ACCY(9) + Kaccy*OP(12,7)*SK_ACCY(8) + 2*Kaccy*OP(12,23)*SK_ACCY(7) - Kaccy*OP(12,24)*SK_ACCY(9));
Kfusion(13) = -SK_ACCY(1)*(Kaccy*OP(13,1)*SK_ACCY(4) + Kaccy*OP(13,2)*SK_ACCY(3) - Kaccy*OP(13,4)*SK_ACCY(2) + Kaccy*OP(13,3)*SK_ACCY(5) - Kaccy*OP(13,5)*SK_ACCY(6) + Kaccy*OP(13,6)*SK_ACCY(9) + Kaccy*OP(13,7)*SK_ACCY(8) + 2*Kaccy*OP(13,23)*SK_ACCY(7) - Kaccy*OP(13,24)*SK_ACCY(9));
Kfusion(14) = -SK_ACCY(1)*(Kaccy*OP(14,1)*SK_ACCY(4) + Kaccy*OP(14,2)*SK_ACCY(3) - Kaccy*OP(14,4)*SK_ACCY(2) + Kaccy*OP(14,3)*SK_ACCY(5) - Kaccy*OP(14,5)*SK_ACCY(6) + Kaccy*OP(14,6)*SK_ACCY(9) + Kaccy*OP(14,7)*SK_ACCY(8) + 2*Kaccy*OP(14,23)*SK_ACCY(7) - Kaccy*OP(14,24)*SK_ACCY(9));
Kfusion(15) = -SK_ACCY(1)*(Kaccy*OP(15,1)*SK_ACCY(4) + Kaccy*OP(15,2)*SK_ACCY(3) - Kaccy*OP(15,4)*SK_ACCY(2) + Kaccy*OP(15,3)*SK_ACCY(5) - Kaccy*OP(15,5)*SK_ACCY(6) + Kaccy*OP(15,6)*SK_ACCY(9) + Kaccy*OP(15,7)*SK_ACCY(8) + 2*Kaccy*OP(15,23)*SK_ACCY(7) - Kaccy*OP(15,24)*SK_ACCY(9));
Kfusion(16) = -SK_ACCY(1)*(Kaccy*OP(16,1)*SK_ACCY(4) + Kaccy*OP(16,2)*SK_ACCY(3) - Kaccy*OP(16,4)*SK_ACCY(2) + Kaccy*OP(16,3)*SK_ACCY(5) - Kaccy*OP(16,5)*SK_ACCY(6) + Kaccy*OP(16,6)*SK_ACCY(9) + Kaccy*OP(16,7)*SK_ACCY(8) + 2*Kaccy*OP(16,23)*SK_ACCY(7) - Kaccy*OP(16,24)*SK_ACCY(9));
Kfusion(17) = -SK_ACCY(1)*(Kaccy*OP(17,1)*SK_ACCY(4) + Kaccy*OP(17,2)*SK_ACCY(3) - Kaccy*OP(17,4)*SK_ACCY(2) + Kaccy*OP(17,3)*SK_ACCY(5) - Kaccy*OP(17,5)*SK_ACCY(6) + Kaccy*OP(17,6)*SK_ACCY(9) + Kaccy*OP(17,7)*SK_ACCY(8) + 2*Kaccy*OP(17,23)*SK_ACCY(7) - Kaccy*OP(17,24)*SK_ACCY(9));
Kfusion(18) = -SK_ACCY(1)*(Kaccy*OP(18,1)*SK_ACCY(4) + Kaccy*OP(18,2)*SK_ACCY(3) - Kaccy*OP(18,4)*SK_ACCY(2) + Kaccy*OP(18,3)*SK_ACCY(5) - Kaccy*OP(18,5)*SK_ACCY(6) + Kaccy*OP(18,6)*SK_ACCY(9) + Kaccy*OP(18,7)*SK_ACCY(8) + 2*Kaccy*OP(18,23)*SK_ACCY(7) - Kaccy*OP(18,24)*SK_ACCY(9));
Kfusion(19) = -SK_ACCY(1)*(Kaccy*OP(19,1)*SK_ACCY(4) + Kaccy*OP(19,2)*SK_ACCY(3) - Kaccy*OP(19,4)*SK_ACCY(2) + Kaccy*OP(19,3)*SK_ACCY(5) - Kaccy*OP(19,5)*SK_ACCY(6) + Kaccy*OP(19,6)*SK_ACCY(9) + Kaccy*OP(19,7)*SK_ACCY(8) + 2*Kaccy*OP(19,23)*SK_ACCY(7) - Kaccy*OP(19,24)*SK_ACCY(9));
Kfusion(20) = -SK_ACCY(1)*(Kaccy*OP(20,1)*SK_ACCY(4) + Kaccy*OP(20,2)*SK_ACCY(3) - Kaccy*OP(20,4)*SK_ACCY(2) + Kaccy*OP(20,3)*SK_ACCY(5) - Kaccy*OP(20,5)*SK_ACCY(6) + Kaccy*OP(20,6)*SK_ACCY(9) + Kaccy*OP(20,7)*SK_ACCY(8) + 2*Kaccy*OP(20,23)*SK_ACCY(7) - Kaccy*OP(20,24)*SK_ACCY(9));
Kfusion(21) = -SK_ACCY(1)*(Kaccy*OP(21,1)*SK_ACCY(4) + Kaccy*OP(21,2)*SK_ACCY(3) - Kaccy*OP(21,4)*SK_ACCY(2) + Kaccy*OP(21,3)*SK_ACCY(5) - Kaccy*OP(21,5)*SK_ACCY(6) + Kaccy*OP(21,6)*SK_ACCY(9) + Kaccy*OP(21,7)*SK_ACCY(8) + 2*Kaccy*OP(21,23)*SK_ACCY(7) - Kaccy*OP(21,24)*SK_ACCY(9));
Kfusion(22) = -SK_ACCY(1)*(Kaccy*OP(22,1)*SK_ACCY(4) + Kaccy*OP(22,2)*SK_ACCY(3) - Kaccy*OP(22,4)*SK_ACCY(2) + Kaccy*OP(22,3)*SK_ACCY(5) - Kaccy*OP(22,5)*SK_ACCY(6) + Kaccy*OP(22,6)*SK_ACCY(9) + Kaccy*OP(22,7)*SK_ACCY(8) + 2*Kaccy*OP(22,23)*SK_ACCY(7) - Kaccy*OP(22,24)*SK_ACCY(9));
Kfusion(23) = -SK_ACCY(1)*(Kaccy*OP(23,1)*SK_ACCY(4) + Kaccy*OP(23,2)*SK_ACCY(3) - Kaccy*OP(23,4)*SK_ACCY(2) + Kaccy*OP(23,3)*SK_ACCY(5) - Kaccy*OP(23,5)*SK_ACCY(6) + Kaccy*OP(23,6)*SK_ACCY(9) + Kaccy*OP(23,7)*SK_ACCY(8) + 2*Kaccy*OP(23,23)*SK_ACCY(7) - Kaccy*OP(23,24)*SK_ACCY(9));
Kfusion(24) = -SK_ACCY(1)*(Kaccy*OP(24,1)*SK_ACCY(4) + Kaccy*OP(24,2)*SK_ACCY(3) - Kaccy*OP(24,4)*SK_ACCY(2) + Kaccy*OP(24,3)*SK_ACCY(5) - Kaccy*OP(24,5)*SK_ACCY(6) + Kaccy*OP(24,6)*SK_ACCY(9) + Kaccy*OP(24,7)*SK_ACCY(8) + 2*Kaccy*OP(24,23)*SK_ACCY(7) - Kaccy*OP(24,24)*SK_ACCY(9));