Localization and mapping

These classes support simulation of vehicle and map estimation in a simple planar world with point landmarks.

State estimation

Two state estimators are included.

Extended Kalman filter

The EKF is capable of vehicle localization, map estimation or SLAM.

class EKF(robot, sensor=None, map=None, P0=None, x_est=None, joseph=True, animate=True, x0=(0, 0, 0), verbose=False, history=True, workspace=None)[source]

Bases: object

Extended Kalman filter for vehicle localization, map estimation and SLAM

__init__(robot, sensor=None, map=None, P0=None, x_est=None, joseph=True, animate=True, x0=(0, 0, 0), verbose=False, history=True, workspace=None)[source]

Extended Kalman filter

Parameters:

This class solves several classical robotic estimation problems, which are selected according to the arguments:

Problem

len(x)

robot

sensor

map

P0

Dead reckoning

3

(veh,V)

None

None

P0

Map-based localization

3

(veh,V)

(smodel,W)

yes

P0

Map creation

2N

(veh,None)

(smodel,W)

None

None

SLAM

3+2N

(veh,V)

(smodel,W)

None

P0

where:

  • veh models the robotic vehicle kinematics and odometry and is a VehicleBase subclass

  • V is the estimated odometry (process) noise covariance as an ndarray(2,2)

  • smodel models the robot mounted sensor and is a SensorBase subclass

  • W is the estimated sensor (measurement) noise covariance as an ndarray(2,2)

The state vector has different lengths depending on the particular estimation problem, see below.

At each iteration of the EKF:

  • invoke the step method of the robot

    • obtains the next control input from the driver agent, and apply it as the vehicle control input

    • the vehicle returns a noisy odometry estimate

  • the state prediction is computed

  • the true pose is used to determine a noisy sensor observation

  • the state is corrected, new landmarks are added to the map

The working area of the robot is defined by workspace or inherited from the landmark map attached to the sensor (see expand_dims()):

workspace

x-range

y-range

A (scalar)

-A:A

-A:A

[A, B]

A:B

A:B

[A, B, C, D]

A:B

C:D

Dead-reckoning localization

The state \(\vec{x} = (x, y, \theta)\) is the estimated vehicle configuration.

Create a vehicle with odometry covariance V, add a driver to it, run the Kalman filter with estimated covariances V and initial vehicle state covariance P0:

V = np.diag([0.02, np.radians(0.5)]) ** 2;
robot = Bicycle(covar=V)
robot.control = RandomPath(workspace=10)

x_sdev = [0.05, 0.05, np.radians(0.5)]
P0 = np.diag(x_sdev) ** 2
ekf = EKF(robot=(robot, V), P0=P0)

ekf.run(T=20)  # run the simulation for 20 seconds

robot.plot_xy(color="b")  # plot the true vehicle path
ekf.plot_xy(color="r")    # overlay the estimated path
ekf.plot_ellipse(filled=True, facecolor="g", alpha=0.3)  # overlay uncertainty ellipses

# plot the covariance against time
t = ekf.get_t();
pn = ekf.get_Pnorm()
plt.plot(t, pn);

Map-based vehicle localization

The state \(\vec{x} = (x, y, \theta)\) is the estimated vehicle configuration.

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

V = np.diag([0.02, np.radians(0.5)]) ** 2;
robot = Bicycle(covar=V)
robot.control = RandomPath(workspace=10)

map = LandmarkMap(20, workspace=10)

W = np.diag([0.1, np.radians(1)]) ** 2
sensor = RangeBearingSensor(robot=robot, map=map, covar=W, angle=[-np.pi/2, np.pi/2], range=4, plot=True)

x_sdev = [0.05, 0.05, np.radians(0.5)]
P0 = np.diag(x_sdev) ** 2
ekf = EKF(robot=(robot, V), P0=P0, map=map, sensor=(sensor, W))

ekf.run(T=20)  # run the simulation for 20 seconds

map.plot()  #  plot the map
robot.plot_xy(color="b")  # plot the true vehicle path
ekf.plot_xy(color="r")    # overlay the estimated path
ekf.plot_ellipse()  # overlay uncertainty ellipses

# plot the covariance against time
t = ekf.get_t();
pn = ekf.get_Pnorm()
plt.plot(t, pn);

Vehicle-based map making

The state \(\vec{x} = (x_0, y_0, \dots, x_{N-1}, y_{N-1})\) is the estimated landmark positions where \(N\) is the number of landmarks. The state vector is initially empty, and is extended by 2 elements every time a new landmark is observed.

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

robot = Bicycle()
robot.add_driver(RandomPath(workspace=10))

map = LandmarkMap(20, workspace=10, seed=0)

W = np.diag([0.1, np.radians(1)]) ** 2
sensor = RangeBearingSensor(robot, map, covar=W)

ekf = EKF(robot=(robot, None), sensor=(sensor, W))

ekf.run(T=20)  # run the simulation for 20 seconds

map.plot()  #  plot the map
robot.plot_xy(color="b")  # plot the true vehicle path

Simultaneous localization and mapping (SLAM)

The state \(\vec{x} = (x, y, \theta, x_0, y_0, \dots, x_{N-1}, y_{N-1})\) is the estimated vehicle configuration followed by the estimated landmark positions where \(N\) is the number of landmarks. The state vector is initially of length 3, and is extended by 2 elements every time a new landmark is observed.

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

V = np.diag([0.02, np.radians(0.5)]) ** 2;
robot = Bicycle(covar=V)
robot.control = RandomPath(workspace=10)

map = LandmarkMap(20, workspace=10)

W = np.diag([0.1, np.radians(1)]) ** 2
sensor = RangeBearingSensor(robot=robot, map=map, covar=W, angle=[-np.pi/2, np.pi/2], range=4, plot=True)

x_sdev = [0.05, 0.05, np.radians(0.5)]
P0 = np.diag(x_sdev) ** 2
ekf = EKF(robot=(robot, V), P0=P0, sensor=(sensor, W))

