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

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 
    % heading angle
    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

Contact us