# nlcontrol

### Stephen vanHook (view profile)

Nonlinear algorithm for controlling dynamical systems, particularly where linear methods fail.

pendemo4.m
```% "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);
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

```