function compara
% Versão: Matlab 2020a
% Disciplina: PME3380 - Modelagem de Sistemas Dinâmicos
% Autores:
% Ives C. Vieira - 10355551
% Mauricio C. Leiman - 10772571
% Vitor Facchini - 10772605
% Yago N. Yang - 10772626

% Essa rotina permite simular a modelagem linear e a nao linear, comentando
% e descomentando as primeiras linhas, fazendo assim, a 'habilitacao' e
% 'desativacao'  de cada uma das condicoes: degrau, rampa e impulso.

% A modelagem esta descrita no relatorio entregue e esta rotina usa
% exatamente o mesmo equacionamento das duas anteriores, ela apenas nao
% plota graficos intermediarios.
close all

%% Valores

tempo_final = 16;
step = 0.005;
t = 0:step:tempo_final;
vet_temp = t;
n = length(t);
u = zeros(1,n); 
% Degrau
% funcao_deflexao = 8;

% Rampa
% funcao_deflexao = zeros(1,n);
% trampa=2;
% funcao_deflexao = min(ones(1,n)*8,8*t);

% Impulso
funcao_deflexao = ones(1,n);
funcao_deflexao(1,100) = -8;

u(1,:) = deg2rad(funcao_deflexao); % = deflexao do prof durante a simulacao, mudar! 

% Constantes 
rho = 1.1062;
g = 9.81;
% Geométricos 
m = 20;
W = m*g;
Iyy = 1.36;
z_motor = 0.4438;
z_cg = 0.4384;
S = 3.2;
Sh = 0.3387;
mac = 0.234;
x1 = -0.0059;
x2 = -0.1242;
xh = -0.2561;
xCG = 0.1509;

% Aerodinâmicos 
dT = -1.7464;
eta = 1;
CL0 = 0.49;
CD0 = 0.06;
CM0 = 0.06;
CL_alpha = 3.94;
CD_alpha = 0.2180;
CM_alpha = -0.3846;
CL_def = -0.3538;
CD_def = -0.008;
CM_def = -0.3470;
CL_alpha_1 = 1.9108;
CL_alpha_2 = 1.7226;
CL_alpha_h = 0.3110;

%% Condicao de trimagem
% Velocidade Inicial 
V = 15.75;

% Chute Inicial 
% alfa def T
x0 = [0;0;60];
rvs = 0.5*rho*V^2*S;

% Equações do Movimento 
fun = @(x) [ (CL0+CL_alpha*x(1)+CL_def*x(2)) + x(3)*sin(x(1))/rvs  - W/rvs;
    x(3)*cos(x(1)) - (CD0+CD_alpha*x(1)+CD_def*x(2));
    (CM0+CM_alpha*x(1)+CM_def*x(2)) - (z_motor-z_cg)*x(3)/(rvs*mac)];

% Variáveis Trimadas 
options = optimoptions('fsolve','Display','off');
y = fsolve(fun,x0,options);
alpha_trim = y(1); def_trim = y(2) ; T_trim = y(3);

% Estados no  sistema de eixos fixo no avião 
Vu = V*cos(alpha_trim);
Vv = 0;
Vw = V*sin(alpha_trim);
% vet_estados = 
%         Vu Vw q xf zf theta
y_trim = [Vu Vw 0 0  0  alpha_trim]';

%% Derivadas de estabilidade
% Coeficientes aerodinâmicos 
CL = CL0 + CL_alpha*alpha_trim + CL_def*def_trim;
CD = CD0 + CD_alpha*alpha_trim + CD_def*def_trim;
CM = CM0 + CM_alpha*alpha_trim + CM_def*def_trim;

% Forca peso 
Wf = [0 0 W]; 
Wb = body(y_trim,Wf);
Xg = Wb(1);
Zg = Wb(3);

