#!/usr/bin/env python3
"""
Python EKF Planner
@Author: Peter Corke, original MATLAB code and Python version
@Author: Kristian Gibson, initial MATLAB port
Based on code by Paul Newman, Oxford University,
http://www.robots.ox.ac.uk/~pnewman
"""
from __future__ import annotations
from typing import Any, NamedTuple
import matplotlib.pyplot as plt
import numpy as np
import scipy as sp
import spatialmath.base as smb
from matplotlib import animation
from matplotlib.axes import Axes
from matplotlib.lines import Line2D
from roboticstoolbox.mobile import VehicleBase
from roboticstoolbox.mobile.landmarkmap import LandmarkMap
from roboticstoolbox.mobile.sensors import RangeBearingSensor
from roboticstoolbox.mobile.workspace import WorkspaceLike, resolve_workspace
from roboticstoolbox.tools.plot import apply_block
from roboticstoolbox.tools.types import ArrayLike, NDArray
"""
Monte-carlo based localisation for estimating vehicle pose based on
odometry and observations of known landmarks.
"""
# TODO: refactor this and EKF, RNG, history, common plots, animation, movie
class PFlog(NamedTuple):
"""One step of the history of a :class:`ParticleFilter`"""
t: float
odo: NDArray
xest: NDArray
std: NDArray
weights: NDArray
[docs]
class ParticleFilter:
"""
Particle filter for vehicle localization
"""
[docs]
def __init__(
self,
robot: VehicleBase,
sensor: RangeBearingSensor,
R: NDArray,
L: NDArray,
nparticles: int = 500,
seed: int | None = 0,
x0: ArrayLike | None = None,
verbose: bool = False,
animate: bool = False,
history: bool = True,
workspace: WorkspaceLike | None = None,
) -> None:
r"""
Particle filter
:param robot: robot motion model
:type robot: :class:`VehicleBase` subclass,
:param sensor: vehicle mounted sensor model
:type sensor: :class:`SensorBase` subclass
:param R: covariance of the zero-mean Gaussian noise added to the particles at each step (diffusion)
:type R: ndarray(3,3)
:param L: covariance used in the sensor likelihood model
:type L: ndarray(2,2)
:param nparticles: number of particles, defaults to 500
:type nparticles: int, optional
:param seed: random number seed, defaults to 0
:type seed: int, optional
:param x0: initial particle states, defaults to None which means the
particles are distributed uniformly randomly over the workspace, with
random headings
:type x0: array_like(3) or ndarray(nparticles,3), optional
:param verbose: display extra debug information, defaults to False
:type verbose: bool, optional
:param history: retain step-by-step history, defaults to True
:type history: bool, optional
:param workspace: dimension of workspace, see :func:`~spatialmath.base.graphics.expand_dims`
:type workspace: scalar, array_like(2), array_like(4)
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 :math:`(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
:func:`~spatialmath.base.graphics.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
:math:`(x, y, \theta)` all particles at this state
ndarray(nparticles, 3) a row :math:`(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 :meth:`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.
.. seealso:: :meth:`run`
"""
self._robot = robot
self._sensor = sensor
self.R = R
self.L = L
self.nparticles = nparticles
self._animate = animate
# self.dim = sensor.map.dim
self._history: list[PFlog] = []
self.x: NDArray = np.empty((0, 3))
self.weight: NDArray = np.empty((0,))
self.w0 = 0.05
self._x0 = x0
self._verbose = verbose
self._t: float = 0
self.h: Line2D
# create a private random number stream if required
self._random = np.random.default_rng(seed)
self._seed = seed
self._keep_history = history # keep history
self._htuple = PFlog
# the workspace, in order of precedence: the explicit argument, the
# sensor's map, the vehicle
self._workspace = resolve_workspace(
workspace, sensor.map if sensor is not None else None, self.robot
)
self._dim = self._workspace
# self._init()
def __str__(self) -> str:
# ParticleFilter.char Convert to string
#
# PF.char() is a string representing the state of the ParticleFilter
# object in human-readable form.
#
# See also ParticleFilter.display.
def indent(s: str, n: int = 2) -> str:
spaces = " " * n
return s.replace("\n", "\n" + spaces)
s = f"ParticleFilter object: {self.nparticles} particles"
s += "\nR: " + smb.array2str(self.R)
s += "\nL: " + smb.array2str(self.L)
if self.robot is not None:
s += indent("\nrobot: " + str(self.robot))
if self.sensor is not None:
s += indent("\nsensor: " + str(self.sensor))
return s
@property
def robot(self) -> VehicleBase:
"""
Get robot object
:return: robot used in simulation
:rtype: :class:`VehicleBase` subclass
"""
return self._robot
@property
def sensor(self) -> RangeBearingSensor:
"""
Get sensor object
:return: sensor used in simulation
:rtype: :class:`SensorBase` subclass
"""
return self._sensor
@property
def map(self) -> LandmarkMap:
"""
Get map object
:return: map used in simulation
:rtype: :class:`LandmarkMap` subclass
"""
return self.sensor.map
@property
def verbose(self) -> bool:
"""
Get verbosity state
:return: verbosity
:rtype: bool
"""
return self._verbose
@property
def history(self) -> list[PFlog]:
"""
Get EKF simulation history
:return: simulation history
:rtype: 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.
.. seealso:: :meth:`get_t` :meth:`get_xyt` :meth:`get_std`
:meth:`get_Pnorm`
"""
return self._history
@property
def workspace(self) -> NDArray | None:
"""
Size of robot workspace
:return: workspace bounds [xmin, xmax, ymin, ymax]
:rtype: ndarray(4)
Returns the bounds of the workspace as specified by constructor
option ``workspace``
"""
return self._workspace
@property
def random(self) -> np.random.Generator:
"""
Get private random number generator
:return: NumPy random number generator
:rtype: :class:`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.
"""
return self._random
def _init(
self,
x0: ArrayLike | None = None,
animate: bool = False,
ax: Axes | None = None,
) -> None:
# Initialize the particle filter: the vehicle and sensor, the history and
# the particle states.
#
# The initial states are, in order of precedence, the x0 argument, the x0
# given to the constructor, or a uniformly random distribution over the
# workspace. A vector gives every particle that state, a matrix has a
# row for each particle. The array is copied, since the particles are
# updated in place.
#
# Invoked by run() and run_animation().
self.robot.init()
self.sensor.init()
# clear the history
self._history = []
# create a new private random number generator
if self._seed is not None:
self._random = np.random.default_rng(self._seed)
self._t = 0
# initialize particles
if x0 is None:
x0 = self._x0
if x0 is None:
# create initial particle distribution as uniformly randomly distributed
# over the map workspace and heading angles
workspace = self.workspace
if workspace is None:
raise ValueError(
"no workspace: give the workspace argument, or use a sensor "
"with a map or a robot that has one"
)
x = self.random.uniform(workspace[0], workspace[1], size=(self.nparticles,))
y = self.random.uniform(workspace[2], workspace[3], size=(self.nparticles,))
t = self.random.uniform(-np.pi, np.pi, size=(self.nparticles,))
self.x = np.c_[x, y, t]
else:
# a copy, since the particles are updated in place and the caller's
# array must not change
x0 = np.array(x0, dtype=float)
if x0.shape == (3,):
self.x = np.tile(x0, (self.nparticles, 1))
elif x0.shape == (self.nparticles, 3):
self.x = x0
else:
raise ValueError(
f"x0 must have shape (3,) or ({self.nparticles}, 3), got {x0.shape}"
)
if animate:
if ax is None:
ax = plt.gca()
# display the initial particles
(self.h,) = ax.plot(
self.x[:, 0],
self.x[:, 1],
"go",
zorder=0,
markersize=3,
markeredgecolor="none",
alpha=0.3,
label="particle",
)
self.weight = np.ones((self.nparticles,))
[docs]
def run(self, T: float = 10, x0: ArrayLike | None = None) -> None:
"""
Run the particle filter simulation
:param T: maximum simulation time in seconds
:type T: float
:param x0: 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.
:type x0: array_like(3) or ndarray(nparticles,3), optional
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
.. seealso:: :meth:`history` :meth:`get_xyt` :meth:`get_t` :meth:`get_std`
:meth:`plot_xy`
"""
self._init(x0=x0)
# anim = Animate(opt.movie)
# display the initial particles
ax = smb.axes_logic(None, 2)
if self._animate:
(self.h,) = ax.plot(
self.x[:, 0],
self.x[:, 1],
"go",
zorder=0,
markersize=3,
markeredgecolor="none",
alpha=0.3,
label="particle",
)
# set(self.h, 'Tag', 'particles')
# self.robot.plot()
# iterate over time
for i in range(round(T / self.robot.dt)):
self._step()
if self._animate:
plt.pause(0.2)
# plt.draw()
# anim.add()
# anim.close()
[docs]
def run_animation(
self,
T: float = 10,
x0: ArrayLike | None = None,
format: str | None = None,
file: str | None = None,
) -> Any:
"""
Run the particle filter simulation
:param T: maximum simulation time in seconds
:type T: float
:param x0: 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.
:type x0: array_like(3) or ndarray(nparticles,3), optional
:param format: Output format
:type format: str, optional
:param file: File name
:type file: str, optional
:return: Matplotlib animation object
:rtype: :meth:`matplotlib.animation.FuncAnimation`
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
.. seealso:: :meth:`history` :meth:`get_xyt` :meth:`get_t` :meth:`get_std`
:meth:`plot_xy`
"""
fig, ax = plt.subplots()
nframes = round(T / self.robot.dt)
anim = animation.FuncAnimation(
fig=fig,
# func=lambda i: self._step(animate=True, pause=False),
# init_func=lambda: self._init(animate=True),
func=lambda i: self._step(),
init_func=lambda: self._init(x0=x0, ax=ax, animate=True),
frames=nframes,
interval=self.robot.dt * 1000,
blit=False,
repeat=False,
)
# anim._interval = self.dt*1000/2
# anim._repeat = True
ret = None
if format == "html":
ret = anim.to_html5_video() # convert to embeddable HTML5 animation
elif format == "jshtml":
ret = anim.to_jshtml() # convert to embeddable Javascript/HTML animation
elif format in ("gif", "mp4"):
if file is None:
raise ValueError(f"format {format!r} needs a file name")
fps = round(1 / self.robot.dt)
if format == "gif":
writer: animation.AbstractMovieWriter = animation.PillowWriter(fps=fps)
else:
writer = animation.FFMpegWriter(fps=fps) # mp4/H264
anim.save(file, writer=writer)
ret = None
elif format is None:
# return the anim object
return anim
else:
raise ValueError("unknown format")
if ret is not None and file is not None:
with open(file, "w") as f:
f.write(ret)
ret = None
plt.close(fig)
return ret
# self._init(x0=x0)
# # anim = Animate(opt.movie)
# # display the initial particles
# ax = smb.axes_logic(None, 2)
# if self._animate:
# (self.h,) = ax.plot(
# self.x[:, 0],
# self.x[:, 1],
# "go",
# zorder=0,
# markersize=3,
# markeredgecolor="none",
# alpha=0.3,
# label="particle",
# )
# # set(self.h, 'Tag', 'particles')
# # self.robot.plot()
# # iterate over time
# import time
# for i in range(round(T / self.robot.dt)):
# self._step()
# # time.sleep(0.2)
# plt.pause(0.2)
# # plt.draw()
# # anim.add()
# # anim.close()
def _step(self) -> None:
"""Advance the simulation and estimate the current particle cloud.
Heading uses the circular mean and circular standard deviation.
The moments describe the equally weighted cloud after resampling,
when an observation is available. A zero mean resultant does not
define a unique heading; SpatialMath's angular mean then selects a
representative.
Heading dispersion is unbounded as the mean resultant tends to zero.
"""
# fprintf('---- step\n')
odo = self.robot.step(animate=self._animate) # move the robot
# update the particles based on odometry
self._predict(odo)
# get a sensor reading
z, lm_id = self.sensor.reading()
if z is not None:
self._observe(z, lm_id)
# fprintf(' observe beacon #d\n', lm_id)
self._select()
# Cartesian moments are linear; heading moments are circular.
x_est = np.r_[self.x[:, :2].mean(axis=0), smb.angle_mean(self.x[:, 2])]
# Centre headings before computing their spread to reduce roundoff.
std_est = np.r_[
self.x[:, :2].std(axis=0),
smb.angle_std(smb.angdiff(self.x[:, 2], x_est[2])),
]
# display the updated particles
# set(self.h, 'Xdata', self.x(:,1), 'Ydata', self.x(:,2), 'Zdata', self.x(:,3))
if self._animate:
self.h.set_xdata(self.x[:, 0])
self.h.set_ydata(self.x[:, 1])
# if ~isempty(self.anim)
# self.anim.add()
if self._keep_history:
hist = self._htuple(
self.robot._t, odo.copy(), x_est, std_est, self.weight.copy()
)
self._history.append(hist)
[docs]
def plot_pdf(self) -> None:
"""
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.
"""
ax = smb.plotvol3()
for (x, y, t), weight in zip(self.x, self.weight):
# ax.plot([x, x], [y, y], [0, weight], 'r')
ax.plot([x, x], [y, y], [0, weight], "skyblue", linewidth=3)
ax.plot(x, y, weight, "k.", markersize=6)
ax.grid(True)
ax.set_xlabel("X")
ax.set_ylabel("Y")
ax.set_xlim()
ax.set_zlabel("particle weight")
ax.view_init(29, 59)
def _predict(self, odo: NDArray) -> None:
# step 2
# update the particle state based on odometry and a random perturbation
# Straightforward code:
#
# for i=1:self.nparticles
# x = self.robot.f( self.x(i,:), odo)' + sqrt(self.R)*self.randn[2,0]
# x[2] = angdiff(x[2])
# self.x(i,:) = x
#
# Vectorized code:
self.x = self.robot.f(self.x, odo) + self.random.multivariate_normal(
(0, 0, 0), self.R, size=self.nparticles
)
self.x[:, 2] = smb.angdiff(self.x[:, 2])
def _observe(self, z: NDArray, lm_id: int | None) -> None:
# step 3
# predict observation and score the particles
# Straightforward code:
#
# for p = 1:self.nparticles
# # what do we expect observation to be for this particle?
# # use the sensor model h(.)
# z_pred = self.sensor.h( self.x(p,:), lm_id)
#
# # how different is it
# innov[0] = z[0] - z_pred[0]
# innov[1] = angdiff(z[1], z_pred[1])
#
# # get likelihood (new importance). Assume Gaussian but any PDF works!
# # If predicted obs is very different from actual obs this score will be low
# # ie. this particle is not very good at predicting the observation.
# # A lower score means it is less likely to be selected for the next generation...
# # The weight is never zero.
# self.weight(p) = exp(-0.5*innov'*inv(self.L)*innov) + 0.05
# end
#
# Vectorized code:
invL = np.linalg.inv(self.L)
z_pred = self.sensor.h(self.x, lm_id)
z_pred[:, 0] = z[0] - z_pred[:, 0]
z_pred[:, 1] = smb.angdiff(z[1], z_pred[:, 1])
LL = -0.5 * np.r_[invL[0, 0], invL[1, 1], 2 * invL[0, 1]]
e = (
np.c_[z_pred[:, 0] ** 2, z_pred[:, 1] ** 2, z_pred[:, 0] * z_pred[:, 1]]
@ LL
)
self.weight = np.exp(e) + self.w0
def _select(self) -> None:
# step 4
# select particles based on their weights
#
# particles with large weights will occupy a greater percentage of the
# y axis in a cummulative plot
cdf = np.cumsum(self.weight) / self.weight.sum()
# so randomly (uniform) choosing y values is more likely to correspond to
# better particles...
iselect = self.random.uniform(0, 1, size=(self.nparticles,))
# find the particle that corresponds to each y value (just a look up)
interpfun = sp.interpolate.interp1d(
cdf,
np.arange(self.nparticles),
assume_sorted=True,
kind="nearest",
fill_value="extrapolate",
)
inextgen = interpfun(iselect).astype(int)
# copy selected particles for next generation..
self.x = self.x[inextgen, :]
[docs]
def get_t(self) -> NDArray:
"""
Get time from simulation
:return: simulation time vector
:rtype: ndarray(n)
Return simulation time vector, starts at zero. The timestep is an
attribute of the ``robot`` object.
"""
return np.array([h.t for h in self._history])
[docs]
def get_xyt(self) -> NDArray:
r"""
Get estimated vehicle trajectory
:return: vehicle trajectory where each row is configuration :math:`(x, y, \theta)`
:rtype: ndarray(n,3)
.. versionchanged:: 1.5.0
Returns the heading as well as the position, as documented and as
:meth:`EKF.get_xyt` does. Previously the result had only the
:math:`(x, y)` columns, ``xyt[:, :2]`` is that.
.. seealso:: :meth:`plot_xy` :meth:`run` :meth:`history`
"""
return np.array([h.xest[:3] for h in self._history])
[docs]
def get_std(self) -> NDArray:
r"""
Get standard deviation of particles
:return: dispersion of position and heading estimates, shape (n, 3)
:rtype: ndarray(n,3)
Return the population standard deviation of position and circular
standard deviation of heading
:math:`(\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.
.. seealso:: :meth:`get_xyt`
"""
return np.array([h.std for h in self._history])
[docs]
def plot_xy(self, block: bool | None = None, **kwargs: Any) -> None:
r"""
Plot estimated vehicle position
:param args: position arguments passed to :meth:`~matplotlib.axes.Axes.plot`
:param kwargs: keywords arguments passed to :meth:`~matplotlib.axes.Axes.plot`
:param block: ``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
Plot the estimated vehicle path in the xy-plane.
.. seealso:: :meth:`get_xyt`
"""
xyt = self.get_xyt()
plt.plot(xyt[:, 0], xyt[:, 1], **kwargs)
apply_block(block)
if __name__ == "__main__":
from roboticstoolbox import *
map = LandmarkMap(20, workspace=10)
V = np.diag([0.02, np.deg2rad(0.5)]) ** 2
robot = Bicycle(covar=V, animation="car", workspace=map)
robot.control = RandomPath(workspace=map)
W = np.diag([0.1, np.deg2rad(1)]) ** 2
sensor = RangeBearingSensor(robot, map, covar=W, plot=True)
R = np.diag([0.1, 0.1, np.deg2rad(1)]) ** 2
L = np.diag([0.1, 0.1])
pf = ParticleFilter(robot, sensor=sensor, R=R, L=L, nparticles=1000, animate=True)
pf.run(T=10)