M-File Help: EKF | View code for 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.
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 |
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) |
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();
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();
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');
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');
Robotics, Vision & Control, Chap 6, Peter Corke, Springer 2011
Inspired by code of Paul Newman, Oxford University, http://www.robots.ox.ac.uk/~pnewman
Vehicle, RandomPath, RangeBearingSensor, Map, ParticleFilter
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.
'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) |
Vehicle, Sensor, RangeBearingSensor, Map
Convert to string
E.char() is a string representing the state of the EKF object in human-readable form.
Display status of EKF object
E.display() displays the state of the EKF object in human-readable form.
Reset the filter
E.init() resets the filter state and clears the history.
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.
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.
'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 |
EKF.plot_xy, EKF.plot_ellipse, EKF.plot_P
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.
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.
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.
EKF.plot_error, EKF.plot_ellipse, EKF.plot_P
Run the filter
E.run(n, options) runs the filter for n time steps and shows an animation of the vehicle moving.
'plot' | Plot an animation of the vehicle moving |
© 1990-2012 Peter Corke.