ekf.run(T=20)  # run the simulation for 20 seconds

map.plot(); # plot true map
ekf.plot_map(); # plot estimated landmark position

robot.plot_xy(); # plot true path
ekf.plot_xy(); # plot estimated robot path
ekf.plot_ellipse(); # plot estimated covariance

# plot the covariance against time
t = ekf.get_t();
pn = ekf.get_Pnorm()
plt.plot(t, pn);

See also

run()

property x_est: ndarray

Get EKF state

Returns:

state vector

Return type:

ndarray(n)

Returns the value of the estimated state vector at the end of simulation. The dimensions depend on the problem being solved.

property P_est: ndarray

Get EKF covariance

Returns:

covariance matrix

Return type:

ndarray(n,n)

Returns the value of the estimated covariance matrix at the end of simulation. The dimensions depend on the problem being solved.

property P0: ndarray | None

Get initial EKF covariance

Returns:

covariance matrix

Return type:

ndarray(n,n)

Returns the value of the covariance matrix passed to the constructor.

property V_est: ndarray | None

Get estimated odometry covariance

Returns:

odometry covariance

Return type:

ndarray(2,2)

Returns the value of the estimated odometry covariance matrix passed to the constructor

property W_est: ndarray | None

Get estimated sensor covariance

Returns:

sensor covariance

Return type:

ndarray(2,2)

Returns the value of the estimated sensor covariance matrix passed to the constructor

property robot: VehicleBase

Get robot object

Returns:

robot used in simulation

Return type:

VehicleBase subclass

property sensor: RangeBearingSensor | None

Get sensor object

Returns:

sensor used in simulation

Return type:

SensorBase subclass

property map: LandmarkMap | None

Get map object

Returns:

map used in simulation

Return type:

LandmarkMap subclass

property verbose: bool

Get verbosity state

Returns:

verbosity

Return type:

bool

property history: list[EKFLog]

Get EKF simulation history

Returns:

simulation history

Return type:

list of namedtuples

At each simulation timestep a namedtuple of is appended to the history list. It contains, for that time step, estimated state and covariance, and sensor observation.

See also

get_t() get_xyt() get_map() get_P() get_Pnorm()

property workspace: ndarray | None

Size of robot workspace

Returns:

workspace bounds [xmin, xmax, ymin, ymax]

Return type:

ndarray(4)

Returns the bounds of the workspace as specified by the constructor option workspace

property landmarks: dict[int, list[int]]

Get landmark information

Returns:

landmark information

Return type:

dict

The dictionary is indexed by the landmark id and gives a 3-tuple:

  • order in which landmark was seen

  • number of times seen

The order in which the landmark was first seen. The first observed landmark has order 0 and so on.

See also

landmark()

landmark(id: int) → tuple[int, int][source]

Landmark information

Parameters:

id (int) – landmark index

Returns:

order in which it was first seen, number of times seen

Return type:

tuple[int, int]

The first observed landmark has order 0 and so on.

See also

landmarks() landmark_index() landmark_mindex()

landmark_index(id: int) → int[source]

Landmark index in complete state vector

Parameters:

id (int) – landmark index

Returns:

index in the state vector

Return type:

int

The return value j is the index of the x-coordinate of the landmark in the EKF state vector, and j+1 is the index of the y-coordinate.

See also

landmark()

landmark_mindex(id: int) → int[source]

Landmark index in map state vector

Parameters:

id (int) – landmark index

Returns:

index in the state vector

Return type:

int

The return value j is the index of the x-coordinate of the landmark in the map vector, and j+1 is the index of the y-coordinate.

See also

landmark()

landmark_x(id: int) → ndarray[source]

Landmark position

Parameters:

id (int) – landmark index

Returns:

landmark position \((x,y)\)

Return type:

ndarray

Returns the landmark position from the current state vector.

init() → None[source]

Reset the filter

Resets the filter state and covariance to their initial values, and clears the landmarks and history.

Return type:

None

run(T: float, animate: bool = False) → None[source]

Run the EKF simulation

Parameters:

  • T (float) – maximum simulation time in seconds

  • animate (bool) – animate motion of vehicle, defaults to False

Return type:

None

Simulates the motion of a vehicle (under the control of a driving agent) and the EKF estimator. The steps are:

  • initialize the filter, vehicle and vehicle driver agent, sensor

  • for each time step:

    • step the vehicle and its driver agent, obtain odometry

    • take a sensor reading

    • execute the EKF

    • save information as a namedtuple to the history list for later display

See also

history() landmark() landmarks() get_xyt() get_t() get_map() get_P() get_Pnorm() plot_xy() plot_ellipse() plot_error() plot_map() run_animation()

run_animation(T: float = 10, x0: ndarray | Sequence[float] | None = None, control: Any = None, format: str | None = None, file: str | None = None) → Any[source]

Run the EKF simulation

Parameters:

  • T (float) – maximum simulation time in seconds

  • format (str | None) – Output format

  • file (str | None) – File name

Returns:

Matplotlib animation object

Return type:

Any

Simulates the motion of a vehicle (under the control of a driving agent) and the EKF estimator for T seconds and returns an animation in various formats:

``format``    ``file``   description
============  =========  ============================
``"html"``    str, None  return HTML5 video
``"jshtml"``  str, None  return JS+HTML video
``"gif"``     str        return animated GIF
``"mp4"``     str        return MP4/H264 video
``None``                 return a ``FuncAnimation`` object

If file can be None then return the video as a string, otherwise it must be a filename.

The simulation steps are:

  • initialize the filter, vehicle and vehicle driver agent, sensor

  • for each time step:

    • step the vehicle and its driver agent, obtain odometry

    • take a sensor reading

    • execute the EKF

    • save information as a namedtuple to the history list for later display

See also

history() landmark() landmarks() get_xyt() get_t() get_map() get_P() get_Pnorm() plot_xy() plot_ellipse() plot_error() plot_map() run_animation()

step(pause: bool = False) → None[source]

Execute one timestep of the simulation

Parameters:

