M-File Help: EKF View code for EKF

EKF

Extended Kalman Filter for navigation

This class can be used for:

It is used in conjunction with:

The EKF object updates its state at each time step, and invokes the state update methods of the Vehicle. The complete history of estimated state and covariance is stored within the EKF object.

Methods

run run the filter
plot_xy plot the actual path of the vehicle
plot_P plot the estimated covariance norm along the path
plot_map plot estimated feature points and confidence limits
plot_ellipse plot estimated path with covariance ellipses
plot_error plot estimation error with standard deviation bounds
display print the filter state in human readable form
char convert the filter state to human readable string

Properties

x_est estimated state
P estimated covariance
V_est estimated odometry covariance
W_est estimated sensor covariance
features maps sensor feature id to filter state element
robot reference to the Vehicle object
sensor reference to the Sensor subclass object
history vector of structs that hold the detailed filter state from each time step
verbose show lots of detail (default false)
joseph use Joseph form to represent covariance (default true)

Vehicle position estimation (localization)

Create a vehicle with odometry covariance V, add a driver to it, create a Kalman filter with estimated covariance V_est and initial state covariance P0

veh = Vehicle(V);
veh.add_driver( RandomPath(20, 2) );
ekf = EKF(veh, V_est, P0);

We run the simulation for 1000 time steps

ekf.run(1000);

then plot true vehicle path

veh.plot_xy('b');

and overlay the estimated path

ekf.plot_xy('r');

and overlay uncertainty ellipses at every 20 time steps

ekf.plot_ellipse(20, 'g');

We can plot the covariance against time as

clf
ekf.plot_P();

Map-based vehicle localization

Create a vehicle with odometry covariance V, add a driver to it, create a map with 20 point features, create a sensor that uses the map and vehicle state to estimate feature range and bearing with covariance W, the Kalman filter with estimated covariances V_est and W_est and initial vehicle state covariance P0

veh = Vehicle(V);
veh.add_driver( RandomPath(20, 2) );
map = Map(20);
sensor = RangeBearingSensor(veh, map, W);
ekf = EKF(veh, V_est, P0, sensor, W_est, map);

We run the simulation for 1000 time steps

ekf.run(1000);

then plot the map and the true vehicle path

map.plot();
veh.plot_xy('b');

and overlay the estimatd path

ekf.plot_xy('r');

and overlay uncertainty ellipses at every 20 time steps

ekf.plot_ellipse([], 'g');

We can plot the covariance against time as

clf
ekf.plot_P();

Vehicle-based map making

Create a vehicle with odometry covariance V, add a driver to it, create a sensor that uses the map and vehicle state to estimate feature range and bearing with covariance W, the Kalman filter with estimated sensor covariance W_est and a "perfect" vehicle (no covariance), then run the filter for N time steps.

veh = Vehicle(V);
veh.add_driver( RandomPath(20, 2) );
sensor = RangeBearingSensor(veh, map, W);
ekf = EKF(veh, [], [], sensor, W_est, []);

We run the simulation for 1000 time steps

ekf.run(1000);

Then plot the true map

map.plot();

and overlay the estimated map with 3 sigma ellipses

ekf.plot_map(3, 'g');

Simultaneous localization and mapping (SLAM)

Create a vehicle with odometry covariance V, add a driver to it, create a map with 20 point features, create a sensor that uses the map and vehicle state to estimate feature range and bearing with covariance W, the Kalman filter with estimated covariances V_est and W_est and initial state covariance P0, then run the filter to estimate the vehicle state at each time step and the map.

veh = Vehicle(V);
veh.add_driver( RandomPath(20, 2) );
map = Map(20);
sensor = RangeBearingSensor(veh, map, W);
ekf = EKF(veh, V_est, P0, sensor, W, []);

We run the simulation for 1000 time steps

ekf.run(1000);

then plot the map and the true vehicle path

map.plot();
veh.plot_xy('b');

and overlay the estimated path

