HW2.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 0;
   1 0;
   0 0;
   0 1;
   0 0;
   0 0];

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

sys=ss(A,B,C,D);
G=tf(sys);
%%
%Smith Mcmillan Form for P22
[M,U,V]=smform(sys);
%Stable Coprime factorizations
lncf(sys);
rncf(sys);
%Transmission Poles and Zeros
tzero(sys)
pole(sys)

%Siso Poles and zeros
sisopoles=[pole(sys(1,1)) pole(sys(1,2)) pole(sys(2,1)) pole(sys(2,2))]
sisozeros=[zero(sys(1,1));0; zero(sys(1,2));0; zero(sys(2,1));0; zero(sys(2,2))]%Using zeros to seperate outputs for visualization

%%
%Which input is best
condc=[cond(gram(sys(1,1),'c')) cond(gram(sys(2,2),'c'))]
condo=[cond(gram(sys(1,1),'o')) cond(gram(sys(2,2),'o'))]


%%
%Parametrize all K controllers
[Kpara,K0]=q_control(sys,eye(2))

%%
%Looking back at project 1

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(1,1),Q,1);
[kalmf,L,P]=kalman(sys(1,1),[1],1);
sys1=ss(A-B(:,1)*K-L*C(1,:),B(:,1),C(1,:),0);

opt=norm(sys1)

%%
%Loop stabilization controller analysis
figure();nyquist(Kpara)
w=logspace(-2,2,100);
figure();gamma=mvar_nyquist(Kpara,w);
plot(real(gamma(1,:)),imag(gamma(1,:)),'b',...
    real(gamma(1,:)),-imag(gamma(1,:)),'b:',...
    real(gamma(2,:)),imag(gamma(2,:)),'r',...
    real(gamma(2,:)),-imag(gamma(2,:)),'r:',...
    'linewidth',2);
grid;
title('Multivariable Nyquist Plot')


%%
%Small gain theorem
sgt=1/norm(Kpara,inf)

%Positive Realness
y=1/abs(min(pr(Kpara,w)))

%SSV
%bounds=mussv(Kpara(1,1),eye(2))