%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%===============triplelqrlqy.m=============================
%系統參數設置、狀態空間方程建立、LQR/LQY控制器設計及各自的
%階躍響應曲線
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%程序如下
%參數設置
M1=1.320;           %小車質量/kg                
M2=0.208;          %1、2擺桿間的質量快/kg 
M3=0.208;          %2、3擺桿間的質量快/kg       
L1=0.090;          %1擺質心到轉軸長度/m         
L2=0.110;          %2擺質心到轉軸長度/m         
L3=0.270;          %3擺質心到轉軸長度/m         
c0=0;              %小車與導軌間的摩擦系數      
c1=0;              %1擺桿與小車間的摩擦系數     
c2=0;              %1、2擺桿間摩擦系數          
c3=0;              %2、3擺桿間摩擦系數          
m1=0.040;          %第一擺桿質量/kg             
m2=0.050;          %第二擺桿質量/kg             
m3=0.132;          %第三擺桿質量/kg             
x=0;               %小車位置/m 
g=9.81;             
%狀態空間方程
k1=m1+m2+m3+M1+M2;
k2=m1+2*m2+2*m3+2*M1+2*M2;
k3=m1/3+m2+m3+M1+M2;
k4=m1+2*m3+2*M2;
k5=m2/3+m3+M2;
G=[k1 k2*L1 k4*L2 k3*L3;
    k2*L1 4*k3*(L1)^2 2*k4*L1*L2 2*k3*L1*L3;
    k4*L2 2*k4*L1*L2 4*k5*(L2)^2 2*k3*L2*L3;
    k3*L3 2*k3*L1*L3 2*k3*L2*L3 4/3*k3*(L3)^3];
H0=[1 0 0 0]';
H1=[-c0 0 0 0;
    0 -c1 0 0;
    0 c2 -c2 0;
    0 0  c3 -c3];
H2=[0 0 0 0;
    0 -k2*g*L1 0 0;
    0 0 -k4*g*L2 0;
    0 0 0 -k3*g*L2];
A=[zeros(4) eye(4);
    inv(G)*H2 inv(G)*H1];
B=[zeros(4,1);
    inv(G)*H0];
C=[eye(4) zeros(4)];
D=[0 0 0 0]';
op=eig(A)
Q0=1*diag([1 1 1 1 0 0 0 0]);%狀態變量的敏感程度
R0=1;%輸入量的敏感度
K0=lqr(A,B,Q0,R0)  % 計算LQR控制矩陣
Ac0=A-B*K0;
Q1 = 8*eye(4);
R1 =0.1; 
K1=lqry(A,B,C,D,Q1,R1)  % 計算LQY控制矩陣
Ac1=A-B*K1;
Bc=[B];
Cc = [C];
Dc = [D];
% 計算LQR增益Nbar0
Cn = [1 0 0 0 0 0 0 0];
Nbar0 = rscale(A,Bc,Cn,0,K0);
Bcn0 = [Nbar0*B];
% 計算LQY增益Nbar1
Nbar1 = rscale(A,Bc,Cn,0,K1);
Bcn1 = [Nbar1*B];
% 求階躍響應并顯示
T = 0:0.005:5;
U = 0.2*ones(size(T));
[Y1,X] = Lsim(Ac0,Bcn0,Cc,Dc,U,T);
plot(T,Y1(:,1),'-.',T,Y1(:,2),'-',T,Y1(:,3),'--',T,Y1(:,4),':')
legend('LQR-Cart Position','LQR-Pendulum1','LQR-Pendulum2','LQR-Pendulum3')
grid
figure
[Y2,X] = Lsim(Ac1,Bcn1,Cc,Dc,U,T);
plot(T,Y2(:,1),'-.',T,Y2(:,2),'-',T,Y2(:,3),'--',T,Y2(:,4),':')
legend('LQY-Cart Position','LQY-Pendulum1','LQY-Pendulum2','LQY-Pendulum3')
grid
%==============end=======================