ekf.plot_xy('r');

and overlay uncertainty ellipses at every 20 time steps

ekf.plot_ellipse([], 'g');

We can plot the covariance against time as

clf
ekf.plot_P();

Then plot the true map

map.plot();

and overlay the estimated map with 3 sigma ellipses

ekf.plot_map(3, 'g');

Reference

Robotics, Vision & Control, Chap 6, Peter Corke, Springer 2011

Acknowledgement

Inspired by code of Paul Newman, Oxford University, http://www.robots.ox.ac.uk/~pnewman

See also

Vehicle, RandomPath, RangeBearingSensor, Map, ParticleFilter


EKF.EKF

EKF object constructor

E = EKF(vehicle, v_est, p0, options) is an EKF that estimates the state of the vehicle with estimated odometry covariance v_est (2x2) and initial covariance (3x3).

E = EKF(vehicle, v_est, p0, sensor, w_est, map, options) as above but uses information from a vehicle mounted sensor, estimated sensor covariance w_est and a map.

Options

'verbose' Be verbose.
'nohistory' Don't keep history.
'joseph' Use Joseph form for covariance
'dim', D Dimension of the robot's workspace. Scalar D is DxD, 2-vector D(1)xD(2), 4-vector is D(1)

Notes

See also

Vehicle, Sensor, RangeBearingSensor, Map


EKF.char

Convert to string

E.char() is a string representing the state of the EKF object in human-readable form.

See also

EKF.display


EKF.display

Display status of EKF object

E.display() displays the state of the EKF object in human-readable form.

Notes

See also

EKF.char


EKF.init

Reset the filter

E.init() resets the filter state and clears the history.


EKF.plot_ellipse

Plot vehicle covariance as an ellipse

E.plot_ellipse() overlay the current plot with the estimated vehicle position covariance ellipses for 20 points along the path.

E.plot_ellipse(i) as above but for i points along the path.

E.plot_ellipse(i, ls) as above but pass line style arguments ls to plot_ellipse. If i is [] then assume 20.

See also

plot_ellipse


EKF.plot_error

Plot vehicle position

E.plot_error(options) plot the error between actual and estimated vehicle path (x, y, theta). Heading error is wrapped into the range [-pi,pi)

out = E.plot_error() is the estimation error versus time as a matrix (Nx3) where each row is x, y, theta.

Options

'bound', S Display the S sigma confidence bounds (default 3). If S =0 do not display bounds.
'boundcolor', C Display the bounds using color C
LS Use MATLAB linestyle LS for the plots

Notes

See also

EKF.plot_xy, EKF.plot_ellipse, EKF.plot_P


EKF.plot_map

Plot landmarks

E.plot_map() overlay the current plot with the estimated landmark position (a +-marker) and a covariance ellipses.

E.plot_map(ls) as above but pass line style arguments ls to plot_ellipse.

p = E.plot_map() returns the estimated landmark locations (2xN) and column I is the I'th map feature. If the landmark was not estimated the corresponding column contains NaNs.

See also

plot_ellipse


EKF.plot_P

Plot covariance magnitude

E.plot_P() plots the estimated covariance magnitude against time step.

E.plot_P(ls) as above but the optional line style arguments ls are passed to plot.

m = E.plot_P() returns the estimated covariance magnitude at all time steps as a vector.


EKF.plot_xy

Plot vehicle position

E.plot_xy() overlay the current plot with the estimated vehicle path in the xy-plane.

E.plot_xy(ls) as above but the optional line style arguments ls are passed to plot.

p = E.plot_xy() returns the estimated vehicle pose trajectory as a matrix (Nx3) where each row is x, y, theta.

See also

EKF.plot_error, EKF.plot_ellipse, EKF.plot_P


EKF.run

Run the filter

E.run(n, options) runs the filter for n time steps and shows an animation of the vehicle moving.

Options

'plot' Plot an animation of the vehicle moving

Notes


 

© 1990-2012 Peter Corke.