% Derivadas 
Fxu = -rho*S*V*CD + dT;
Fzu = -rho*S*V*CL;
Myu = rho*S*mac*V*CM + dT*(-z_motor+z_cg);
Fxw = 0.5*rho*S*V*(CL - CD_alpha);
Fzw = 0.5*rho*S*V*(-CD - CL_alpha);
Myw = 0.5*rho*S*V*mac*CM_alpha;
Fxq = 0;
Fzq = 0.5*rho*V*(S*x1*CL_alpha_1 + S*x2*CL_alpha_2 + eta*Sh*xh*CL_alpha_h);
Myq = -0.5*rho*V*(S*x1^2*CL_alpha_1 + S*x2^2*CL_alpha_2 + eta*Sh*xh^2*CL_alpha_h);;
Fxd = -0.5*rho*S*V^2*CD_def;
Fzd = -0.5*rho*S*V^2*CL_def;
Myd =  0.5*rho*S*V^2*mac*CM_def;


%% Parametros
% Matrizes do Sistema 
Inv = inv([W/g 0 0 0 0 0
    0 W/g 0 0 0 0
    0 0 Iyy 0 0 0
    0 0 0 1 0 0
    0 0 0 0 1 0
    0 0 0 0 0 1]);

A = [Fxu Fxw Fzq 0 0 -W*cos(alpha_trim)
Fzu Fzw Fzq+W*V/g 0 0 -W*sin(alpha_trim)
Myu Myw Myq 0 0 0
cos(alpha_trim) sin(alpha_trim) 0 0 0 -V*sin(alpha_trim)
-sin(alpha_trim) cos(alpha_trim) 0 0 0 -V*cos(alpha_trim)
0 0 1 0 0 0 ];

B = [Fxd
    Fzd
    Myd
    0
    0
    0];

A = Inv*A;
B = Inv*B;

% C = zeros(2,4);
% C(1,:) = rad2deg([0 1/V0 0 0]); C(2,:) = A(2,:)/g;
C = eye(6);
D = zeros(6,1);

% Parâmetros da Simulação 
y_0 = zeros(6,1);
def = rad2deg(u);

%% Solucao do sistema
% Solução do Sistema por ODE 
fun = @(t,x) odefun_linear(t,x,A,B,u,vet_temp);

[t_tot,y_tot] = ode45(fun, t, y_0);

% Plots 
ulin=y_tot(:,1);
wlin=y_tot(:,2);
qlin=y_tot(:,3);
xlin=y_tot(:,4);
zlin=y_tot(:,5);
thelin=rad2deg(y_tot(:,6));
tlin=t_tot;


%% Solucao nao linear -----------------------------------------------------
% -------------------------------------------------------------------------
% -------------------------------------------------------------------------
% -------------------------------------------------------------------------
% -------------------------------------------------------------------------

%% Valores
% Constantes 
rho = 1.1062;
g = 9.81;

% Geométricos 
m = 20;
W = m*g;
Iyy = 1.36;
z_motor = 0.4438;
z_cg = 0.4384;
S = 3.2;
Sh = 0.3387;
mac = 0.234;
x1 = -0.0059;
x2 = -0.1242;
xh = -0.2561;
xCG = 0.1509;

% Aerodinâmicos 
dT = -1.7464;
eta = 1;
CL0 = 0.49;
CD0 = 0.06;
CM0 = 0.06;
CL_alpha = 3.94;
CD_alpha = 0.2180;
CM_alpha = -0.3846;
CL_def = -0.3538;
CD_def = -0.008;
CM_def = -0.3470;
CL_alpha_1 = 1.9108;
CL_alpha_2 = 1.7226;
CL_alpha_h = 0.3110;

%% Condicao de trimagem
% Velocidade Inicial 
V = 15.75;

% Chute Inicial 
% alfa def T
x0 = [0;0;60];
rvs = 0.5*rho*V^2*S;

