Code covered by the BSD License

# Optimal Control Using Control Vector Parameterization

### Chang Duan (view profile)

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

animateopt( topt,xopt,uopt,thetaopt,beta,N,rho )
function M = animateopt( topt,xopt,uopt,thetaopt,beta,N,rho )
%ANIMATEOPT animate the optimal state trajectory
[toptu idx] = unique(topt);
xoptu = xopt(idx,:); %unique(xopt,'rows');
uoptu = uopt(idx);
thetaoptu = thetaopt(idx); % remove duplicate values
t = 0:0.1:6;
xoi = interp1(toptu,xoptu,t);
uoi = interp1(toptu,uoptu,t); % trust $u$
haoi = atan(xoi(:,4)./xoi(:,3)); % heading angle
haoi(1) = haoi(2);
toi = interp1(toptu,thetaoptu,t); % trust angle $\theta$
[x,y] = meshgrid(-1:0.5:5);
fx = (beta*(3-x))./((y.^2+(x-3).^2).^(3/2));
fy = (-beta*y)./((y.^2+(x-3).^2).^(3/2));
figure;
for k=1:length(t)
% contruct the shape of a boat around the point with respect to
bx= xoi(k,1)+ [0.2*cos(haoi(k)-0.5) 0.3*cos(haoi(k)) ...
0.2*cos(haoi(k)+0.5) 0.2*cos(haoi(k)-0.5) ...
-0.2*cos(haoi(k)+0.5) -0.3*cos(haoi(k)) ...
-0.2*cos(haoi(k)-0.5) -0.2*cos(haoi(k)+0.5) ...
-0.2*cos(haoi(k)-0.5) 0.2*cos(haoi(k)+0.5)];
by= xoi(k,2)+ [0.2*sin(haoi(k)-0.5) 0.3*sin(haoi(k)) ...
0.2*sin(haoi(k)+0.5) 0.2*sin(haoi(k)-0.5) ...
-0.2*sin(haoi(k)+0.5) -0.3*sin(haoi(k)) ...
-0.2*sin(haoi(k)-0.5) -0.2*sin(haoi(k)+0.5) ...
-0.2*sin(haoi(k)-0.5) 0.2*sin(haoi(k)+0.5)];
plot(xoi(1:k,1),xoi(1:k,2),'b-',bx,by,'b-',[0 3 4],[0 0 4],'ko',...
[xoi(k,1),xoi(k,1)+uoi(k)/max(abs(uoptu))*cos(toi(k))],...
[xoi(k,2),xoi(k,2)+uoi(k)/max(abs(uoptu))*sin(toi(k))],...
'r:','LineWidth',2); % using red dotted line to denote direction of
% the trust, and its length is proportional to the control effort $u$
hold on;quiver(x,y,fx,fy,1,'LineWidth',2,'Color','black');hold off
xlabel('x_1(t)','fontsize',14)
ylabel('x_2(t)','fontsize',14)
set(gca,'FontSize',12)
title(['\beta=',int2str(beta),' N=',int2str(N),' \rho=',int2str(rho)])
xlim([-1 5]);ylim([-1 5]);
axis square;
M(k) = getframe(gcf);
end
end