HW1.m

%%Setup of the state space system
A=[0 1 0 0 0 0;
   -5.5 -0.3 5 0.1 0 0;
   0 0 0 1 0 0;
   5 0.1 -10 -0.4 5 0.1;
   0 0 0 0 0 1;
   0 0 5 0.1 -5 -0.3];

B=[0;
    sym('u');
    0;
    sym('d');
    0;
    0];

B=[0;
    sym('u');
    0;
    0;
    0;
    0];

C=[0 0 0 0 1 0];
D=0;

test=(s*eye(6,6)-A);

G=C*inv(test)*B;
Gsimp=simplify(G);

Gsnum=[1 50];
Gdnum=[10 503 205 2750];
Gden=[200 200 4162 2276 16791 3400 2500];

Gs=tf(Gsnum,Gden);
Gds=tf(Gdnum,Gden);


% margin(Gs)
% figure()
% step(Gs)
% figure()
% rlocus(Gs)
% stepinfo(Gs)
% sisotool(Gs)

D=tf([1 8],[1 1]);
figure()
GCL=feedback(D*Gs*.5,1);
step(GCL*13.5)
hold on
step(Gs*50)
stepinfo(GCL)

%%
%LQR Controller Design
Bss=[0;1;0;0;0;0];
sys=ss(A,Bss,C,0);
Q=[1 0 0 0 0 0;
    0 1 0 0 0 0;
    0 0 1 0 0 0;
    0 0 0 1 0 0;
    0 0 0 0 1 0;
    0 0 0 0 0 1];
[K,S,P]=lqr(sys,Q,1);
[kalmf,L,P]=kalman(sys,[1],1);



sys1=ss(A-Bss*K-L*C,Bss,C,0);
G3CL=feedback(sys1,1);
step(G3CL*4.878)
legend('Open Loop','Closed Loop','LQR')
stepinfo(G3CL)
figure()
margin(G3CL)
figure()
nyquist(G3CL)

%%
%Adjusting the system in order to check for noise

n=tf([20],[1 0 20^2]);
noise=feedback(sys1,n,-1);
figure()
step(noise*4.878)

%%
%Adjusting the system in order to check for disturbance

d=tf([0.2*2],[0.2 0 0.2*2*2]);
cont=tf([1 1],[1 10]);
dist=feedback((sys1+d)*cont*9.5,1);
step(dist*2.38)

%%
%Adjusting the input

r=tf([15],[3 0 3*5*5]);
rin=Gs*r;
sisotool(rin)