%%% 図解でわかる制御工学I
%%% 状態空間表現と安定性

clear;

%% 伝達関数（ブラックボックスモデル）の定義
s = tf('s');
G = (2*s+1)/(s^2+2*s+3);

%% 伝達関数から状態空間表現を求める（状態空間実現）
% MATLAB での状態空間表現 （ss 表現）
Gss = ss(G); 
% 行列A,B,C,Dを求める
[A,B,C,D] = ssdata(Gss);
% 可制御性行列 M_R を求める
MR = ctrb(A,B);
% 可観測性行列 M_O を求める
MO = obsv(A,C);
% Hankel 行列 H を求める
H = MO*MR;

%% システムの応答を求める
% インパルス応答
figure;impulse(G);
% 零入力応答
x0 = [1;1]; % 初期値
[y1,t]=initial(Gss,x0);
% 零状態応答（ステップ応答）
[y2,t]=step(G,t);
% LTIシステムの応答＝零入力応答＋零状態応答
figure;
plot(t,y1,'--b');
hold on
plot(t,y2,'-.k');
plot(t,y1+y2,'-r');
legend('Zero-input response','Zero-state response','Output');
xlabel('time (sec)');
ylabel('amplitude');
grid on;

%% 安定性 by Lyapunov
% 行列の固有値
N=size(A,1);
lambda_A = eig(A)
if isempty(find(real(lambda_A>=0)))
    disp('A is stable since Re(lambda_A) < 0.');
else
    disp('A is unstable since Re(lambda_A) >= 0.');
end
% Lyapunov 方程式
Q = eye(N);
P = lyap(A',Q);
[P2,r]=chol(P);
P
if r==0
    disp('A is stable since P > 0.');
else
    disp('A is unstable since P > 0.');
end



