HW4.m

close all;clear all;

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);
%%

% set sampling time and total time (and # of samples)
ts=.001;T=50;Ns=fix(T/ts); % 1ms sampling period

% maximum frequency separation of 
fmax=1/ts*(Ns/2-1)/Ns;fs=1/ts/Ns; % 1/2 of the frequency response
                                  % due to symmetry
% set excitation frequency band 0-5Hz
fband=50;
Nband=fix(fband/fs);

% set desired magnitude
uf_mag=zeros(1,Ns/2);
uf_mag(1:Nband)=1;

usat=10;
[u_schroed,t,mags,phs]=schroed(ts,Ns,uf_mag,usat);

%%

tvec=(0:Ns-1)*ts;
figure,plot(tvec,u_schroed);
title('Schroder phase signal (time domain)');
xlabel('time (sec)');ylabel('input (N)')

fvec=(0:Ns/2-1)*fs;
fvec_band=fvec(1:Nband);
u_f=t2f(u_schroed)*ts;
figure,plot(fvec,abs(u_f),'x');
axis([0 10 0 35])
title('Schroder phase signal (frequency domain)');
xlabel('frequency (Hz)');ylabel('input magnitude')
figure,plot(fvec,unwrap(angle(u_f)),'o');
axis([0 10 0 200])
title('Schroder phase signal (frequency domain)');
xlabel('frequency (Hz)');ylabel('input phase')

%%
% excite each input channel

numrun=10;
u_std=1;
y_std=0.04;
u_excite=kron(ones(numrun,1),u_schroed')';

tvec_numrun=(0:numrun*Ns-1)*ts;
u_noise1=randn(1,length(tvec)*numrun)*u_std;
y_noise1=randn(1,length(tvec)*numrun)*y_std;
y_u1_full=lsim(G(:,1),u_excite+u_noise1,tvec_numrun)+y_noise1';
u_noise2=randn(1,length(tvec)*numrun)*u_std;
y_noise2=randn(1,length(tvec)*numrun)*y_std;
y_u2_full=lsim(G(:,2),u_excite+u_noise2,tvec_numrun)+y_noise2';

y1_u1=mean(reshape(y_u1_full(:,1),Ns,numrun)');
y2_u1=mean(reshape(y_u1_full(:,2),Ns,numrun)');
y1_u2=mean(reshape(y_u2_full(:,1),Ns,numrun)');
y2_u2=mean(reshape(y_u2_full(:,2),Ns,numrun)');

figure,plot(tvec,y1_u1,tvec,y1_u2,tvec,y2_u1,tvec,y2_u2);

y11=t2f(y1_u1)*ts/abs(u_f(2));
y21=t2f(y2_u1)*ts/abs(u_f(2));
y12=t2f(y1_u2)*ts/abs(u_f(2));
y22=t2f(y2_u2)*ts/abs(u_f(2));

[mag,phase]=bode(G,2*pi*fvec);

figure(21),semilogx(fvec,squeeze(mag(1,1,:)),'o',fvec,abs(y11),'x');
figure(22),semilogx(fvec,squeeze(mag(2,1,:)),'o',fvec,abs(y21),'x');
figure(23),semilogx(fvec,squeeze(mag(1,2,:)),'o',fvec,abs(y12),'x');
figure(24),semilogx(fvec,squeeze(mag(2,2,:)),'o',fvec,abs(y22),'x');

%%
% frd object

fid_band=15;
Nid_band=fix(fid_band/fs);
G_freq=zeros(2,2,Nid_band);
G_freq(1,1,:)=y11(1:Nid_band);
G_freq(1,2,:)=y12(1:Nid_band);
G_freq(2,1,:)=y21(1:Nid_band);
G_freq(2,2,:)=y22(1:Nid_band);
G_frd=frd(G_freq,fvec(1:Nid_band),'frequencyunit','Hz');
figure(25),bode(G,G_frd)

%%
% sysid toolbox (frequency domain)

dat_f=iddata(G_frd,'Domain','Frequency');
G_est_f=ssest(dat_f,6);
figure(26);compare(dat_f,G_est_f);
figure(27);bode(G,G_est_f);legend('true','identified');


%%
% sysid toolbox (time domain)

u_in=[u_excite;u_excite];
y_full=lsim(G,u_in,tvec_numrun)+[y_noise1' y_noise2'];
y1=mean(reshape(y_full(:,1),Ns,numrun)');
y2=mean(reshape(y_full(:,2),Ns,numrun)');
dat=iddata([y1' y2'],[u_schroed' u_schroed'],ts);
G_est=ssest(dat,6,'Ts',ts);
figure(30),compare(dat,G_est);
G_disc=ss(G_est.a,G_est.b,G_est.c,G_est.d,ts);
G_cont=d2c(G_disc,'tustin');
figure(31);bode(G,G_cont);legend('true','id');

%%
%Uncertainty Analysis
norm(G_est_f-G_cont,'inf')
eps1=1;eps2=1;

%%
%Controller Design

B=[zeros(6,3) G_est.B];
C=[zeros(3,6);G_est.C];
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 G_est.D];
P=ss(A,B,C,D);

[K,CLperf]=h2syn(P,2,2);

Gcl=feedback(K*sys,eye(2,2));

[G_b,S]=balreal(Gcl);
r=8;
Ar=Gcl.A(1:r,1:r);
Br=Gcl.B(1:r,:);
Cr=Gcl.C(:,1:r);
Gr=ss(Ar,Br,Cr,Gcl.d);
disp(norm(G-Gr));
bode(Gcl,Gr);legend('Closed Loop','Reduced')