pause (bool) – pause for the vehicle’s time step after drawing the animation, defaults to False

Return type:

None

get_t() → ndarray[source]

Get time from simulation

Returns:

simulation time vector

Return type:

ndarray

Return simulation time vector, starts at zero. The timestep is an attribute of the robot object.

See also

run() history()

get_xyt() → ndarray | None[source]

Get estimated vehicle trajectory

Returns:

vehicle trajectory where each row is configuration \((x, y, \theta)\)

Return type:

ndarray | None

See also

plot_xy() run() history()

plot_xy(*args: Any, block: bool | None = None, **kwargs: Any) → None[source]

Plot estimated vehicle position

Parameters:

  • args (Any) – position arguments passed to plot()

  • kwargs (Any) – keywords arguments passed to plot()

  • block (bool | None) – None leaves showing the figure to the environment (no plt.show call), False shows it and returns at once, True shows it and waits until it is closed; ignored by notebook backends, defaults to None

Return type:

None

Plot the estimated vehicle path in the xy-plane.

See also

get_xyt() plot_error() plot_ellipse() plot_P run() history()

plot_ellipse(confidence: float = 0.95, N: int = 10, block: bool | None = None, **kwargs: Any) → None[source]

Plot uncertainty ellipses

Parameters:

  • confidence (float) – ellipse confidence interval, defaults to 0.95

  • N (int) – number of ellipses to plot, defaults to 10

  • block (bool | None) – None leaves showing the figure to the environment (no plt.show call), False shows it and returns at once, True shows it and waits until it is closed; ignored by notebook backends, defaults to None

  • kwargs (Any) – arguments passed to spatialmath.base.graphics.plot_ellipse()

Return type:

None

Plot N uncertainty ellipses spaced evenly along the trajectory.

See also

get_P() run() history()

plot_error(bgcolor: str = 'r', confidence: float | None = 0.95, ax: Any = None, block: bool | None = None, **kwargs: Any) → None[source]

Plot error with uncertainty bounds

Parameters:

  • bgcolor (str) – color of the confidence bound polygons, defaults to ‘r’

  • confidence (float | None) – confidence interval, defaults to 0.95, None for no bounds

  • ax (Any) – axes to plot into, one for each of \((x, y, \theta)\), defaults to a new figure with three stacked axes

  • block (bool | None) – None leaves showing the figure to the environment (no plt.show call), False shows it and returns at once, True shows it and waits until it is closed; ignored by notebook backends, defaults to None

Raises:

ValueError – if the vehicle pose is not part of the estimated state

Return type:

None

Plot the error between actual and estimated vehicle path \((x, y, \theta)\) versus time as three stacked plots. Heading error is wrapped into the range \([-\pi,\pi)\)

Behind each line draw a shaded polygon bgcolor showing the specified confidence bounds based on the covariance at each time step. Ideally the lines should be within the shaded polygon confidence of the time.

Note

Observations will decrease the uncertainty while periods of dead-reckoning increase it.

Changed in version 1.5.0: - The confidence bounds are the interval that contains that fraction of a one-dimensional Gaussian, \(\pm 1.96\sigma\) for 0.95. Earlier versions used the radius of a two-dimensional confidence region, \(\pm 2.45\sigma\) for 0.95, which made the shaded region about 25% wider than its label. Plots produced with earlier versions show wider bounds. - The error is plotted against time. Earlier versions plotted it against sample number on axes limited to the time range, so only the start of a run was visible. - bgcolor is used, it was previously ignored. - Works for SLAM, where the state also holds the landmarks, and raises ValueError if the vehicle pose is not estimated.

See also

get_P() run() history()

get_map() → ndarray[source]

Get estimated map

Returns:

landmark coordinates \((x, y)\)

Return type:

ndarray

Landmarks are returned in the order they were first observed.

See also

landmarks() run() history()

plot_map(marker_style: dict[str, Any] | None = None, ellipse_style: dict[str, Any] | None = None, confidence: float = 0.95, block: bool | None = None, *, marker: dict[str, Any] | None = None, ellipse: dict[str, Any] | None = None) → None[source]

Plot estimated landmarks

Parameters:

  • marker_style (dict[str, Any] | None) – Matplotlib options for the landmark marker, defaults to a red +

  • ellipse_style (dict[str, Any] | None) – options passed to plot_ellipse(), ellipses are drawn only if this is given

  • confidence (float) – ellipse confidence interval, defaults to 0.95

  • block (bool | None) – None leaves showing the figure to the environment (no plt.show call), False shows it and returns at once, True shows it and waits until it is closed; ignored by notebook backends, defaults to None

  • marker (dict[str, Any] | None) – deprecated, use marker_style

  • ellipse (dict[str, Any] | None) – deprecated, use ellipse_style

Return type:

None

Plot a marker and, if ellipse_style is given, a covariance ellipse for each estimated landmark.

Deprecated since version 1.5.0: marker and ellipse are deprecated, use marker_style and ellipse_style.

Changed in version 1.5.0: Each ellipse is now drawn with the covariance of its own landmark. Previously the covariance was taken from the wrong place in the state covariance matrix, so that the ellipses after the first were wrong, and in SLAM were not landmark covariances at all.

See also

get_map() run() history()

get_P(k: int | None = None) → ndarray | list[ndarray][source]

Get covariance matrices from simulation

Parameters:

k (int | None) – timestep, defaults to None

Returns:

covariance matrix

Return type:

ndarray | list[ndarray]

If k is given return covariance from simulation timestep k, else return a list of all covariance matrices.

See also

get_Pnorm() run() history()

get_Pnorm(k: int | None = None) → float | ndarray[source]

Get covariance norm from simulation

Parameters:

k (int | None) – timestep, defaults to None

Returns:

covariance matrix norm

Return type:

float | ndarray

If k is given return covariance norm from simulation timestep k, else return all covariance norms as a 1D NumPy array.

See also

get_P() run() history()

disp_P(P: ndarray, colorbar: bool | dict[str, Any] = False) → None[source]

Display covariance matrix

