clear all;
close all;

n = 20; % プラントの次数 
load data/spk1.dat;
load data/spk2.dat;

fl = 10;   % グラフの最低周波数
fh = 2000; % グラフの最高周波数

[tmp, index] = sort(spk1(:,1));
tmp = spk1(:,1);
spk1(:,1) = tmp(index); % 1 列目: 周波数
tmp = spk1(:,2);
spk1(:,2) = tmp(index); % 2 列目: ゲイン(エラーマイクが出力)
tmp = spk1(:,3);
spk1(:,3) = tmp(index); % 3 列目: 位相(エラーマイクが出力)
tmp = spk1(:,4);
spk1(:,4) = tmp(index); % 4 列目: ゲイン(リファレンスマイクが出力)
tmp = spk1(:,5);
spk1(:,5) = tmp(index); % 5 列目: 位相(リファレンスマイクが出力)

[tmp, index] = sort(spk2(:,1));
tmp = spk2(:,1);
spk2(:,1) = tmp(index); % 1 列目: 周波数
tmp = spk2(:,2);
spk2(:,2) = tmp(index); % 2 列目: ゲイン(エラーマイクが出力)
tmp = spk2(:,3);
spk2(:,3) = tmp(index); % 3 列目: 位相(エラーマイクが出力)
tmp = spk2(:,4);
spk2(:,4) = tmp(index); % 4 列目: ゲイン(リファレンスマイクが出力)
tmp = spk2(:,5);
spk2(:,5) = tmp(index); % 5 列目: 位相(リファレンスマイクが出力)

f = spk1(:,1); % 周波数は，spk1 で代表する．
w = 2*pi*f;
num = size(spk1,1); % 周波数応答のデータ数

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

for k = 1:num
  resp(1,1,k) = Gzw_jw(k);
  resp(1,2,k) = Gzu_jw(k);
  resp(2,1,k) = Gyw_jw(k);
  resp(2,2,k) = Gyu_jw(k);
end
%G_Hz = frd(resp, w, 'Units', 'Hz');
G_g = frd(resp, w);

%tmp = fselect(G_g, 2*pi*100, 2*pi*1500);
%G0 = n4sid(tmp, 24, 'co', 'none', 'Focus', 'Stability'); % 安定なノミナルプラントを得る
%G0 = n4sid(G_g, 24, 'co', 'none', 'Focus', 'Stability'); % 安定なノミナルプラントを得る
G0 = n4sid(G_g, n, 'co', 'none', 'Focus', 'Stability'); % 安定なノミナルプラントを得る

G0 = ss(G0.a, G0.b, G0.c, G0.d); % 余分な情報を除去
G0_g = frd(G0, w);

% Gzw
figure(1);
bode(G_g(1,1), 'b-', G0_g(1,1), 'r-', G0(1,1), 'm');
legend('exp.', 'nominal plant');

% Gzu
figure(2);
bode(G_g(1,2), 'b-', G0_g(1,2), 'r-', G0(1,2), 'm');
legend('exp.', 'nominal plant');
print -djpeg -r70 foo.jpg;

% Gyw
figure(3);
bode(G_g(2,1), 'b-', G0_g(2,1), 'r-', G0(2,1), 'm');
legend('exp.', 'nominal plant');

% Gyu
figure(4);
bode(G_g(2,2), 'b-', G0_g(2,2), 'r-', G0(2,2), 'm');
legend('exp.', 'nominal plant');

save data/nominal.mat G0 G_g G0_g w n
