%図14.6

clear all; close all;

%システムパラメータを与える
A = [0 1;-6 -5]; %行列A
b = [0;1]; %ベクトルb
c = [1 0]; %ベクトルc
d = 0; %スカラーd

%状態フィードバックによる閉ループの極
fbp = [-3; -3];

%フィードバックゲインを求める
disp('求めるフィードバックゲイン');
f =  acker(A,b,fbp)
Af = A - b * f
disp('閉ループ極')
eig(A - b * f)

%システムの状態空間表現を求める
cp = eye(2); %オブザーバの状態変数をプロットするためにCを単位行列にする
dp = [0;0]; %cに合わせてDは零ベクトルとする

sys_s_fbk = ss(Af, b, cp, dp); %システムの状態空間表現

%時間変数の定義
t = 0:0.01:5; %0から5まで0.01刻み

%システムの初期値を与える
x0 = [-1;0]; %システムの初期値

%初期値応答の計算
y_sf = initial(sys_s_fbk,x0,t); %状態フィードバック系の初期値応答

%図14.6(a)のプロット
figure(1) %図のウィンドウを開く
plot(t,y_sf(:,1),t,y_sf(:,2)); %各状態を抽出してプロット
xlim([0 5]); %横軸（時間軸）の範囲の指定
ylim([-1 2]); %縦軸の範囲の設定
grid; %罫線を表示
xlabel('time t[s]'); %横軸のラベル表示
ylabel('state'); %縦軸のラベル表示
legend('x_1(t)','x_2(t)'); %凡例の表示

%%%%%%%%%%%%%
% LQ 制御
%%%%%%%%%%%%%

Q1=[13 0; 0 1];
R1=1;
[f_lqr_1, P1, e1] = lqr(A,b,Q1,R1);
disp('最適レギュレータによるフィードバックゲイン')
f_lqr_1

A_lqr1 = A - b * f_lqr_1; %最適レギュレータによるAf=A-bfの計算
sys_s_lqr1 = ss(A_lqr1, b, cp, dp); %最適レギュレータによるシステムの状態空間表現
%初期値応答の計算
y_lqr1 = initial(sys_s_lqr1,x0,t); %状態フィードバック系の初期値応答

%図14.6(b)のプロット
figure(2) %図のウィンドウを開く
plot(t,y_lqr1(:,1),t,y_lqr1(:,2)); %各状態を抽出してプロット
xlim([0 5]); %横軸（時間軸）の範囲の指定
ylim([-1 2]); %縦軸の範囲の設定
grid; %罫線を表示
xlabel('time t[s]'); %横軸のラベル表示
ylabel('state'); %縦軸のラベル表示
legend('x_1(t)','x_2(t)'); %凡例の表示

%図14.6(c)

u_sf = - f(1) * y_sf(:,1) - f(2) * y_sf(:,2); %極配置の場合の入力の計算
u_lqr1 = - f_lqr_1(1) * y_lqr1(:,1) - f_lqr_1(2) * y_lqr1(:,2); %最適制御の場合の入力の計算（r=の場合）

%図14.6(c)のプロット
figure(3) %図のウィンドウを開く1
plot(t,u_sf,t,u_lqr1); %プロット
xlim([0 5]); %横軸（時間軸）の範囲の指定
ylim([0 3]); %縦軸の範囲の設定
grid; %罫線を表示
xlabel('time t[s]'); %横軸のラベル表示
ylabel('u(t)'); %縦軸のラベル表示
legend('極配置法', '最適制御則'); %凡例の表示

%%%%%%%%%%%%%%%%%%%%%%
Q2=[1 0; 0 1];
R2=3;
[f_lqr_2, P2, e2] = lqr(A,b,Q2,R2);
disp('最適レギュレータによるフィードバックゲイン')
f_lqr_2

A_lqr2 = A - b * f_lqr_2; %最適レギュレータによるAf=A-bfの計算
sys_s_lqr2 = ss(A_lqr2, b, cp, dp); %最適レギュレータによるシステムの状態空間表現
%初期値応答の計算
y_lqr2 = initial(sys_s_lqr2,x0,t); %状態フィードバック系の初期値応答

%図14.6(d)

u_lqr2 = - f_lqr_2(1) * y_lqr2(:,1) - f_lqr_2(2) * y_lqr2(:,2); %最適制御の場合の入力の計算（r=3の場合）

%図14.6(d)のプロット
figure(4) %図のウィンドウを開く
plot(t,u_lqr1,t,u_lqr2); %プロット
xlim([0 5]); %横軸（時間軸）の範囲の指定
ylim([-0.2 1.2]); %縦軸の範囲の設定
grid; %罫線を表示
xlabel('time t[s]'); %横軸のラベル表示
ylabel('u(t)'); %縦軸のラベル表示
legend('r=1', 'r=3'); %凡例の表示

求めるフィードバックゲイン

f =

     3     1


Af =

     0     1
    -9    -6

閉ループ極

ans =

  -3.0000 + 0.0000i
  -3.0000 - 0.0000i

最適レギュレータによるフィードバックゲイン

f_lqr_1 =

    1.0000    0.2915

最適レギュレータによるフィードバックゲイン

f_lqr_2 =

    0.0277    0.0387

Published with MATLAB® R2016b