EE3901/EE5901 Sensor Technologies

Assignment 1 — Implementing an extended Kalman filter

Introduction

A vehicle or robot in the field can track its position using GNSS (Global Navigation Satellite System), e.g. GPS, GLONAS, etc. However, GNSS radio signals are easily blocked by obstacles in the environment, so a vehicle cannot always rely upon having continuous position data. In challenging environments, a vehicle may go for some time between GNSS fixes. Hence, position data is often obtained by sensor fusion in applications such as field robotics, unmanned aerial vehicles, etc.

Your task is to design, implement, and evaluate a sensor fusion scheme for locating the weed-spraying vehicle shown in Figure 1. This vehicle was developed by JCU spinout company AutoWeed, who have generously agreed to provide real sensor data for this assignment task.

You are provided with 7 data recordings. In each recording, there is approximately 30 seconds of movement in which full GNSS fixes are available. Then there is a period of approximately another 30 seconds where the GNSS fixes have been deliberately removed, to simulate the case where global positioning is temporarily unavailable. Your task is to ensure that the vehicle’s control system is still able to obtain position estimates even if GNSS fixes are denied.

The first six recordings are considered to be a “training set”. The withheld GNSS fixes are provided to you in a separate file, so that you can compare the true position data against your Kalman filter’s predictions. The seventh and final recording has the true position data kept secret. Your task is to predict the position for this recording as accurately as possible. After you submit your assignment, your predictions will be compared to the real positions.

Vehicle sensor data

The vehicle’s navigation data are provided by a Advanced Navigation Spatial Dual. Data from this device has been extracted for your use in this assignment.

Click here to download the sensor data (2MB zip file). Please refer to the “Data Explanation” spreadsheet inside the zip file that explains each of the data columns.

Your task

Your task is to design, implement, and evaluate an extended Kalman filter to reconstruct the vehicle position during the GNSS outages. For each of the recordings, your task is to produce a CSV file with three columns:

This is the same data format as the “true positions” files that are provided for recordings 1-6. However, your calculations should be labelled “estimated positions”. You must produce a file “Recording 7 estimated positions.csv” that will be compared against the (secret) “Recording 7 true positions.csv” file to assess the performance of your Kalman filter.

Recommended approach

You may assume that the vehicle is always operated on flat ground. Consequently, all your vehicle dynamics are two dimensional, and you avoid the complexity of 3D rotations in your system model. The actual AutoWeed vehicle is typically operated on flat terrain, so this is a reasonable assumption.

When designing your Kalman filter, the following state vector is recommended:

You will require two different measurement models. When GNSS is available, you will receive measurements for all 5 state elements. When GNSS is denied, you will receive measurements for the last 3 elements only. Recall that the height of the measurement matrix will be the number of measurements that are received, i.e. you’ll have having 5 rows and a separate with 3 rows.

Hints:

• When developing your prediction step, be careful with the definition of heading (i.e. 0 heading meaning travelling north), and use the appropriate trig functions to calculate changes in the easting and northing. Make sure you draw a picture to ensure that you have it the right way around.
• Your prediction step will be non-linear due to the presence of the trig functions. Hence you’ll need to use the extended Kalman filter for the prediction step. You can use the symbolic toolbox in Matlab to calculate the Jacobian of the prediction function.
• Your measurement model is (most likely) linear, hence you can use the equations for the classic Kalman filter for the update step.
• Recordings 1 and 3 have a lengthy period at the beginning where the vehicle is not moving. You can use that portion of the recording to estimate the standard deviation of each of the sensors, which is necessary information for your measurement covariance matrix.
• The time steps in the recordings are not quite uniform. Your prediction model will need to use the actual for that timestep.
• Heading is an angle, i.e. radians = radians. Hence in your update step, you should compute . Otherwise you could have what appears to be a large “error” in heading where your prediction is slightly above but the sensor reading is close to .

Written report

You must also write a report that describes your results. The following report structure is recommended:

1. Introduction
   • Briefly describe the motivation and the problem that you are addressing.
2. Literature Review [EE5901 students only]
   • Review some aspect of the sensor fusion literature and describe how this relates to your task. (See hints below).
3. Sensor fusion method
   • Describe the derivation of your model, e.g. describe the derivation of the prediction step, the measurement model, etc.
   • Show the working for how you obtained these equations.
4. Results and discussion
   • Analyse the performance of your method for the 6 recordings where you have known GNSS fixes. For example, compare the accuracy over time. Is the error growing over time? Is it more or less reliable when the heading is changing? What is the worst case error that is observed in the provided recordings and how does this compare to the mean error?
   • Comment on the implications of these results for the scenario of navigation in a GNSS denied environment. Is the proposed sensor fusion method a success? What criteria do you think would define success and how does your system compare?
5. Conclusion
   • Summarise the report and give recommendations for future work (i.e. how could the system be improved, what else could be investigated, are there other sensors that would have helped, etc).

Literature review for EE5901 students

If you are enrolled in the Masters level version of this class, then you are required to engage more critically with the research in this field. In discussion with your lecturer, you should identify some aspect of sensor fusion that interests you to investigate for a brief literature review.

Some possible topics:

• Methods of determining the process covariance matrix.
• Applications of inertial measurement unit (IMU) sensor fusion in robotics or another field like healthcare, sports science, aerospace, etc.
• Methods and applications of indoor positioning or navigation in other GNSS-denied environments.

Your literature review should be relatively brief and focus on giving a survey of the field without going into all the technical details. Aim for 1-2 pages of text. The IEEE reference style is encouraged (but any format is acceptable so long as you use it consistently).

Your lecturer will be able to help you in formulating your review and critically analysing the research papers.

Submission and assessment criteria

Prepare the following items for submission:

• Your Kalman filter outputs in files “Recording 1 estimated position.csv”, “Recording 2 estimated position.csv”, etc. Make sure that you include the 7th recording, since the performance evaluation will mostly be conducted on that file.
• Your written report in PDF format.
• Your software source code (e.g. as a Zip file containing all source code).

Submit your work via LearnJCU before the due date.

Assessment will be based on the rubric in the subject outline.