% 図解でわかる制御工学I
% 可観測性とオブザーバ

% 倒立振子のパラメータ値
ac=5; bc=4;
a1=39.2; a2=-0.05; a3=20; b1=-16;

% 倒立振子の実現 A,B,C
% D=[0;0] なので，今後計算では明記しない
A=[0 0 1 0;0 0 0 1;a1 0 a2 a3;0 0 0 -ac];
B=[0;0;b1;bc];
C=[1 0 0 0;0 1 0 0];

% 可制御性行列
Mc=ctrb(A,B);
% rank(Mc)=4 より，可制御である

% 状態フィードバックの極を指定
Ps=[-3 -4 -10 -12];
% 状態フィードバックゲインFの計算
F=place(A,B,Ps);

% 可観測性行列
Mo=obsv(A,C);
% rank(Mo)=4 より，可観測である

% オブザーバの極を指定
Po=[-45 -60 -150 -180];
% オブザーバゲインLの計算
L=(place(A',C',Po))';

% 状態フィードバック・オブザーバ併合系の実現
Ae=[A -B*F;L*C A-B*F-L*C];
Be=[B;B];
Ce=[C zeros(2,4)];

% 状態xの初期状態
x0=[0.1;0;0;0];
% 推定値xhの初期状態
xh0=[0;0;0;0];
% 閉ループ系の初期状態
xe0=[x0;xh0];

% 倒立振子の出力
ipen=ss(Ae,Be,Ce,zeros(2,1));
% シミュレーション時間として，0.001[s]間隔で2[s]まで指定
T1=[0:0.001:2];
figure(1)
% レギュレータ問題なので，初期値応答としてシミュレーション可能
initial(ipen,xe0,T1);
title('出力応答')

% 倒立振子の状態の真値xを測定
% xe=[x;xh]より，xを出力として選ぶ
Cex=[eye(4) zeros(4)];
ipenx=ss(Ae,Be,Cex,zeros(4,1));
% シミュレーション時間として，0.001[s]間隔で0.25[s]まで指定
T2=[0:0.001:0.25];
figure(2)
initial(ipenx,xe0,T2);
title('倒立振子の状態の真値xと推定値xhの比較')

% xの推定値xhを測定
hold on
% xe=[x;xh]より，xhを出力として選ぶ
Cexh=[zeros(4) eye(4)];
ipenxh=ss(Ae,Be,Cexh,zeros(4,1));
initial(ipenxh,xe0,T2);


