% ISCIE システム／制御／情報，2012年6月号
% 「離散時間制御」
% サンプル値最適レギュレータ【例題２】のデモ
% ISCIE 解説

%% 初期化
clear
format compact

s = tf('s');

%% 制御対象（状態空間表現のA行列とB行列）
a = 8.5879;
b = 36.081;
A = [0,1;0,-a];
B = [0;b];

%% 最適レギュレータの重み行列の設定
[n,m]=size(B);
Q = eye(n); % 状態 x に対する重み行列
R = eye(m); % 入力 u に対する重み行列
P = [Q, zeros(n,m);zeros(m,n),R]; % P=[Q,0;0,R]

%% 評価関数の離散化
h = 0.06; % サンプリング周期
M = [A, B;zeros(m,n), zeros(m,m)];
MM = [-M', P; zeros(n+m,n+m), M];
expMM = expm(MM*h);
M12 = expMM(1:n+m,n+m+1:2*(n+m));
M22 = expMM(n+m+1:2*(n+m),n+m+1:2*(n+m));
Pd = M22'*M12;
Qd = Pd(1:n,1:n);
Rd = Pd(n+1:n+m,n+1:n+m);
Nd = Pd(1:n,n+1:n+m);

%% 制御対象の離散化
[Ad,Bd] = c2d(A,B,h);

%% サンプル値最適レギュレータゲインを求める
[Kd,Sd,Ed] = dlqr(Ad,Bd,Qd,Rd,Nd);

%% 連続時間最適レギュレータゲインを求める（比較用）
[K,S,E]=lqr(A,B,Q,R,zeros(n,1));

%% シミュレーション
initialx = [1,1];
sim iscie_sim_lqr;

%% |x(t)| の時間応答
time = ysd.time;
normsd = diag(ysd.signals.values*ysd.signals.values');
normc = diag(yc.signals.values*yc.signals.values');
normcsd = diag(ycsd.signals.values*ycsd.signals.values');

figure;plot(time,sqrt(normsd),'r-');
hold on
plot(time,sqrt(normcsd),'--');
plot(time,sqrt(normc),':k');
legend('Sampled-data design', 'Discretization of Kc', 'Ideal response');
xlabel('Time t [s]');
ylabel('|x(t)|');
grid on;