Parameters:
Type:

bool or dict

Return type:

None

Plot the elements of the covariance matrix as an image. If colorbar is True add a color bar, if colorbar is a dict add a color bar with these options passed to colorbar.

Note

A log scale is used.

See also

imshow() matplotlib.pyplot.colorbar()

get_transform(map: dict[int, ndarray | Sequence[float]] | ndarray) → SE2[source]

Transformation from estimated map to true map frame

Parameters:

map (dict[int, ndarray | Sequence[float]] | ndarray) – known landmark positions

Returns:

transform from map to estimated map frame

Return type:

SE2

Uses a least squares technique to find the transform between the landmark is world frame and the estimated landmarks in the SLAM reference frame.

See also

points2tr2()

Particle filter

The particle filter is capable of map-based vehicle localization.

class ParticleFilter(robot, sensor, R, L, nparticles=500, seed=0, x0=None, verbose=False, animate=False, history=True, workspace=None)[source]

Bases: object

Particle filter for vehicle localization

__init__(robot, sensor, R, L, nparticles=500, seed=0, x0=None, verbose=False, animate=False, history=True, workspace=None)[source]

Particle filter

Parameters:

  • robot (VehicleBase) – robot motion model

  • sensor (RangeBearingSensor) – vehicle mounted sensor model

  • R (ndarray) – covariance of the zero-mean Gaussian noise added to the particles at each step (diffusion)

  • L (ndarray) – covariance used in the sensor likelihood model

  • nparticles (int) – number of particles, defaults to 500

  • seed (int | None) – random number seed, defaults to 0

  • x0 (ndarray | Sequence[float] | None) – initial particle states, defaults to None which means the particles are distributed uniformly randomly over the workspace, with random headings

  • verbose (bool) – display extra debug information, defaults to False

  • history (bool) – retain step-by-step history, defaults to True

  • workspace (Union[float, ndarray, Sequence[float], HasWorkspace, None]) – dimension of workspace, see expand_dims()

This class implements a Monte-Carlo estimator or particle filter for vehicle state, based on odometry, a landmark map, and landmark observations. The state of each particle is a possible vehicle configuration \((x,y,\theta)\). Bootstrap particle resampling is used.

The working area is defined by workspace or inherited from the landmark map attached to the sensor (see expand_dims()):

workspace

x-range

y-range

A (scalar)

-A:A

-A:A

[A, B]

A:B

A:B

[A, B, C, D]

A:B

C:D

Particles are initially distributed uniform randomly over this area, unless x0 is given:

x0

initial particles

None

uniformly random over the workspace

\((x, y, \theta)\)

all particles at this state

ndarray(nparticles, 3)

a row \((x, y, \theta)\) for each particle

Particles that start at the same state are spread apart by the noise R added at each step. The array is copied, it is not changed by the filter. This is the default for the x0 argument of run().

Example:

V = np.diag([0.02, np.radians(0.5)]) ** 2
robot = Bicycle(covar=V, animation="car", workspace=10)
robot.control = RandomPath(workspace=robot)

map = LandmarkMap(20, workspace=robot.workspace)

W = np.diag([0.1, np.radians(1)]) ** 2
sensor = RangeBearingSensor(robot, map, covar=W, plot=True)

R = np.diag([0.1, 0.1, np.radians(1)]) ** 2
L = np.diag([0.1, 0.1])
pf = ParticleFilter(robot, sensor, R, L, nparticles=1000)

pf.run(T=10)

map.plot()
robot.plot_xy()
pf.plot_xy()

plt.plot(pf.get_std()[:100,:])

Note

Set seed=None to get different behaviour from run to run.

See also

run()

property robot: VehicleBase

Get robot object

Returns:

robot used in simulation

Return type:

VehicleBase subclass

property sensor: RangeBearingSensor

Get sensor object

Returns:

sensor used in simulation

Return type:

SensorBase subclass

property map: LandmarkMap

Get map object

Returns:

map used in simulation

Return type:

LandmarkMap subclass

property verbose: bool

Get verbosity state

Returns:

verbosity

Return type:

bool

property history: list[PFlog]

Get EKF simulation history

Returns:

simulation history

Return type:

list of namedtuples

At each simulation timestep a namedtuple of is appended to the history list. It contains, for that time step, estimated state and covariance, and sensor observation.

See also

get_t() get_xyt() get_std() get_Pnorm()

property workspace: ndarray | None

Size of robot workspace

Returns:

workspace bounds [xmin, xmax, ymin, ymax]

Return type:

ndarray(4)

Returns the bounds of the workspace as specified by constructor option workspace

property random: Generator

Get private random number generator

Returns:

NumPy random number generator

Return type:

numpy.random.Generator

Has methods including:

  • integers(low, high, size, endpoint)

  • random(size)

  • uniform

  • normal(mean, std, size)

  • multivariate_normal(mean, covar, size)

The generator is initialized with the seed provided at constructor time every time init is called.

run(T: float = 10, x0: ndarray | Sequence[float] | None = None) → None[source]

Run the particle filter simulation

Parameters:

  • T (float) – maximum simulation time in seconds

  • x0 (ndarray | Sequence[float] | None) – initial particle states for this run, in the forms that the constructor accepts. Defaults to the x0 given to the constructor, and if that was None the particles are uniformly random over the workspace.

Return type:

None

Simulates the motion of a vehicle (under the control of a driving agent) and the particle-filter estimator. The steps are:

  • initialize the filter, vehicle and vehicle driver agent, sensor

  • for each time step:

    • step the vehicle and its driver agent, obtain odometry

    • take a sensor reading

    • execute the particle filter

    • save information as a namedtuple to the history list for later display

See also

history() get_xyt() get_t() get_std() plot_xy()

run_animation(T: float = 10, x0: ndarray | Sequence[float] | None = None, format: str | None = None, file: str | None = None) → Any[source]

Run the particle filter simulation

