pendemo4.m

非线性控制 Matlab编译

% "Swing up" control of the pedulum
%
% Copyright (c) 1998 The University of Texas at Austin

clear all
clf
u2=0;


% INITIAL STATE

angle = .2;
velocity = 0;
x0=[angle velocity]; 

u = 0; % initial control force

% TIME SPECIFICATIONS

final_time = 12;   % seconds
dt= 0.01;	  % Time for simulation step
DSample = 5;      % update control every Sample*dt seconds

T = dt:dt:final_time;
steps = length(T);
plotstep = 1;     % update graphics every plotstep time steps

%To record positions and control force.
X = zeros(2,steps);
U = zeros(1,steps);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% DESIGN LINEAR CONTROLLER

m=2; %Length of the observation sequence
d=0; %Perturbation propagation delay. Bridgesim.m has the same parameter.
n=2; %Length of the controlling sequence
neighbors=20; %Number of nearest neighbors for local plane  approximation
niter=150;    %Total number of points for surface reconstruction
echo off
%%%%%%%%%%%%%%%%%%%%%
goaly=[pi];  %Objective: vertical position
goalu=Inf;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Simulate controlled pendulum

x = x0'; i = 1; j = 0;

while (i<steps)
 i = i+1;
 % UPDATE STATE
 % state simulation based on Lagrange method

 x = pend_sim(x,u,dt) + 0.001*randn(2,1);%Process noise is added;
 obs=x(1)+0.001*randn;  %Measurement noise

 % Update Control force

 if rem(i,DSample) <= eps,   % Apply control once in DSample time steps.

  j = j + 1;
  u2=u;

if (j==1)
disp '1. Autonomous dynamics, no perturbations applied.'
disp '****************************************************'
end

if (j==50)
disp '2. Random perturbations applied to interrogate the system.'
disp '*********************************************************'
end

  if(j > 50 & j < 101)
   u=contr(obs, goaly, goalu, 0, 10., niter, m, n, d, neighbors); 
  end

if (j==100)
disp '3. Adaptive approach to the upward position.'
disp '**********************************************************'
end

 if(j > 101 & j < 151)
  u=contr(obs, goaly, goalu, 2, 10.0, niter, m, n, d, neighbors); 
 end

if (j==150)
disp '4. Control is ON.'
disp '**********************************************************'
end

 if(j > 150)
   u=contr(obs, goaly, goalu, 1, 100, niter, m, n, d, neighbors); 
 end
   
end

% Draw pendulum and timeseries
	if( i > 2 & rem(i,10)==0)
    clf  
    ind=(1:i-1);
    subplot('position',[0.1 0.5 0.4 0.4]);
    plot(ind,X(2,ind)), axis([0 steps -10 10]);
    
    subplot('position',[0.1 0.05 0.8 0.4]);
    plot(ind,U(ind)), axis([0 steps -200 200]);
    
    subplot('position',[0.55 0.55 0.4 0.4]), axis([0 1 0 1]);
     ax=get(gcf,'currentaxes');
     % cla;
     set(ax,'box','off','xtick',[],'ytick',[],'xcolor',[0 0 0],'ycolor',[0 0 0],...
         'xlim',[0,1],'ylim',[0,1],'vis','off', 'DrawMode', 'Fast');
     xpl(1)=0.5;
     ypl(1)=0.5;
     xpl(2)=0.4*sin(obs)+0.5;
     ypl(2)=-0.4*cos(obs)+0.5;
     set(ax,'NextPlot','Replace');
	  plot(xpl,ypl,'r-','Linewidth',3);
     set(ax,'vis','off','NextPlot','Add','xlim',[0,1],'ylim',[0,1]);
     plot(xpl(2),ypl(2),'b.','Markersize',40);
     drawnow
   end
 
 % SAVE VALUES
 X(:,i) = x;
 U(:,i) = u;

end


    ind=(1:i-1);
    subplot(2,1,1), plot(ind,X(2,ind),'g-');
    subplot(2,1,2), plot(ind,U(ind));
    drawnow