%cl_setup.m

clc

A=[1 0 0; 0 1 4; 0 4 5]

eig(A)

B1=[1;0;0]
Cont1 = ctrb(A,B1)
[B1 A*B1 A^2*B1]
rankcont1=rank(Cont1)
display(['Rank of controllability matrix for (A,B1) = ',num2str(rankcont1)])

%pause

B2=[1;1;0]
Cont2 = ctrb(A,B2)
[B2 A*B2 A^2*B2]
rankcont2=rank(Cont2)
display(['Rank of controllability matrix for (A,B2) = ',num2str(rankcont2)])

%Desired poles for closed-loop system
Pc=[-4 -2+i*2 -2-i*2];

despolycont=poly(Pc)

%pause

K = place(A,B2,Pc)

eig(A-B2*K)

%pause

C1=[1 0 0]
Obsv1 = obsv(A,C1)
[C1;C1*A;C1*A^2]
rankobsv1=rank(Obsv1)
display(['Rank of observability matrix for (A,C1) = ',num2str(rankobsv1)])

%pause

C2=[1 1 1]
Obsv2 = obsv(A,C2)
[C2;C2*A;C2*A^2]
rankobsv2=rank(Obsv2)
display(['Rank of observability matrix for (A,C2) = ',num2str(rankobsv2)])

%pause

%Desired poles for observer error dynamics
%Po=[-30 -10+i*5 -10-i*5];
Po=[-3 -1+i*0.5 -1-i*0.5];

despolyobsv=poly(Po)

L = place(A',C2',Po)
L=L'

eig(A-L*C2)

%pause

%Simulation setup
x0=[1;2;3]

x0_obs=[0;0;0]

C2_aug=[C2; eye(3)];

B2_aug=[B2 L];

ctrl_switch=input('Do you want to run an open-loop (0) or closed-loop (1) simulation? ')

est_switch=input('Do you want to use state (0) or estimation (1) for feedback? ')

% figure(1)
% plot(ystate.time,ystate.signals.values,'r-',yobsfast.time,yobsfast.signals.values,'b:',yobsslow.time,yobsslow.signals.values,'m-.')
% xlabel('Time')
% ylabel('Output')
% legend('State','Fast Estimate','Slow Estimate')
