Slalom Manoeuvre For term project 2, find the guidance command that solves the following optimisation, \[ \begin{array}{c} u^{*}=\arg \min _{u} J(u)=\int_{4_{0}}^{t_{f}}|| x_{d}(t)-x(t) \| d t \\ \text { s.t. } \\ |u|<<0 \end{array} \] Figure TP2: Helicopter slalom manoeuvre. This exercise illustrates the two complementary functions of the guidance algorithm- finding a feasible path and calculating the correct commands for the flight controller. Step 1 - Define the trajectory The maximum lateral extent $(h)$ is $20 \mathrm{~m}$. You should define the desired lateral motion of each segment using a polynomial for $y$. \[ y(\tau)=a_{0}+a_{1} \tau+a_{2} \tau^{2}+a_{3} r^{3} \] To calculate the polynomial coefficients $\left(a_{0}, a_{1}, a_{2}, a_{3}\right)$ of each trajectory segment, express the equations in vector/matrix form i.e., \[ \Phi a=b \] where, \[ \Phi=\left[\begin{array}{cccc} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 1 & t_{f} & t_{f}^{2} & t_{f}^{3} \\ 0 & 1 & 2 t_{f} & 3 t_{f}^{2} \end{array}\right], a=\left[\begin{array}{c} a_{0} \\ a_{1} \\ a_{2} \\ a_{3} \end{array}\right], b=\left[\begin{array}{c} y(0) \\ y(0) \\ y\left(t_{f}\right) \\ \dot{y}\left(t_{f}\right) \end{array}\right] \]
\[ \Phi=\left[\begin{array}{cccc} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 1 & t_{f} & t_{f}^{2} & t_{f}^{3} \\ 0 & 1 & 2 t_{f} & 3 t_{f} \end{array}\right], a=\left[\begin{array}{c} a_{0} \\ a_{1} \\ a_{2} \\ a_{3} \end{array}\right], b=\left[\begin{array}{c} y(0) \\ y(0) \\ y\left(t_{f}\right) \\ y\left(t_{f}\right) \end{array}\right] \] Then \[ a=\Phi^{-1} b \] Remember you need to perform this calculation for each segment! Once you have all the coefficients, the last step is computing the desired trajectories for both $x$ and $y$ axes. The polynomials only define the position and velocity of desired motion in the $y$-axis. To obtain the desired position in the $x$-axis, use the constraint of constant airspeed, \[ \dot{x}_{d}=\sqrt{V^{2}-\dot{y}_{d}^{2}} \] Step 2 - Find the Controls This problem is best formulated using a double integrator model. Recall from the notes that the difference equations for this type of model are (for constant $V_{f}$ ), \[ \begin{array}{l} x_{k+1}=x_{k}+v_{x} \Delta t \\ y_{k+1}=y_{k}+v_{y} \Delta t \\ v_{k+1}=v_{k}+a_{x} \Delta t \\ v_{k+1}=v_{k}+a_{y} \Delta t \end{array} \] calculating the error at each time step ( $k k$ - see cost function loop below) using the inner product, i.e. $J_{\mu}=\left(X_{d_{\mu}}-X_{\mu}\right)^{\mathrm{T}} C^{\mathrm{T}} C\left(X_{d_{\mu}}-X_{\mu}\right.$ ), while $X$ is the state vector and $C$ is the output selection matrix. Using the MATLAB function fmincon, solve the optimisation problem ( $i$ ). Once your code have passed all of the MATLAB Grader tests, you MUST press SUBMIT for marking. Script $\odot$

SKELETON CODE

%% Define the problem

% Constants

Vf = 5; % UAV flight speed

t0 = 0; tseg = 5; % Initial and segment time

dt = 0.1; % sampling interval

%--------------------------------------

% Double Integrator model (ENG5017)

%--------------------------------------

A = [1 dt 0 0;

0 1 0 0;

0 0 1 dt;

0 0 0 1]; % state transition matrix

B = [0;dt;0;dt]; % control transition matrix

C = [0 0 1 0]; % output selection matrix

% Discretisation limits

npts(1) = floor((tseg-t0)/dt); % number of optimisation points, seg1

npts(2) = floor(1.5*npts(1)); % number of optimisation points, seg2

npts(3) = npts(1); % number of optimisation points, seg3

%% Trajectory generation

xd = [0;0;0;0]; % placeholder for desired trajectory

lim = 20; % maximum lateral offset

% Calculate the array of boundary conditions 'bca' defining trajectory polynomials for each segment

% ------ INSERT YOUR CODE HERE ------

nseg = 3; % number of polynomial segments

% Loop over each trajectory segment

for jj=1:nseg

tf = floor(npts(jj))*dt; % segment duration (s)

bc = bca(jj,:);

% Calculate the polynomial coefficient matrix 'Phi'

% Calculate the boundary condition vector 'b'

% ------ INSERT YOUR CODE HERE ------

a = Phi\b;

for ii=1:floor(npts(jj))

tau = (ii-1)* dt;

% Now calculate the trajectory coordinates using polynomials

% The variables to be calculated include position (xp and yp) and velocity (xpd and ypd)

% ------ INSERT YOUR CODE HERE ------

% append the trajectory array

xd = [xd [xp;xpd;yp;ypd]];

end

end

%% Solve the optimisation

% Define the initial conditions

x0 = [0;Vf;0;0];

CC = C'*C;

tp = sum(npts);

tend = tp*dt;

t = t0:dt:tend;

U0 = zeros(tp,1);

Ulower = -50*ones(tp,1);

Uupper = 50*ones(tp,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 defined in the cost function: costFun(U,A,B,x0,CC,dt,tp,Xd).

% Return the optimal values in 'U_opt'.

% Remember to include the upper and lower constraints.

% ------ INSERT YOUR CODE HERE ------

%% display the results

% Calculate the complete trajectory using the optimal control vector

xv = x0;

x = x0;

for jj=1:tp

x = A*x + B * U_opt(jj,1);

xv = [xv x];

end

% Now plot the UAV position time-series

figure(1)

plot(t,xv(3,:)); hold on;

plot(t(1:length(xd(1,:))),xd(3,:),'--g');

legend('Planned optimal trajectory','Polynomial generated trajectory');

xlabel('t (s)');

ylabel('y (m)');

grid on; hold off;

% and the control inputs

figure(2)

stairs(t,[U_opt;U_opt(end)],'r'); grid on;

xlabel('t (s)');

ylabel('u (N)');

%% Calculate the cost function

function J = costFun(U,A,B,x0,CC,dt,tp,xd)

J = 0.0;

x = x0;

for kk=1:tp-1

x = A*x + B*U(kk,1);

diff = (xd(:,kk+1) - x)'*CC*(xd(:,kk+1) - x)*dt;

J = J + diff;

end

end