Velocity/Heading Model in a Slalom Manoeuvre

Hi everyone,

I've been going through this project for Uni but I am stuck at where I need to calculate the complete trajectory with the optimal control vector. Here is the code:

%% Define the problem
% Constants
Vf = 25;                            % Helicopter flight speed
t0 = 0; tseg = 5;                   % Initial and segment time
dt = 0.1;                           % sampling interval
d2r = pi/180;
% Discretisation limits
npts(1) = floor((tseg-t0)/dt);      % number of optimisation points, seg1
npts(2) = floor(1.75*npts(1));      % number of optimisation points, seg2
npts(3) = npts(1);                  % number of optimisation points, seg3
%% Trajectory generation
xd = [0;0];                     % placeholder for desired trajectory
lim = 50;                       % maximum lateral offset
% Define a (3x4) array of boundary conditions defining trajectory polynomials for each segment.
% Each row should contain the boundary conditions for the appropriate segment.
% Call the array 'bca'.
bca = [0 Vf lim Vf;lim Vf -lim Vf;-lim Vf 0 Vf];
nseg = 3;                       % number of polynomial segments
% Loop over each trajectory segment
for jj=1:nseg
    tf = npts(jj)*dt;           % segment duration (s)
    % Calculate the polynomial coeffs for segment jj using the vector/matrix method. 
    % Return the result in vector 'a'.
    psi = [1 0 0 0;0 1 0 0;1 tf tf^2 tf^3;0 1 2*tf 3*tf^2];
    b = bca(jj,:);
    counter_psi = inv(psi);
    a = counter_psi.*b;
    % Now calculate the trajectory coordinates using polynomials
    % and store in vectors 'xp' and 'yp'.
    for ii=1:floor(npts(jj))
        tau = (ii-1)* dt;
        yp = a(1)+a(2)*tau+a(3)*tau^2+a(4)*tau^3;
        ypd = a(2)+2*a(3)*tau+3*a(4)*tau^2;
        xpd = sqrt(Vf^2-ypd^2);
        xp = xpd*dt;
        % append the trajectory array
        xd = [xd [xp;yp]]; %#ok<*AGROW>
    end
end
tp = sum(npts);
tend = tp*dt;
t = t0:dt:tend;
%% Solve the optimisation
% Define the initial conditions
x0 = [0;0;Vf;0;0];
nc = 1;
U0 = zeros(tp*nc,1);
uSat = 20;
Ulower = -uSat*ones(tp*nc,1);
Uupper = uSat*ones(tp*nc,1);
% Overwrite some of the default optimisation properties
options = optimset('TolFun',1e-3,...
                   'Display','iter',...
                   'MaxFunEvals',50000);
%---------------------------------------
% Run the optimisation...!!!
%---------------------------------------
% Call fmincon using an anonymous function to pass the extra parameters
% Vf,x0,tp,dt,d2r,xd,nc. Return the optimal values in 'U_opt'.
% Remember to include the upper and lower constraints.
U_opt = fmincon(@costFun,U0,[],[],[],[],Ulower,Uupper,[],options,Vf,x0,tp,dt,d2r,xd,nc);
%% display the results
% Calculate the complete trajectory using the optimal control vector.
% Use the difference equations shown above to integrate the helicopter
% trajectory. Store states in (5x1) vector 'xv'. Access appropriate U_opt value using 'idx' as index.
xvec = x0;
xv = x0;
idx = 1;
UoptVec = [];
for jj=1:tp
    dxy = diff(xd);
    angle1 = atan2(dxy(jj+1), dxy(jj))*d2r^-1;
    angle2 = atan2(dxy(jj+2), dxy(jj+1))*d2r^-1;
    angled = angle2-angle1
    Vx = diff(xd(1,:))
    Vy = diff(xd(2,:))
    xv(1,jj) = xp(1,jj)+Vf*cos(angle1)*dt;
    xv(2,jj) = yp(1,jj)+Vf*sin(angle1)*dt;
    xv(3,jj) = Vx(1,jj)-Vf*sin(angle1)*angled*dt
    xv(4,jj) = Vy(1,jj)+Vf*cos(angle1)*angled*dt
    xv(5,jj) = angle1+angled*dt
    idx = idx+nc;
    xvec = [xvec xv];
    if(jj < tp)
        UoptVec = [UoptVec U_opt(idx)];
    end
end

I am not quite sure of what I wrote in that part so if you could just give me a hand, that'd be helpful. I am referring to this part especially :

for jj=1:tp
    dxy = diff(xd);
    angle1 = atan2(dxy(jj+1), dxy(jj))*d2r^-1;
    angle2 = atan2(dxy(jj+2), dxy(jj+1))*d2r^-1;
    angled = angle2-angle1
    Vx = diff(xd(1,:))
    Vy = diff(xd(2,:))
    xv(1,jj) = xp(1,jj)+Vf*cos(angle1)*dt;
    xv(2,jj) = yp(1,jj)+Vf*sin(angle1)*dt;
    xv(3,jj) = Vx(1,jj)-Vf*sin(angle1)*angled*dt
    xv(4,jj) = Vy(1,jj)+Vf*cos(angle1)*angled*dt
    xv(5,jj) = angle1+angled*dt
    idx = idx+nc;
    xvec = [xvec xv];
    if(jj < tp)
        UoptVec = [UoptVec U_opt(idx)];
    end
end

Thanks a lot for your time and comprehension :)!

[taiyu JU]

Hi, would you mind sharing the coes of the costfun parts

[Heather Lomax]

did you ever get an answer to this?