HW3.m

%%Setup of the state space sys[tem
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];

B2=[0 0;
   1 0;
   0 0;
   0 1;
   0 0;
   0 0];

C2=[0 0 0 0 1 0;
   1 0 0 0 0 0];

D22=zeros(2);

sys=ss(A,B2,C2,D22);
G=tf(sys);

eps1=1;eps2=1;

B=[zeros(6,3) B2];
C=[zeros(3,6);C2];
D11=[zeros(3,3)]; %3x3
D12=[zeros(1,2);diag([eps1,eps2])]; %2x3
D21=[1 0 0;0 1 0]; %2x3
D=[D11 D12;D21 D22];
P=ss(A,B,C,D);


%%
%Notes

%2a is analysis on nominal system no need for weightings at first

%nyquist open loop
%ssv closed loop

%Uncertainty analysis is closed loop, look at Hinf norm to understand
%tolerance for uncertainty

%use worst case gains and sigmas to understand robustness

%3a is going to be zero because the system is open loop stable so no reason
%to add a controller

%3b shows that the robust controller is able to perform still with higher
%ranges of uncertainty 


%%
%RGA Calculations
rga=G.*(G^-1).';

%%
%Determining the h2 norm for Twz
nw=3;nu=2;nz=3;ny=2;

Wr=tf(15,[1 25]);
Wu=tf(0.4,[1 4]);
b1=20;b2=50;Wn=[tf([1 0],[1 b1]) 0 ;0 tf([1 0],[1 b2])];
%b1=15;b2=15;Wn=[tf([1 0],[1 b1]) 0 ;0 tf([1 0],[1 b2])];

% P(1,1) is the tracking error (from r to y1-r)
P_w = P;
P_w(2:3,:)=P(2:3,:)*Wu;
P_w(:,1)=P(:,1)*Wr;
[K_2,Twz_2,gam_2,info2]=h2syn(P_w,ny,nu);
[K_inf,Twz_inf,gam_inf,infoinf]=hinfsyn(P_w,ny,nu);
Gcl_inf=lft(P,K_inf,nu,ny);
Gcl_2=lft(P,K_2,nu,ny);
figure(1),step(Gcl_2(2,2));legend('H_2');
figure();step(Gcl_inf(1,2));legend('H_\infty')



%%
%Uncertain model
Wd = 100*makeweight(1,[5 .7],0);

b1=ureal('b1',1,'Percentage',10);
b2=ureal('b2',1,'Percentage',10);

Bd=[0 0;
   b1 0;
   0 0;
   0 b2;
   0 0;
   0 0];

Bu=[zeros(6,3) Bd];

Pu=ss(A,Bu,C,D);
w=logspace(-2,2,100);

figure();gamma=mvar_nyquist(tf(Pu),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')

figure();nyquist(Pu(4,4));legend('Input 1')
figure();nyquist(Pu(5,5));legend('Input 2')

Pu_w=Pu;
Pu_w(:,1:2)=P_w(:,1:2)*Wd;

[K_2u,Twz_2u,gam_2u]=h2syn(Pu_w,ny,nu);
[K_infu,Twz_infu,gam_infu,infoinfu]=hinfsyn(Pu_w,ny,nu);
Gcl_infu=lft(Pu,K_infu);
Gcl_2u=lft(Pu,K_2u);
figure();step(Gcl_infu(2,1))
figure();step(Gcl_2u(2,2))

[K,CLperf,info] = musyn(Pu_w,nu,ny); 
Gmusyn=lft(Pu,K,nu,ny);
figure();step(Gmusyn(2,2))


%%
%Part 4 siso pairing

[K_2_1,Twz_2_1,gam_2_1]=h2syn(P_w(4,4),1,1);
Gcl_2_1=lft(P,K_2_1,1,1);


[K_2_2,Twz_2_2,gam_2_2]=h2syn(P_w(5,5),1,1);
Gcl_2_2=lft(P,K_2_1,1,1);
figure();step(Gcl_2_1(4,4),Gcl_2_2(4,4));legend('Input 1','Input 2')

Gcl_f=lft(Gcl_2_1,K_2_2);
figure();step(Gcl_f(2,2));legend('Combined Controllers')