Scribd
Upload
55 views9 pages

2D Vehicle EKF Prediction Step Exercise

Uploaded by

sergiosantana
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as PDF, TXT or read online on Scribd
55 views9 pages

2D Vehicle EKF Prediction Step Exercise

Uploaded by

sergiosantana
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as PDF, TXT or read online on Scribd
You are on page 1/ 9

Advanced Kalman Filtering and Sensor Fusion

2D Vehicle Extended
Kalman Filter:
Prediction Step
EKF Exercise 1
2D Vehicle EKF:
Prediction Step Exercise
Overview
Implement the Extended Kalman Filter Prediction Equations and the 2D Non-Linear Vehicle Process
Model.

Step 1 (Setup)
● Open the c++ file “kalmanfilter.cpp” which will be the file used in this exercise (it should be a
new copy of the file “kalmanfilter_ekf_student.cpp” file).
● Compile the run the simulation as is, using profile 3. See that the car starts at the origin (0,0)
and moves at 5 m/s while performing a series of turns.
● Note: The GPS measurement model and state initialisation is copied from the previous.Linear
Kalman Filter exercises as the filter equations and logic are the same, so the filter will start to
run (but using GPS only and without an process model)

EKF Exercise 1 | 2
2D Vehicle EKF:
Prediction Step Exercise

Constants that you


can modify and use

GPS Measurement
is already done
(Same as LKF)

EKF Exercise 1 | 3
2D Vehicle EKF:
Prediction Step Exercise

GPS Measurement
is already done
(Same as LKF)

EKF Exercise 1 | 4
2D Vehicle EKF:
Prediction Step Exercise
Step 2 (Implement the Prediction Step (Process Model))
● Modify the predictionStep() function
● Use the time step and gyroscope input. Assume zero acceleration.
● Normalise the heading angle to +/- PI after prediction!

EKF Exercise 1 | 5
2D Vehicle EKF:
Prediction Step Exercise
Step 3 (Implement the Prediction Step (Covariance))
● Modify the predictionStep() function
● Calculate the Jacobian State matrix (F matrix)
● Define the Q matrix for gyroscope noise and acceleration noise (Assume zero positional noise)
● Implement the covariance prediction step

EKF Exercise 1 | 6
2D Vehicle EKF:
Prediction Step Exercise
Step 4 (Run the Simulation)
● Check out how the simulation runs with profiles 1 - 4.

EKF Exercise 1 | 7
2D Vehicle EKF:
Prediction Step Exercise
Step 5 (You can quickly compare to the LKF Solution for the Error
Statistics)
Note: There will be slight differences in the results due to tuning, etc.

EKF Exercise 1 | 8
2D Vehicle EKF:
Prediction Step Exercise
Step 6 (Play around with your code and make sure it is working as
expected)
● Is this filter performing better than the LKF? Is that to be expected? Is it more robust?
● Can you think of a better way to initialise the state? (i.e how to initialize velocity and heading
without assume them to be zero or a known constant)

EKF Exercise 1 | 9

You might also like