Parameters:

  • T (float) – maximum simulation time in seconds

  • x0 (ndarray | Sequence[float] | None) – initial particle states for this run, in the forms that the constructor accepts. Defaults to the x0 given to the constructor, and if that was None the particles are uniformly random over the workspace.

  • format (str | None) – Output format

  • file (str | None) – File name

Returns:

Matplotlib animation object

Return type:

Any

Simulates the motion of a vehicle (under the control of a driving agent) and the particle-filter estimator and returns an animation in various formats:

``format``    ``file``   description
============  =========  ============================
``"html"``    str, None  return HTML5 video
``"jshtml"``  str, None  return JS+HTML video
``"gif"``     str        return animated GIF
``"mp4"``     str        return MP4/H264 video
``None``                 return a ``FuncAnimation`` object

The allowables types for file are given in the second column. A str value is the file name. If None is an option then return the video as a string.

For the last case, a reference to the animation object must be held if used for animation in a Jupyter cell:

anim = robot.run_animation(T=20)

The steps are:

  • initialize the filter, vehicle and vehicle driver agent, sensor

  • for each time step:

    • step the vehicle and its driver agent, obtain odometry

    • take a sensor reading

    • execute the particle filter

    • save information as a namedtuple to the history list for later display

See also

history() get_xyt() get_t() get_std() plot_xy()

plot_pdf() → None[source]

Plot particle PDF

Displays a discrete PDF of vehicle position. Creates a 3D plot where the x- and y-axes are the estimated vehicle position and the z-axis is the particle weight. Each particle is represented by a a vertical line segment of height equal to particle weight.

Return type:

None

get_t() → ndarray[source]

Get time from simulation

Returns:

simulation time vector

Return type:

ndarray

Return simulation time vector, starts at zero. The timestep is an attribute of the robot object.

get_xyt() → ndarray[source]

Get estimated vehicle trajectory

Returns:

vehicle trajectory where each row is configuration \((x, y, \theta)\)

Return type:

ndarray

Changed in version 1.5.0: Returns the heading as well as the position, as documented and as EKF.get_xyt() does. Previously the result had only the \((x, y)\) columns, xyt[:, :2] is that.

See also

plot_xy() run() history()

get_std() → ndarray[source]

Get standard deviation of particles

Returns:

dispersion of position and heading estimates, shape (n, 3)

Return type:

ndarray

Return the population standard deviation of position and circular standard deviation of heading \((\sigma_x, \sigma_y, \sigma_\theta)\) of the equally weighted particle cloud at each time step. Heading dispersion is in radians and is unbounded as the mean resultant tends to zero.

See also

get_xyt()

plot_xy(block: bool | None = None, **kwargs: Any) → None[source]

Plot estimated vehicle position

Parameters:

  • args – position arguments passed to plot()

  • kwargs (Any) – keywords arguments passed to plot()

  • block (bool | None) – None leaves showing the figure to the environment (no plt.show call), False shows it and returns at once, True shows it and waits until it is closed; ignored by notebook backends, defaults to None

Return type:

None

Plot the estimated vehicle path in the xy-plane.

See also

get_xyt()

Sensor models

class RangeBearingSensor(robot, map, line_style=None, poly_style=None, covar=None, range=None, angle=None, plot=False, seed=0, **kwargs)[source]

Bases: SensorBase

Range and bearing angle sensor

__init__(robot, map, line_style=None, poly_style=None, covar=None, range=None, angle=None, plot=False, seed=0, **kwargs)[source]

Range and bearing angle sensor

Parameters:

  • robot (Any) – model of robot carrying the sensor

  • map (Any) – map of landmarks

  • line_style (dict[str, Any] | None) – style of the line to the observed landmark, defaults to None

  • poly_style (dict[str, Any] | None) – style for the sensing region polygon, defaults to None

  • covar (ndarray | Sequence[float] | None) – covariance matrix for sensor readings, or the variance of each of range and bearing, defaults to None (noise free)

  • range (float | ndarray | Sequence[float] | None) – maximum range \(r_{max}\) or range span \([r_{min}, r_{max}]\), defaults to None

  • angle (float | ndarray | Sequence[float] | None) – angular field of view, either \(\theta\) for the span \([-\theta, \theta]\) or the span \([\theta_{min}, \theta_{max}]\), defaults to None

  • plot (bool) – plot the map and a line to each observed landmark, defaults to False

  • seed (int | None) – random number seed, defaults to 0

  • kwargs (Any) – arguments passed to SensorBase

Sensor object that returns the range and bearing angle \((r, \beta)\) to a point landmark from a robot-mounted sensor. The sensor measurements are corrupted with zero-mean Gaussian noise with covariance covar.

The sensor can have a maximum range, or a minimum and maximum range. The sensor can also have a restricted angular field of view.

If plot is True the landmark map is displayed and a line is drawn from the robot to the landmark observed each time reading() is called.

line_style styles the landmark beam and poly_style styles the sensing region when plot=True. The region is shown when both range and angle limits are supplied.

>>> from roboticstoolbox import Bicycle, LandmarkMap, RangeBearingSensor
>>> from math import pi
>>> robot = Bicycle()
>>> map = LandmarkMap(20)
>>> sensor = RangeBearingSensor(robot, map, range=(0.5, 20), angle=pi/4)
>>> print(sensor)
RangeBearingSensor sensor class
  LandmarkMap object with 20 landmarks, workspace=(-10.0: 10.0, -10.0: 10.0)
  W = [ 0, 0 | 0, 0 ]
  sampled every 1 samples
  range: (0.5: 20.0)
  angle: (-0.785: 0.785)

See also

LandmarkMap EKF

Deprecated since version 1.4.0: The animate keyword, use plot instead.

init()[source]

Initialize sensor

  • reseed the random number generator

  • reset the counter for handling the every and fail options

  • reset the landmark log

  • initialize plots

See also

SensorBase.init()

Return type:

None

property W: ndarray

Get sensor covariance

Returns:

sensor covariance

Return type:

ndarray(2,2)

Returns the value of the sensor covariance matrix passed to the constructor.

reading()[source]

Choose landmark and return observation

