M-File Help: EKF

M-File Help: EKF | View code for EKF

# EKF

Extended Kalman Filter for navigation

Extended Kalman filter for optimal estimation of state from noisy measurments given a non-linear dynamic model. This class is specific to the problem of state estimation for a vehicle moving in SE(2).

This class can be used for:

- dead reckoning localization
- map-based localization
- map making
- simultaneous localization and mapping (SLAM)

It is used in conjunction with:

- a kinematic vehicle model that provides odometry output, represented by a Vehicle object.
- The vehicle must be driven within the area of the map and this is achieved by connecting the Vehicle object to a Driver object.
- a map containing the position of a number of landmark points and is represented by a Map object.
- a sensor that returns measurements about landmarks relative to the vehicle's location and is represented by a Sensor object subclass.

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');

## References

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

Stochastic processes and filtering theory, AH Jazwinski Academic Press 1970

## 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

- If MAP is [] then it will be estimated.
- If V_EST and P0 are [] the vehicle is assumed error free and the filter will only estimate the landmark positions (map).
- If V_EST and P0 are finite the filter will estimate the vehicle pose and the landmark positions (map).
- EKF subclasses Handle, so it is a reference object.
- Dimensions of workspace are normally taken from the map if given.

## 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

- This method is invoked implicitly at the command line when the result of an expression is a EKF object and the command has no trailing semicolon.

## 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

- The bounds show the instantaneous standard deviation associated with the state. Observations tend to decrease the uncertainty while periods of dead-reckoning increase it.
- Ideally the error should lie "mostly" within the +/-3sigma bounds.

## 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() is 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() is 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() is 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

- All previously estimated states and estimation history are initially cleared.

© 1990-2014 Peter Corke.