% Equações do Movimento 
fun2 = @(x) [ (CL0+CL_alpha*x(1)+CL_def*x(2)) + x(3)*sin(x(1))/rvs  - W/rvs;
    x(3)*cos(x(1)) - (CD0+CD_alpha*x(1)+CD_def*x(2));
    (CM0+CM_alpha*x(1)+CM_def*x(2)) - (z_motor-z_cg)*x(3)/(rvs*mac)];

% Variáveis Trimadas 
options = optimoptions('fsolve','Display','off');
y = fsolve(fun2,x0,options);
alpha_trim = y(1); 
def_trim = y(2) ; 
T_trim = y(3);

% Estados no  sistema de eixos fixo no avião 
Vu = V*cos(alpha_trim);
Vv = 0;
Vw = V*sin(alpha_trim);
% vet_estados = 
%         Vu Vw q xf zf theta
y_trim = [Vu Vw 0 0  0  alpha_trim]';

% Parâmetros da Simulação 
u(1,:) = def_trim  + deg2rad(funcao_deflexao); % = def do prof durante a simulacao, mudar! 

y_0 = y_trim;
def =  rad2deg(u);

%% Solucao do sistema
% Solução do Sistema por ODE 
fun2 = @(t,y) odefun_nlinear(t, y, u, vet_temp, T_trim);

[t_tot,y_tot] = ode45(fun2, t, y_0);

% Plots 
unl=y_tot(:,1);
wnl=y_tot(:,2);
qnl=y_tot(:,3);
xnl=y_tot(:,4);
znl=y_tot(:,5);
thenl=rad2deg(y_tot(:,6));
tnl=t_tot;

figure
subplot(2,1,1)
plot(tlin,ulin,tnl,unl-y_0(1),'LineWidth',2)
xlabel('Tempo [s]')
ylabel('u [m/s]')
legend('Linear','Não linear')
grid on
subplot(2,1,2)
plot(tlin,wlin,tnl,wnl-y_0(2),'LineWidth',2)
xlabel('Tempo [s]')
ylabel('w [m/s]')
grid on
legend('Linear','Não linear')


figure
subplot(2,1,1)
plot(tlin,thelin,tnl,thenl-thenl(1),'LineWidth',2)
xlabel('Tempo [s]')
ylabel('\theta [°]')
legend('Linear','Não linear')
grid on
subplot(2,1,2)
plot(tlin,qlin,tnl,qnl-y_0(3),'LineWidth',2)
xlabel('Tempo [s]')
ylabel('q [°/s]')
legend('Linear','Não linear')
grid on

end

function yp = odefun_nlinear(t,y,u,vet_temp,T_trim)
%% Valores
% ============================= Constantes ============================== %
rho = 1.1062;
g = 9.81;

% Geométricos 
m = 20;
W = m*g;
z_motor = 0.4438;
z_cg = 0.4384;
S = 3.2;
Sh = 0.3387;
mac = 0.234;
x1 = -0.0059;
x2 = -0.1242;
xh = -0.2561;
xCG = 0.1509;
Iner = [0.36 0 0.0071
    0 1.36 0
    0.0071 0 0.46];
Ixxb = Iner(1,1);
Iyyb = Iner(2,2);
Izzb = Iner(3,3);
Ixzb = Iner(1,3);

% Aerodinâmicos 
dT = -1.7464;
eta = 1;
CL0 = 0.49;
CD0 = 0.06;
CM0 = 0.06;
CL_alpha = 3.94;
CD_alpha = 0.2180;
CM_alpha = -0.3846;
CL_def = -0.3538;
CD_def = -0.008;
CM_def = -0.3470;
CL_alpha_1 = 1.9108;
CL_alpha_2 = 1.7226;
CL_alpha_h = 0.3110;
CD_q = 0.0049;
CL_q = .0380;
CM_q = -0.0187;

% Acha def no instante 
[~,instante] = min(abs(vet_temp-t)); 
def = u(:,instante);

