www.pudn.com > controlsystem.ZIP > EX3052.M

% Example 3.52 
% 
% Design of an inverted pendulum control system 
%  using optimal regulator 
% 
% Design 
a=[0 1 0 0; 20.601 0 0 0; 0 0 0 1; -0.4905 0 0 0]; 
b=[0; -1; 0; 0.5]; 
c=[0 0 1 0]; 
d=[0]; 
[g,h]=c2d(a,b,0.1) 
g1=[g,zeros(4,1); -c*g, 1]; 
h1=[h; -c*h]; 
q=diag([10,1,100,1,1]); r=[1]; 
[k1,p,e]=dlqr(g1,h1,q,r); 
disp('The optimal feedback gain matrix k is') 
ki=-k1(1,5) 
k=k1(1,1:4) 
% 
% Step response 
% 
% The close loop state system is denoted as (gc,hc,cc,dc) 
% 
gc=g1-h1*k1; 
hc=[0; 0; 0; 0; 1]; 
cc=[0 0 1 0 0]; dc=[0]; 
figure(1) 
v=[0 100 0 1.2]; 
[y,x]=dstep(gc,hc,cc,dc,1,100); 
kk=1:length(y); 
plot(kk,y,'oy',kk,y,'-w') 
axis(v) 
title('Step Response of Servo Control system') 
xlabel('Sample') 
ylabel('Output y(position of cart)') 
 
figure(2) 
v=[0 100 -0.2 0.8]; 
subplot(2,2,1) 
ww=[1,0,0,0,0]; y1=x*ww'; 
plot(kk,y1,'oy',kk,y1,'-w'); axis(v) 
title('Amgular Displacement Theta'); ylabel('x1') 
 
subplot(2,2,2) 
ww=[0,1,0,0,0]; y1=x*ww'; 
plot(kk,y1,'oy',kk,y1,'-w'); axis(v) 
title('Amgular Velocity Theta Dot'); ylabel('x2') 
 
subplot(2,2,3) 
ww=[0,0,0,1,0]; y1=x*ww'; 
plot(kk,y1,'oy',kk,y1,'-w'); axis(v) 
title('Velocity of Cart'); xlabel('Sample'); ylabel('x4') 
 
v=[0 100 -5 25]; 
subplot(2,2,4) 
ww=[0,0,0,0,1]; y1=x*ww'; 
plot(kk,y1,'oy',kk,y1,'-w'); axis(v) 
title('Output of Integrator'); xlabel('Sample'); ylabel('x5')