Returns:

range and bearing angle to a landmark, and landmark id

Return type:

tuple[ndarray | None, int | None]

Returns an observation of a random visible landmark (range, bearing) and the id of that landmark. The landmark is chosen randomly from the set of all visible landmarks, those within the angular field of view and range limit.

If constructor argument every is set then only return a valid reading on every every calls.

If constructor argument fail is set then do not return a reading during that specified time interval.

If no valid reading is available then return (None, None)

>>> from roboticstoolbox import Bicycle, LandmarkMap, RangeBearingSensor
>>> from math import pi
>>> robot = Bicycle()
>>> map = LandmarkMap(20)
>>> sensor = RangeBearingSensor(robot, map, range=(0.5, 20), angle=pi/4)
>>> print(sensor.reading())
(array([ 7.2691, -0.039 ]), 16)

Note

  • Noise with covariance W (set by constructor) is added to the reading

  • If animate option is set then show a line from the vehicle to the landmark

  • If animate option set and the angular and distance limits are set then display the sensor field of view as a polygon.

See also

h()

plot(id)[source]

Update the sensing region and the beam to the observed landmark.

Parameters:

id (int | None) – landmark index, or None if no landmark is observed

Return type:

None

visible()[source]

List of all visible landmarks

Returns:

list of visible landmarks

Return type:

list[tuple[ndarray, int]]

Return a list of the id of all landmarks that are visible, that is, it lies with the sensing range and field of view of the sensor at the robot’s current configuration.

See also

isvisible() h()

isvisible(id)[source]

Test if landmark is visible

Parameters:

id (int) – landmark id

Returns:

visibility

Return type:

bool

The landmark id is visible if it lies with the sensing range and field of view of the sensor at the robot’s current configuration.

See also

visible() h()

h(x, landmark=None)[source]

Landmark observation function

Parameters:
Returns:

range and bearing angle to landmark \((r,\beta)\)

Return type:

ndarray

Return the range and bearing to a landmark:

  • .h(x) is range and bearing to all landmarks, one row per landmark, including maps with just one landmark

  • .h(x, id) is range and bearing to landmark id

  • .h(x, p) is range and bearing to landmark with coordinates p

For an explicit landmark, a single vehicle pose returns a vector of two elements. An ndarray of vehicle poses returns one row per pose, including a batch containing just one pose.

>>> from roboticstoolbox import Bicycle, LandmarkMap, RangeBearingSensor
>>> from math import pi
>>> robot = Bicycle()
>>> map = LandmarkMap(20)
>>> sensor = RangeBearingSensor(robot, map, range=(0.5, 20), angle=pi/4)
>>> z = sensor.h((1, 2, pi/2), 3)
>>> print(z)
[10.7111  1.4826]

Note

  • Noise with covariance (property W) is added to each row of z.

  • Performs fast vectorized operation where x is an ndarray(n,3).

  • The landmark is assumed to be visible, field of view and range limits are not applied.

See also

reading() Hx() Hw() Hp()

Hx(x, landmark)[source]

Jacobian dh/dx

Parameters:

  • x (ndarray | Sequence[float]) – vehicle state \((x, y, \theta)\)

  • arg (int or array_like(2)) – landmark id or coordinate

Returns:

Jacobian matrix

Return type:

ndarray

Compute the Jacobian of the observation function with respect to vehicle configuration \(\partial h/\partial x\)

  • sensor.Hx(q, id) is Jacobian for landmark id

  • sensor.h(q, p) is Jacobian for landmark with coordinates p

See also

h() Hp() Hw()

Hp(x, landmark)[source]

Jacobian dh/dp

Parameters:

  • x (ndarray | Sequence[float]) – vehicle state \((x, y, \theta)\)

  • arg (int or array_like(2)) – landmark id or coordinate

Returns:

Jacobian matrix

Return type:

ndarray

Compute the Jacobian of the observation function with respect to landmark position \(\partial h/\partial p\)

  • sensor.Hp(x, id) is Jacobian for landmark id

  • sensor.Hp(x, p) is Jacobian for landmark with coordinates p

See also

h() Hx() Hw()

Hw(x, landmark)[source]

Jacobian dh/dw

Parameters:

  • x (ndarray | Sequence[float]) – vehicle state \((x, y, \theta)\)

  • arg (int or array_like(2)) – landmark id or coordinate

Returns:

Jacobian matrix

Return type:

ndarray

Compute the Jacobian of the observation function with respect to sensor noise \(\partial h/\partial w\)

  • sensor.Hw(x, id) is Jacobian for landmark id

  • sensor.Hw(x, p) is Jacobian for landmark with coordinates p

Note

x and landmark are not used to compute this.

See also

h() Hx() Hp()

property map: Any

Landmark map associated with sensor (superclass)

Returns:

robot

Return type:

LandmarkMap

property random: Generator

Get private random number generator (superclass)

Returns:

NumPy random number generator

Return type:

numpy.random.Generator

Has methods including:

The generator is initialized with the seed provided at constructor time every time init() is called.

See also

init()

property robot: Any

Robot associated with sensor (superclass)

Returns:

robot

Return type:

VehicleBase subclass

property verbose: bool

Get verbosity state

Returns:

verbosity

Return type:

bool

g(x, z)[source]

Landmark position from sensor observation

Parameters:
Returns:

landmark position \((x, y)\)

Return type:

ndarray

Compute the world coordinate of a landmark given the observation z from a vehicle state with x.

See also

h() Gx() Gz()

Gx(x, z)[source]

Jacobian dg/dx

Parameters:
Returns:

Jacobian matrix

Return type:

ndarray

Compute the Jacobian of the landmark position function with respect to landmark position \(\partial g/\partial x\)

See also

g()

Gz(x, z)[source]

Jacobian dg/dz

Parameters:
Returns:

Jacobian matrix

Return type:

ndarray

Compute the Jacobian of the landmark position function with respect to sensor observation \(\partial g/\partial z\)

See also

g()

