Module:

Lecturers: Dr Hae-In Lee, Dr Namhoon Cho, Dr Sabyasachi Mondal Date issued: 15th January 2024

Submission date: 16th February 2023 Estimatedtimerequired: 50hours

The assignment should be submitted electronically as a pdf, in the form of a brief individual report (4000 words limit applies) explaining the methods and giving a clear statement of the principal results. Students should highlight the basis for any assumptions made wherever possible.

The work must be presented as follows:

1. A pdf document which describes the answers to the questions in Task 1 and Task 2 with results and discussions. This document should be submitted on the CANVAS website through Turnitin following the standard procedure on or before the submis- sion deadline.

2. A zipped folder containing all MATLAB codes created to answer the questions. This zipped folder should be named as ‘GNAS_[student_number]_[name]’ and sent to {haein.lee ; n.cho ; sabyasachi.mondal}@cranfield.ac.uk before the submission dead- line.

You are reminded that in the absence of exceptional circumstances (supported by written evidence) late submissions will be penalised.

Assignments for Guidance and Navigation for Autonomous Systems

1 Task 1 – Path Generation and Tracking [50 marks]

Task 1 considers the problem of planning and tracking a path in two-dimensional space (horizontal plane) for a simplified fixed-wing aircraft to fly from p0 to pf .

In Task_1_Path.zip, the code for parameter optimisation solver based on a simple primal-dual interior-point method is provided.

1.1 Path Planning [30 marks]

The coordinates of the initial and final points are

p0 = (0,0) pf = (xf,0) = (400,0) Inaddition,thevehicleisrequiredtoarriveatthegoalpointwiththeflightpathangleχfd whichhas90deg

measured from the y-axis in clockwise direction.

P1. Formulate a parameter optimisation problem for the path planning task to satisfy the given initial and final conditions [5 marks]. Justify the reasoning behind the problem formulation, i.e., the choice of the objective and constraint functions [5 marks]. Modify the solver code and solve the formulated problem [5 marks]. Discuss the trend observed in the resulting path depending on the formulation choices, for example, explain how the relative difference between the weighting values for the state and input variables in the objective function affects the aggressiveness of the generated path [bonus marks].

(Hint 1: The independent variable of the dynamics is also of the designer’s choice. Consider x-axis position coordinate as the independent variable, and take the double integrator system addressed in the lab session as an example.)

(Hint 2: One might incorporate proper scaling of the variables for reformulate the problem in terms of normalised variables, if necessary.)

P2. Let us consider the presence of rectangular obstacles in the flight area as shown in Fig. 1.

4

 Figure 1: Obstacle Field Definition

Page 2 of 5

Assignments for Guidance and Navigation for Autonomous Systems

The parameters in Fig. 1 are as follows:

• xf =400[m]

• yU =100[m]

• yL = −100 [m]

Formulate the path planning problem for reaching the goal point while avoiding obstacles as a constrained optimisation problem [5 marks]. Solve the formulated optimisation problem and compare the result with that obtained in P1. For example, one can discuss how the generated path depends on specification of the constraint values or step size chosen for discretisation of the associated system dynamics, how the specifi- cation of optimisation problem can affect the safety of the planned path, etc [10 marks].

1.2 Path-Following [20 marks]

Consider the simplified simulation model for a fixed-wing aircraft described by

x ̇ = V sin χ y ̇ = V cos χ χ ̇ = Vg tanφ

where χ represents the horizontal flight path angle defined to be positive clockwise from the y-axis, g is the gravitational acceleration, φ is the bank angle, and the speed V is assumed to be controlled to maintain 20m/s. The initial condition is given by (x0, y0, χ0) = (0, 0, 90 deg). Note that φ is the input variable for this model for which admissible range is [−90, 90] deg.

