Fig_12

%図12.3

clear all; close all;
set(0, 'DefaultAxesLineWidth', 1);
set(0, 'DefaultLineLineWidth', 2);
set(0, 'defaultAxesFontSize', 12);
%
%システムパラメータを与える
A = [0 1;-6 -5]; %行列A
b = [0;1]; %ベクトルb
c = [1 0]; %ベクトルc
d = 0; %スカラーd

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

%オブザーバゲインを求める．双対システム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); %システムの状態空間表現

AE = [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(AE, 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.3(a)のプロット
figure(1) %図のウィンドウを開く
plot(t,ye(:,1),t,ye(:,2),t,y(:,1),t,y(:,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.3(b)のプロット
figure(2) %図のウィンドウを開く
plot(t,ye(:,1),t,ye(:,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 =

     6
    -6

A-hcの固有値

ans =

    -6
    -5

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

f =

    24     6


Af =

     0     1
   -30   -11

閉ループ極

ans =

   -5.0000
   -6.0000