%% Estados
% vet_estados = 
% Vu Vw q xf zf theta
u   = y(1);
v   = 0;
w   = y(2);
p   = 0;
q   = y(3);
r   = 0;
phi = 0;
the = y(6);
psi = 0;

V = [u;v;w];
if u == 0
    alpha = 0;
else
    alpha = atan(w/u);
end
T = T_trim;

%% Forcas e momentos
V0=(u*u + v*v + w*w)^0.5;

CL = CL0 + CL_alpha*the + CL_def*def + CL_q*q;
CD = CD0 + CD_alpha*the + CD_def*def + CD_q*q;
CM = CM0 + CM_alpha*the + CM_def*def + CM_q*q;
rsv = 0.5*rho*S*V0^2;

F = [rsv*(CL*sin(alpha) - CD*cos(alpha)) * T*cos(alpha) - W*sin(the) 0,...
    rsv*(-CL*cos(alpha) - CD*sin(alpha)) + W*cos(phi)*cos(the)]; 
M = [0 rsv*mac*CM + T*(z_cg-z_motor) 0]; 

%% Derivadas do vetor de estados
% Aceleracoes lineares
up = F(1)/m + r*v - q*w;
vp = 0;
wp = F(3)/m + q*u - p*v;

% Aceleracoes angulares
aang = (Iner^-1)*[
     M(1) + (Iyyb -Izzb)*q*r + Ixzb*p*q;
     M(2) + (Izzb -Ixxb)*p*r + Ixzb*(r^2-p^2);
     M(3) + (Ixxb -Iyyb)*p*q - Ixzb*q*r];
pp = 0;
qp = aang(2);
rp = 0;

% Velocidades em relacao ao solo
vf = [
    cos(the)*cos(psi), sin(phi)*sin(the)*cos(psi) - cos(phi)*sin(psi), cos(phi)*sin(the)*cos(psi) + sin(phi)*sin(psi);
    cos(the)*sin(psi), sin(phi)*sin(the)*sin(psi) + cos(phi)*cos(psi), cos(phi)*sin(the)*sin(psi) - sin(phi)*cos(psi);
    -sin(the), sin(phi)*cos(the), cos(phi)*cos(the)] * V;
xp = vf(1);
yp = 0;
zp = vf(3);

% Variacao dos angulos de Euler
dang = [1, sin(phi)*sin(the)/cos(the), cos(phi)*sin(the)/cos(the);
    0, cos(phi), -sin(phi);
    0, sin(phi)/cos(the), cos(phi)/cos(the)]*[p;q;r];
phip = 0;
thep = dang(2);
psip = 0;

% Resultado
yp = [up;wp;qp;xp;zp;thep];

end

function x_ponto = odefun_linear(t,x,A,B,u,vet_temp)
[~,instante] = min(abs(vet_temp-t)); 
u_t = u(:,instante);
x_ponto = A*x + B*u_t;
end

function Vb = body(y, V)
% Essa funcao transforma um vetor da base fixa no solo em uma na base fixa
% no aviao (body), pode ser linha ou coluna.
% y eh o vetor de estados no instante, pode ser linha ou coluna, apenas
% importam os angulos de Euler
% V eh o vetor a ser transformado

[l,~] = size(V);
vet_col=false;
if l==3
    vet_col = true;
    V = V';
end
M = [cos(y(6)) *cos(0), cos(y(6)) * sin(0), -sin(y(6));
    sin(0) *sin(y(6)) *cos(0) - cos(0) *sin(0), sin(0) *sin(y(6)) *sin(0) + cos(0) *cos(0), sin(0) *cos(y(6));
    cos(0) *sin(y(6)) *cos(0) + sin(0) *sin(0), cos(0) *sin(y(6)) *sin(0) - sin(0) *cos(0), cos(0) *cos(y(6))];
Vb = (M*V')';
if vet_col
    Vb = Vb';
end
end