P3. Choose and implement a path-following guidance algorithm to track the paths designed in P1 and P2 [5 marks]. Discuss how the closed-loop trajectory depends on the choice of design parameters in the chosen path-following method, such as the feedback gains in a control-theoretic path-following guidance law or the look-ahead distance in a virtual-target-following guidance method [5 marks].

P4. Suppose that the aircraft flight envelope protection system prohibits the bank angle from exceeding φlim = 45 deg in its magnitude, which leads to reduced range of admissible turning acceleration. Simulate the model with the bank angle limit for the same task considered in P3 and compare the result with that of P3 [5 marks]. Modify either the optimisation problem considered for path planning and/or the tunable elements of the path-following guidance algorithm for the aircraft to accomplish the autonomous flight task successfully without colliding into obstacles with limited available bank angle [5 marks].

2 Task 2 – Simultaneous Localisation and Mapping (SLAM) [50 marks]

In Task_2_SLAM.zip, the particle filter SLAM algorithm has been designed to navigate two UAVs flying over the landmarks. In the current settings, relative position of the landmarks is measured from the UAVs in

Page 3 of 5

Assignments for Guidance and Navigation for Autonomous Systems

x-y axis. The details of the code are given in the Lab Sessions on SLAM.

2.1 Particle filter SLAM with range and bearing measurements [12.5 marks]

Let us assume that the vehicle is equipped with a LIDAR sensor, which returns the range and bearing measurements of the landmarks. Hence, the measurement equation is changed as follows:

"p(xL(k)−x(k))2+(yL(k)−y(k))2 #

z(k) ≜ h(xL(k), x(k)) + v = tan−1 ((yL(k) − y(k))/(xL(k) − x(k))) + v, (1)

wherethemeasurementnoiseischangedtov1 ∼N(0,12)mforrangemeasurements,andv2 ∼N(0,0.12) rad for bearing measurements.

Derive the measurement Jacobian matrix and write the equation in the report [2.5 marks]. Modify the MATLAB codes accordingly to implement the SLAM algorithm with range and bearing measurements, and write the modified parts of the code in the report [5 marks]. Compare the estimation error with the x-y relative position measurements, discuss the reason why the differences occur, and propose alternative methods to improve the estimation accuracy [5 marks].

Hint: Difference in angular measurements should be wrapped to [−π, π], and plot the animations with new types of measurements.

2.2 Particle filter SLAM with bearing-only measurements [12.5 marks]

Let us assume that the vehicle is equipped with a monocular camera, measuring only the relative bearing angle to the landmarks. Considering that only the bearing measurements are available, re-formulate the measurement model and derive the measurement Jacobian matrix. Write their equations in the report [2.5 marks]. Modify the codes accordingly to implement the SLAM algorithm with bearing-only measurements, and write the modified parts of the code in the report [5 marks]. Compare the estimation error with the range

  Page 4 of 5

Assignments for Guidance and Navigation for Autonomous Systems

and bearing measurement results, and discuss how to improve the estimation accuracy using bearing-only measurements [5 marks].

2.3 Data association [10 marks]

In Task_2_SLAM.zip, it is assumed that the assignments of the measurements to landmarks are known, i.e., data association algorithm is not implemented. Assuming that this is no longer known, using the relative x-y position measurements as in Task_2_SLAM.zip, implement a simple data association by associating the nearest landmark to the measurement [5 marks]. Discuss the difficulties that arise and suggest ways to improve the accuracy, comparing the different types of measurements available [5 marks].

2.4 EKF SLAM with range and bearing measurements [15 marks]

Another commonly used filter for SLAM is Extended Kalman Filter (EKF). Formulate the EKF SLAM algo- rithm using both the range and bearing measurements, by defining appropriate state and measurement mod- els with augmented vectors [2.5 marks], and by describing the predication and update steps [2.5 marks]. Implement the EKF SLAM algorithm by modifying the given code [5 marks]. Compare the estimation error with the original results, and discuss the differences between the particle filter SLAM and the EKF SLAM algorithm [5 marks].

Page 5 of 5