% nominal.m
% Purpose: to obtain nominal plant from frequency response
% Input: data/spk1.dat, data/spk2.dat; n:order of nominal plant defiend in nominal.m
% Output: data/nominal.mat
% Usage: just type `nominal' on command prompt of Matlab

close all; % close all figures
clear;

% z <-----|  Gzw   Gzu  |<----- w
% y <-----|  Gyw   Gyu  |<----- u
% 
% spk1.dat ... holds frequency data from w to z and y
% spk2.dat ... holds frequency data from u to z and y

n = 24; % the order of nominal plant which is specified by user

fl = 10;   % the lowest frequency to make graph
fh = 2000; % the highest frequency to make graph

load spk1.dat;  % load frequency data
[tmp, index] = sort(spk1(:,1));
tmp = spk1(:,1);
spk1(:,1) = tmp(index); % 1st column: frequency
tmp = spk1(:,2);
spk1(:,2) = tmp(index); % 2nd column: gain (from w to z)
tmp = spk1(:,3);
spk1(:,3) = tmp(index); % 3rd column: phase (from w to z)
tmp = spk1(:,4);
spk1(:,4) = tmp(index); % 4th column: gain (from w to y)
tmp = spk1(:,5);
spk1(:,5) = tmp(index); % 5th column: phase (from w to y)

load spk2.dat;  % load frequency data
[tmp, index] = sort(spk2(:,1));
tmp = spk2(:,1);
spk2(:,1) = tmp(index); % 1st column: frequency
tmp = spk2(:,2);
spk2(:,2) = tmp(index); % 2nd column: gain (from u to z)
tmp = spk2(:,3);
spk2(:,3) = tmp(index); % 3rd column: phase (from u to z)
tmp = spk2(:,4);
spk2(:,4) = tmp(index); % 4th column: gain (from u to y)
tmp = spk2(:,5);
spk2(:,5) = tmp(index); % 5th column: phase (from u to y)

f = spk1(:,1); % frequency data in spk1 is used but in spk2 by assuming that spk1 and spk2 have same frequency data
w = 2*pi*f; % angular frequency
M = size(spk1,1); % number of test frequencies

Gzw_jw = spk1(:,2).*exp(i*pi/180.0*spk1(:,3));
Gyw_jw = spk1(:,4).*exp(i*pi/180.0*spk1(:,5));
Gzu_jw = spk2(:,2).*exp(i*pi/180.0*spk2(:,3));
Gyu_jw = spk2(:,4).*exp(i*pi/180.0*spk2(:,5));

G_jw = [];
for k=1:M
  tmp = [Gzw_jw(k), Gzu_jw(k); Gyw_jw(k), Gyu_jw(k)];
  G_jw = [G_jw; tmp];
end

G = vpck(G_jw, w);
m = 2;
p = 2;

[A, B, C, D] = subspace(G, m, p, n, 2*n);

tmp = [A, B; C, D];
G0 = pss2sys(tmp, n);
G0_g = frsp(G0, w);
G0_Hz = scliv(G0_g, 1/(2*pi));



%%%%%%%
% Gzw %
%%%%%%%
figure(1);
clf;
gzw_jw = vpck(Gzw_jw, w);
Gzw0 = sel(G0,1,1);
Gzw0_g = frsp(Gzw0, w);
Gzw0_Hz = scliv(Gzw0_g, 1/(2*pi));
gzw_jw_Hz = scliv(gzw_jw, 1/(2*pi));

subplot(2,1,1), vplot('liv,lm', xtract(gzw_jw_Hz, fl, fh), 'bo', xtract(Gzw0_Hz, fl, fh), 'r');
axis([10, 2000, 1e-4, 10]);
ylabel('Magnitude');
set(findobj('type','line'),'linewidth',2);
set(findobj('type','line'),'markersize',3);
title('Gzw');


subplot(2,1,2), vplot('liv,p', xtract(gzw_jw_Hz, fl, fh), 'bo', xtract(Gzw0_Hz, fl, fh), 'r');
axis([10, 2000, -2500, 500]);
xlabel('Frequency [Hz]');
ylabel('Phase [deg]');
set(findobj('type','line'),'linewidth',2);
set(findobj('type','line'),'markersize',3);
legend('Exp. data', 'Approximated',3);
print -djpeg90 -r70 data/Gzw.jpg;


%%%%%%%
% Gyw %
%%%%%%%
figure(2);
clf;
gyw_jw = vpck(Gyw_jw, w);
Gyw0 = sel(G0,2,1);
Gyw0_g = frsp(Gyw0, w);
Gyw0_Hz = scliv(Gyw0_g, 1/(2*pi));
gyw_jw_Hz = scliv(gyw_jw, 1/(2*pi));

subplot(2,1,1), vplot('liv,lm', xtract(gyw_jw_Hz, fl, fh), 'bo', xtract(Gyw0_Hz, fl, fh), 'r');
axis([10, 2000, 1e-4, 10]);
ylabel('Magnitude');
set(findobj('type','line'),'linewidth',2);
set(findobj('type','line'),'markersize',3);
title('Gyw');

subplot(2,1,2), vplot('liv,p', xtract(gyw_jw_Hz, fl, fh), 'bo', xtract(Gyw0_Hz, fl, fh), 'r');
axis([10, 2000, -2500, 500]);
xlabel('Frequency [Hz]');
ylabel('Phase [deg]');
set(findobj('type','line'),'linewidth',2);
set(findobj('type','line'),'markersize',3);
legend('Exp. data', 'Approximated',3);
print -djpeg90 -r70 data/Gyw.jpg;


%%%%%%%
% Gzu %
%%%%%%%
figure(3);
clf;
gzu_jw = vpck(Gzu_jw, w);
Gzu0 = sel(G0,1,2);
Gzu0_g = frsp(Gzu0, w);
Gzu0_Hz = scliv(Gzu0_g, 1/(2*pi));
gzu_jw_Hz = scliv(gzu_jw, 1/(2*pi));

subplot(2,1,1), vplot('liv,lm', xtract(gzu_jw_Hz, fl, fh), 'bo', xtract(Gzu0_Hz, fl, fh), 'r');
axis([10, 2000, 1e-4, 10]);
ylabel('Magnitude');
set(findobj('type','line'),'linewidth',2);
set(findobj('type','line'),'markersize',3);
title('Gzu');

subplot(2,1,2), vplot('liv,p', xtract(gzu_jw_Hz, fl, fh), 'bo', xtract(Gzu0_Hz, fl, fh), 'r');
axis([10, 2000, -2500, 500]);
xlabel('Frequency [Hz]');
ylabel('Phase [deg]');
set(findobj('type','line'),'linewidth',2);
set(findobj('type','line'),'markersize',3);
legend('Exp. data', 'Approximated',3);
print -djpeg90 -r70 data/Gzu.jpg;


%%%%%%%
% Gyu %
%%%%%%%
figure(4);
clf;
gyu_jw = vpck(Gyu_jw, w);
Gyu0 = sel(G0,2,2);
Gyu0_g = frsp(Gyu0, w);
Gyu0_Hz = scliv(Gyu0_g, 1/(2*pi));
gyu_jw_Hz = scliv(gyu_jw, 1/(2*pi));

subplot(2,1,1), vplot('liv,lm', xtract(gyu_jw_Hz, fl, fh), 'bo', xtract(Gyu0_Hz,fl, fh), 'r');
axis([10, 2000, 1e-4, 10]);
ylabel('Magnitude');
set(findobj('type','line'),'linewidth',2);
set(findobj('type','line'),'markersize',3);
title('Gyu');

subplot(2,1,2), vplot('liv,p', xtract(gyu_jw_Hz, fl, fh), 'bo', xtract(Gyu0_Hz, fl, fh), 'r');
axis([10, 2000, -2500, 500]);
xlabel('Frequency [Hz]');
ylabel('Phase [deg]');
set(findobj('type','line'),'linewidth',2);
set(findobj('type','line'),'markersize',3);
legend('Exp. data', 'Approximated',3);
print -djpeg90 -r70 data/Gyu.jpg;

save data/vmat.mat gzw_jw_Hz gzu_jw_Hz gyw_jw_Hz gyu_jw_Hz Gzw_jw Gyw_jw Gzu_jw Gyu_jw Gyu0_Hz w

%save data/nominal.mat Gzw0 Gzu0 Gyu0 Gyw0
save data/nominal.mat G0
