lordemo.m

非线性控制 Matlab编译

%  LORENZDEMO Demonstrates targeting of unstable states in Lorenz
%  Equations
%  Visualiztion and integration procedure 
%  adapted from lorenz.m by Mathworks

clear all
clf
%Controller parameters
    %dimensionality of the system 	 
    m=1;
    d=0;
    n=1;
    neighbors=30;
    niter=400;
    
% Set two targeting values
    goaly1=[27; 8.5; 8.5];
    goaly2=[27; -8.5; -8.5];
    goalu=Inf; %Assume du/dt=0 at the target


%step counter for control algorithm 
    step = -1000;
  
   % Information regarding the play status will be held in
% the axis user data according to the following table:
play= 1;
stop=-1;

    figure(gcf);
    axHndl=gca;
    figNumber=gcf;
        % ====== Start of Demo
    %set(figNumber,'Backingstore','off');
    % The graphics axis limits are set to values known 
    % to contain the solution.
    set(axHndl, ...
        'XLim',[0 40],'YLim',[-35 10],'ZLim',[-10 40], ...
	'Userdata',play, ...
	'Drawmode','fast', ...
        'Visible','on', ...
	'NextPlot','add', ...
        'Userdata',play, ...
	'View',[-37.5,30]);
    xlabel('X');
    ylabel('Y');
    zlabel('Z');

    % The values of the global parameters are
    global SIGMA RHO BETA
    SIGMA = 10.;
    RHO = 28.;
    BETA = 8./3.;
 
    % The orbit ranges chaotically back and forth around two different points,
    % or attractors.  It is bounded, but not periodic and not convergent.
    % The numerical integration, and the display of the evolving solution,
    % are handled by the function ODE23P.
 
    FunFcn='lorenzeq';
    % The initial conditions below will produce good results
    %y0 = [300 1 0];
    % set initial perturbation
    u = [0;0;0];
    % Random initial conditions
    y0(1)=rand*30+5;
    y0(2)=rand*35-30;
    y0(3)=rand*40-5;
    t0=0;
    tfinal=140;
    pow = 1/3;
    tol = 0.001;
 
    t = t0;
    hmax = (tfinal - t)/5;
    hmin = (tfinal - t)/200000;
    h = (tfinal - t)/100;
    y = y0(:);
    tau = tol * max(norm(y,'inf'),1);
 
    % Save L steps and plot like a comet tail.
    L = 50;
    Y = y*ones(1,L);
 
    cla;
    head = line( ...
	'color','r', ...
	'linestyle','.', ...
	'markersize',25, ...
	'erase','xor', ...
	'xdata',y(1),'ydata',y(2),'zdata',y(3));
    body = line( ...
	'color','y', ...
	'linestyle','-', ...
	'erase','none', ...
	'xdata',[],'ydata',[],'zdata',[]);
    tail=line( ...
	'color','b', ...
	'linestyle','-', ...
	'erase','none', ...
	'xdata',[],'ydata',[],'zdata',[]);

	    
    % The main loop
    while  (h >= hmin)
   	if t + h > tfinal, h = tfinal - t; end
        if (step == 1)
	  disp 'Applying random perturbations.'
	elseif (step == 401)
	  disp 'Targeting upper state'
	elseif (step == 501)
	  disp 'Targeting lower state'
	elseif (step == 601)
	  disp 'Targeting upper state again'
	elseif (step == 701)
	  disp 'Targeting lower state again'
	elseif (step == 801)
	  disp 'Let it go'
	end
        % Compute the slopes
      	s1 = feval(FunFcn, t, y);
      	s2 = feval(FunFcn, t+h, y+h*s1);
      	s3 = feval(FunFcn, t+h/2, y+h*(s1+s2)/4);
 
      	% Estimate the error and the acceptable error
      	delta = norm(h*(s1 - 2*s3 + s2)/3,'inf');
      	tau = tol*max(norm(y,'inf'),1.0);
 
      	% Update the solution only if the error is acceptable
      	ts = t;
      	ys = y;
      	if delta <= tau
            step = step + 1;
	    t = t + h;
	    y_last = y;
            y = y + h*(s1 + 4*s3 + s2)/6;
            % add the perturbation 	    
            y = y + u;
            % apply algorithm
        
	    if (step > 0)	    
	      if (step < 401)
             u = contr(y,goaly1,goalu,0,[1; 1; 1],niter,m,n,d,neighbors);
                elseif (step > 400 & step < 501)
	          u = contr(y,goaly1,goalu,1,[1; 1; 1],niter,m,n,d,neighbors);
                elseif (step > 500 & step < 601)
	          u = contr(y,goaly2,goalu,1,[1; 1; 1],niter,m,n,d,neighbors);
                elseif (step > 600 & step < 701)
	          u = contr(y,goaly1,goalu,1,[1; 1; 1],niter,m,n,d,neighbors);
                 elseif (step > 700 & step < 801)
	          u = contr(y,goaly2,goalu,1,[1; 1; 1],niter,m,n,d,neighbors);
          			elseif (step == 801)
              u=[0.5; 0.5; 0.5]; %Kick it out or it will take some time
          		else
	          u = [0; 0; 0];
	      end
	    end	   		
            
	    % Update the plots
            Y = [y Y(:,1:L-1)];
            set(head,'xdata',Y(1,1),'ydata',Y(2,1),'zdata',Y(3,1))
            set(body,'xdata',Y(1,1:2),'ydata',Y(2,1:2),'zdata',Y(3,1:2))
            set(tail,'xdata',Y(1,L-1:L),'ydata',Y(2,L-1:L),'zdata',Y(3,L-1:L))
	    if (step > 1)
	      xs(:,step) = y-y_last;
	      us(:,step) = u;
	      ind=(max(1,step-29):step);
	      %figure(2)
	      %subplot(2,1,1),plot(ind,xs(1,ind),'y-',ind,xs(2,ind),'g-',ind,xs(3,ind),'b-'),title 'y';
	      %subplot(2,1,2),plot(ind,us(1,ind),'y-',ind,us(2,ind),'g-',ind,us(3,ind),'b-'),title 'u';
	    end
	    drawnow;
        end
 
      	% Update the step size
      	if delta ~= 0.0
            h = min(hmax, 0.9*h*(tau/delta)^pow);
      	end

    end;    % Main loop ...