EE3901/EE5901 Sensor Technologies
Assignment 1 — Implementing an extended Kalman filter

Written by A/Prof Bronson Philippa and Laurance Papale

College of Science and Engineering, James Cook University

Last updated 6 March 2025

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.

Figure 1:
AutoWeed prototype vehicle showing cameras at the front and herbicide sprayers at the back. The white antennas are for receiving GNSS signals. The IMU and control system electronics are located inside the black case at the front of the vehicle. The photo shows a small scale research prototype, which was used to gather the data that you will analyse in this assignment.
Zoom:

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:

Time	Eastings	Northings

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:

Introduction
- Briefly describe the motivation and the problem that you are addressing.
Sensor fusion method
- Describe the derivation of your model, e.g. describe the derivation of the prediction step, the measurement model, any values that you derived etc.
- Show the working for how you obtained these equations.
- Describe how you tuned your Kalman filter, e.g. how you determined the process covariance matrix, the measurement covariance matrix, etc.
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?
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).

Submission

Submit your work via LearnJCU before the due date.

You must submit the following items exactly as specified below. Any deviation from these instructions will result in rejection of your submission. You will be asked to resubmit and late penalties will apply.

1. Kalman Filter Output Files

Submit your estimated position files with the exact filenames:
- Recording 1 estimated position.csv
- Recording 2 estimated position.csv
- Recording 3 estimated position.csv
- Recording 4 estimated position.csv
- Recording 5 estimated position.csv
- Recording 6 estimated position.csv
- Recording 7 estimated position.csv (This file is critical for evaluation. Ensure it is included.)
The files must be in CSV format with appropriate data outlined above.

2. Written Report

Submit your report in PDF format only.
The file must be properly formatted and contain all required content.

3. Source Code

Submit all source code as a single ZIP file.
Ensure the ZIP file contains all necessary code files for running your implementation.
You should use relative paths in your code to access any data.

Do not submit any additional files. Only the files listed above should be included.

Assessment Rubric

Criteria	High Distinction	Distinction	Credit	Pass	Fail
Kalman Filter Implementation	Correctly implements the EKF with appropriate prediction and update steps. Handles GNSS availability transitions seamlessly.	EKF is implemented correctly with only minor errors that do not significantly impact performance. Handles GNSS availability transitions but with some inefficiencies.	EKF is mostly functional but has notable errors in state transition or update equations that reduce accuracy. GNSS handling is inconsistent.	EKF is incomplete or contains major flaws leading to significant inaccuracies. GNSS transitions are not handled correctly.	EKF implementation is missing, non-functional, or fundamentally incorrect.
Accuracy of Estimated Positions	Estimated positions closely match ground truth for recordings 1-6, with minimal drift. Recording 7 estimation is reasonable.	Estimated positions are generally accurate, with small but noticeable drift over time. Recording 7 estimation is reasonable but not as precise.	Estimated positions show moderate drift or increasing errors due to weaknesses in process modelling or parameter tuning. Recording 7 has significant inaccuracies.	Large errors in estimated positions, with position estimates diverging significantly from ground truth. Recording 7 is highly inaccurate.	No valid estimated positions submitted, or estimates are completely unreliable.
Process and Measurement Model Development	Clearly and correctly derives the process and measurement models, explaining all assumptions and derivations with strong justification.	Process and measurement models are mostly correct, with clear explanations and reasonable justifications, though minor gaps exist.	Models contain errors or inconsistencies in derivations, with limited justifications provided. Some assumptions are weak or unexplained.	Models are incomplete, unclear, or contain significant formulation errors. Justifications are mostly missing.	No process or measurement model provided, or models are entirely incorrect.
Parameter Tuning and Justification	Process and measurement covariance matrices are well-tuned and justified based on sensor characteristics, with a clear explanation of tuning choices.	Covariance matrices are reasonably tuned with mostly correct justifications, though some choices lack explanation.	Basic tuning is performed but lacks strong justification for chosen values. Some parameters may be poorly selected.	Tuning is ineffective or poorly justified, leading to inaccurate results. Minimal effort in explaining parameter selection.	No tuning information provided, or tuning choices make the EKF non-functional.
Analysis of Results	Provides a detailed and insightful analysis of filter performance, including error trends, sensitivity to different conditions, and discussion of limitations.	Provides a reasonable analysis with discussion of error trends and filter behavior. Some evaluation of limitations is present but not in depth.	Basic analysis of results is provided but lacks key insights or depth. Limited discussion of limitations or accuracy over time.	Analysis is minimal, lacks discussion of key aspects like error trends, and does not critically evaluate performance.	No results analysis provided, or analysis is superficial and lacks meaningful evaluation.
Report Clarity and Structure	Report is well-organized, clearly written, and follows the recommended structure. Figures and equations are properly formatted and well-integrated.	Report is mostly well-structured and clear but contains minor formatting or explanation issues. Figures and equations are mostly correct.	Report follows a reasonable structure but contains unclear explanations, formatting issues, or disorganized sections.	Report lacks organization, clarity, or proper formatting, making it difficult to understand. Explanations are vague or incomplete.	No report submitted, or report is severely incomplete and unreadable.

General Grading Scale:

High Distinction: Outstanding work with minimal errors and a well-executed EKF.
Distinction: Good work with minor issues in implementation or analysis.
Credit: Satisfactory work but with notable issues in modelling, accuracy, or documentation.
Pass: Significant flaws in implementation, analysis, or documentation, but some functionality is present.
Fail: Incomplete or non-functional submission.