clear,clc;
% 系统参数
m=2;
g=9.81;
I=1;
L=0.5;

% 状态空间模型
A = [0 1 0 0;
    0 0 -m*g*L^2/I 0;
    0 0 0 1;
    0 0 m*g*L/I 0]
B = [0;
    (I-m*L^2)/(I*m);
    0;
    L/I]
C = [1 0 0 0;
    0 0 1 0]
D=[0;
    0]
sys_ss = ss(A,B,C,D)

% 可控可观性判断
cont = ctrb(sys_ss)
if rank(cont)==4
    disp("该系统可控!")
else
    disp("该系统不可控!")
end
obser = obsv(sys_ss)
if rank(obser)==4
    disp("该系统可观!")
else
    disp("该系统不可观!")
end

disp("-------------------------------")

% LQR
Q = C'*C;
Q(1,1) = 5000;
Q(3,3) = 100;
R = 1;
K = lqr(A,B,Q,R) %状态反馈控制增益矩阵

% 新的状态空间模型
Ac = [(A-B*K)]
Bc = [B]
Cc = [C]
Dc = [D]
sys_cl = ss(Ac,Bc,Cc,Dc)
% 可控可观性判断
cont = ctrb(sys_cl)
if rank(cont)==4
    disp("该系统可控!")
else
    disp("该系统不可控!")
end
obser = obsv(sys_ss)
if rank(obser)==4
    disp("该系统可观!")
else
    disp("该系统不可观!")
end
t = 0:0.01:5;
r =0.2*ones(size(t));
[y,t,x]=lsim(sys_cl,r,t);
[AX,H1,H2] = plotyy(t,y(:,1),t,y(:,2),'plot');
set(get(AX(1),'Ylabel'),'String','cart position (m)')
set(get(AX(2),'Ylabel'),'String','pendulum angle (radians)')
title('Step Response with LQR Control')