Introduction to Estimation and the Kalman Filter (KC-1)
Extended Kalman Filter Navigation System Example Documentation
The following Matlab code implements the extended Kalman filter navigation
system example described in Section 8 of the course notes. The code is
divided into two main parts:
1. A simulation that generates a vehicle trajectory and observations of
landmarks,
2. An extended Kalman filter which takes as input the control and
observation inputs generated during the simulation and estimates
vehicle position, orientation and effective wheel radius.
To run the code:
1. Start up Matlab and change to the directory containing the source
code.
2. Run the script file “set_up”. This sets up a graphical window that allows
input of beacons and vehicle trajectory. Follow the instructions as
printed in the Matlab command window. Note, the trajectory generation
function uses the Matlab “spline” commands, so trajectories that loop
back (have more than one y value for a given x value) are not allowed.
The function “set_up” uses the trajectory input to compute control
inputs for the vehicle to maintain track. These are used later in the
estimation cycle.
3. Run the script file “run_obs”. This simulates the operation of a scanning
radar mounted on the vehicle and generates simulated observation
information to drive the estimator.
4. Run the script file “run_filt”. This runs the filter itself, using data from
control inputs and observations. The run_filt script file also initialises
state and state covariance.
5. Run the script file “plots”. This plots out the results of the filtering
process in the form found in Section 8 of the notes.
6. The file “ginit” contains all global definitions for variances, vehicle
parameters, etc.
The code is initially set up to run a conventional and well tuned filter. The
objective of the laboratory is:
• To see how the EKF is coded and how it works.
• To understand the process of tuning and operating an EKF.
• To see the effects of faults and changes in initial conditions.
The filtering code structure is reasonably generic. It simply calls two functions
to predict (using a platform model) and then update (using an observation
model), the state and state covariance. It should not be hard to modify these
for other filtering problems.
