Code covered by the BSD License  

Highlights from
Optimal Control Using Control Vector Parameterization

image thumbnail

Optimal Control Using Control Vector Parameterization

by

 

An example of using control vector parameterization to solve an optimal control problem

[topt,xopt,uopt,thetaopt]=plotopt1( x0,N,ts,dvarO,beta,rho,optODE )
function [topt,xopt,uopt,thetaopt]=plotopt1( x0,N,ts,dvarO,beta,rho,optODE )
%PLOTOPT1 plot function for piecewise constant control
z0 =  x0;
topt = [];
xopt = [];
uopt = [];
thetaopt = [];
u = dvarO(1:N);
theta = dvarO(N+1:2*N);
[x,y] = meshgrid(-1:0.5:5);
fx = (beta*(3-x))./((y.^2+(x-3).^2).^(3/2)); % force along x axis
fy = (-beta*y)./((y.^2+(x-3).^2).^(3/2)); % force along y axis

% forward state integration
for ks = 1:N
    [tspan,zs] = ode15s( @(t,x)dyneqn1(t,x,u,theta,beta,ks), ...
        [ts(ks),ts(ks+1)], z0, optODE );
    z0 = zs(end,:)';
    topt = [ topt; tspan ];
    xopt = [ xopt; zs ];
    uopt = [ uopt; u(ks)*ones(length(tspan),1) ];
    thetaopt = [thetaopt; theta(ks)*ones(length(tspan),1) ];
end

% Display Optimal Cost
disp(sprintf('Optimal cost for N=%d: %d', N, zs(end,5)));
figure(1) % state trajectory
plot(xopt(:,1),xopt(:,2),'k-',[0 3 4],[0 0 4],'ko','LineWidth',2)
hold on
quiver(x,y,fx,fy,1,'LineWidth',2,'Color','black') % force vector field
axis([-1 5 -1 5]);
xlabel('x_1(t)','fontsize',14)
ylabel('x_2(t)','fontsize',14)
set(gca,'FontSize',12)
title(['\beta=',int2str(beta),' N=',int2str(N),' \rho=',num2str(rho)])
hold off
%saveas(gcf,['x2(x1)_',int2str(ns),'stg.eps'], 'psc2')

figure(2) % control signal $u$
plot(topt,uopt,'k-','LineWidth',2)
xlabel('t','fontsize',14)
ylabel('u(t)','fontsize',14)
set(gca,'FontSize',12)
title(['\beta=',int2str(beta),' N=',int2str(N),' \rho=',num2str(rho)])
%saveas(gcf,['u(t)_',int2str(ns),'stg.eps'], 'psc2')

figure(3) % control signal $\theta$
plot(topt,thetaopt,'k-','LineWidth',2)
xlabel('t','fontsize',14)
ylabel('\theta(t)','fontsize',14)
set(gca,'FontSize',12)
title(['\beta=',int2str(beta),' N=',int2str(N),' \rho=',num2str(rho)])
end

Contact us