class SensorBase(robot, map, every=1, fail=[], plot=False, delay=0.1, seed=0, verbose=False, **kwargs)[source]

Bases: ABC

Superclass for sensors

__init__(robot, map, every=1, fail=[], plot=False, delay=0.1, seed=0, verbose=False, **kwargs)[source]

Sensor.Sensor Sensor object constructor % # S = Sensor(VEHICLE, MAP, OPTIONS) is a sensor mounted on a vehicle # described by the Vehicle subclass object VEHICLE and observing landmarks # in a map described by the LandmarkMap class object self. % # Options:: # ‘animate’ animate the action of the laser scanner # ‘ls’,LS laser scan lines drawn with style ls (default ‘r-‘) # ‘skip’, I return a valid reading on every I’th call # ‘fail’,T sensor simulates failure between timesteps T=[TMIN,TMAX] % # Notes:: # - Animation shows a ray from the vehicle position to the selected # landmark.

init()[source]

Initialize sensor (superclass)

  • reseed the random number generator

  • reset the counter for handling the every and fail options

Return type:

None

__str__()[source]

Convert sensor parameters to a string (superclass) % # s = self.char() is a string showing sensor parameters in # a compact human readable format.

Return type:

str

property robot: Any

Robot associated with sensor (superclass)

Returns:

robot

Return type:

VehicleBase subclass

property map: Any

Landmark map associated with sensor (superclass)

Returns:

robot

Return type:

LandmarkMap

property random: Generator

Get private random number generator (superclass)

Returns:

NumPy random number generator

Return type:

numpy.random.Generator

Has methods including:

The generator is initialized with the seed provided at constructor time every time init() is called.

See also

init()

property verbose: bool

Get verbosity state

Returns:

verbosity

Return type:

bool

plot(id)[source]

Plot sensor observation

Parameters:

id (int) – landmark id

Return type:

None

Draws a line from the robot to landmark id.

Note

  • The line is drawn using the line_style given at constructor time

Map models

LANDMARK_STYLE: dict[str, Any] = {'color': 'black', 'linestyle': 'None', 'linewidth': 0, 'marker': 'x'}

default Matplotlib style of the landmarks, a user’s options are merged over this

class LandmarkMap(landmarks=None, workspace=10, seed=0, verbose=None, map=None)[source]

Bases: object

Map of planar point landmarks

A LandmarkMap object represents a rectangular 2D environment with a number of point landmarks.

The landmarks can be specified explicitly or be uniform randomly positioned inside a region defined by the workspace. The workspace can be numeric:

workspace

x-range

y-range

A (scalar)

-A:A

-A:A

[A, B]

A:B

A:B

[A, B, C, D]

A:B

C:D

or any object that has a workspace attribute, see The workspace.

Example:

>>> from roboticstoolbox import LandmarkMap
>>> map = LandmarkMap(20)
>>> print(map)
LandmarkMap object with 20 landmarks, workspace=(-10.0: 10.0, -10.0: 10.0)
>>> print(map[3])  # coordinates of landmark 3
[-9.6694  2.9438]

The object is an iterator that returns consecutive landmark coordinates.

Reference:

Robotics, Vision & Control in Python, 3rd edition, Chap 6, Peter Corke, Springer 2023

See also

RangeBearingSensor EKF

__init__(landmarks=None, workspace=10, seed=0, verbose=None, map=None)[source]

Create a landmark map

Parameters:

  • landmarks (NDArray | int | None) – the landmark coordinates, one column per landmark, or the number of landmarks to place at random in the workspace

  • workspace (Union[float, ndarray, Sequence[float], HasWorkspace]) – workspace or map bounds, defaults to 10

  • seed (int | None) – random number seed used when placing landmarks at random, defaults to 0

  • verbose (bool | None) – deprecated, it had no effect

  • map (NDArray | None) – deprecated, use landmarks

Raises:

ValueError – landmarks is not given, or is not a 2xN array or an integer

Deprecated since version 1.5.0: verbose has no effect and map is renamed landmarks.

__str__()[source]

Compact string description of the map

Return type:

str

Returns:

number of landmarks and the workspace

__repr__()[source]

Compact string description of the map, same as __str__()

Return type:

str

__len__()[source]

Number of landmarks in map

Return type:

int

Returns:

number of landmarks in the map

property landmarks: NDArray

xy-coordinates of all landmarks

Returns:

xy-coordinates for landmark points, one column per landmark

Return type:

ndarray(2, n)

property workspace: ndarray[Tuple[Literal[4]], dtype[floating]]

Size of map workspace

Returns:

workspace bounds [xmin, xmax, ymin, ymax]

Returns the bounds of the workspace as specified by constructor option workspace.

__getitem__(k)[source]

Get landmark coordinates from map

Parameters:

k (int) – landmark index

Return type:

ndarray[Tuple[Literal[2]], dtype[floating]]

Returns:

coordinate \((x,y)\) of k’th landmark

plot(labels=False, block=None, **kwargs)[source]

Plot landmark map

Parameters:

  • labels (bool) – number the points on the plot, defaults to False

  • block (bool | None) – None leaves showing the figure to the environment (no plt.show call), False shows it and returns at once, True shows it and waits until it is closed; ignored by notebook backends, defaults to None

  • kwargs (Any) – plot() options, merged over LANDMARK_STYLE

Return type:

None

Plot landmark points using Matplotlib options. Default style is black crosses, and any options given override the corresponding default.

Pose graph optimization

A pose graph is the other approach to SLAM: the poses of the vehicle are the vertices of a graph, and the measurements between them are the edges. Optimizing the graph finds the poses that agree best with all of the measurements.

class PoseGraph(filename, lidar=False, verbose=False)[source]

Bases: object

Pose graph optimization for SLAM

Parameters:

  • filename (str) – name of file to load, G2O or TORO format, can be zipped

  • lidar (bool) – load lidar scans, defaults to False

  • verbose (bool) – print a summary of the file when loaded and the cost of each optimization iteration, defaults to False

