# 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-1.a¶
Histogram Filter State Transition and Prediction¶
- In this exercise, you will be asked to implement a histogram filter for a linear dynamic system studied in the previous chapter.
a. Implement a histogram filter for the dynamical system described in Exercise 1 of the previous chapter (see page 81). Use the filter to predict a sequence of posterior distributions for $t = 1, 2, ..., 5$. For each value of $t$, plot the joint posterior over $x$ and $\dot x$ into a diagram, where $x$ is the horizontal and $\dot x$ is the vertical axis.
Solution¶
Define a Python Object to Contain Histogram Data¶
I created a Python class to represent a two-dimensional numerical array. The Grid2D class refers to the array of possible states as a grid, and each individual location on the grid is called a bin. Position is represented by the x-axis and velocity by the y-axis.
class Grid2D(object):
"""Two demensional grid to be used with a histogram filter.
The grid supports histogram filters with two state variables. State
variable maps to the x-axis, and the other maps to the y-axis.
Grid2D[x_idx, y_idx] will return the probability that the system in
the state x, y corresponding to indices x_idx and y_idx.
Attributes:
x_min (float): minimum value on x axis
x_max (float): maximum value on x axis
x_size (float): size of individual bin on x axis
x_n (int): Number of bins along x axis
y_min (float): minimum value on y_axis
y_max (float): maximum value on y axis
y_size (float): size of individual bin on y axis
y_n (int): Number of bins along y axis
grid (numpy array): Two-dimensional array of probabiliites that
state is represented by variables x and y
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(x, y) (int, int): Returns the grid indices corresponding to
the bin containing x and y.
x(x_index) (float): Returns the center of the bin on the x axis for
all bins with the given x index.
y(y_index) (float): Returns the center of the bin on the y axis for
all bins with the given y index.
set_scale() (None): Scales probabilities so they all add up to one.
dataframe() (Pandas DataFrame): Dataframe containing all nonzero
data.
"""
def __init__(self, x_min, x_max, x_size, y_min, y_max, y_size):
"""Constructes a new Grid2D object.
"""
self.x_min = x_min
self.x_max = x_max
self.x_size = x_size
self.x_n = int(1 + (x_max - x_min) / x_size)
self.y_min = y_min
self.y_max = y_max
self.y_size = y_size
self.y_n = int(1 + (y_max - y_min) / y_size)
self.volume = self.x_size * self.y_size
self.grid = np.zeros((self.x_n, self.y_n))
self.sig_digits = 8
self.zero_threshold = 0.0001
self.nonzero_bins = set()
self.scale = 1
@property
def bin_volume(self):
return round(self.x_size * self.y_size, self.sig_digits)
@property
def N(self):
return self.x_n * self.y_n
def __setitem__(self, idc, value):
"""Allows setting probabilities without without Grid2D.grid attribute.
Assigning probabilites using `Grid2D[x_idx, y_idx]` syntax will
automatically update list of nonzero bins and enforce the zero
threshhold. Assigning probabilities using
`Grid2D.grid[x_idx, y_idx]` 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[0], idc[1]] = value
def __getitem__(self, idc):
return self.grid[idc[0], idc[1]]
def __iter__(self):
return self.grid
def indices(self, x, y):
"""Returns grid indices for bin specified position and velocity.
"""
if math.fabs(x) > self.x_max + self.x_size/2:
raise Exception("Position {} is out of range.".format(x))
if math.fabs(y) > self.y_max + self.y_size/2:
raise Exception("Position {} is out of range.".format(y))
x_idx = int((x - self.x_min + self.x_size / 2) // self.x_size)
y_idx = int((y - self.y_min + self.y_size / 2) // self.y_size)
return x_idx, y_idx
def x(self, x_idx):
if x_idx > self.x_n:
raise Exception("Index is out or range.")
return round(self.x_min + x_idx * self.x_size, self.sig_digits)
def y(self, y_idx):
if y_idx > self.y_n:
raise Exception("Index is out or range.")
return round(self.y_min + y_idx * self.y_size, self.sig_digits)
def tpl(self, x_idx, y_idx):
return self.x(x_idx), self.y(y_idx), self[x_idx, y_idx]
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 dataframe(self):
"""Returns a Pandas dataframe containing all nonzero bins.
"""
pts = sorted(list(self.nonzero_bins), key=lambda tpl: tpl[0])
x_idx = [pt[0] for pt in pts]
y_idx = [pt[1] for pt in pts]
X = [self.x(pt[0]) for pt in pts]
Y = [self.y(pt[1]) for pt in pts]
Z = [self[pt[0], pt[1]] for pt in pts]
data = collections.OrderedDict([("x_index", x_idx), ("y_index", y_idx),
("x", X), ("y", Y), ("prob", Z)])
return pd.DataFrame(data)
Define Grid for One-Dimensional Robot Problem¶
Now I'll create a grid object to represent the robot's state at time $t=0$. We'll put position $x$ on the x-axis and velocity $\dot x$ on the y-axis. Position will range from -20 to 20 and velocity from -40 to 40. The bin will be 0.25 position units by 0.5 velocity units.
For example, the bin at position (0, 0) contains the probability that the robot position is between [-20.125 and -19.875) and velocity is between [-40.25 and 39.75). The indices (80, 80) corresponds to the bin containing state (0, 0). The (0, 0) bin extends from [-0.125 to 0.125) on the position $x$ axis and [-0.25 to 0.25) on the velocity $\dot x$ axis. The square brackets indicate that the left (smaller) bin edge is included in the bin, but the rigt edge is included in the adjacent bin. The maximum value of both the position and velocity index is 160.
# Calculate number of cells bins
grid0 = Grid2D(-20, 20, 0.25, -40, 40, 0.5)
The Grid2D object initally assumes the robot is nowhere (all probabilities are set to zero). In this exercise we know the robot position with certainty at time $t=0$: $x = 0$ and $\dot x = 0$. Therefore all probability should be placed in the bin containing (0, 0), and all other bins should be assigned probability 0. The following Python statement assigns probability 1 to the bin containing position 0 and velocity 0:
# Set probability to 1 for bin containing state (0, 0)
grid0[grid0.indices(0, 0)] = 1
Discrete Bayes Filter Algorithm¶
The Grid2D object does not contain any of the histogram filter logic. It can store probabilities for each bin but that's it. Here is the algorithm:
- Given the set $\{p_{k, t-1}\}$, $\vec u_t$, and $\vec z_t$:
- for all $k$ do
- $\qquad$ $\overline{p_{k, t}} = \sum_i p(X_i = \vec{x_k} \mid \vec{u_t}, X_{t-1} = \vec{x_i}) p_{i, t-1}$
- $\qquad$ $p_{k, t} = \eta \, p(\vec{z_t} \mid X_t = \vec{x_k}) \, \overline{p_{k, t}}$
- end for
- return $\{p_{k, t}\}$
In this example, $k$ is an index representing each bin in the Grid2D object. The index $k$ represents all combinations of position and velocity, which each range from 0 to 161, or 25,921 discrete states ($161^2$, grid0.N will return the number of bins).
$p_{k, t}$ is the probability that the robot's state falls within bin $k$ at time $t$. Velocity $\dot x$ and position $x$ are continuous variables but we've converted the continuous model into a discrete system by assigning all values of $\dot x$ and $x$ to a finite number of states (i.e., bins). Since $p_{k, t}$ represents a discrete probability, all values of $p$ for a specific time $t$ will sum to 1.
We can also think of the grid as representing a stepwise continuous probability distribution, where the value of the distribution follows this relation:
$$ \tag{1} p(\vec x_t) = \frac{p_{k, t}}{\mid \mathbf{x}_{k, t} \mid}$$
$\mid \mathbf{x}_{k, t} \mid$ is the area of the bin. In this example, it's $0.25 \times 0.5 = 0.125$. Since $p((0, 0)) = 1$, $p(\vec x_t) = 8$.
Algorithm Discussion¶
Line 1¶
This system has no controls, so we can ignore the $\vec{u_t}$ term. Also, at this point we have no measurements, so we can ignore $\vec z_t$. Our inputs to the algorithm consist solely of the grid at time $t=0$ (i.e., grid-0). As previously discussed, all grid-0 probabilities are zero, with the exception of $p_{(x=0, v=0)}$, which is equal to 1.
Speeding Up Lines 2 and 3¶
Line 2 indicates that we'll have to loop over every bin in the grid and calculate the probability that each bin represents the robot state at time $t = 1$. Note that line 3 itself contains a summation over every single bin. Our Python code could represent the summation inside a for loop with a nested for loop. The problem with this approach is that the inner for loop would be executed 671,898,241 times ($25921^2$). Even for modern computers, that's a lot.
However, line 3 contains the term $p_{i, t-1}$. At time $t = 0$ this term is zero for all bins except for the bin with $x = 0$ and $\dot x = 0$. If we can keep the robot's uncertainty fairly small, most of the bins will be zero. So if we can loop over only the non-zero bins, then we can speed up the computation significantly. The Grid2D class is designed to do this. It has a nonzero_bins attribute that contains a set of tuples with the indices for all non-zero bins. Grid2D automatically updates the nonzero_bins set when assigning a probability to a bin.
Another way to speed up the calculation is to set a minimum probability, below which the bin contents will be set to zero. By default, Grid2D sets this threshold at 0.0001. This speeds up the computation because we don't have to loop over bins with insignificant but non-zero probabilities.
State Transition Probability¶
The term $\sum_i p(X_i = \vec{x_k} \mid \vec{u_t}, X_{t-1})$ is the probability of the system transitioning between any two arbitrary states in one time interval (e.g., transitioning from state $i$ at $t = 0$ to state $k$ at $t = 1$).
First, let's review the equations that govern the motion from exercise 3-1:
$$ \tag{2} x_t = x_{t-1} + \dot{x}_{t-1} + \frac{\ddot{x}_t}{2}$$
$$ \tag{3} \ddot{x}_t = \dot{x}_{t} - \dot{x}_{t-1} $$
So the acceleration necessary to transition from state $i$ to state $k$ is just the difference in velocities between states $i$ and $k$. Once we know the acceleration, we can calculate the probability of observing that acceleration because we know it's normally distributed with a mean of zero and a standard deviation of one:
$$ \tag{4} p(\ddot{x}) = \frac{1}{\sqrt{2\pi}} \exp\left({-\frac{\ddot{x}^2}{2}}\right)$$
So to calculate the probability of being in a certain bin at time $t = 1$, we calculate the acceleration that corresponds to the difference in velocity occurring between times 0 and 1, calculate the probability corresponding to that acceleration, and multiply that acceleration probability times the probability that the system was in the specific state at time 0 corresponding to that acceleration, right?
Not so fast. You can't always get here from there. In other words, it's physically impossible to transition between most states (i.e., bins) in just one time interval. Suppose at time 1 the system has position 2.0 and velocity 2.0. Can the system be at position 2.0 and velocity 3.0 at time 2? If no acceleration is applied at time 2, the position will be 4.0 because of the initial velocity. If we apply an acceleration of 1.0, resulting in the velocity being 3.0, then the position will be 4.5 (check equations 2 and 3). Any given acceleration uniquely determines both the position and velocity for the next time interval. We have a bivariate system (i.e., two variables), but only one input that determines the next state. This situation is referred to as a degenerate distribution. See https://en.wikipedia.org/wiki/Degenerate_distribution.
So if we have a specific state $k$ at time $t$ $(x_t, \dot{x}_t)$ how do we determine the other states $(x_t, \dot{x}_t)$ from which it's possible to transition to state $k$ in one time interval?
Easy. Substitute equation (3) into equation (2) and solve for $\dot{x}_{t-1}$:
$$ \tag{5} x_t = x_{t-1} + \dot{x}_{t-1} + \dot{x}_t/2 - \dot{x}_{t-1}/2 $$ $$ \tag{6} 2x_t = 2x_{t-1} + 2\dot{x}_{t-1} + \dot{x}_t - \dot{x}_{t-1}$$ $$ \tag{7} \dot{x}_{t-1} = 2x_t - 2x_{t-1} - \dot{x}_t $$ $$ \tag{8} \dot{x}_{t-1} = -2 x_{t-1} + 2 x_t - \dot{x}_t $$
When plotted on a vertical axis corresponding to position and a vertical axis corresponding to velocity, equation (8) forms a line with slope -2 and vertical axis intercept $ x_t - \dot{x}_t/2 $. For every state $k$, our algorithm must calculate this line and only loop over bins that fall on this line. This isn't a matter of computational speed -- our results will be dead wrong if we don't filter out initial states that don't fall on the line.
Equation (8) allows us to calculate the possible positions at time $t-1$ given a position at time $t$. With some algebra, we can calculate all possible positions at time $t$ from a give position at time $t-1$.
$$ \tag{9} \dot{x}_t = 2 x_t - (2x_{t-1} + \dot{x}_{t-1}) $$
Suppose at time $t=0$ the system is in state (2, 2). Substituting 2 for $x_{t-1}$ and 2 for $\dot{x}_{t-1}$ in equation (9) gives us the equation: $$ \tag{10} \dot{x}_t = 2 x_t -6 $$
Assume that at time $t=1$, the system transitions to state (0, -6), which falls on the line defined by equation (10). Substituting 0 and -6 for the time $t$ variables in equation (8) gives us: $$ \tag{11} \dot{x}_{t-1} = -2 x_{t-1} + 6 $$
But the system at time 1 could be at any point on the line defined by equation (10). What it it's at point (10, 14)? Then the equation for all possible states still works out to be equation (11), the exact same line.
Let's plot this:
fig_lines = plt.figure()
plt.plot([-5, 10], [-16, 14])
plt.annotate("t = 1", (6, 6), (12, 6), arrowprops={"facecolor": 'black'})
plt.plot([-5, 10], [16, -14])
plt.annotate("t = 0", (0, 6), (-8, 6), arrowprops={"facecolor": 'black'})
plt.scatter([2], [2])
ax = plt.gca()
ax.set_aspect('equal', adjustable='box')
ax.set_xticks(range(-25, 25, 5))
ax.set_xticks(range(-25, 25, 1), True)
ax.set_yticks(range(-15, 20, 5))
ax.set_yticks(range(-15, 20, 1), True)
ax.set_xlim([-10, 15])
ax.set_ylim([-10, 10])
plt.title("Possible Start and End Points")
plt.xlabel("Position")
plt.ylabel("Velocity")
plt.grid()
plt.show()
The blue dot is our start point at $t=0$ at (2, 2). The blue line is the set of all possible states that the system can reach at time $t=1$ from the state (2, 2). The orange line is the set of all possible states from which the system can reach any state on the blue line.
The intersection of the two lines on the x axis indicates that states on the x axis are the only states that can remain constant between two adjacent points in time. This makes intuitive sense since the x axis represents all states with velocity zero.
It's easy to graphically determine the set of all possible states at the next point in time. Draw a line with slope -2 through the point at time $t-1$. Where that line intersects the x axis, draw a line with slope +2. The line with slope +2 is the set of all states that are possible at time $t$.
This process can be reversed to find the set of all states at time $t-1$ from which the system can reach a specific state. Draw a line with slope +2 through the point at time $t=1$. Where that line intersects the x axis, draw a line with slope -2. The line with slope -2 represents the set of all states at time $t-1$ that can reach the state at time $t$.
Intersection Algorithm¶
The following function determines if a specific point at time $t=1$ is reachable from another specific point at time $t=0$.
def reachable(pos1_idx, vel1_idx, pos0_idx, vel0_idx, grid):
"""Return True if state 1 is reachable from state 0.
Args:
pos1_idx (int): Index of state 1 position
vel1_idx (int): Index of state 1 velocity
pos1_idx (int): Index of state 1 position
vel1_idx (int): Index of state 1 velocity
grid (Grid2D): Two dimensional historgram object representing
current state of system
Returns: (Boolean)
"""
# Convert indices to positions and velocities at center of bin
p1 = grid.x(pos1_idx)
v1 = grid.y(vel1_idx)
p0 = grid.x(pos0_idx)
v0 = grid.y(vel0_idx)
# Calculate location of edges of state 0 bin
top_edge = v0 + grid.y_size / 2
bottom_edge = v0 - grid.y_size / 2
right_edge = p0 + grid.x_size / 2
left_edge = p0 - grid.x_size / 2
# Create functions to calculate coordinates on start-line.
# Start-line is set of all states from which the system can reach
# state 1 in one time interval
def make_vert_func(p_1, v_1): # Will return velocity given position
return lambda p_start: -2 * p_start + 2 * p_1 - v_1
get_vel = make_vert_func(p1, v1)
def make_horz_func(p_1, v_1): # Will return position given velocity
return lambda v_start: p_1 - (v_start + v_1) / 2
get_pos = make_horz_func(p1, v1)
# Check if start-line intersects bin edges of state 0
sides_crossed = 0
# Top Boundary (includes top-right corner)
if left_edge < get_pos(top_edge) <= right_edge:
sides_crossed = sides_crossed + 1
offset = (1 ,1)
# Right Boundary (includes lower-right corner)
if bottom_edge <= get_vel(right_edge) < top_edge:
sides_crossed = sides_crossed + 1
offset = (1, -1)
# Bottom Boundary (includes lower-left corner)
if left_edge <= get_pos(bottom_edge) < right_edge:
sides_crossed = sides_crossed + 1
offset = None
# Left Boundary (includes top-left corner)
if bottom_edge < get_vel(left_edge) <= top_edge:
sides_crossed = sides_crossed + 1
offset = None
if sides_crossed == 0: # No intersection, state 1 not reachable
return False
elif sides_crossed == 1 and offset is not None: # Might intersect
# Start
next_bin_indices = (pos0_idx + offset[0], vel0_idx + offset[1])
if next_bin_indices[0] < grid.x_n and next_bin_indices[1] < grid.y_n:
if grid[next_bin_indices]:
return True
elif sides_crossed >= 2: # Definitely intersects, state 1 reachable
return True
else:
return False
Determining Probability for Single Bin¶
The following function, bin_pred(), determines the probability that the system state is represented by a single bin (represented by position and velocity index values).
def bin_pred(pos_idx, vel_idx, grid):
"""Calculates the probability for a specific bin.
Args:
pos_idx (int): Position index
vel_idx (int): Velocity index
grid (Grid2D): Two dimensional grid object representing prior
system state.
Returns: (float) probability that bin represents system state
"""
p_k = 0
# Loop through all nonzero bins in prior grid
for bn in grid.nonzero_bins:
# Calculate probability if current bin is reachable from prior bin.
if reachable(pos_idx, vel_idx, bn[0], bn[1], grid):
acc1 = grid.y(vel_idx) - grid.y(bn[1])
prob = stats.norm.pdf(acc1) * grid[bn]
# If not reachable, probability is zero
else:
prob = 0
p_k = p_k + prob
return p_k
Determining Probability for Entire Grid¶
The following function, grid_pred() creates a new grid with all bins initialized to zero probability. Then it calls bin_pred() on every single bin in the grid to calculate the probability for that bin.
def grid_pred(grid):
"""Create new grid for next step in time and calculate proabilities
Args:
grid (Grid2D): Two dimensional grid object representing prior
system state.
Returns (Grid2D): New grid object with probabilities for next step
in time.
"""
# Create new grid object, initialized to zero
grid_next = Grid2D(grid.x_min, grid.x_max, grid.x_size, grid.y_min,
grid.y_max, grid.y_size)
# Calculate probability for every bin in grid
for pos_idx_k in range(0, grid_next.x_n):
for vel_idx_k in range(0, grid_next.y_n):
grid_next[pos_idx_k, vel_idx_k] = bin_pred(pos_idx_k,
vel_idx_k, grid)
# Normalize histogram grid so all probabilities sum to 1
grid_next.set_scale()
return grid_next
Time t = 1¶
# Prediction at time t=1
grid1 = grid_pred(grid0)
# Save grid to disk (grids take a long time to calculate)
with open("nb07/grid1.pickle", "wb") as file:
pickle.dump(grid1, file)
# Reload grid from disk
with open("nb07/grid1.pickle", "rb") as file:
grid1 = pickle.load(file)
# View results in tabular form
grid1.dataframe()
# View results as 3D scatter plot
pts = sorted(list(grid1.nonzero_bins), key=lambda tpl: tpl[0])
X = [grid1.x(pt[0]) for pt in pts]
Y = [grid1.y(pt[1]) for pt in pts]
Z = [grid1[pt[0], pt[1]] for pt in pts]
zer = [0 for idx in range(len(X))]
fig1 = plt.figure()
ax = fig1.add_subplot(111, projection='3d')
ax.scatter(X, Y, Z)
ax.plot(X, Y, zer)
plt.xlabel("Position")
plt.ylabel("Velocity")
plt.title("Robot State at Time 1")
plt.show()
# View results as 3D bar plot (stepwise continuous probability distribution)
# Normalize bins so volumes add up to 1 per equation (1)
Zh = [z/grid1.volume for z in Z]
fig1a = plt.figure()
ax = fig1a.add_subplot(111, projection='3d')
ax.bar3d(X, Y, zer, grid1.x_size, grid1.y_size, Zh)
plt.xlabel("Position")
plt.ylabel("Velocity")
plt.title("Robot State at Time 1")
plt.show()
Time t = 2¶
# Calculate system state at time 2
# grid2 = grid_pred(grid1)
# with open("nb07/grid2.pickle", "wb") as file:
# pickle.dump(grid2, file)
with open("nb07/grid2.pickle", "rb") as file:
grid2 = pickle.load(file)
# Display results in 3D bar plot
pts = list(grid2.nonzero_bins)
pts.sort()
X = [grid2.x(pt[0]) for pt in pts]
Y = [grid2.y(pt[1]) for pt in pts]
Z = [grid2[pt[0], pt[1]] for pt in pts]
zer = [0 for idx in range(len(X))]
Zh = [z/grid2.volume for z in Z]
fig2 = plt.figure()
ax = fig2.add_subplot(111, projection='3d')
ax.bar3d(X, Y, zer, grid2.x_size, grid2.y_size, Zh)
plt.xlabel("Position")
plt.ylabel("Velocity")
plt.title("Robot State at Time 1")
plt.show()
# Display Results as 3D surface plot
pts_x = sorted([pt[0] for pt in grid2.nonzero_bins])
pts_y = sorted([pt[1] for pt in grid2.nonzero_bins])
X_idx = list(range(pts_x[0], pts_x[-1]+1))
Y_idx = list(range(pts_y[0], pts_y[-1]+1))
Xval = [grid2.x(idx) for idx in X_idx]
Yval = [grid2.y(idx) for idx in Y_idx]
Xv, Yv = np.meshgrid(Xval, Yval)
XX, YY = np.meshgrid(X_idx, Y_idx)
def prob(x, y, grid):
return grid[x, y]
vprob = np.vectorize(prob, excluded=["grid"])
ZZ = vprob(XX, YY, grid2)
fig2a = plt.figure()
ax = fig2a.add_subplot(111, projection='3d')
ax.plot_surface(Xv, Yv, ZZ, cmap=matplotlib.cm.coolwarm, rstride=1, cstride=1)
plt.xlabel("Position")
plt.ylabel("Velocity")
plt.title("Robot State at Time 2")
plt.show()
# Display results as contour plot
fig2 = plt.figure()
ax = fig2.add_subplot(111)
ax.contourf(Xv, Yv, ZZ, cmap=cm.PuBuGn)
plt.xlabel("Position")
plt.ylabel("Velocity")
plt.title("Robot State at Time 2")
plt.show()
Comparison of Kalman and Histogram Filters¶
I'll use use the contour plot to compare the histogram filter to the Kalman filter results from earlier. First I'll bring in the get_eigen_data() function from notebook #3.
def get_eigen_data(eigenvects):
if eigenvects[0][0] > eigenvects[1][0]:
idx_max = 0
idx_min = 1
else:
idx_max = 1
idx_min = 0
return {"eigvec_major": list(eigenvects[idx_max][2][0]),
"eigvec_minor": list(eigenvects[idx_min][2][0]),
"eigval_major": eigenvects[idx_max][0],
"eigval_minor": eigenvects[idx_min][0],
"theta_major": math.atan(eigenvects[idx_max][2][0][1]/
eigenvects[idx_max][2][0][0])*180/math.pi,
"theta_minor": math.atan(eigenvects[idx_min][2][0][1]/
eigenvects[idx_min][2][0][0])*180/math.pi,
}
This function will plot the contour chart and the 95% uncertainty ellipse.
def contour_plot(grid, title, sigma=None, state=(0,0), xlims=None, ylims=None):
pts_x = sorted([pt[0] for pt in grid.nonzero_bins])
pts_y = sorted([pt[1] for pt in grid.nonzero_bins])
X_idx = list(range(pts_x[0], pts_x[-1]+1))
Y_idx = list(range(pts_y[0], pts_y[-1]+1))
Xval = [grid.x(idx) for idx in X_idx]
Yval = [grid.y(idx) for idx in Y_idx]
Xv, Yv = np.meshgrid(Xval, Yval)
XX, YY = np.meshgrid(X_idx, Y_idx)
def prob(x, y, grid_):
return grid_[x, y]
vprob = np.vectorize(prob, excluded=["grid"])
ZZ = vprob(XX, YY, grid)
contour_fig = plt.figure()
ax = contour_fig.add_subplot(111)
ax.contourf(Xv, Yv, ZZ, cmap=cm.PuBuGn)
if sigma is not None:
ed = get_eigen_data(sigma.eigenvects())
ell = mpatches.Ellipse(state, math.sqrt(ed["eigval_major"]*5.992)*2,
math.sqrt(ed["eigval_minor"]*5.992)*2,
ed["theta_major"], fill=False, edgecolor="red")
ax.add_patch(ell)
if xlims is not None:
ax.set_xlim(xlims)
if ylims is not None:
ax.set_ylim(ylims)
plt.title(title)
plt.xlabel("Position")
plt.ylabel("Velocity")
plt.grid()
plt.show()
Now to show the results.
# Covariance Matrix at time 2 from Notebook #3
sigma2 = sympy.Matrix([[2.5, 2], [2, 2]])
contour_plot(grid2, "Robot state at Time 2", sigma2, xlims=(-8, 8), ylims=(-6, 6))
The red line is the 95% uncertainty ellipse calculated from the Kalman covariance matrix. The blue-green shading represents the results from the Kalman filter. There is good correlation between the Kalman filter and histogram filter results. The histogram filter does show less variance than the Kalman filter and the numerical simulation. This may be due to the zero threshold. I'll try changing the zero threshold for future calculations.
The contour chart allows comparison of the Kalman and histogram filter results, so I'll use contour charts from here on.
Time t = 3¶
# grid3 = grid_pred(grid2)
# with open("nb07/grid3.pickle", "wb") as file:
# pickle.dump(grid3, file)
with open("nb07/grid3.pickle", "rb") as file:
grid3 = pickle.load(file)
# Covariance Matrix at time 3 from Notebook #3
sigma3 = sympy.Matrix([[8.75, 4.5], [4.5, 3]])
contour_plot(grid3, "Robot state at Time 3", sigma3)
Time t = 4¶
# grid4 = grid_pred(grid3)
# with open("nb07/grid4.pickle", "wb") as file:
# pickle.dump(grid4, file)
with open("nb07/grid4.pickle", "rb") as file:
grid4 = pickle.load(file)
# Covariance Matrix at time 4 from Notebook #3
sigma4 = sympy.Matrix([[21, 8], [8, 4]])
contour_plot(grid4, "Robot State at Time 4", sigma4)
Time t = 5¶
# grid5 = grid_pred(grid4)
# with open("nb07/grid5.pickle", "wb") as file:
# pickle.dump(grid5, file)
with open("nb07/grid5.pickle", "rb") as file:
grid5 = pickle.load(file)
# Covariance Matrix at time 4 from Notebook #3
sigma5 = sympy.Matrix([[41.25, 12.5], [12.5, 5]])
contour_plot(grid5, "Robot State at Time 5", sigma5)
2. Exercise 4-1.b¶
Histogram Filter Measurement Update¶
- In this exercise, you will be asked to implement a histogram filter for a linear dynamic system studied in the previous chapter.
a. Now implement the measurement update step into your histogram filter, as described in Exercise 2 of the previous chapter (page 82). Suppose at time $t = 5$, we observe a measurement $z = 5$. State and plot the posterior before and after updating the histogram filter.
Review of Exercise 2 from Previous Chapter¶
According to exercise 2, the expected value of the position measurement is the actual position of the robot. The position measurement is normally distributed but noisy, with variance = 10.
Solution¶
Measurement Update¶
The equation for the measurement update: $$ \tag{12} p_{k, t} = \eta \, p(\vec{z_t} \mid X_t = \vec{x_k}) \, \overline{p_{k, t}}$$
Equation (12) is calculated for every state (i.e., for every bin in the grid). $\overline{p_{k, t}}$ is the pre-measurement (prediction) probability from exercise 4-1.a. $p(\vec{z_t} \mid X_t = \vec{x_k})$ is the probability of receiving a specific measurement $z$, such as position = 5, if the system is in state $k$. $\eta$ is a scaling factor that will be adjusted as necessary for probabilities to sum to 1.
The probability of receiving measurement $z$ is $$ \tag{13} p(\vec{z_t} \mid X_t = \vec{x_k}) = N(x_{max}, z, 10) - N(x_{min}, z, 10) $$
$N(x, z, 10)$ is the cumulative normal probability distribution with mean $z$ and variance 10. $x_{max}$ is the position at the right edge of the bin centered on $x$ and $x_{min}$ is position at the left edge of the bin centered on x. The following function will calculate $p(\vec{z_t} \mid X_t = \vec{x_k})$ for a single bin.
def bin_measurement(z, x_idx, grid):
x = grid.x(x_idx)
x_max = x + grid.x_size/2
x_min = x - grid.x_size/2
std_dev = math.sqrt(10)
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
grid_measurement will calculate $p_{k, t}$ for every bin in the grid.
def grid_measurement(z, grid):
# Create new grid object, initialized to zero
grid_meas = Grid2D(grid.x_min, grid.x_max, grid.x_size, grid.y_min,
grid.y_max, grid.y_size)
grid_meas.zero_threshold = 0.000001
# Calculate probability for every bin in grid
for pos_idx_k in range(0, grid_meas.x_n):
for vel_idx_k in range(0, grid_meas.y_n):
bin_meas = bin_measurement(z, pos_idx_k, grid)
prob = bin_meas * grid[pos_idx_k, vel_idx_k]
grid_meas[pos_idx_k, vel_idx_k] = prob
# Normalize histogram grid so all probabilities sum to 1
grid_meas.set_scale()
return grid_meas
# Calculate the post measurement grid
grid5_meas = grid_measurement(5, grid5)
# Post measurement covariance and mean from notebook #4
sigma5_meas = sympy.Matrix([[8.048, 2.439], [2.439, 1.951]])
state5_meas = (4.024, 1.220)
contour_plot(grid5_meas, "Robot State at Time 5, z = 5", sigma5_meas,
state5_meas, xlims=(-5, 15), ylims=(-4, 6))
The red ellipse is the 95% uncertainty ellipse calculated from the Kalman filter algorithm. It corresponds well with the post measurement results from the histogram filter.
Stacy Irwin, 5 August 2018