%  FZH.M  倒擺系統(tǒng)演示
   clear all, close all, nfig=0;
   delete fzh.out      
   diary fzh.out
   M=2.0;m=0.1;
   len=0.5;
   g=9.81;
   c1=M*len; c2=m*len; c3=m*g; c4=(M+m)*g;
   A=[0 1 0 0;c4/c1 0 0 0;0 0 0 1;-c3/M 0 0 0];
   B1=[0 -1/c1 0 1/M]'; B2=[0 1/c2 0 0]';
   C=[0 0 1 0];  D=[0];
   disp('Controllability Matrix for this system'), CM=ctrb(A,B1)
   disp('Rank of Controllability Matrix'), rank(CM)
   clp=[-1.5+3.0j -1.5-3.0j -5.0 -4.0];
   Ks=place(A,B1,clp);
   Nr=-1.0/(C*inv(A-B1*Ks)*B1);
   BB=[B1 B2];D=[0 0];    
   sysc1=ss(A-B1*Ks,BB,C,D);
   t0=0; tf=5; Nt=101; t=linspace(t0,tf,Nt)';
   u1=zeros(size(t));
   rd=ones(size(t)); 
   v1=zeros(size(t)); 
   w1=[Nr*rd v1];
   [y1,t,x1]=lsim(sysc1,w1,t);
   for i=1:Nt, u1(i)=Nr*rd(i)-Ks*x1(i,:)'; end   
   nfig=nfig+1; figure(nfig)
   subplot(2,1,1),plot(t,y1,'r-'),grid
   ylabel('Cart Position(m)'),xlabel('Time(sec)')
   title('Linear Inverted Pendulum (Case 1:Fw=0)')
   subplot(2,1,2),plot(t,u1,'b-'),grid
   ylabel('Inputs(N)'),xlabel('Time(sec)')
   diary off