%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% ISCIE 56-4 2012
% 初学者のための図解でわかる制御工学I
% サーボ系・内部モデル原理（MATLABファイル）
% 第3章数値例
% 目標信号：ステップ
% 外乱信号：ステップ
% ------------------------------
% | 外乱無し | pend_lpr.mdl     | 
% | 外乱有り | pend_lqr_dis.mdl |
% ------------------------------
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

close all;
clear all;

%% 台車型倒立振子のパラメータ

%----------------------------------
% シミュレーションパラメータ
x0 = [0;0;0;0;]; % 倒立振子の初期値
                 %角度，角速度，位置，速度
time = 3; %シミュレーション時間
r0 = 0.5; %ステップ目標信号の大きさ[m]
d0 = 5; %ステップ外乱信号の大きさ[v]
st_time=0; %ステップ信号の開始時間
% グラフ表示パラメータ
ymin = -0.1;
ymax = r0*1.1;
% 評価関数用のパラメータ：（24）式
q1 = 4; % 台車位置偏差に対する重み
q2 = 100; % 積分補償に対する重み
R = 0.005; % 入力重み

%----------------------------------
%第1表倒立振子の物理定数
mp = 0.06; %振子の質量
mc = 0.6; %台車の質量
J = 8.*10^(-4); % 振子の重心周りの慣性モーメント
l = 0.2; %振子の端から重心までの長さ
mup = 1.2*10^(-5); %振子軸の粘性摩擦の比例定数
muc = 2.5; %台車の粘性摩擦の比例定数
alpha = 0.3; %モータ電圧から台車の駆動力へのゲイン
g = 9.80; % 重力加速度

%% 台車型倒立振子の制御（3章）

%----------------------------------
% システム行列の定義
I = (mp+mc)*J+mp*mc*l^2;
A = [ 0 1 0 0;
    mp*g*l*(mp+mc)/I -mup*(mp+mc)/I 0 muc*mp*l/I;
    0 0 0 1;
    -mp^2*g*l^2/I mup*mp*l/I 0 -muc*(J+mp*l^2)/I];
B = [0;
    -alpha*mp*l/I;
    0;
    alpha*(J+mp*l^2)/I];
C = [0 0 1 0];
% システムのサイズ
n = size(A,1);

%----------------------------------
% 拡大形の定義：(21)式
Ah = [A zeros(n,1);
     -C  zeros(1)];
Bh = [B;
     0];
Ch = [-C zeros(1);
    zeros(1,n) eye(1)];

%----------------------------------
% フィードバックゲインの計算
Q = [q1 0;
     0 q2];
F=lqr(Ah,Bh,Ch'*Q*Ch,R);
% 閉ループ系の極の表示
eig(Ah-Bh*F)
% ゲインの分解：(20)式と(23)式
F1 = -F(1,1:4);
F2 = -F(1,5);

%----------------------------------
% シミュレーション(外乱なし)
sim('pend_lqr',time); 
% シミュレーション(外乱あり)
sim('pend_lqr_dis',time); 

figure(1)
    subplot(211)
        hold on;
        grid on;
        box on;
        plot(r.time,r.signals.values,'k-','linewidth',1);
        plot(zf.time,zf.signals.values,'k--','linewidth',2);
        plot(zfd.time,zfd.signals.values,'k-','linewidth',2);
        set(gca,'Ylim',[ymin ymax]);
        set(gca,'Xlim',[0 time]);
        xlabel('time t [s]','FontSize',15);
        ylabel('z(t) and r(t)','FontSize',15);  
        legend('r_0=0.5','外乱無し','外乱有り','FontSize',12)
    subplot(212)
        hold on;
        grid on;
        box on;
        plot(thetaf.time,thetaf.signals.values,'k--','linewidth',2);
        plot(thetafd.time,thetafd.signals.values,'k-','linewidth',2);
        set(gca,'Ylim',[-0.15 0.15]);
        set(gca,'Xlim',[0 time]);
        xlabel('time t [s]','FontSize',15);
        ylabel('\theta(t)','FontSize',15);    
