lqr.rar_LQR 响应_lq_lq matlab_二级倒立摆lqr_倒立摆 matlab

clc A=[0 1 0 0 0 0;0 -16.6601 -1.2973 0 0.0857 0; 0 0 0 1 0 0;0 39.0555 18.054 0 -7.863 0; 0 0 0 0 0 1;0 -68.512 -14.4458 0 25.9635 0]; B=[0;0.72;0;-1.7044;0;0.2069]; C=[1 0 0 0 0 0;0 1 0 0 0 0; 0 0 1 0 0 0;0 0 0 1 0 0; 0 0 0 0 1 0;0 0 0 0 0 1]; D=0; Q=[100000 0 0 0 0 0 ;0 0 0 0 0 0; 0 0 10 0 0 0;0 0 0 0 0 0; 0 0 0 0 10 0;0 0 0 0 0 0]; R=1; [P,L,K]=care(A,B,Q,R) Aq=[A-B*K]; Bq=[B]; Cq=[C]; Dq=[D]; sys=ss(A,B,C,D); [y,t,x]=step(sys) sys1=ss(Aq,Bq,Cq,Dq); [y1,t1,x1]=step(sys1); p=eig(A) q=eig(Aq) u=-K*x1'; figure(1) subplot(2,2,2),plot(t,y(:,1)),title('小车位移阶跃响应曲线'),grid subplot(2,2,[1,3]),plot(t,y(:,3)),title('下摆角响应曲线'),grid subplot(2,2,4),plot(t,y(:,5)),title('上摆角响应曲线'),grid figure(2) subplot(2,2,1),plot(t1,y1(:,1)),title('小车位移响应曲线'),grid subplot(2,2,2),plot(t1,y1(:,3)),title('下摆角响应曲线'),grid subplot(2,2,3),plot(t1,y1(:,5)),title('上摆角响应曲线'),grid subplot(2,2,4),plot(t1,u),title('最优控制曲线'),grid