forked from Ghost1995/Estimation-of-Quadrotor-State
-
Notifications
You must be signed in to change notification settings - Fork 0
/
jacobianMatrix_A.m
87 lines (83 loc) · 6.71 KB
/
jacobianMatrix_A.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
function A = jacobianMatrix_A( X, IMU, n )
% this function is to compute the JACOBIAN MATRIX A
% This function was generated by the Symbolic Math Toolbox version 8.0.
% 16-Nov-2017 14:19:23
if length(n) == 1
n = n*ones(12,1);
end
t2 = cos(X(5));
t3 = sin(X(5));
t4 = cos(X(4));
t5 = 1.0./t4;
t6 = sin(X(4));
t7 = 1.0./t4.^2;
t8 = t2.*2.682502464768751e15;
t9 = t3.*4.416535607028606e16;
t10 = n(1).*t2.*2.305398575877086e18;
t11 = n(2).*t2.*4.493290657188787e16;
t12 = n(3).*t2.*5.51574025573632e15;
t13 = t2.*X(10).*2.305398575877086e18;
t14 = t2.*X(11).*4.493290657188787e16;
t15 = t2.*X(12).*5.51574025573632e15;
t16 = n(1).*t3.*4.507532929405568e16;
t17 = t3.*IMU(5).*2.304412144238161e18;
t18 = t3.*IMU(6).*6.756380182005043e16;
t19 = t3.*X(10).*4.507532929405568e16;
t20 = t8+t9+t10+t11+t12+t13+t14+t15+t16+t17+t18+t19-n(2).*t3.*2.304412144238161e18-n(3).*t3.*6.756380182005043e16-t2.*IMU(4).*2.305398575877086e18-t2.*IMU(5).*4.493290657188787e16-t3.*IMU(4).*4.507532929405568e16-t2.*IMU(6).*5.51574025573632e15-t3.*X(11).*2.304412144238161e18-t3.*X(12).*6.756380182005043e16;
t21 = t3.*2.682502464768751e15;
t22 = n(2).*t2.*2.304412144238161e18;
t23 = n(3).*t2.*6.756380182005043e16;
t24 = t2.*IMU(4).*4.507532929405568e16;
t25 = t2.*X(11).*2.304412144238161e18;
t26 = t2.*X(12).*6.756380182005043e16;
t27 = n(1).*t3.*2.305398575877086e18;
t28 = n(2).*t3.*4.493290657188787e16;
t29 = n(3).*t3.*5.51574025573632e15;
t30 = t3.*X(10).*2.305398575877086e18;
t31 = t3.*X(11).*4.493290657188787e16;
t32 = t3.*X(12).*5.51574025573632e15;
t33 = t2.*-4.416535607028606e16+t21+t22+t23+t24+t25+t26+t27+t28+t29+t30+t31+t32-n(1).*t2.*4.507532929405568e16-t2.*IMU(5).*2.304412144238161e18-t3.*IMU(4).*2.305398575877086e18-t2.*IMU(6).*6.756380182005043e16-t3.*IMU(5).*4.493290657188787e16-t3.*IMU(6).*5.51574025573632e15-t2.*X(10).*4.507532929405568e16;
t34 = sin(X(6));
t35 = IMU(1).*1.954830797844587e-2;
t36 = IMU(2).*9.993794612339975e-1;
t37 = IMU(3).*2.93011282858715e-2;
t38 = n(4).*1.954830797844587e-2;
t39 = n(5).*9.993794612339975e-1;
t40 = n(6).*2.93011282858715e-2;
t41 = X(14).*9.993794612339975e-1;
t42 = X(15).*2.93011282858715e-2;
t52 = X(13).*1.954830797844587e-2;
t43 = t35-t36-t37-t38+t39+t40+t41+t42-t52-1.915367e-2;
t44 = cos(X(6));
t45 = n(4).*9.9980725776437e-1;
t46 = n(5).*1.948654196853161e-2;
t47 = n(6).*2.392071027254028e-3;
t48 = X(13).*9.9980725776437e-1;
t49 = X(14).*1.948654196853161e-2;
t50 = X(15).*2.392071027254028e-3;
t53 = IMU(1).*9.9980725776437e-1;
t54 = IMU(2).*1.948654196853161e-2;
t55 = IMU(3).*2.392071027254028e-3;
t51 = t45+t46+t47+t48+t49+t50-t53-t54-t55+1.16335e-3;
t56 = IMU(3).*9.995677675262321e-1;
t57 = n(4).*1.81960898838246e-3;
t58 = n(5).*2.934224166204635e-2;
t59 = X(13).*1.81960898838246e-3;
t60 = X(14).*2.934224166204635e-2;
t62 = IMU(1).*1.81960898838246e-3;
t63 = IMU(2).*2.934224166204635e-2;
t64 = n(6).*9.995677675262321e-1;
t65 = X(15).*9.995677675262321e-1;
t61 = t56+t57+t58+t59+t60-t62-t63-t64-t65+9.811681999999999e-2;
t66 = t3.*t34;
t67 = t66-t2.*t6.*t44;
t68 = t2.*t34;
t69 = t3.*t6.*t44;
t70 = t68+t69;
t71 = t2.*t44;
t72 = t71-t3.*t6.*t34;
t73 = t3.*t44;
t74 = t2.*t6.*t34;
t75 = t73+t74;
A = reshape([0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t7.*t20.*(-4.336808689942018e-19),t6.*t7.*t20.*4.336808689942018e-19,t6.*t34.*t61-t3.*t4.*t34.*t43+t2.*t4.*t34.*t51,-t6.*t44.*t61+t3.*t4.*t43.*t44-t2.*t4.*t44.*t51,t4.*t61+t3.*t6.*t43-t2.*t6.*t51,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t2.*1.16335e-3+t3.*1.915367e-2+n(1).*t2.*9.9980725776437e-1+n(1).*t3.*1.954830797844587e-2+n(2).*t2.*1.948654196853161e-2-n(2).*t3.*9.993794612339975e-1+n(3).*t2.*2.392071027254028e-3-n(3).*t3.*2.93011282858715e-2-t2.*IMU(4).*9.9980725776437e-1-t2.*IMU(5).*1.948654196853161e-2-t3.*IMU(4).*1.954830797844587e-2-t2.*IMU(6).*2.392071027254028e-3+t3.*IMU(5).*9.993794612339975e-1+t3.*IMU(6).*2.93011282858715e-2+t2.*X(10).*9.9980725776437e-1+t2.*X(11).*1.948654196853161e-2+t3.*X(10).*1.954830797844587e-2+t2.*X(12).*2.392071027254028e-3-t3.*X(11).*9.993794612339975e-1-t3.*X(12).*2.93011282858715e-2,t5.*t6.*t33.*4.336808689942018e-19,t5.*t33.*(-4.336808689942018e-19),-t43.*t75+t51.*t72,-t43.*t67+t51.*t70,-t2.*t4.*t43-t3.*t4.*t51,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,-t43.*t70-t51.*t67-t4.*t44.*t61,t43.*t72+t51.*t75-t4.*t34.*t61,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t2.*(-1.954830797844587e-2)+t3.*9.9980725776437e-1,t5.*(t4.*-3.2779161448157e13+t2.*t6.*1.801092637403974e16+t3.*t6.*3.5215101010981e14).*(-5.551115123125783e-17),t5.*(t2.*9.005463187019868e15+t3.*1.76075505054905e14).*1.110223024625157e-16,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t2.*9.993794612339975e-1+t3.*1.948654196853161e-2,t5.*(t4.*2.64291417230814e14-t2.*t6.*1.75519166296437e14+t3.*t6.*9.001609938430316e15).*1.110223024625157e-16,t5.*(t2.*1.75519166296437e14-t3.*9.001609938430316e15).*1.110223024625157e-16,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t2.*2.93011282858715e-2+t3.*2.392071027254028e-3,t5.*(t4.*9.003306050725395e15+t2.*t6.*2.154586037397e13-t3.*t6.*2.63921100859572e14).*(-1.110223024625157e-16),t5.*(t2.*1.0772930186985e13-t3.*1.31960550429786e14).*2.220446049250313e-16,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t4.*t34.*(-1.81960898838246e-3)-t2.*t44.*1.954830797844587e-2+t3.*t44.*9.9980725776437e-1+t2.*t6.*t34.*9.9980725776437e-1+t3.*t6.*t34.*1.954830797844587e-2,t2.*t34.*(-1.954830797844587e-2)+t3.*t34.*9.9980725776437e-1+t4.*t44.*1.81960898838246e-3-t2.*t6.*t44.*9.9980725776437e-1-t3.*t6.*t44.*1.954830797844587e-2,t6.*1.81960898838246e-3+t2.*t4.*9.9980725776437e-1+t3.*t4.*1.954830797844587e-2,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t4.*t34.*(-2.934224166204635e-2)+t2.*t44.*9.993794612339975e-1+t3.*t44.*1.948654196853161e-2+t2.*t6.*t34.*1.948654196853161e-2-t3.*t6.*t34.*9.993794612339975e-1,t2.*t34.*9.993794612339975e-1+t3.*t34.*1.948654196853161e-2+t4.*t44.*2.934224166204635e-2-t2.*t6.*t44.*1.948654196853161e-2+t3.*t6.*t44.*9.993794612339975e-1,t6.*2.934224166204635e-2+t2.*t4.*1.948654196853161e-2-t3.*t4.*9.993794612339975e-1,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t4.*t34.*9.995677675262321e-1+t2.*t44.*2.93011282858715e-2+t3.*t44.*2.392071027254028e-3+t2.*t6.*t34.*2.392071027254028e-3-t3.*t6.*t34.*2.93011282858715e-2,t2.*t34.*2.93011282858715e-2+t3.*t34.*2.392071027254028e-3-t4.*t44.*9.995677675262321e-1-t2.*t6.*t44.*2.392071027254028e-3+t3.*t6.*t44.*2.93011282858715e-2,t6.*(-9.995677675262321e-1)+t2.*t4.*2.392071027254028e-3-t3.*t4.*2.93011282858715e-2,0.0,0.0,0.0,0.0,0.0,0.0],[15,15]);
end