M-File Help: EKF View code for EKF

EKF

Extended Kalman Filter for vehicle pose and map estimation

This class can be used for: It is used in conjunction with: The EKF object.

Methods

run run the filter
plot_xy return/plot the actual path of the vehicle
plot_P return/plot the estimate covariance
plot_map plot feature points and confidence limits
plot_ellipse plot path with covariance ellipses
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 map book keeping, maps sensor feature id to filter state
robot reference to the robot object
sensor reference to the sensor object
history vector of structs that hold the detailed information from each time step

Vehicle position estimation

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, then run the filter for N time steps.
veh = Vehicle(V);
veh.add_driver( RandomPath(20, 2) );
ekf = EKF(veh, V_est, P0);
ekf.run(N);

Vehicle map based 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, then run the filter for N time steps.
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);
ekf.run(N);

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, []);
ekf.run(N);

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 for N time steps 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, []);
ekf.run(N);

Reference

Robotics, Vision & Control, Peter Corke, Springer 2011

See also

Vehicle, RandomPath, RangeBearingSensor, Map, ParticleFilter


EKF.EKF

EKF object constructor

E = EKF that estimates the state of the vehicle with estimated odometry covariance vest (2x2) and initial covariance (3x3). E = Vehicle, Sensor, RangeBearingSensor, Map


EKF.char

Convert EKF object to string

E.EKF object in human-readable form.

EKF.display

Display status of EKF object

E.display the state of the EKF.char


EKF.plot_P

Plot covariance magnitude

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

EKF.plot_ellipse

Plot vehicle covariance as an ellipse

E.plot_ellipse() as above but i=20. E.plot_ellipse.

See also

plot_ellipse


EKF.plot_map

Plot landmarks

E.plot_map() as above but i=20. E.plot_ellipse


EKF.plot_xy

Plot vehicle position

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

EKF.run

Run the EKF

E.run the filter for n time steps.

Notes

  • all previously estimated states and estimation history is cleared.

 

© 1990-2011 Peter Corke.