forked from Ghost1995/Estimation-of-Quadrotor-State
-
Notifications
You must be signed in to change notification settings - Fork 0
/
getXdot.m
54 lines (50 loc) · 4.4 KB
/
getXdot.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
function Xdot = getXdot( X, IMU, n )
% GET XDOT
% This function was generated by the Symbolic Math Toolbox version 8.0.
% 16-Nov-2017 14:20:26
if length(n) == 1
n = n*ones(12,1);
end
t2 = cos(X(5));
t3 = sin(X(5));
t4 = t2.^2;
t5 = t3.^2;
t6 = t4+t5;
t7 = 1.0./t6;
t8 = sin(X(4));
t9 = cos(X(4));
t10 = 1.0./t9;
t11 = cos(X(6));
t12 = sin(X(6));
t13 = IMU(1).*1.954830797844587e-2;
t14 = IMU(2).*9.993794612339975e-1;
t15 = IMU(3).*2.93011282858715e-2;
t16 = n(4).*1.954830797844587e-2;
t17 = n(5).*9.993794612339975e-1;
t18 = n(6).*2.93011282858715e-2;
t19 = X(14).*9.993794612339975e-1;
t20 = X(15).*2.93011282858715e-2;
t39 = X(13).*1.954830797844587e-2;
t21 = t13-t14-t15-t16+t17+t18+t19+t20-t39-1.915367e-2;
t22 = n(4).*9.9980725776437e-1;
t23 = n(5).*1.948654196853161e-2;
t24 = n(6).*2.392071027254028e-3;
t25 = X(13).*9.9980725776437e-1;
t26 = X(14).*1.948654196853161e-2;
t27 = X(15).*2.392071027254028e-3;
t40 = IMU(1).*9.9980725776437e-1;
t41 = IMU(2).*1.948654196853161e-2;
t42 = IMU(3).*2.392071027254028e-3;
t28 = t22+t23+t24+t25+t26+t27-t40-t41-t42+1.16335e-3;
t29 = IMU(3).*9.995677675262321e-1;
t30 = n(4).*1.81960898838246e-3;
t31 = n(5).*2.934224166204635e-2;
t32 = X(13).*1.81960898838246e-3;
t33 = X(14).*2.934224166204635e-2;
t35 = IMU(1).*1.81960898838246e-3;
t36 = IMU(2).*2.934224166204635e-2;
t37 = n(6).*9.995677675262321e-1;
t38 = X(15).*9.995677675262321e-1;
t34 = t29+t30+t31+t32+t33-t35-t36-t37-t38+9.811681999999999e-2;
Xdot = [X(7);X(8);X(9);t7.*(t2.*-4.416535607028606e16+t3.*2.682502464768751e15-n(1).*t2.*4.507532929405568e16+n(1).*t3.*2.305398575877086e18+n(2).*t2.*2.304412144238161e18+n(2).*t3.*4.493290657188787e16+n(3).*t2.*6.756380182005043e16+n(3).*t3.*5.51574025573632e15+t2.*IMU(4).*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+t2.*X(11).*2.304412144238161e18+t3.*X(10).*2.305398575877086e18+t2.*X(12).*6.756380182005043e16+t3.*X(11).*4.493290657188787e16+t3.*X(12).*5.51574025573632e15).*4.336808689942018e-19;t7.*t10.*(t2.*t8.*2.682502464768751e15+t3.*t8.*4.416535607028606e16-t4.*t9.*2.262419834832783e17-t5.*t9.*2.262419834832783e17+n(1).*t2.*t8.*2.305398575877086e18+n(1).*t3.*t8.*4.507532929405568e16+n(2).*t2.*t8.*4.493290657188787e16-n(2).*t3.*t8.*2.304412144238161e18+n(3).*t2.*t8.*5.51574025573632e15-n(1).*t4.*t9.*4.195732665364096e15-n(3).*t3.*t8.*6.756380182005043e16-n(1).*t5.*t9.*4.195732665364096e15-n(2).*t4.*t9.*6.765860281108838e16-n(2).*t5.*t9.*6.765860281108838e16+n(3).*t4.*t9.*2.304846348985701e18+n(3).*t5.*t9.*2.304846348985701e18-t2.*t8.*IMU(4).*2.305398575877086e18-t2.*t8.*IMU(5).*4.493290657188787e16-t3.*t8.*IMU(4).*4.507532929405568e16-t2.*t8.*IMU(6).*5.51574025573632e15+t3.*t8.*IMU(5).*2.304412144238161e18+t3.*t8.*IMU(6).*6.756380182005043e16+t4.*t9.*IMU(4).*4.195732665364096e15+t4.*t9.*IMU(5).*6.765860281108838e16+t5.*t9.*IMU(4).*4.195732665364096e15-t4.*t9.*IMU(6).*2.304846348985701e18+t5.*t9.*IMU(5).*6.765860281108838e16-t5.*t9.*IMU(6).*2.304846348985701e18+t2.*t8.*X(10).*2.305398575877086e18+t2.*t8.*X(11).*4.493290657188787e16+t3.*t8.*X(10).*4.507532929405568e16+t2.*t8.*X(12).*5.51574025573632e15-t3.*t8.*X(11).*2.304412144238161e18-t3.*t8.*X(12).*6.756380182005043e16-t4.*t9.*X(10).*4.195732665364096e15-t4.*t9.*X(11).*6.765860281108838e16-t5.*t9.*X(10).*4.195732665364096e15+t4.*t9.*X(12).*2.304846348985701e18-t5.*t9.*X(11).*6.765860281108838e16+t5.*t9.*X(12).*2.304846348985701e18).*(-4.336808689942018e-19);t7.*t10.*(t2.*2.682502464768751e15+t3.*4.416535607028606e16+n(1).*t2.*2.305398575877086e18+n(1).*t3.*4.507532929405568e16+n(2).*t2.*4.493290657188787e16-n(2).*t3.*2.304412144238161e18+n(3).*t2.*5.51574025573632e15-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.*IMU(5).*2.304412144238161e18+t3.*IMU(6).*6.756380182005043e16+t2.*X(10).*2.305398575877086e18+t2.*X(11).*4.493290657188787e16+t3.*X(10).*4.507532929405568e16+t2.*X(12).*5.51574025573632e15-t3.*X(11).*2.304412144238161e18-t3.*X(12).*6.756380182005043e16).*4.336808689942018e-19;t21.*(t2.*t11-t3.*t8.*t12)+t28.*(t3.*t11+t2.*t8.*t12)-t9.*t12.*t34+2.874440684843995e-1;t21.*(t2.*t12+t3.*t8.*t11)+t28.*(t3.*t12-t2.*t8.*t11)+t9.*t11.*t34-9.805759799432337;t8.*t34-t3.*t9.*t21+t2.*t9.*t28+2.346621677736202e-2;n(7);n(8);n(9);n(10);n(11);n(12)];
end