%図12.2

clear all; close all;

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

%配置するオブザーバの極を与える
op = [-1;-2]; %-1, -2をオブザーバの極とする

%オブザーバゲインを求める.双対システムA^Tとc^Tの状態フィードバックゲインを求め,転置を取るとオブザーバゲインとなる
h = acker(A',c',op)' %双対システムで極配置し,転置を求める

%Ah = A-hcとオブザーバの極の計算と表示
Ah = A - h*c; %Ah = A - h*cの計算
disp('A-hcの固有値');
eig(A - h * c) %オブザーバの極を計算し表示

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

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

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

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

E = [Af -b*f;zeros(2) Ah]; %拡大システム(12.13)のA行列
bE = [0;0;0;1]; %併合システム(12.13)のベクトルb
cE = eye(4); %併合システム(12.13)のベクトルc
dE = [0;0;0;0]; %併合システム(12.13)のベクトルd

e_sys = ss(E, bE, cE, dE); %併合システムの状態空間表現

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

%システムとオブザーバの初期値を与える
x0 = [1;1]; %システムの初期値
x0_ob = [0;0]; %オブザーバの初期値

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

ye = initial(e_sys,[x0;x0_ob-x0],t); %併合システムの初期値応答
%
h_x_1 = ye(:,3) + ye(:,1); %併合システムの初期値応答
h_x_2 = ye(:,4) + ye(:,2); %併合システムの初期値応答

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

%図12.2(b)のプロット
figure(2) %図のウィンドウを開く
plot(t,y(:,1),t,y(:,2),t,h_x_1,t,h_x_2); %各状態を抽出してプロット
xlim([0 5]); %横軸(時間軸)の範囲の指定
ylim([-2.0 3]); %縦軸の範囲の設定
grid; %罫線を表示
xlabel('time t[s]'); %横軸のラベル表示
ylabel('state'); %縦軸のラベル表示
legend('x_1(t)','x_2(t)','hat{x}_1(t)','hat{x}_2(t)'); %凡例の表示
h =

    -2
     6

A-hcの固有値

ans =

   -1.0000
   -2.0000

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

f =

    24     6


Af =

     0     1
   -30   -11

閉ループ極

ans =

   -5.0000
   -6.0000