M=1.096;                                   %小車質(zhì)量
m=0.109;                                   %擺桿質(zhì)量
b=0.1;                                     %小車摩擦系數(shù)
l=0.25;                                    %擺桿轉(zhuǎn)動軸心到桿質(zhì)心的長度
I=0.0034;                                  %擺桿慣量
T=0.005;                                   %采樣時間
g=9.8;
A1=[0               1                           0                       0;
   0 -((I+m*l^2)*b)/(I*(M+m)+M*m*l^2)   m^2*g*l^2/(I*(M+m)+M*m*l^2)   0;
   0               0                           0                       1;
   0   -m*l*b/(I*(M+m)+M*m*l^2)       m*g*l*(M+m)/(I*(M+m)+M*m*l^2)    0]
B1=[ 0;   (I+m*l^2)/(I*(M+m)+M*m*l^2);    0;    m*l/(I*(M+m)+M*m*l^2) ]
C1=[1 0 0 0;
   0 0 1 0]
D1=[0; 0]
sys=ss(A1,B1,C1,D1);%改此處了
T=0.005;%改此處了
sysd=c2d(sys,T,'zof')
[A,B,C,D,TS]=ssdata(sysd)

Gf=M*sysd
Gfs=ss(Gf)
[A2,B2,C2,D2]=ssdata(Gfs)
po=pole(Gfs);    % 開環(huán)系統(tǒng)極點
Co=ctrb(Gfs);
Ob = obsv(Gfs);
unctrb=length(A)-rank(Co)                 %可控性
unobv = length(A)-rank(Ob)                %可觀性
key=1;
while key~=0
  q11=input('please input q11 \n q11=');
  q33=input('please input q33 \n q33=');
  %disp('your Q matrix is');
  Q=[q11 0 0   0;
      0  0 0   0;
      0  0 q33 0;
      0  0 0   0];
  R=1;
  disp('your feedback matrix ')
  [K,S,e]=dlqry(A2,B2,C2,D2,Q,R)    %s為對應(yīng)黎卡提方程的唯一正定解P，E為A-BK的特征值%改此處了
  Ky=[K(3),K(4),K(5),K(6)]
  %sys2=ss(A-B*K,B,C,D);
 % [G,H,Cd,Dd,ts]=ssdata(sys2)
  %pc=pole(sys2)                           %閉環(huán)系統(tǒng)極點
  
  %pc=eig(A-B*K)
  key=input('Would you continue? Select 0 to end program '); 
end

disp('call simulation dlqr003');
dlqr003;                                      %調(diào)用仿真模型