%% 第６～８図

%% LQレギュレータの設計

% R = 1 の場合
Cmax = [ eye(4), zeros(4,1) ];
Clq = Cmax*Ch1;
Dmax1 = [ 0, 0, 0, 0, 1 ];
Dlq1 = Dmax1*Dh12;
Klq1 = -lqr(Ah,Bh2,Clq'*Clq,Dlq1'*Dlq1);

% R = 10^6 の場合
Dmax2 = [ 0, 0, 0, 0, 1e3 ];
Dlq2 = Dmax2*Dh12;
Klq2 = -lqr(Ah,Bh2,Clq'*Clq,Dlq2'*Dlq2);

%% 外乱応答の計算

% 外乱のサンプル周期を 1e-2
% 外乱が 0.9e-8 だと LQ では不安定
% robust では外乱が 11e-8 まで安定化できている

dist_int = 1e-9;
% dist_int = 1e-10;

K = Krp;
[Trobust,X,Yrobust] = sim('clsys');

K = Klq1;
[Tlq1,X,Ylq1] = sim('clsys');

K = Klq2;
[Tlq2,X,Ylq2] = sim('clsys');

%% 外乱応答の図

fig_basenumber = gcf + 1;

for k = 1:5
  figure;
  plot(Trobust,Yrobust(:,k),'k','LineWidth',1.5);
  hold on
  plot(Tlq1,Ylq1(:,k),'k:','LineWidth',1.5);
  plot(Tlq2,Ylq2(:,k),'k-.','LineWidth',1.5);
  plot(Trobust,0,'k');
  hold off
end

%% q1 の応答

figure(fig_basenumber);
set(gca,'FontSize', 16);
legend('robust control', 'optimal control (R=1)', 'optimal control (R=1000)', 'Location', 'NorthWest');
xlabel('t');
ylabel('q_1');
print -depsc2 q1.eps

%% q2 の応答

figure(fig_basenumber+1);
set(gca,'FontSize', 16);
legend('robust control', 'optimal control (R=1)', 'optimal control (R=1000)', 'Location', 'SouthWest');
xlabel('t');
ylabel('q_2');
print -depsc2 q2.eps

%% 外乱

figure;
set(gca,'FontSize', 16);
plot(Trobust,Yrobust(:,6),'k');
hold on
plot(Trobust,0,'k');
hold off
set(gcf,'PaperUnits','inches','PaperPosition',[0 0 6 3]);
xlim([0 50]);
xlabel('t');
ylabel('\eta');
ylim([-7e-5,7e-5]);
print -depsc2 eta.eps

