clear; clc;
format short;
k12 = 86.69; k13 = -21.62; k17 = 6.64;
k22 = -40.31; k23 = 39.45; k27 = -0.088;
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 k12 k13 0 0 0;
0 k22 k23 0 0 0];
B = [0 0 0 1 k17 k27]';
C = [1 0 0 0 0 0;
0 1 0 0 0 0;
0 0 1 0 0 0];
D = [0; 0 ;0];
Q11 = 500; %代表小车位置的权重
Q22 = 500; %代表摆杆 1 角度的权重
Q33 = 500; %代表摆杆 2 角度的权重
Q44 = 0; %小车速度的权重
Q55 = 0; %摆杆 1 角速度的权重
Q66 = 0; %摆杆 2 角速度的权重
Q = [Q11 0 0 0 0 0 ;
0 Q22 0 0 0 0 ;
0 0 Q33 0 0 0 ;
0 0 0 Q44 0 0 ;
0 0 0 0 Q55 0 ;
0 0 0 0 0 Q66];
R = 1; %控制能量的限制
K = lqr(A,B,Q,R)
% Wc = -C/(A-B*K)*B
% L = 1/Wc(1)
Ang1_0 = -3/180*pi;
Ang2_0 = 5/180*pi;
sim('lip2_lqr.slx',6) ;
figure(1)
set(gcf,'Units','centimeters','Position',[10 10 5*1.45 4*1.45]);%设置图片大小为 5*1.45cm× 4*1.45cm
plot(T, Ang1*180/pi,'b'); hold on
plot(T,Ang2*180/pi,'r');
axis([0 4 -14 16])
% hold off
legend('Ang1','Ang2')
xlabel('t(s)');
ylabel('\theta (°)' );
title('角度变化曲线')
grid on;
hold on;
figure(2)
set(gcf,'Units','centimeters','Position',[20 10 5*1.45 4*1.45]);%设置图片大小为 5*1.45cm× 4*1.45cm
plot(T, Pos, 'b');
xlabel('t(s)');
ylabel('x (m)');
title('位置变化曲线')
grid on;
hold on;
% Ac = [(A-B*K)];
%
% Bc = B;
% Cc = [C];
% Dc = [D];
% Cn = [1 0 0 0 0 0];
%
% s = size(A,1);
% Z = [zeros(1,s) 1];
%
% N = inv([A,B;Cn,0])*Z';
% Nx = N(1:s);
% Nu = N(1+s);
% Nbar = Nu + K*Nx;
%
% Bcn = [Nbar*B];
% x0 = [0 0 0.05 0 0 0];
% T = 0:0.01:10;
% U = zeros(size(T));
% [Y,X] = lsim(Ac,Bcn,Cc,Dc,U,T,x0);
% xpos = Y(:,1);
% xangle = Y(:,2);
% xangle2 = Y(:,3);
%
% plot(T,xpos,':');
% hold on
% plot(T,xangle,'--')
% plot(T,xangle2,'-')