10-04-2017, 07:58 PM
%%
%
% This program implements a controller that uses a planning
% strategy for the surge tank example.
%
% Kevin Passino
% Version: 4/19/01
%
%%
% Initialize variables
clear
% Set the length of the simulation
Nnc=300;
T=0.1; % Sampling rate
% As a reference input, we use a square wave (define one extra
% point since at the last time we need the reference value at
% the last time plus one)
timeref=1:Nnc;
r(timeref)=3.25-3*square((2*pi/150)*timeref); % A square wave input
%r(timeref)=3.25*ones(1,Nnc+1)-3*(2*rand(1,Nnc+1)-ones(1,Nnc+1)); % A noise input
ref=r(1:Nnc); % Then, use this one for plotting
time=1:Nnc;
time=T*time; % Next, make the vector real time
% We assume that the parameters of the surge tank (truth model) are:
abar=0.01; % Parameter characterizing tank shape (nominal value is 0.01)
%abar=0.05; % Parameter characterizing tank shape
bbar=0.2; % Parameter characterizing tank shape (nominal value is 0.2)
cbar=1; % Clogging factor representing dirty filter in pump (nominally, 1)
%cbar=0.8; % Clogging factor representing dirty filter in pump (dirty case)
dbar=1; % Related to diameter of output pipe
g=9.8; % Gravity
% Controller parameters
% Model used in planning strategy:
abarm=0.002; % Parameter characterizing tank shape
bbarm=0.2; % Parameter characterizing tank shape
cbarm=0.9; % Clogging factor representing dirty filter in pump
dbarm=0.8; % Related to diameter of output pipe
% Next, use a set of preset controllers (think of each as a plan template)
% In this case we use PI controllers, with a grid of Kp, Ki gains around the
% ones that we had designed for the PI controller for the **model**
% Guess at values
Kp=0.01;
Ki=0.3;
%Kpvec=Kp; % Vector of gains in the region of Kp, Ki
%Kivec=Ki;
Kpvec=0:0.05:0.2; % Vector of gains in the region of Kp, Ki
%Kivec=Ki;
Kivec=0.15:0.05:0.4;
%Kpvec=0.2:0.2:1.8; % Vector of gains in the region of Kp, Ki
%Kivec=0.05:0.01:0.11;
% Set weights of cost function used to select plans
w1=1;
w2=1;
% Set the length of time that will project into the future, N
NN=[20]; % To test just one value for N
%NN=[1,5,10,15,17,20,25,30,33,35,36,37,38,39,40,45,50]; % To test multiple values of the projection length
N=max(NN); % For initialization, and allocation of variables below
% First, compare the truth model characteristics to the model
height=0:.1:10;
for i=1:length(height)
crossect_truth(i,1)=abs(abar*height(i)+bbar);
crossect_model(i,1)=abarm*height(i)^2 +bbarm;
end
figure(1)
clf
plot(height,crossect_truth,'k-',height,crossect_model,'k--')
grid
ylabel('Cross-sectional area')
xlabel('Height')
title('Cross-sectional area for plant (solid) and model (dashed)')
%%
% Next, start the main loop:
%%
for kk=1:length(NN)
N=NN(kk); % Next projection length
h=0*ones(1,Nnc+1); % Allocate memory
hp=0*ones(1,Nnc+1);
hmm=0*ones(1,Nnc+1);
h(1)=1; % Initial liquid level
hp(1)=1; % Initial liquid level (for plant with a PI controller)
hmm(1)=1; % Initial liquid level (for model with a PI controller)
% Allocate memory for projection variables
hm=0*ones(N+1,length(Kpvec),length(Kivec));
em=0*ones(N,length(Kpvec),length(Kivec));
um=0*ones(N,length(Kpvec),length(Kivec));
J=0*ones(length(Kpvec),length(Kivec));
rowindex=0*ones(1,Nnc);
colindex=0*ones(1,Nnc);
% Also, allocate memory for the control inputs and error
up=0*ones(1,Nnc);
umm=0*ones(1,Nnc);
u=0*ones(1,Nnc);
ep=0*ones(1,Nnc);
emm=0*ones(1,Nnc);
e=0*ones(1,Nnc);
% Initialize controller integrator:
sume=0;
sumep=0;
sumemm=0;
for k=1:Nnc
%--
% Define the controller (for plant), where test single Kp, Ki gains
ep(k)=r(k)-hp(k);
sumep=sumep+ep(k); % Compute integral approx.
up(k)=Kp*ep(k)+Ki*sumep;
% In implementation, the input flow is restricted
% by some physical contraints. So we have to put a
% limit for the input flow that is chosen as 50.
if up(k)>50
up(k)=50;
end
if up(k)<-50
up(k)=-50;
end
% Plant equations
Ap=abs(abar*hp(k)+bbar); % Cross-sectional area
alphap=hp(k)-T*dbar*sqrt(2*g*hp(k))/Ap; % Nonlinear dynamics
betap=T*cbar/Ap;
hp(k+1)=alphap+betap*up(k); % Compute plant output
hp(k+1)=max([0.001,hp(k+1)]); % Constraint liquid not to go
% negative
%--
% Define the controller (for model), also where test single Kp, Ki
% gains - here for the purpose of seeing how good the model is
% (test model validity by seeing if it reacts similarly to the plant
% for the same control system).
emm(k)=r(k)-hmm(k);
sumemm=sumemm+emm(k); % Compute integral approx.
umm(k)=Kp*emm(k)+Ki*sumemm;
% In implementation, the input flow is restricted
% by some physical contraints. So we have to put a
% limit for the input flow that is chosen as 50.
if umm(k)>50
umm(k)=50;
end
if umm(k)<-50
umm(k)=-50;
end
% Model equations
Amm=abarm*hmm(k)^2 +bbarm; % Cross-sectional area (model)
alphamm=hmm(k)-T*dbarm*sqrt(2*g*hmm(k))/Amm; % Nonlinear dynamics (model)
betamm=T*cbarm/Amm;
hmm(k+1)=alphamm+betamm*umm(k); % Compute plant output
hmm(k+1)=max([0.001,hmm(k+1)]); % Constraint liquid not to go
% negative
%--
% Define the planner that uses the model to control the plant
% (of course this is the primary thing that we want to study)
e(k)=r(k)-h(k); % Compute error
sume=sume+e(k); % Compute integral approx.
% Projection into the future for each controller:
hm(1,:,=h(k)*ones(length(Kpvec),length(Kivec)); % Initialize prediction with current
% liquid level
for ii=1:length(Kpvec)
for jj=1:length(Kivec) % The first two loops pick a controller
% Initialize integral with current error integral sume, or 0
% (analgous to how we start the loop for the main controller)
sumem=sume;
for j=1:N % This loop simulates it at N time points
em(j,ii,jj)=r(k)-hm(j,ii,jj); % Error for each model (assumes that r(k) will stay
% constant over the projection window)
sumem=sumem+em(j,ii,jj); % Compute integator for each controller
um(j,ii,jj)=Kpvec(ii)*em(j,ii,jj)+Kivec(jj)*sumem; % Compute controller output
if um(j,ii,jj)>50 % Compute saturation
um(j,ii,jj)=50;
end
if um(j,ii,jj)<-50
um(j,ii,jj)=-50;
end
Am=abarm*hm(j,ii,jj)^2 +bbarm; % Cross-sectional area (model)
alpham=hm(j,ii,jj)-T*dbarm*sqrt(2*g*hm(j,ii,jj))/Am; % Nonlinear dynamics (model)
betam=T*cbarm/Am;
hm(j+1,ii,jj)=alpham+betam*um(j,ii,jj); % Compute plant output
hm(j+1,ii,jj)=max([0.001,hm(j+1,ii,jj)]); % Constraint liquid not to go
% negative
end
% Compute the cost function for the controller that was just simulated
J(ii,jj)=w1*(r(k)*ones(N+1,1)-hm(1:N+1,ii,jj))'*(r(k)*ones(N+1,1)-hm(1:N+1,ii,jj))+..
w2*um(1:N,ii,jj)'*um(1:N,ii,jj);
end
end
% Find the indices of the best controller (plan)
[val,rowindex(k)]=min(min(J')); % Gives the min value (val) and its row index
[val,colindex(k)]=min(min(J)); % Gives the min value and its column index
% Pick the input to be the one that came from the plan that did best in
% the projection
u(k)=um(1,rowindex(k),colindex(k));
% Next, put the chosen input to the plant
% Input saturation
if u(k)>50
u(k)=50;
end
if u(k)<-50
u(k)=-50;
end
% Plant equations
A=abs(abar*h(k)+bbar); % Cross-sectional area
alpha=h(k)-T*dbar*sqrt(2*g*h(k))/A; % Nonlinear dynamics
beta=T*cbar/A;
h(k+1)=alpha+beta*u(k); % Compute plant output
h(k+1)=max([0.001,h(k+1)]); % Constraint liquid not to go
% negative
end
hh=h(timeref);
trackerrorenergy(kk)=0.5*(ref'-hh')'*(ref'-hh');
inputenergy(kk)=0.5*u*u';
end
%%
% Plot the results
% Plant: Controlled by PI controller
% Strip off last computed value of h
hhp=hp(timeref);
figure(2)
clf
subplot(211)
plot(time,hhp,'k-',time,ref,'k--')
grid
ylabel('Liquid height, h')
title('Liquid level h and reference input r (plant)')
subplot(212)
plot(time,up,'k-')
grid
title('Tank input, u')
xlabel('Time, k')
axis([min(time) max(time) -50 50])
% Model: Controlled by PI controller
% Strip off last computed value of hmm
hhm=hmm(timeref);
figure(3)
clf
subplot(211)
plot(time,hhm,'k-',time,ref,'k--')
grid
ylabel('Liquid height, h_m')
title('Liquid level h_m and reference input r (model)')
subplot(212)
plot(time,umm,'k-')
grid
title('Tank input, u_m')
xlabel('Time, k')
axis([min(time) max(time) -50 50])
% Difference between plant and model: Controlled by PI controller
% Strip off last computed value of hmm
figure(4)
clf
subplot(211)
plot(time,hhp-hhm,'k-')
grid
title('Liquid height error')
subplot(212)
plot(time,up-umm,'k-')
grid
title('Tank input error')
xlabel('Time, k')
%--
% Planning system results
% Strip off last computed value of h
hh=h(timeref);
figure(5)
clf
subplot(211)
plot(time,hh,'k-',time,ref,'k--')
grid
ylabel('Liquid height, h')
title('Liquid level h and reference input r')
subplot(212)
plot(time,u,'k-')
grid
title('Tank input, u')
xlabel('Time, k')
axis([min(time) max(time) -50 50])
figure(6)
clf
plot(time,rowindex,'k-',time,colindex,'k--')
axis([min(time) max(time) 0 max(length(Kpvec),length(Kivec))])
grid
title('Indices of plan (row=solid, column=dashed)')
ylabel('Row and column indices')
xlabel('Time, k')
% Next, study the effect of the projection length N
figure(7)
clf
plot(NN,trackerrorenergy,'b-',NN,trackerrorenergy,'ro')
title('Tracking energy vs. projection length N')
xlabel('Projection length N')
figure(8)
clf
plot(NN,inputenergy,'b-',NN,inputenergy,'ro')
title('Control energy vs. projection length N')
xlabel('Projection length N')
%%
% End of program
%%