%% CEE P10

% April 2016, J. Gaspar

%% P10.a) a)  Encontre o vector K tal que sendo u=-Kx+Nr
% o sistema em malha fechada tem valores próprios em -2±2j

A= diag([-1 -2]);
B= [1; 1];
C= [1 3];

K= acker(A,B, [-2+2*j -2-2*j])


%% P10.b) Dimensione N tal que sendo r=r_\inf constante, então y=y_\inf=r_\inf,
% i.e. o erro estático de posição é nulo. Mostre que esta propriedade (erro estático de posição nulo) não é robusta a alterações na matriz A.

%u=-K*x, dx= Ax+Bu= Ax-BKx= (A-BK)x
%eig(A-B*K)

s2= ss(A-B*K, B, C, 0);
pole(s2)
[num, den]= ss2tf(A-B*K, B, C, 0); s2b= tf(num,den);
G0= bode(s2b, 0)
sz= zero(s2b)
sp= pole(s2b)

% G0*N = 1
H0= G0*8/5

syms s
Hs= C*inv([s 0; 0 s]-(A-B*K))*B;
pretty(simplify(Hs))


%% P10.c) Juntar ao sistema um integrador
% escolher os ganhos K e ki, tais que sendo u=-[K ki][x \eta]^T, os valores
% próprios da malha fechada estejam  em  –2 e -1+/-\sqrt(3) Mostre  que
% neste caso o  sistema tem erro estático de posição nulo, e que esta propriedade é robusta em relações a alterações na matriz A, desde que o sistema em malha fechada permaneça estável

A= diag([-1 -2]);
B= [1; 1];
C= [1 3];

A2= [A 0*A(:,1); C 0*C(:,1)];
B2= [B; 0];
p= [-2, -1+sqrt(3)*j, -1-sqrt(3)*j];

K= acker(A2,B2,p)

s3= ss(A2-B2*K, [0 0 -1]', [C 0], 0);
sp= pole(s3)

% doing by hand
Ctr= [B2 A2*B2 A2*A2*B2]
CtrInv= inv(Ctr)
a= poly(p); a= a/a(1);
AA= polyvalm(a, A2);
K_by_hand= CtrInv(end,:)*AA