# Setup notebook for calculations and plotting
# Plotting
# Comment following line (%matplotlib inline) for interactive inline plots
# %matplotlib inline
import matplotlib
# Uncomment following line for interactive inline plots
matplotlib.use('nbAgg')
from mpl_toolkits.mplot3d import axes3d
import matplotlib.pyplot as plt
import matplotlib.patches as mpatches
import matplotlib.cm as cm
# Numerical simulations
import math
import numpy as np
import scipy.stats as stats
import pandas as pd
import collections
# Matrix calculations
import sympy
from IPython.display import display
sympy.init_printing(use_latex=True)
# Saving grids to a file
import pickle
1. Exercise 4-2.a¶
Scenario¶
- You are now asked to implement the histogram filter for the nonlinear studied in Exercise 4 in the previous chapter (page 83). There we studied a nonlinear system defined over three state variables, and with the deterministic state transition
$$ \tag{1} \left\lgroup \begin{matrix} x^\prime \cr y^\prime \cr \theta^\prime \end{matrix} \right\rgroup = \left\lgroup \begin{matrix} x + \cos \theta \cr y + \sin \theta \cr \theta \end{matrix} \right\rgroup $$
The initial state estimate was as follows:
$$ \tag{2} \vec \mu = \lgroup \begin{matrix}0 & 0 & 0 \end{matrix} \rgroup $$
$$ \tag{3} \mathbf{\Sigma} = \left\lgroup \begin{matrix}0.01 & 0 & 0 \cr 0 & 0.01 & 0 \cr 0 & 0 & 10000 \end{matrix} \right\rgroup $$
a. Propose a suitable initial estimate for a histogram filter, which reflects the state of knowledge in the Gaussian prior.
Solution¶
Define a Python Object to Contain Histogram Data¶
Here is a three dimensional version of the Grid2D object from the last exercise.
class GridXD(object):
"""Three demensional grid to be used with a histogram filter.
The grid supports histogram filters with three state variables: x,
y, and z.Grid3D[x_idx, y_idx, z_idx] will return the probability
that the system in the state x, y corresponding to indices x_idx,
y_idx, and z_idx.
Attributes:
dims int: Number of grid dimensions
mins ndarray(int16): array of minimum values on each axis
maxs ndarray(int16): array of maximum values on each axis
sizes ndarray(int16): array of sizes of individual bin on each axis
labels [str]: Python list of dimension labels. Defaults to "D0",
"D1", "D2", ...
lengths ndarray(int16): array of number of bins along each axis
grid (numpy array): Multi-dimensional array of probabiliites
zero_threshold (float): Below this value, Grid2D will assign
probability of zero. Defaults to 0.0001
nonzero_bins (set): A set of tuples of length 2. Each tuple
contains the indices for a grid location with non-zero
probability.
bin_volume float: Volume of individual bin (x_size * y_size)
N int: Number of bins.
indices(D0_val, D1_val, ...) ndarray(int16, int16, ...): Returns the
grid indices corresponding to thebin containing the indices
passed as arguments
center(D0_idx, D1_idx, ...) float: Returns the center value of the
represented by the indices passed as arguments.
set_scale() None: Scales probabilities so they all add up to one.
tpl(D0_idx, D1_idx, ...) tuple: Tuple of length GridXD.dims + 1
containing indices and probability at that bin location
dic(D0_idx, D1_idx, ...) dict: Python dictionary containing
indices, axes values, and probability
dataframe() Pandas DataFrame: Dataframe containing all nonzero
data.
"""
def __init__(self, mins, maxs, sizes):
"""Constructes a new GridXD object.
"""
self.dims = len(mins)
if len(maxs) != self.dims or len(sizes) != self.dims:
raise Exception("Arguments must all be same length.")
self.mins = np.array(mins)
self.maxs = np.array(maxs)
self.sizes = np.array(sizes)
self.labels = ["D" + str(x) for x in range(self.dims)]
lengths = (self.maxs - self.mins) / self.sizes
self.lengths = lengths.astype(np.int16)
self.volume = np.prod(self.sizes)
self.N = np.prod(self.lengths)
self.grid = np.zeros(self.lengths)
self.zero_threshold = 0.0001
self.nonzero_bins = set()
self.scale = 1
def __setitem__(self, idc, value):
"""Allows setting probabilities without without GridXD.grid attribute.
Args
idc: tuple of integer indices
value: probability
Assigning probabilites using `Grid2D[(idc)]` syntax will
automatically update list of nonzero bins and enforce the zero
threshhold. Assigning probabilities using
`Grid2D.grid[idc]` will bypass this functionality.
"""
if value >= self.zero_threshold:
self.nonzero_bins.add(idc)
else:
value = 0
if idc in self.nonzero_bins:
self.nonzero_bins.remove(idc)
self.grid[idc] = value
def __getitem__(self, idc):
return self.grid[idc]
def __iter__(self):
return self.grid
def indices(self, *values):
"""Returns grid indices for bin specified position and velocity.
"""
if (np.absolute(values) > self.maxs + self.sizes/2).all():
raise Exception("Position is out of range.")
idc = (values - self.mins + self.sizes / 2) // self.sizes
return idc.astype(np.int16)
def center(self, *indices):
idc = np.array(indices)
if (idc > self.lengths).any():
raise Exception("Index is out of range.")
return self.mins + idc * self.sizes
def set_scale(self):
"""Ensures all probabilities add up to 1.
The output of a histogram filter is a discrete probability
distribution. Assuming all possible states are represented by
the output of the filter, the discrete probabilities should
add up to 1.
"""
total = sum([self.grid[idc] for idc in self.nonzero_bins])
self.scale = 1 / total
for bn in self.nonzero_bins:
self.grid[bn] = self[bn] * self.scale
def tpl(self, *indices):
return indices + (self.grid[indices],)
def dic(self, *indices):
idx_dict = {dim[0] + "_idx": dim[1]
for dim in zip(self.labels, indices)}
val_dict = {dim[0] + "_val": dim[1]
for dim in zip(self.labels, self.center(*indices))}
full_dict = {**idx_dict, **val_dict}
full_dict["P"] = self[indices]
return full_dict
def dataframe(self):
"""Returns a Pandas dataframe containing all nonzero bins.
"""
pts = [self.dic(*idc) for idc in self.nonzero_bins]
sorted_pts = sorted(pts, key=lambda dic: dic[self.labels[0] + "_idx"])
df = pd.DataFrame(sorted_pts).sort_values("\u03B8_val")
return df.reset_index(drop=True)
def copy(self, copy_probs=False):
grid = GridXD(self.mins, self.maxs, self.sizes)
grid.labels = self.labels
if copy_probs:
grid.grid = np.copy(self.grid)
return grid
def pickel(self, filename):
with open(filename, "wb") as file:
pickel.dump(self, filename)
Create the Initial Estimate¶
Instantiate a grid object with $x$ and $y$ ranging from -10 to 10 and $\theta$ ranging from 0 to 360:
grid0 = GridXD((-10, -10, 0), (10, 10, 360), (0.1, 0.1, 1))
grid0.labels = ["x", "y", "\u03B8"]
grid0.indices(0, 0, 0)
All probability is concentrated at $x = 0$ and $y = 0$. However we don't know the heading ($\theta$), so the probability will be uniform and non-zero at all values of $\theta$:
for theta in range(360):
grid0[100, 100, theta] = 1
All probabilities must add up to 1.0:
grid0.set_scale()
Now we have our initial estimate for the histogram filter. To view all non-zero cells:
grid0.dataframe()
Verify cells add up to 1.0:
assert grid0.dataframe()["P"].sum() == 1.0
2. Exercise 4-2.b¶
b. Implement a histogram filter and run its prediction step. Compare the resulting posterior with the one from the EKF and from your intuitive analysis. What can you learn about the resolution of the x-y coordinates and the orientation of $\theta$ in your histogram filter?
The Motion Model¶
The histogram filter algorithm requires that we calculate the probability of a transition between all possible pairs of system states (i.e., grid bins). For many transitions, the probability will be zero, meaning the one state is not reachable from the other. For example, suppose at time $t=0$ the system is in state $(x, y, \theta)$. For the system to be in state $(x^{\prime}, y^{\prime}, \theta^{\prime})$ at time $t=1$ there must be a solution to the following equations:
$$ \tag{4} \left\lgroup \begin{matrix} x \cr y\cr \theta \end{matrix} \right\rgroup = \left\lgroup \begin{matrix} x^\prime - \cos \theta^\prime \cr y^\prime - \sin \theta^\prime \cr \theta^\prime \end{matrix} \right\rgroup $$
Equation (4) is a rearrangement of equation (1). Contrary to the histogram model in notebook 7 (the linear robot), a state is only reachable from one other state, or a single point. In the linear robot model, the set of all possible earlier states fell on a line.
The following function will calculate the point from which the current point can be reached (the origin). It returns the indices of this origin point.
def destination(idc_s0, grid):
# Get state values that correspond to indices for time t=1
val_s0 = grid.center(idc_s0)[0]
x0 = val_s0[0]
y0 = val_s0[1]
th0 = val_s0[2]
# Calculate the destination state
xd = x0 + math.cos(th0 * math.pi/180)
yd = y0 + math.sin(th0 * math.pi/180)
thd = th0
idc = grid.indices(xd, yd, thd)
return (idc[0], idc[1], idc[2])
Note home much shorter the reachable() function is compared to the linear model in notebook 07. This is due to the earlier possible state being limited to a single point instead of a line.
destination((40, 40, 180), grid0)
def grid_pred(grid):
# Create new grid object, initialized to zero
grid_next = grid.copy()
for bin_ in grid.nonzero_bins:
prob = grid[bin_]
next_bin = destination(bin_, grid)
grid_next[next_bin] = prob
return grid_next
grid1 = grid_pred(grid0)
df1 = grid1.dataframe()
df1.shape
def plot_grid(grid, title, xlim=(-1.2, 1.2), ylim=(-1.2, 1.2)):
df = grid.dataframe().sort_values("\u03B8_val")
u_coords = df["\u03B8_val"].apply(lambda theta: math.cos(theta * math.pi/180))
v_coords = df["\u03B8_val"].apply(lambda theta: math.sin(theta * math.pi/180))
fig2 = plt.figure()
plt.quiver(df["x_val"], df["y_val"], u_coords, v_coords)
plt.xlabel("x position")
plt.ylabel("y position")
plt.xlim(xlim)
plt.ylim(ylim)
plt.grid()
plt.title(title)
plt.show()
plot_grid(grid1, "Robot State at Time 1")
Figure 1 corresponds well to our intuitive analysis from the previous chapter's exercises (see notebook 06). The direction of the arrows corresponds to the robot heading, $\theta$.
Figure 1 does not correspond at all to the results from the extended Kalman filter (EKF). The EKF predicted the robot would be on a vertical line intersecting the x-axis at 1.0. The 95% probability ellipse extended from -200 to 200.
Heading Verses X-Y Resolution¶
At time 0 there were exactly 360 non-zero bins, corresponding to a non-zero probability at the (0, 0) x-y position for every possible value of $\theta$ (0, 1, 2, ..., 358, 359). There are only about 80 points plotted in Figure 1. However our model maps one bin at time $t$ to one and only one bin at time $t+1$, so there are actually 360 non-zero bins. Different bins at the same x-y position are plotting on top of each other. The heading arrows are plotting on top of each other because the different headings that are possible for any given x-y position are very close together.
The data frame on non-zero bins shows this more clearly:
df1 = grid1.dataframe()
df1.query("x_idx==110").head(10)
Consider the state $x = 1$, $y = 0$, and $\theta = 1$. If the system were modeled exactly, the position at time $t=1$ for a robot starting at (0, 0) with heading of 001 should be 0.017. But since our bin is 0.1 units high in the y direction, the model approximates the y position as 0. The y position will continue to be 0 no matter how far into the future we continue the model. In the real world, a robot with heading 001 would slowly diverge from the x-axis. In summary, we effectively lose resolution on heading if our resolution in the x and y directions is not sufficiently fine.
To have a resolution of 1 degree in heading, then we could try setting the bin size to 0.01 in both the x and y directions. Our calculation times should stay the same, but our array size will increase by a factor of 100. At a size of 0.1 units, we have 14.4 million bins, so reducing the size in this manner increases our array size to 1.44 billion bins, or about 2 GB assuming our grid array contains 16-bit floats. Since it's a sparse array (all but 360 bins are zero) we could modify the algorithm to only store the non-zero bins.
Alternative Representation¶
Another way to represent the system is to sum the probabilities across the different headings for each value of x and y, and then take the median of the different headings.
df1_g = df1.groupby(["x_idx", "y_idx", "x_val", "y_val"], as_index=False).agg({"P": "sum", "\u03B8_val": "median"})
df1_g["\u03B8_val"] = df1_g["\u03B8_val"].apply(int)
df1_g
We'll plot this with a combination of a contour and quiver plot.
def plot_contour(grid, title, xlim=(-1.2, 1.2), ylim=(-1.2, 1.2)):
df = grid.dataframe()
df_g = df.groupby(["x_idx", "y_idx", "x_val", "y_val"],
as_index=False).agg({"P": "sum",
"\u03B8_val": "median"})
df_g["\u03B8_val"] = df_g["\u03B8_val"].apply(int)
contour_fig = plt.figure()
ax = contour_fig.add_subplot(111)
# Prep data for countour plot
x_vals = np.arange(df_g["x_val"].min(),
df_g["x_val"].max() + grid.sizes[0]/2,
grid.sizes[0]).round(2)
x_idc = np.arange(df_g["x_idx"].min(), df_g["x_idx"].max() + 1)
y_vals = np.arange(df_g["y_val"].min(),
df_g["y_val"].max() + grid.sizes[1]/2,
grid.sizes[1]).round(2)
y_idc = np.arange(df_g["y_idx"].min(), df_g["y_idx"].max() + 1)
xv_msh, yv_msh = np.meshgrid(x_vals, y_vals)
xi_msh, yi_msh = np.meshgrid(x_idc, y_idc)
grid_g = grid.copy()
for _, row in df_g.iterrows():
grid_g[int(row["x_idx"]), int(row["y_idx"]), 0] = row["P"]
def prob(x, y, grid_):
return grid_[x, y, 0]
vprob = np.vectorize(prob, excluded=["grid"])
ZZ = vprob(xi_msh, yi_msh, grid_g)
# Countour plot
ax.contourf(x_vals, y_vals, ZZ, cmap=cm.PuBuGn)
# Quiver plot
u_coords = df_g["\u03B8_val"].apply(
lambda theta: math.cos(theta * math.pi/180))
v_coords = df_g["\u03B8_val"].apply(
lambda theta: math.sin(theta * math.pi/180))
plt.quiver(df_g["x_val"], df_g["y_val"], u_coords, v_coords,
width=0.003)
# Plot adjustments
plt.xlim(xlim)
plt.ylim(ylim)
plt.grid()
plt.xlabel("X Position")
plt.ylabel("Y Position")
plt.title(title)
plt.show()
plot_contour(grid1, "Position at Time 1")
3. Exercise 4-2.c¶
c. Now incorporate a measurement into your estimate. As before, the measurement shall be a noisy projection of the x-coordinate of the robot, with covariance $Q = 0.01$. Implement the step, compute the result, plot it, and compare it with the result of the EKF and your intuitive analysis.
Solution¶
Review of Measurement Update Formulas¶
The equation for the measurement update: $$ \tag{5} p_{k, t} = \eta \, p(\vec{z_t} \mid X_t = \vec{x_k}) \, \overline{p_{k, t}}$$
The probability of receiving a specific measurement $z_t$ given that $X_t=x_k$ is: $$ \tag{6} p(\vec{z_t} \mid X_t = \vec{x_k}) = N(x_{max}, z, 0.01) - N(x_{min}, z, 0.01) $$
def bin_measurement(z, x_idx, grid):
x = grid.center(x_idx, 0, 0)[0]
x_max = x + grid.sizes[0]/2
x_min = x - grid.sizes[0]/2
std_dev = 0.1 # square root of variance 0.01
p_max = stats.norm.cdf(x_max, loc=z, scale=std_dev)
p_min = stats.norm.cdf(x_min, loc=z, scale=std_dev)
return p_max - p_min
def grid_measurement(z, grid):
# Create new grid object, initialized to zero
grid_meas = grid.copy()
grid_meas.zero_threshold = 0.0001
# Calculate probability for every bin in grid
for bin_ in grid.nonzero_bins:
grid_meas[bin_] = grid[bin_] * bin_measurement(z, bin_[0], grid)
# Normalize histogram grid so all probabilities sum to 1
grid_meas.set_scale()
return grid_meas
Measurement: x = 1¶
grid1m = grid_measurement(1, grid1)
plot_grid(grid1m, "Robot State at Time 1\nX-measurement = 1")
The results are as expected. The quiver plot does not show different probabilities for different x-y locations. Let's try a contour plot:
plot_contour(grid1m, "Robot State at Time 1\nX-measurement = 1")
Again, the results are expected. The probability decreases as x decreases from 1, reflecting the lower probability of receiving a measurement of 1.0 as the true location diverges from $x = 1.0$.
Measurement: x = 0¶
grid1b = grid_measurement(0, grid1)
plot_contour(grid1b, "Robot State at Time 1\nX-measurement = 0")
Unlike the EKF, the histogram filter can handle multi-modal probability distributions.
Stacy Irwin, 13 August 2018