Raises:

  • RuntimeError – file syntax error

  • ValueError – an edge references an unknown vertex, or a lidar record precedes any vertex

A pose graph has a vertex for each robot pose \((x, y, \theta)\) and an edge for each relative pose measurement between two poses. Optimization adjusts the poses to best satisfy all the measurements.

The vertices are numbered in the order they appear in the file, and pose(), scan() and time() take this number, not the vertex name which is the identifier in the file.

Example:

>>> from roboticstoolbox import PoseGraph
>>> pg = PoseGraph("data/pg1.g2o")
>>> pg.optimize(iterations=3)
References:

  • Robotics, Vision & Control for Python, §6.5, P. Corke, Springer 2023.

  • A Tutorial on Graph-Based SLAM, G. Grisetti, R. Kümmerle, C. Stachniss and W. Burgard, in IEEE Intelligent Transportation Systems Magazine, vol. 2, no. 4, pp. 31-43, winter 2010, doi: 10.1109/MITS.2010.939925.

See also

optimize() plot() scanmap()

__init__(filename, lidar=False, verbose=False)[source]
scan(i: int) → tuple[ndarray, ndarray][source]

Lidar scan at a vertex

Parameters:

i (int) – vertex number

Raises:
Returns:

range and bearing of each beam

Return type:

tuple[ndarray, ndarray]

See also

scanxy()

scanxy(i: int) → ndarray[source]

Lidar scan at a vertex as points in the sensor frame

Parameters:

i (int) – vertex number

Raises:
Returns:

points, one per column, NaN for beams that returned no range less than the maximum range of the sensor

Return type:

ndarray

See also

scan()

plot_scan(n: int | ndarray | Sequence[float], pause: float = 1, **kwargs: Any) → None[source]

Plot lidar scans

Parameters:

  • n (int | ndarray | Sequence[float]) – vertex number, or a sequence of them

  • pause (float) – time to display each scan, defaults to 1 second

  • kwargs (Any) – Matplotlib line options, merged over a marker size of 10

Return type:

None

Each scan is plotted in the sensor frame, in turn.

See also

scanxy()

pose(i: int) → ndarray[source]

Pose of a vertex

Parameters:

i (int) – vertex number

Raises:

IndexError – there is no such vertex

Returns:

pose \((x, y, \theta)\), or position \((x, y)\) for a landmark, as stored in the graph so that it reflects any optimization

Return type:

ndarray

time(i)[source]

Timestamp of the lidar scan at a vertex

Parameters:

i (int) – vertex number

Raises:
Return type:

float

Returns:

timestamp, as a Unix time

plot(vertex_style: dict[str, Any] | None = None, edge_style: dict[str, Any] | None = None, *, vopt: dict[str, Any] | None = None, eopt: dict[str, Any] | None = None, **kwargs: Any) → None[source]

Plot the pose graph

Parameters:

  • vertex_style (dict[str, Any] | None) – Matplotlib options for the vertices, merged over VERTEX_STYLE

  • edge_style (dict[str, Any] | None) – Matplotlib options for the edges, merged over EDGE_STYLE

  • vopt (dict[str, Any] | None) – deprecated, use vertex_style

  • eopt (dict[str, Any] | None) – deprecated, use edge_style

  • kwargs (Any) – options passed to pgraph.UGraph.plot

Return type:

None

Deprecated since version 1.5.0: vopt and eopt are deprecated, use vertex_style and edge_style.

See also

optimize()

scanmatch(s1: int, s2: int) → SE2[source]

Relative pose between two scans by scan matching

Parameters:

  • s1 (int) – vertex number of the first scan

  • s2 (int) – vertex number of the second scan

Raises:
Return type:

SE2

Returns:

relative pose, found by iterative closest point

See also

spatialmath.base.ICP2d()

scanmap(occgrid: BaseOccupancyGrid, maxrange: float | None = None, step: int = 5) → None[source]

Build an occupancy grid from the lidar scans

Parameters:

  • occgrid (BaseOccupancyGrid) – occupancy grid to update

  • maxrange (float | None) – ignore range readings beyond this distance, defaults to None for all

  • step (int) – use every step-th scan, defaults to 5

Raises:

ValueError – the graph has no lidar scans

Return type:

None

Each scan is placed at the pose of its vertex, and the cells along each ray are incremented as free, and the cell it ends in is decremented as occupied. Afterwards cells are -1 (occupied), +1 (free) or 0 (unknown).

Note

Scans are taken in vertex order, so the result depends on the current vertex poses, and is best made after optimize().

See also

scanxy() line_world()

optimize(iterations: int = 10, animate: bool = False, retain: bool = True, vertex_style: dict[str, Any] | None = None, edge_style: dict[str, Any] | None = None, **kwargs: Any) → None[source]

Optimize the pose graph

Parameters:

  • iterations (int) – maximum number of iterations, defaults to 10

  • animate (bool) – show the graph after each iteration, defaults to False

  • retain (bool) – retain the graph of each iteration in the animation, in increasingly dark gray, defaults to True

  • vertex_style (dict[str, Any] | None) – Matplotlib options for the vertices when animating

  • edge_style (dict[str, Any] | None) – Matplotlib options for the edges when animating, the color is set by retain

  • kwargs (Any) – options passed to pgraph.UGraph.plot when animating

Raises:

NotImplementedError – the graph has landmark vertices

Return type:

None

Deprecated since version 1.5.0: vopt and eopt are deprecated, use vertex_style and edge_style.

Each iteration solves the linearized least-squares problem for a correction to all the poses, which are updated in place. Iteration stops early if the total error does not decrease.

See also

linearize_and_solve() plot()

linearize_and_solve() → float[source]

One iteration of pose graph optimization

Return type:

float

Returns:

total cost, the sum of squared errors of all the edges, before the poses are updated

If the graph is verbose the cost and the time taken are printed.

The first vertex is anchored, since the measurements are all relative. The system matrix is assembled from sparse 3-by-3 blocks, so assembly storage grows with the graph’s vertices and edges rather than the square of its vertex count.

See also

optimize()