%% 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 (3×4) 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’.
%
% — INSERT CODE HERE —
%
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’.
%
% — INSERT CODE HERE —
%
% Now calculate the trajectory coordinates using polynomials
% and store in vectors ‘xp’ and ‘yp’.
for ii=1:floor(npts(jj))
tau = (ii-1)* dt;
%
% — INSERT CODE HERE —
%
% 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.
%
% — INSERT CODE HERE —
%
%% 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 (5×1) vector ‘xv’. Access appropriate U_opt value using ‘idx’ as index.
xvec = x0;
xv = x0;
idx = 1;
UoptVec = [];
for jj=1:tp
%
% — INSERT CODE HERE —
%
idx = idx+nc;
xvec = [xvec xv];
if(jj < tp)
UoptVec = [UoptVec U_opt(idx)];
end
end
% Now plot the helicopter position time-series
figure(1);
plot(xvec(1,:),xvec(2,:)); hold on;
plot(xd(1,:),xd(2,:),'--g');
xlabel('x(m)'); ylabel('y(m)'); title('Demanded vs. actual ground track');
legend('actual','demanded');
grid on; hold off;
% and the control inputs
figure(2);
[haxes,hline1,hline2] = plotyy(t(1:length(UoptVec(1,:))),UoptVec(1,:),t(1:length(xvec(1,:))),xvec(5,:)/d2r,'stairs','plot');
ylabel(haxes(1),'\psi_d (deg/s)');
ylabel(haxes(2),'\psi (deg)');
xlabel('t(s)');
grid on;
%
% Calculate the cost function
%
function J = costFun(U,Vf,x0,tp,dt,d2r,xd,nc)
J = 0.0;
x = x0;
%--------------------------------------
% Velocity-Heading model
%--------------------------------------
idx = 1;
for kk=1:tp-1
% Use the difference equations again to calculate the state
% at time kk. Find the trajectory error using the inner product
% of the error vector. Add this to the cost 'J' at each timestep.
%
% --- INSERT CODE HERE ---
%
idx = idx+nc;
end
end