%Lab 5 - ELG 4152/SYS 5100

%SS Representation of the open-loop system
A=[0 1;0 0];
B=[0;1];
C=[1 0];
D=[0];

%Check controllability
CO=ctrb(A,B);
if rank(CO)==2
    disp('The system is controllable!');
end

%Let us design our full order controller
P=[-0.707+0.707j;-0.707-0.707j];
K=place(A,B,P);

%Check observability
OB=obsv(A,C);
if rank(OB)==2
    disp('The system is observable!');
end

%Design our full order observer
omega_n=5;
tzeta=0.5;
pol_caract=[1 2*tzeta*omega_n omega_n^2];
Pe=roots(pol_caract);
Lt=place(A',C',Pe);
L=Lt';


%Calculate Nbar
F=[A B;C D];
vect_xss_uss=inv(F)*[0;0;1];
Nu=vect_xss_uss(3);
Nx=vect_xss_uss(1:2);
Nbar=Nu+K*Nx;


%The optimal problem - case 1
k1=1;
fh=@myfun;
fplot(fh,[0.8 4]);
axis([0 5 0 6]);
grid on;
k2=fminbnd(fh,0.2,4);
K_new=[k1 k2];

sys1=ss(A-B*K,B,C,D);
sys2=ss(A-B*K_new,B,C,D);

%ltiview(sys1,sys2);