# 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
1. Exercise 4-4¶
- Implement Exercise 1 using particle filters instead of histograms, and plot and discuss the results.
See notebook 07 for the results of exercise 4-1. Exercise 4-1 is an extension of exercise 3-1 in notebook 03, which uses a Kalman filter (KF).
Description of Exercise 3-1¶
Movement Model¶
In this and the following exercise, you are asked to design a Kalman filter for a simple dynamical system: a car with linear dynamics moving in a linear environment. Assume $\Delta T = 1$ for simplicity. The position of the car at time $t$ is given by $x_t$. Its velocity is $\dot x_t$, and its acceleration is $\ddot x_t$.Suppose the acceleration is set randomly at each point in time, according to a Gaussian with zero mean and covariance $\sigma^2 = 1$.
$$ \tag{1} \dot x_t = \dot x_{t-1} + \ddot x_t $$
$$ \tag{2} x_t = x_{t-1} + \dot x_{t-1} + \frac{\ddot x_t}{2} $$
Equations (1) and (2) show how we can generate the state at time $t$ from the state at time $t-1$, given an acceleration $\ddot x_t$.
Measurement Model¶
We will now add measurements to our Kalman filter. Suppose at time $t$, we can receive a noisy observation of $x$. In expectation, our sensor measures the true location. However this measurement is corrupted by Gaussian noise with covariance $\sigma^2 = 10$.
Particle Filter Overview¶
- Particle Filter Algorithm ($X_{t-1}, u_t, z_t$):
- $\quad \overline{X_t} = X_t = \emptyset$
- $\quad$for $m = 1$ to $M$ do
- $\qquad$sample $x_t^{[m]} \sim p(x_t \mid u_t, x_{t-1}^{[m]})$
- $\qquad w_t^{[m]} = p(z_t \mid x_t^{[m]})$
- $\qquad \overline X_t = \overline X_t + \langle x_t^{[m]}, w_t^{[m]}\rangle$
- $\quad$endfor
- $\quad$for $m = 1$ to $M$ do
- $\qquad$draw $i$ with probability $\propto w_t^{[i]}$
- $\qquad$add $x_t^{[i]}$ to $X_i$
- $\quad$endfor
- $\quad$return $X_t$
Particle Set¶
Our particle set $X$ will consist of 250 particles.
M = 1000 # Number of particles
The class PSet uses a numpy structured array to represent a particle set. It also contains array columns for weights and an empirical cumulative distribution function that will be used for the measurement update portion of the PF algorithm.
class PSet(object):
def __init__(self, num_particles):
"""Initializes a PSet object with all particles set to (0, 0)"""
self.num_particles = num_particles
self.ptcs = np.zeros(num_particles,
dtype=[("pos", np.float16),
("vel", np.float16),
("weights", np.float32),
("cdf", np.float32)])
self.belief = False
self.measurement = None
self.posterior = False
def __getitem__(self, key):
return self.ptcs[key]
def __setitem__(self, key, value):
self.ptcs[key] = value
@property
def shape(self):
return self.ptcs.shape
def set_cdf(self):
"""Populates the 'cdf' column, based on particle weights.
"""
weight_sum = 0
for row in self.ptcs:
weight_sum = row["weights"] + weight_sum
row["cdf"] = weight_sum
def resample(self):
"""Returns new Posterior PSet resulting from PF resampling step
"""
sum_weights = np.sum(self["weights"])
rand_samples = np.sort(np.random.random_sample(size=M) * sum_weights)
num_ptcl = self.num_particles
posterior = PSet(num_ptcl)
post_idx = 0
bel_idx = 0
while bel_idx < num_ptcl:
while ((post_idx < num_ptcl and bel_idx < num_ptcl) and
rand_samples[post_idx] < self["cdf"][bel_idx]):
posterior.ptcs["pos"][post_idx] = self["pos"][bel_idx]
posterior.ptcs["vel"][post_idx] = self["vel"][bel_idx]
post_idx = post_idx + 1
bel_idx = bel_idx + 1
posterior.posterior = True
posterior.belief = False
return posterior
def clear_weights(self):
"""Sets weights and CDF to zero."""
self.ptcs["weights"] = 0
self.ptcs["cdf"] = 0
def covariance(self):
"""Calculates the coviance matrix of the particle set."""
return np.cov(self.ptcs["pos"], self.ptcs["vel"])
def mean(self):
"""Returns the means for the particle set"""
return (np.mean(self["pos"]), np.mean(self["vel"]))
def scatter_plot(self, title, lims=None):
fig = plt.figure()
plt.plot(self["pos"], self["vel"], 'b.')
plt.title(title)
plt.xlabel("Position")
plt.ylabel("Velocity")
if lims is not None:
plt.xlim(lims[0])
plt.ylim(lims[1])
plt.grid()
plt.show()
plt.close()
def contour_plot(self, title, lims=None):
hist, pos_edges, vel_edges = np.histogram2d(self.ptcs["pos"],
self.ptcs["vel"])
pos_ctrs = pos_edges[0:-1] + (pos_edges[1] - pos_edges[0])/2
vel_ctrs = vel_edges[0:-1] + (vel_edges[1] - vel_edges[0])/2
pos_mesh, vel_mesh = np.meshgrid(pos_ctrs, vel_ctrs)
contour_fig = plt.figure()
ax = contour_fig.add_subplot(111)
ax.contourf(pos_mesh, vel_mesh, hist, cmap=cm.PuBuGn)
plt.title(title)
plt.xlabel("Position")
plt.ylabel("Velocity")
if lims is not None:
plt.xlim(lims[0])
plt.ylim(lims[1])
plt.grid()
plt.show()
plt.close()
Particle Set at Time 0¶
At time $t = 0$ we know the state of the robot precisely. It is at position 0 and velocity 0. The probability distribution looks like this:
$$ \tag{1} p(\vec x_0) = \left\lgroup \begin{matrix}1: & x = 0 \: \mathsf{and} \: \dot x = 0 \cr 0: & x \neq 0 \: \mathsf{or} \: \dot x \neq 0 \end{matrix} \right\rgroup $$
No matter how many particles we draw from this distribution, every particle will be (0, 0). We can create such a particle set simply by initializing our PSet class with the number of particles. All particles in the PSet class are initially (0, 0).
pset0 = PSet(M)
State Transition¶
The state transition portion of the generic Bayes filter algorithm is addressed by lines 3 and 4 in the particle filter algorithm. Lines 3 and 4 require us to determine the subsequent robot state (position and velocity) at time $t + 1$ for every single particle in the set at time $t$. The equations for this update (from notebook 03) are:
$$ \tag{2} \dot x_t = \dot x_{t-1} + \ddot x_t $$
$$ \tag{3} x_t = x_{t-1} + \dot x_{t-1} + \frac{\ddot x_t}{2} $$
The next_step function implements these equations to return a new particle set for time $t + 1$, given a particle set for time $t$.
def next_step(pset):
"""Applies a state transition to creates a new particle set for time t + 1.
"""
num_particles = pset.num_particles
pset_next = PSet(num_particles)
acc = stats.norm.rvs(size=num_particles)
pset_next["vel"] = pset["vel"] + acc
pset_next["pos"] = pset["pos"] + pset["vel"] + acc / 2
pset_next.belief = True
return pset_next
Time $t = 1$¶
pset1 = next_step(pset0)
pset1.mean()
pset1.covariance()
How does the particle filter, which is basically a numerical simulation, compare with the closed form solution that was generated by the Kalman filter (KF)? Not bad.
KF results at time $t = 1$:
$$ \tag{4} \vec \mu_0 = \left\lgroup \begin{matrix}0 \cr 0 \end{matrix} \right\rgroup \\ \bar{\mathbf{\Sigma}_1} = \left\lgroup \matrix{\frac{1}{4} & \frac{1}{2} \cr \frac{1}{2} & 1} \right\rgroup $$
pset2 = next_step(pset1)
pset2.mean()
pset2.covariance()
KF Results¶
$$ \tag{5} \vec \mu_0 = \left\lgroup \begin{matrix}0 \cr 0 \end{matrix} \right\rgroup \\ \bar{\mathbf{\Sigma}_1} = \left\lgroup \matrix{2.5 & 2 \cr 2 & 2} \right\rgroup $$
pset3 = next_step(pset2)
pset3.mean()
pset3.covariance()
KF Results¶
$$ \tag{6} \vec \mu_0 = \left\lgroup \begin{matrix}0 \cr 0 \end{matrix} \right\rgroup \\ \bar{\mathbf{\Sigma}_1} = \left\lgroup \matrix{8.75 & 4.5 \cr 4.5 & 3} \right\rgroup $$
pset4 = next_step(pset3)
pset4.mean()
pset4.covariance()
KF Results¶
$$ \tag{7} \vec \mu_0 = \left\lgroup \begin{matrix}0 \cr 0 \end{matrix} \right\rgroup \\ \bar{\mathbf{\Sigma}_1} = \left\lgroup \matrix{21 & 8 \cr 8 & 4} \right\rgroup $$
pset5 = next_step(pset4)
pset5.mean()
pset5.covariance()
KF Results¶
$$ \tag{8} \vec \mu_0 = \left\lgroup \begin{matrix}0 \cr 0 \end{matrix} \right\rgroup \\ \bar{\mathbf{\Sigma}_1} = \left\lgroup \matrix{41.25 & 12.5 \cr 12.58 & 5} \right\rgroup $$
Measurement Update¶
The remainder of the particle filter, lines 5 through 12, all address the measurement update. For this exercise, we will assume that we receive a measurement of position at time $t = 5$. The measurement is centered at the true position with a covariance of 10. The function weights implements lines 5 and 6 of the PF algorithm by calculating the probability of receiving a measurement of 5 for every particle in the set. Specifically:
$$\tag{9} w_t^{[m]} = p(z_t \mid x_t^{[m]})$$
MEASUREMENT_VARIANCE = 10
def weight(pos, meas, var):
return stats.norm.pdf(meas, loc=pos, scale=math.sqrt(var))
weights = np.vectorize(weight, excluded=["pos", "var"], otypes=[np.float16])
Now we have to:
- Assign the weights to the weights column of the structured array contained within our PSet object.
- Create an empirical cumulative distribution function (CDF) based on the particle weights.
pset5["weights"] = weights(pset5["pos"], 5,
MEASUREMENT_VARIANCE) # Assign weights
pset5.set_cdf() # Create empirical CDF from weights.
pset5.measurement = 5
Let's take a look at the first few lines of the particle set:
pset5[0:10]
The closer the position (1st column) is to 5, the higher the weights (3rd column), as expected. The 4th column contains the empirical probability distribution function that will be used for resampling.
Finally, we'll complete lines 8 through 11 of the PF algorithm via the PSet.resample() method.
post5 = pset5.resample()
Here are the results:
post5.mean()
post5.covariance()
Note that the mean position is skewed towards the pre-measurement belief, even though our algorithm assigns the highest probability for particles with positions closest to 5, our measurement. This happens because there are just a lot more particles close to the pre-measurement mean than there are close to the measurement. So our resulting mean is affected by both the particle density before the measurement and the measurement weights. Just like the Kalman filter, the particle filter will discount the measurement if it's unlikely that the robot was in a state that was likely to result in that measurement.
KF Results¶
$$ \tag{10} \vec \mu_0 = \left\lgroup \begin{matrix}4.2 \cr 1.22 \end{matrix} \right\rgroup \\ \bar{\mathbf{\Sigma}_1} = \left\lgroup \matrix{8.05 & 2.44 \cr 2.44 & 1.95} \right\rgroup $$
The PF results compare favorably with the results from the Kalman filter. We could improve the accuracy by adding more particles, at the cost of requiring more computation.
Visualization¶
Comparison of belief and posterior at time $t = 5$:
pset5.scatter_plot("Robot State at Time 5")
post5.scatter_plot("Posterior at Time 5, z = 5", lims=[(-15, 15), (-5, 5)])
The post-measurement plot suggests the position is somewhere between 0 and 10. But keep in mind that the posterior particle set has many duplicate particles, which are plotting on top of each other in the scatter plot. Consequently the scatter plot gives the impression that there is more uncertainty than what is indicated by the covariance matrix. We'll have to use a histogram or other sort of plot that accounts for duplicate particles to improve the visualization.
Let's take a loot at the posterior to see the duplicate particles. There should be more duplicate particles for particles with positions closer to our measurement or the mean of the pre-measurement belief.
post5.contour_plot("Posterior at Time 5, z = 5")
Stacy Irwin, August 2018