echo off;
close all; % figure を全部閉じる
clear;

load data/vmat.mat; % nominal.m で作成した，周波数応答データをロードする

sample = 0.0002;

f = getiv(gzw_jw_Hz);
w = 2*pi*f;

% 補償器の周波数応答
load data/controller.mat; % 補償器のデータをロードする
Hinf = pck(Ac, Bc, Cc, Dc);
Hinf_g = frsp(Hinf, w);
Hinf_Hz = scliv(Hinf_g, 1/(2*pi));

% 閉ループ系 Gcl = GZW + GZU K / (1 - GYU K) GYW の周波数応答
L = mmult(gyu_jw_Hz, Hinf_Hz);
tmp1 = msub(1, L);
tmp2 = mmult(gzu_jw_Hz, Hinf_Hz);
tmp2 = mmult(tmp2, gyw_jw_Hz);
Gcl_Hz = madd(gzw_jw_Hz, mmult(tmp2, minv(tmp1)));

% 一巡伝達関数のナイキストプロット
figure(1);
clf;
load data/nominal.mat; % ノミナルプラントのデータを読み込む
Gyu0 = sel(G0, 2, 2); % u→y のノミナル伝達関数
Gyu0_g = frsp(Gyu0, w);
Gyu0_Hz = scliv(Gyu0_g, 1/(2*pi));
load data/weight.mat; % 重みのデータを読み込む
W_g = frsp(W, w);
W_Hz = scliv(W_g, 1/(2*pi));
L0 = mmult(Gyu0_Hz, Hinf_Hz); % Gyu がノミナル伝達関数のときの，一巡伝達関数
vplot('nyq', mmult(-1, L0), 'r.-'); % 一巡伝達関数を -1 倍してからプロットする．本閉ループ系では正帰還がかかっているため
hold on;
vplot('nyq', mmult(-1, L), 'b.-');
plot(-1, 0, 'g*');
for phi = 0:30:330 %% Δ(jw) を単位円上の点として与える
  Gyu_Hz = madd(Gyu0_Hz, mmult(W_Hz, exp(i*phi/180*pi))); %% 摂動後の Gyu
  L0_delta = mmult(Gyu_Hz, Hinf_Hz);
  vplot('nyq', mmult(-1, L0_delta), 'w.');
end
vplot('nyq', mmult(-1, L0), 'r.-', mmult(-1, L), 'b.-'); % 再度プロット
hold off;
xlabel('Real');
ylabel('Imaginaly');
title('Nyquist plot');
legend('plant model (delta = 0)', 'true plant', '(-1,0)', 'plant model (delta = unit disk)', 1);
set(findobj('type','line'),'linewidth',3);
set(findobj('type','line'),'markersize',10);
print -djpeg90 -r70 data/nyquist.jpg;

% 閉ループ系の安定解析
fprintf('nominal transfer function for Gyu: ');
[a, b, c, d] = unpck(Gyu0);
val = max(real(eig(a)));
if val < 0      
  fprintf('stable\n');
else
  fprintf('unstable\n');
end

fprintf('controller: ');
[a, b, c, d] = unpck(Hinf);
val = max(real(eig(a)));
if val < 0      
  fprintf('stable\n');
else
  fprintf('unstable\n');
end

fprintf('closed-loop system: ');
Gcl = starp(G0, Hinf, 1, 1);
[a, b, c, d] = unpck(Gcl);
val = max(real(eig(a)));
if val < 0      
  fprintf('stable\n');
else
  fprintf('unstable\n');
end

%figure(2);
%clf;
%vplot('liv,m', msub(L0, 1), 'r', msub(L, 1), 'b');
%axis([10, 2000, 0, 6]);
%xlabel('Frequency [Hz]');
%ylabel('Distance from (-1,0) to Nyquist locus');
%title('Distance from (-1,0) to Nyquist locus');
%legend('plant model (delta = 0)', 'true plant');
%set(findobj('type','line'),'linewidth',2);
%print -djpeg90 -r70 data/nyquist_dist.jpg;

figure(2);
clf;
vplot('liv,lm', gzw_jw_Hz, 'b', Gcl_Hz, 'r');
axis([10, 2000, 1e-4, 10]);
xlabel('Frequency [Hz]');
ylabel('Magnitude');
title('Performance estimation');
legend('without control', 'with control', 3);
set(findobj('type','line'),'linewidth',2);
print -djpeg90 -r70 data/perf.jpg;

