close all
clc

% Problema do Navio

M = 10^7; %Momento de Inércia do navio em torno do eixo vertical
d = 10^6; %Coeficiente de arrasto associado à rotação
c = 5000; %Coeficiente relacionando o ângulo do leme e o torque aplicado

A = [0 1 0;
    0 -d/M -c/M;
    0 0 -0.1];

B1 = [0;
    0;
    0.1];

B2 = [0;
    1/M;
    0];

B = [B1 B2];

C = [1 0 0];

%% Realimentação de Estados

p = [-0.02+0.03j -0.02-0.03j -0.1]; %Dinâmica desejada

K = place(A, B1, p); %Ganho de realimentação

figure(1);
plot(state_var_feed1.time, state_var_feed1.signals.values);%Sem perturbação
legend('theta','theta ponto','alfa');
title('Realimentação de estados sem perturbação');
figure(2);
plot(state_var_feed2.time, state_var_feed2.signals.values);%Com perturbação
legend('theta','theta ponto','alfa');
title('Realimentação de estados com perturbação');
%% Realimentação de Estados com integrador
pint = [-0.02+0.03j -0.02-0.03j -0.1 -1];

Ai = [0 C;
    zeros(3,1) A];

Bi = [0;
    B1];

Ci = [0 1 0 0];
Kint = place(Ai, Bi, pint);
Ki = Kint(1);
Kint = Kint(2:4);

figure(3);
plot(state_var_int.time, state_var_int.signals.values)
legend('theta');
title('Realimentação de estados com integrador');
%% Realimentação de estados com observador

%Primeiro, deve-se checar se o sistema é observável e controlável
Ob = obsv(A,C);
rank(Ob);

Contrl = ctrb(A,B);
rank(Contrl);

%polos do observador: devem ser mais rápidos que os do sistema real
pobs = [-0.2 -0.25 -0.1];

%Ganho do observador
Kobs = place(A',C', pobs);
Kobs = Kobs';
Kobs2 = [0.52;
    0.0396;
    -2.1600];
%Ganho de realimentação
Kfeed = place(A, B1, p);


figure(4);
plot(state_var_obs1.time, state_var_obs1.signals.values);
legend('theta','theta estimado');
title('Comparação entre theta real e o theta estimado (observador)');
%% Realimentação de estados com observador e integrador


figure(5);
plot(state_var_obsint.time, state_var_obsint.signals.values)
legend('theta estimado','theta estimado + integrador');
title('Comparação entre o theta estimado sem integrador e com integrador');