%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%-----------------------doublelqrlqy.m---------------------
%% 最優控制（量綱匹配）:為了使輸入與反饋的量綱互相匹配，給輸入乘以增益,
%在前面的設計方法中，是把輸出信號反饋回來乘以一個系數矩陣,然后與輸入量 
%相減得到控制信號 
% 確定開環極點的程序如下
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%系統狀態空間方程
A=[0 0 0 1 0 0;
    0 0 0 0 1 0;
    0 0 0 0 0 1;
    0 0 0 0 0 0; 
    0 64.966 -14.865 0 0 0;
    0 -32.483 31.933 0 0 0];
B=[0;0;0;1;5.1124;0.0562];
C=[eye(3) zeros(3)];
D=[0;0;0];
%
Q0=1*diag([1 1 1 0 0 0]);
R0=0.8;
Q1=8*diag([1 1 1]);
R1=0.1;
% 計算LQR控制矩陣
K0 = lqr(A,B,Q0,R0)
% 計算LQY控制矩陣
K1=lqry(A,B,C,D,Q1,R1)
Ac0 = [(A-B*K0)];
Ac1 = [(A-B*K1)];
Bc = [B];
Cc = [C];
Dc = [D];
% 計算LQR增益Nbar0
Cn = [1 0 0 0 0 0];
Nbar0 = rscale(A,B,Cn,0,K0);
Bcn0 = [Nbar0*B];
% 計算LQY增益Nbar1
Nbar1 = rscale(A,B,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),'--')
hold on
[Y2,X] = Lsim(Ac1,Bcn1,Cc,Dc,U,T);
plot(T,Y2(:,1),'-.',T,Y2(:,2),'-',T,Y2(:,3),'--')
hold off
grid
% 計算LQR控制矩陣
K0 = lqr(A,B,Q0,R0)
% 計算LQY控制矩陣
K1=lqry(A,B,C,D,Q1,R1)
Ac0 = [(A-B*K0)];
Ac1 = [(A-B*K1)];
Bc = [B];
Cc = [C];
Dc = [D];
% 計算LQR增益Nbar0
Cn = [1 0 0 0 0 0];
Nbar0 = rscale(A,B,Cn,0,K0);
Bcn0 = [Nbar0*B];
% 計算LQY增益Nbar1
Nbar1 = rscale(A,B,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),'--')
hold on
[Y2,X] = Lsim(Ac1,Bcn1,Cc,Dc,U,T);
plot(T,Y2(:,1),'-.',T,Y2(:,2),'-',T,Y2(:,3),'--')
hold off
grid
legend('LQR-Cart Position','LQR-Pendulum1','LQR-Pendulum2','LQY-Cart Position','LQY-Pendulum1','LQY-Pendulum2')
%=====================end==========================