% "Swing up" control of the pedulum%% Copyright (c) 1998 The University of Texas at Austinclear allclfu2=0;% INITIAL STATEangle = .2;velocity = 0;x0=[angle velocity]; u = 0; % initial control force% TIME SPECIFICATIONSfinal_time = 12;   % secondsdt= 0.01;	  % Time for simulation stepDSample = 5;      % update control every Sample*dt secondsT = 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 CONTROLLERm=2; %Length of the observation sequenced=0; %Perturbation propagation delay. Bridgesim.m has the same parameter.n=2; %Length of the controlling sequenceneighbors=20; %Number of nearest neighbors for local plane  approximationniter=150;    %Total number of points for surface reconstructionecho off%%%%%%%%%%%%%%%%%%%%%goaly=[pi];  %Objective: vertical positiongoalu=Inf;%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Simulate controlled pendulumx = 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 '****************************************************'endif (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);   endif (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);  endif (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