%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% SPSA for Design of a
% Proportional-Derivative Controller for
% Tanker Ship Heading Regulation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% By: Kevin Passino
% Version: 4/13/01
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear % Clear all variables in memory
% Proportional and derivative gains (the input space for the design)
%Kp=-1.5; Some reasonable size gains - found manually
%Kd=-250;
Kpmin=-5;
Kpmax=-0.5; % Program below assumes this
Kdmin=-500;
Kdmax=-100; % Program below assumes this
% Define scale parameters for performance measure
w1=1;
w2=0.01;
% Next, define SPSA parameters:
p=2; % Dimension of the search space
thetamin=[Kpmin; Kdmin]; % Set edges of region want to search in
thetamax=[Kpmax;Kdmax];
Nspsa=50; % Maximum number of iterations to produce
% Next set the parameters of the algorithm:
lambdap=1; % Use two step sizes due to scale differences in two dimensions
lambdad=500;
lambda0=1;
alpha1=0.602;
alpha2=0.101;
alpha1=0.602; %Max of 1, min of 0.602
alpha2=0.101; % Max of 1/6=0.1666666, min of 0.101
% Create two sizes since the sizes of P gain and D gain are quite different
cp=0.5;
cd=50;
% Next, pick the initial value of the parameter vector
theta(:,1)=[-0.5; -300]; % Pick one that found was reasonable
% Allocate memory
theta(:,2:Nspsa)=0*ones(p,Nspsa-1);
Jplus=zeros(Nspsa,1);
Jminus=zeros(Nspsa,1);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Start the SPSA loop
for j=1:Nspsa
% Use projection in case update (or initial values) out of range
theta(:,j)=min(theta(:,j),thetamax);
theta(:,j)=max(theta(:,j),thetamin);
% Set parameters and perturb parameters
lambdajp=lambdap/(lambda0+j)^alpha1;
lambdajd=lambdad/(lambda0+j)^alpha1;
cjp=cp/j^alpha2; % Need two of these since have cp, cd
cjd=cd/j^alpha2;
Delta=2*round(rand(p,1))-1; % According to a Bernoulli +- 1 dist.
thetaplus=theta(:,j)+[cjp*Delta(1,1); cjd*Delta(2,1)];
thetaminus=theta(:,j)-[cjp*Delta(1,1); cjd*Delta(2,1)];
% Use projection in case perturbed out of range.
thetaplus=min(thetaplus,thetamax);
thetaplus=max(thetaplus,thetamin);
thetaminus=min(thetaminus,thetamax);
thetaminus=max(thetaminus,thetamin);
% Next we must compute the cost function for thetaplus and thetaminus
Jplus(j,1)=pdtanker([thetaplus(1,1);thetaplus(2,1)],w1,w2);
Jminus(j,1)=pdtanker([thetaminus(1,1);thetaminus(2,1)],w1,w2);
% Next, compute the approximation to the gradient
g=(Jplus(j,1)-Jminus(j,1))./(2*[cjp*Delta(1,1); cjd*Delta(2,1)]);
% Then, update the parameters
theta(:,j+1)=theta(:,j)-[lambdajp*g(1,1);lambdajd*g(2,1)];
end % End main loop...
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Provide a plot of the costs that were computed
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
t=0:Nspsa;
tprime=0:Nspsa-1;
figure(1)
clf
plot(tprime,Jplus(:,1),'-',tprime,Jminus(:,1),'--')
xlabel('Iteration, j')
ylabel('Cost values')
title('Costs: J_p_l_u_s (solid) J_m_i_n_u_s (dashed)')
figure(2)
clf
subplot(211)
plot(t,theta(1,:),'-')
ylabel('K_p')
title('PD controller parameters (K_p solid, K_d dashed)')
subplot(212)
plot(t,theta(2,:),'-')
xlabel('Iteration, j')
ylabel('K_d')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Find the best set of gains - just take to be last ones computed
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Kpbest=theta(1,Nspsa+1)
Kdbest=theta(2,Nspsa+1)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Next, we provide plots of the input and output of the ship
% along with the reference heading that we want to track
% for the best design found.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Initialize ship parameters
% (can test two conditions, "ballast" or "full"):
ell=350; % Length of the ship (in meters)
u=5; % Nominal speed (in meters/sec)
abar=1; % Parameters for nonlinearity
bbar=1;
% Define the reference model (we use a first order transfer function
% k_r/(s+a_r)):
a_r=1/150;
k_r=1/150;
flag=1;
%flag=0; % Test under off-nominal conditions
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Simulate the controller regulating the ship heading
% Next, we initialize the simulation:
t=0; % Reset time to zero
index=1; % This is time's index (not time, its index).
tstop=1200; % Stopping time for the simulation (in seconds) - normally 20000
step=1; % Integration step size
T=10; % The controller is implemented in discrete time and
% this is the sampling time for the controller.
% Note that the integration step size and the sampling
% time are not the same. In this way we seek to simulate
% the continuous time system via the Runge-Kutta method and
% the discrete time controller as if it were
% implemented by a digital computer. Hence, we sample
% the plant output every T seconds and at that time
% output a new value of the controller output.
counter=10; % This counter will be used to count the number of integration
% steps that have been taken in the current sampling interval.
% Set it to 10 to begin so that it will compute a controller
% output at the first step.
% For our example, when 10 integration steps have been
% taken we will then we will sample the ship heading
% and the reference heading and compute a new output
% for the controller.
eold=0; % Initialize the past value of the error (for use
% in computing the change of the error, c). Notice
% that this is somewhat of an arbitrary choice since
% there is no last time step. The same problem is
% encountered in implementation.
cold=0; % Need this to initialize phiold below
psi_r_old=0; % Initialize the reference trajectory
ymold=0; % Initial condition for the first order reference model
x=[0;0;0]; % First, set the state to be a vector
x(1)=0; % Set the initial heading to be zero
x(2)=0; % Set the initial heading rate to be zero.
% We would also like to set x(3) initially but this
% must be done after we have computed the output
% of the controller. In this case, by
% choosing the reference trajectory to be
% zero at the beginning and the other initial conditions
% as they are, and the controller as designed,
% we will know that the output of the controller
% will start out at zero so we could have set
% x(3)=0 here. To keep things more general, however,
% we set the intial condition immediately after
% we compute the first controller output in the
% loop below.
% Next, we start the simulation of the system. This is the main
% loop for the simulation of the control system.
psi_r=0*ones(1,tstop+1);
psi=0*ones(1,tstop+1);
e=0*ones(1,tstop+1);
c=0*ones(1,tstop+1);
s=0*ones(1,tstop+1);
w=0*ones(1,tstop+1);
delta=0*ones(1,tstop+1);
ym=0*ones(1,tstop+1);
while t <= tstop
% First, we define the reference input psi_r (desired heading).
if t>=0, psi_r(index)=0; end % Request heading of 0 deg
if t>=100, psi_r(index)=45*(pi/180); end % Request heading of 45 deg
if t>=1500, psi_r(index)=0; end % Request heading of 0 deg
if t>=3000, psi_r(index)=45*(pi/180); end % Request heading of -45 deg
if t>=4500, psi_r(index)=0; end % Request heading of 0 deg
if t>=6000, psi_r(index)=45*(pi/180); end % Request heading of 45 deg
if t>=7500, psi_r(index)=0; end % Request heading of 0 deg
if t>=9000, psi_r(index)=45*(pi/180); end % Request heading of 45 deg
if t>=10500, psi_r(index)=0; end % Request heading of 0 deg
if t>=12000, psi_r(index)=45*(pi/180); end % Request heading of -45 deg
if t>=13500, psi_r(index)=0; end % Request heading of 0 deg
if t>=15000, psi_r(index)=45*(pi/180); end % Request heading of 45 deg
if t>=16500, psi_r(index)=0; end % Request heading of 0 deg
if t>=18000, psi_r(index)=45*(pi/180); end % Request heading of 45 deg
if t>=19500, psi_r(index)=0; end % Request heading of 0 deg
% Next, suppose that there is sensor noise for the heading sensor with that is
% additive, with a uniform distribution on [- 0.01,+0.01] deg.
s(index)=0.01*(pi/180)*(2*rand-1); %else
%s(index)=0; % This allows us to remove the noise.
%end
psi(index)=x(1)+s(index); % Heading of the ship (possibly with sensor noise).
if counter == 10, % When the counter reaches 10 then execute the
% controller
counter=0; % First, reset the counter
% Reference model calculations:
% The reference model is part of the controller and to simulate it
% we take the discrete equivalent of the
% reference model to compute psi_m from psi_r (if you use
% a continuous-time reference model you will have to augment
% the state of the closed-loop system with the state(s) of the
% reference model and hence update the state in the Runge-Kutta
% equations).
%
% For the reference model we use a first order transfer function
% k_r/(s+a_r) but we use the bilinear transformation where we
% replace s by (2/step)(z-1)/(z+1), then find the z-domain
% representation of the reference model, then convert this
% to a difference equation:
ym(index)=(1/(2+a_r*T))*((2-a_r*T)*ymold+...
k_r*T*(psi_r(index)+psi_r_old));
ymold=ym(index);
psi_r_old=psi_r(index);
% This saves the past value of the ym and psi_r so that we can use it
% the next time around the loop
% Controller calculations:
e(index)=psi_r(index)-psi(index); % Computes error (first layer of perceptron)
c(index)=(e(index)-eold)/T; % Sets the value of c
eold=e(index); % Save the past value of e for use in the above
% computation the next time around the loop
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% A proportional-derivative controller:
delta(index)=Kpbest*e(index)+Kdbest*c(index);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
else % This goes with the "if" statement to check if the counter=10
% so the next lines up to the next "end" statement are executed
% whenever counter is not equal to 10
% Now, even though we do not compute the controller at each
% time instant, we do want to save the data at its inputs and output at
% each time instant for the sake of plotting it. Hence, we need to
% compute these here (note that we simply hold the values constant):
e(index)=e(index-1);
c(index)=c(index-1);
delta(index)=delta(index-1);
ym(index)=ym(index-1);
end % This is the end statement for the "if counter=10" statement
% Next, the Runge-Kutta equations are used to find the next state.
% Clearly, it would be better to use a Matlab "function" for
% F (but here we do not, so we can have only one program).
time(index)=t;
% First, we define a wind disturbance against the body of the ship
% that has the effect of pressing water against the rudder
%if flag==0, w(index)=0.5*(pi/180)*sin(2*pi*0.001*t); % This is an additive sine disturbance to
% the rudder input. It is of amplitude of
% 0.5 deg. and its period is 1000sec.
%delta(index)=delta(index)+w(index);
%end
% Next, implement the nonlinearity where the rudder angle is saturated
% at +-80 degrees
if delta(index) >= 80*(pi/180), delta(index)=80*(pi/180); end
if delta(index) <= -80*(pi/180), delta(index)=-80*(pi/180); end
% The next line is used in place of the line following it to
% change the speed of the ship
%if t>=1000000,
%if t>=9000, % This switches the ship speed (unrealistically fast)
%if flag==0,
%u=3; % A lower speed
%else
u=5;
%end
% Next, we change the parameters of the ship to tanker to reflect
% changing loading conditions (note that we simulate as if
% the ship is loaded while moving, but we only change the parameters
% while the heading is zero so that it is then similar to re-running
% the simulation, i.e., starting the tanker operation at different
% times after loading/unloading has occurred).
% The next line is used in place of the line following it to keep
% "ballast" conditions throughout the simulation
%if t>=1000000,
%if t>=9000, % This switches the parameters in the middle of the simulation
if flag==0,
K_0=0.83; % These are the parameters under "full" conditions
tau_10=-2.88;
tau_20=0.38;
tau_30=1.07;
else
K_0=5.88; % These are the parameters under "ballast" conditions
tau_10=-16.91;
tau_20=0.45;
tau_30=1.43;
end
% The following parameters are used in the definition of the tanker model:
K=K_0*(u/ell);
tau_1=tau_10*(ell/u);
tau_2=tau_20*(ell/u);
tau_3=tau_30*(ell/u);
% Next, comes the plant:
% Now, for the first step, we set the initial condition for the
% third state x(3).
if t==0, x(3)=-(K*tau_3/(tau_1*tau_2))*delta(index); end
% Next, we use the formulas to implement the Runge-Kutta method
% (note that here only an approximation to the method is implemented where
% we do not compute the function at multiple points in the integration step size).
F=[ x(2) ;
x(3)+ (K*tau_3/(tau_1*tau_2))*delta(index) ;
-((1/tau_1)+(1/tau_2))*(x(3)+ (K*tau_3/(tau_1*tau_2))*delta(index))-...
(1/(tau_1*tau_2))*(abar*x(2)^3 + bbar*x(2)) + (K/(tau_1*tau_2))*delta(index) ];
k1=step*F;
xnew=x+k1/2;
F=[ xnew(2) ;
xnew(3)+ (K*tau_3/(tau_1*tau_2))*delta(index) ;
-((1/tau_1)+(1/tau_2))*(xnew(3)+ (K*tau_3/(tau_1*tau_2))*delta(index))-...
(1/(tau_1*tau_2))*(abar*xnew(2)^3 + bbar*xnew(2)) + (K/(tau_1*tau_2))*delta(index) ];
k2=step*F;
xnew=x+k2/2;
F=[ xnew(2) ;
xnew(3)+ (K*tau_3/(tau_1*tau_2))*delta(index) ;
-((1/tau_1)+(1/tau_2))*(xnew(3)+ (K*tau_3/(tau_1*tau_2))*delta(index))-...
(1/(tau_1*tau_2))*(abar*xnew(2)^3 + bbar*xnew(2)) + (K/(tau_1*tau_2))*delta(index) ];
k3=step*F;
xnew=x+k3;
F=[ xnew(2) ;
xnew(3)+ (K*tau_3/(tau_1*tau_2))*delta(index) ;
-((1/tau_1)+(1/tau_2))*(xnew(3)+ (K*tau_3/(tau_1*tau_2))*delta(index))-...
(1/(tau_1*tau_2))*(abar*xnew(2)^3 + bbar*xnew(2)) + (K/(tau_1*tau_2))*delta(index) ];
k4=step*F;
x=x+(1/6)*(k1+2*k2+2*k3+k4); % Calculated next state
t=t+step; % Increments time
index=index+1; % Increments the indexing term so that
% index=1 corresponds to time t=0.
counter=counter+1; % Indicates that we computed one more integration step
end % This end statement goes with the first "while" statement
% in the program so when this is complete the simulation is done.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Next, we provide plots of data from the simulation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% First, we convert from rad. to degrees
psi_r=psi_r*(180/pi);
psi=psi*(180/pi);
delta=delta*(180/pi);
e=e*(180/pi);
c=c*(180/pi);
ym=ym*(180/pi);
% Next, we provide plots showing the performance of the best design
figure(3)
clf
subplot(211)
plot(time,psi,'k-',time,ym,'k--',time,psi_r,'k-.')
zoom
grid on
title('Ship heading (solid) and desired ship heading (dashed), deg.')
subplot(212)
plot(time,delta,'k-')
zoom
grid on
title('Rudder angle, output of controller (input to the ship), deg.')
xlabel('Time, sec')
figure(4)
clf
plot(time,psi-ym,'k-')
zoom
grid on
title('Ship heading error between ship heading and reference model heading, deg.')
xlabel('Time, sec')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Just repeat above code, but for off-nominal case
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% End of program %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%