-
Notifications
You must be signed in to change notification settings - Fork 0
/
jacM.m
80 lines (79 loc) · 4.29 KB
/
jacM.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
function Jac=jacM(tau,X,xp,param,Foguete)
% tf=t*param; % Arrumar para pegar o tempo certo, quem entra é o tau
if tau<1
t=tau*param(1);
region=1;
elseif tau<2
region=2;
t=(tau-region+1)*(param(region)-param(region-1))+param(region-1);
elseif tau<=3
region=3;
t=(tau-region+1)*(param(region)-param(region-1))+param(region-1);
end
r=X(1); sigma=X(2); u=X(3); v=X(4); m=X(5); lr=X(6); ls=X(7); lu=X(8); lv=X(9); lm=X(10);
U=controle(t,X,region,Foguete);
beta=U(1);
[FA,FN,FAr,FAu,FAv,FNr,FNu,FNv,FNbeta,HessFA,HessFN]=aerodin(t,X(1:5),U,region,Foguete);
[g,gr,grr]=AcelGrav(r);
switch region
case 1
T=Foguete.Otimo.T1;
case 2
T=Foguete.Otimo.T2;
case 3
T=Foguete.Otimo.T3;
end
%% Extrai valores de HessFA
FArr=HessFA(1,1); FAru=HessFA(1,2); FArv=HessFA(1,3);
FAur=HessFA(2,1); FAuu=HessFA(2,2); FAuv=HessFA(2,3);
FAvr=HessFA(3,1); FAvu=HessFA(3,2); FAvv=HessFA(3,3);
%% Extrai valores de HessFN
FNrr=HessFN(1,1); FNru=HessFN(1,2); FNrv=HessFN(1,3);
FNur=HessFN(2,1); FNuu=HessFN(2,2); FNuv=HessFN(2,3);
FNvr=HessFN(3,1); FNvu=HessFN(3,2); FNvv=HessFN(3,3);
Jac=[0,0,1,0,0,0,0,0,0,0;(-1).*r.^(-2).*v,0,0,r.^(-1),0,0,0,0,0,0;gr+( ...
-1).*r.^(-2).*v.^2+m.^(-1).*(FNr.*cos(beta)+(-1).*FAr.*sin(beta)), ...
0,m.^(-1).*(FNu.*cos(beta)+(-1).*FAu.*sin(beta)),2.*r.^(-1).*v+ ...
m.^(-1).*(FNv.*cos(beta)+(-1).*FAv.*sin(beta)),(-1).*m.^(-2).*( ...
FN.*cos(beta)+((-1).*FA+T).*sin(beta)),0,0,0,0,0;r.^(-2).*u.*v+( ...
-1).*m.^(-1).*(FAr.*cos(beta)+FNr.*sin(beta)),0,(-1).*m.^(-1).* ...
r.^(-1).*(m.*v+FAu.*r.*cos(beta)+FNu.*r.*sin(beta)),(-1).*m.^(-1) ...
.*r.^(-1).*(m.*u+FAv.*r.*cos(beta)+FNv.*r.*sin(beta)),m.^(-2).*( ...
FA.*cos(beta)+(-1).*T.*cos(beta)+FN.*sin(beta)),0,0,0,0,0;0,0,0,0, ...
0,0,0,0,0,0;(-2).*r.^(-3).*v.*(ls+(-1).*lv.*u+lu.*v)+m.^(-1).*(( ...
-1).*grr.*lu.*m+FArr.*(lv.*cos(beta)+lu.*sin(beta))+FNrr.*((-1).* ...
lu.*cos(beta)+lv.*sin(beta))),0,(-1).*lv.*r.^(-2).*v+m.^(-1).*( ...
FAru.*(lv.*cos(beta)+lu.*sin(beta))+FNru.*((-1).*lu.*cos(beta)+ ...
lv.*sin(beta))),r.^(-2).*(ls+(-1).*lv.*u+2.*lu.*v)+m.^(-1).*( ...
FArv.*(lv.*cos(beta)+lu.*sin(beta))+FNrv.*((-1).*lu.*cos(beta)+ ...
lv.*sin(beta))),m.^(-2).*((-1).*FAr.*(lv.*cos(beta)+lu.*sin(beta)) ...
+FNr.*(lu.*cos(beta)+(-1).*lv.*sin(beta))),0,r.^(-2).*v,(-1).*gr+ ...
r.^(-2).*v.^2+m.^(-1).*((-1).*FNr.*cos(beta)+FAr.*sin(beta)),(-1) ...
.*r.^(-2).*u.*v+m.^(-1).*(FAr.*cos(beta)+FNr.*sin(beta)),0;0,0,0, ...
0,0,0,0,0,0,0;(-1).*lv.*r.^(-2).*v+m.^(-1).*(FAur.*(lv.*cos(beta)+ ...
lu.*sin(beta))+FNur.*((-1).*lu.*cos(beta)+lv.*sin(beta))),0,m.^( ...
-1).*(FAuu.*(lv.*cos(beta)+lu.*sin(beta))+FNuu.*((-1).*lu.*cos( ...
beta)+lv.*sin(beta))),m.^(-1).*r.^(-1).*(lv.*m+FAuv.*r.*(lv.*cos( ...
beta)+lu.*sin(beta))+FNuv.*r.*((-1).*lu.*cos(beta)+lv.*sin(beta))) ...
,m.^(-2).*((-1).*FAu.*(lv.*cos(beta)+lu.*sin(beta))+FNu.*(lu.*cos( ...
beta)+(-1).*lv.*sin(beta))),(-1),0,m.^(-1).*((-1).*FNu.*cos(beta)+ ...
FAu.*sin(beta)),m.^(-1).*r.^(-1).*(m.*v+FAu.*r.*cos(beta)+FNu.*r.* ...
sin(beta)),0;r.^(-2).*(ls+(-1).*lv.*u+2.*lu.*v)+m.^(-1).*(FAvr.*( ...
lv.*cos(beta)+lu.*sin(beta))+FNvr.*((-1).*lu.*cos(beta)+lv.*sin( ...
beta))),0,m.^(-1).*r.^(-1).*(lv.*m+FAvu.*r.*(lv.*cos(beta)+lu.* ...
sin(beta))+FNvu.*r.*((-1).*lu.*cos(beta)+lv.*sin(beta))),m.^(-1).* ...
r.^(-1).*((-2).*lu.*m+FAvv.*r.*(lv.*cos(beta)+lu.*sin(beta))+ ...
FNvv.*r.*((-1).*lu.*cos(beta)+lv.*sin(beta))),m.^(-2).*((-1).* ...
FAv.*(lv.*cos(beta)+lu.*sin(beta))+FNv.*(lu.*cos(beta)+(-1).*lv.* ...
sin(beta))),0,(-1).*r.^(-1),m.^(-1).*r.^(-1).*((-2).*m.*v+(-1).* ...
FNv.*r.*cos(beta)+FAv.*r.*sin(beta)),m.^(-1).*r.^(-1).*(m.*u+FAv.* ...
r.*cos(beta)+FNv.*r.*sin(beta)),0;m.^(-2).*((-1).*FAr.*(lv.*cos( ...
beta)+lu.*sin(beta))+FNr.*(lu.*cos(beta)+(-1).*lv.*sin(beta))),0, ...
m.^(-2).*((-1).*FAu.*(lv.*cos(beta)+lu.*sin(beta))+FNu.*(lu.*cos( ...
beta)+(-1).*lv.*sin(beta))),m.^(-2).*((-1).*FAv.*(lv.*cos(beta)+ ...
lu.*sin(beta))+FNv.*(lu.*cos(beta)+(-1).*lv.*sin(beta))),(-2).* ...
m.^(-3).*(lv.*T.*cos(beta)+lu.*T.*sin(beta)+(-1).*FA.*(lv.*cos( ...
beta)+lu.*sin(beta))+FN.*(lu.*cos(beta)+(-1).*lv.*sin(beta))),0,0, ...
m.^(-2).*(FN.*cos(beta)+((-1).*FA+T).*sin(beta)),m.^(-2).*(((-1).* ...
FA+T).*cos(beta)+(-1).*FN.*sin(beta)),0];
end