## Kalman Filter 1D¶

In [1]:
import daft

In [2]:
from __future__ import division, print_function

In [3]:
%matplotlib inline

#from IPython.core.pylabtools import figsize
from matplotlib import pyplot as plt
import numpy as np


### Markov Chain¶

In [4]:
def plot_markov_chain():
pgm = daft.PGM([8.3, 1.05], origin=[-2., .3], aspect=1.)
pgm.add_node(daft.Node("H_1", r"$H_1$", 0., 1.))
pgm.add_node(daft.Node("H_2", r"$H_2$", 1., 1.))
pgm.add_node(daft.Node("H_3", r"$H_3$", 2., 1.))
pgm.add_node(daft.Node("H_4", r"$H_4$", 3., 1.))

pgm.render()

In [5]:
plot_markov_chain()


The following independence assumption (Markov Assumption) holds for the graph:

$$P(H_t \mid H_1, \dots ,H_{t-1}) = P(H_t \mid H_{t-1})$$

In other words, the current state is statistically independent of the previous states given the state one time step before, i.e. if the chain starts at time $t=1$:

$$\forall t \in \mathbb N^{+}, \forall t' \in \mathbb N^{+}: (t-t' \geq 2) \Rightarrow H_t \perp H_{t'} \mid H_{t-1}$$

### Localization with motion on a grid¶

$H_t$ is the random variable for the state of a robot, $Val(H_t)$ are the grid indices.

• e.g. $H_t=3$: the robot is located at time $t$ in grid cell with index 3.

#### Motion equation:¶

$$P(H_t=i) = \sum_j P(H_{t-1}=j)P(H_t=i \mid H_{t-1}=j)$$

• $P(H_t=i)$: Probability that the robot is located at grid cell index $i$ at time $t$
• $P(H=i \mid H=j)$: Transition Probability

For localization the (relative) transition probabilities are typically translation invariant, e.g.

$$P(H_t=2 \mid H_{t-1}=3) = P(H_t=1 \mid H_{t-1}=2)$$

The transition probability depends only on the local differenz between the states: $H_t-H_{t-1}$.

$$P(H_t) = \sum_{H_{t-1}} P(H_{t-1})P(H_t - H_{t-1})$$

So the motion equation is mathematically a convolution.

$$(f * g)(H_t) = \sum_{H_{t-1}} f(H_{t-1}) g(H_t - H_{t-1})$$

So if we know the localization of the robot at time $t=1$ and we do not observe any variable (no measurement), our knowledge of the robot location becomes more "blurry" with time.

##### Example:¶
• 10 cells
• the robot is located at time 0 in cell with grid index 4
• motion:
• go left with probability 10%
• stay at position: 70%
• go right: 20%
In [6]:
p = np.zeros([5,10])
p[0, 4] = 1 # the robot is located at time 0 in cell with grid index 4

# transition probabilities for
# [P(H=1|H=i-1), P(H=1|H=i), P(H=1|H=i+1)] all other tp are zero
tp = np.array([0.1, 0.7, 0.2])
np.set_printoptions(precision=3)
for t in range(4):
p[t+1] = np.convolve(p[t], tp, mode='same')
print (p)

[[0.000e+00 0.000e+00 0.000e+00 0.000e+00 1.000e+00 0.000e+00 0.000e+00
0.000e+00 0.000e+00 0.000e+00]
[0.000e+00 0.000e+00 0.000e+00 1.000e-01 7.000e-01 2.000e-01 0.000e+00
0.000e+00 0.000e+00 0.000e+00]
[0.000e+00 0.000e+00 1.000e-02 1.400e-01 5.300e-01 2.800e-01 4.000e-02
0.000e+00 0.000e+00 0.000e+00]
[0.000e+00 1.000e-03 2.100e-02 1.530e-01 4.270e-01 3.060e-01 8.400e-02
8.000e-03 0.000e+00 0.000e+00]
[1.000e-04 2.800e-03 3.020e-02 1.540e-01 3.601e-01 3.080e-01 1.208e-01
2.240e-02 1.600e-03 0.000e+00]]

In [7]:
plt.figure(figsize=(10,8))
for t in range(4):
plt.subplot(4, 1, t+1)
plt.plot(range(10), p[t],'bo')
plt.ylim(-0.0,1.1)
plt.ylabel("probability")
_ = plt.xlabel("grid cells")


Note: This also works with a multimodal distribution.

### Hidden Markov Models¶

In [8]:
def plot_HMM():
pgm = daft.PGM([8.3, 2.05], origin=[-1., -0.3], aspect=1.)
pgm.add_node(daft.Node("Z_1", r"$Z_1$", 0., 0., observed=True))
pgm.add_node(daft.Node("Z_2", r"$Z_2$", 1., 0., observed=True))
pgm.add_node(daft.Node("Z_3", r"$Z_3$", 2., 0., observed=True))
pgm.add_node(daft.Node("Z_4", r"$Z_4$", 3., 0., observed=True))

pgm.add_node(daft.Node("H_1", r"$H_1$", 0., 1.))
pgm.add_node(daft.Node("H_2", r"$H_2$", 1., 1.))
pgm.add_node(daft.Node("H_3", r"$H_3$", 2., 1.))
pgm.add_node(daft.Node("H_4", r"$H_4$", 3., 1.))

pgm.render()

In [9]:
plot_HMM()


The following conditional independence assumptions encodes the graph $\mathcal G$:

$$P(Z_t \mid Z_1, \dots, Z_{t-1}, H_1, \dots, H_{t}) = P(Z_t \mid H_t)$$

$$P(H_t \mid Z_1, \dots, Z_{t-1}, H_1, \dots, H_{t-1}) = P(H_t \mid H_{t-1})$$

#### Example: Robot locatization with observations¶

Addtionally we have a sensor which gives us an observation $Z_t$ which indicates, where the robot is located.

Bayes rule can be use to update our belief of the robot location:

$$P(H_t=i \mid Z_t) = \frac{P(Z_t \mid H_t=i)P(H_{t}=i)}{P(Z_t)}$$

• the Prior $P(H_{t}=i)$ is computed by the motion equation.
• Measurement (Likelihood): $P(Z_t \mid H_t=i)$
• Normalizer $P(Z_t) = \sum_i P(Z_t \mid H_t=i) P(H_{t}=i)$
In [10]:
prior = p[4]

In [11]:
plt.figure(figsize=(8,10))
plt.subplot(4, 1, 1)
plt.plot(range(10), prior,'ro')
plt.ylim(-0.0,1.1)
plt.ylabel("probability")
plt.xlabel("grid cells")
plt.title('Prior')

Out[11]:
Text(0.5,1,'Prior')
In [12]:
plt.figure(figsize=(8,10))
p_measurement = np.array([0., 0., 0., 0.05, 0.18, 0.54, 0.18, 0.05, 0., 0.])
plt.subplot(4, 1, 2)
plt.plot(range(10), p_measurement,'go')
plt.ylim(-0.0,1.1)
plt.ylabel("probability")
plt.xlabel("grid cells")
plt.title('Likelihood')

Out[12]:
Text(0.5,1,'Likelihood')
In [13]:
plt.figure(figsize=(8,10))
posterior = p_measurement * prior
posterior = posterior / posterior.sum()
plt.subplot(4, 1, 3)
plt.plot(range(10), posterior,'bo')
plt.ylim(-0.0,1.1)
plt.ylabel("probability")
plt.xlabel("grid cells")
plt.title('Posterior')

Out[13]:
Text(0.5,1,'Posterior')

### 1D Continuous State Space¶

#### Motion and Prediction¶

Assumption:

• The Position is described by a Gaussian.

Limitation due to the Gaussian: unimodal

A Kalman Filter is a instance of an Bayes Filter: The representation of information is a probability density function.

Here we investigate the 1D Kalman Filter, i.e. we have a constant motion model.

Assumption:

• the state transition probability $p(h_t \mid h_{t-1}, v_t)$ is linear with Gaussian noise (called linear Gaussian), in general for 1d:

$$h_t = h_{t-1} + v_t + \epsilon$$

• $v_t$ is the traveling distance in time $\Delta t$ (one time step).
• $\epsilon$ is Gaussian noise with $\sigma_v$ (the noise of $v_t$ in $\Delta t$)

$$p(h_t \mid h_{t-1}, v_t) = \mathcal N(h_{t-1} + v_t, \sigma_v)$$

So the motion is described by a Gaussian $\mathcal N$.

The inital position at $t=1$ changes to due the motion. The initial uncertainty of the position is increased by the uncertainty of the state transition probability.
This is described mathematically by the convolution of the two Gaussians.

The convolution of two Gaussians is again a Gaussian with (see e.g. [Thr06][Bro14]:

• $\mu' = \mu + \mu_v$
• $\sigma'^2 = \sigma^2 + \sigma^2_v$

Function for the prediction:

In [14]:
def predict(mu, sigma, mu1, sigma1):
# convolution of two gaussians
mu_ = mu + mu1
sigma_ = np.sqrt(sigma**2+sigma1**2)
return mu_, sigma_

In [15]:
import scipy.stats

In [16]:
h_space = np.arange(-10.,10.,.05)
v_space = np.arange(-5.,9.,.05)
mu0 = -2.
sigma0 = 1.5
mu_v = 2.5
sigma_v = 1.9

In [17]:
h0_pdf = scipy.stats.norm(loc=mu0, scale=sigma0).pdf(h_space)
v_pdf = scipy.stats.norm(loc=mu_v, scale=sigma_v).pdf(v_space)
mu1,sigma1 = predict(mu0, sigma0, mu_v, sigma_v)
h1_pdf = scipy.stats.norm(loc=mu1, scale=sigma1).pdf(h_space)

In [18]:
plt.figure(figsize= (8,3))
plt.title("traveling distance in time $\Delta t$")
plt.plot(v_space, v_pdf,'g-')
plt.ylabel("probability density")
plt.xlabel("v")

Out[18]:
Text(0.5,0,'v')
In [19]:
plt.figure(figsize=(8,3))
plt.plot(h_space, h0_pdf,'b-', label="Position at $t$")
plt.plot(h_space, h1_pdf,'r-', label="Predicted Position at $t+\Delta t$")
plt.ylabel("probability density")
plt.xlabel("h")
plt.legend()

Out[19]:
<matplotlib.legend.Legend at 0x117091438>

#### Measurement¶

Assumption: The measurement probability is linear

$$z_t = h_t + \delta$$ with:

• $\delta$: zero-mean Gaussian noise of the measurement

or $$h_t = z_t - \delta$$

(Note: the pdf of the Gaussian noise is symmetric.)

$$p(z_t \mid h_t) = \mathcal N(z_t, \sigma_o)$$

For the measurement we use Bayes rule with the probability densities

$$p(h_t \mid z_t) = \frac{p(z_t \mid h_t)p(h_t)}{p(z_t)}$$

with

• $h \in \mathbb R$
• The prior is the predicted position after a motion (step): $p(H_{t}=h)=\mathcal N(\mu',\sigma')$
• Measurement Likelihood: $p(z_t \mid h_t) = \mathcal N(\mu_o,\sigma_o)$
• The measurement locates the robot at position $z_t = \mu_o$.
• $\sigma_o$ is the standard deviation of the measurement (e.g. sensor uncertainty).
• Normalizer $p(z_t) = \int_{\mathcal H} p(z_t \mid H_t=h) p(H_{t}=h) dh$
##### Product of two Gaussians¶

$$\mu = \frac{\sigma_o^2 \mu' +\sigma'^2\mu_o}{\sigma'^2+\sigma_o^2}$$

$$\sigma^2 = \frac{1}{1/\sigma'^2 + 1/\sigma_o^2}$$

(for the derivation see e.g. [Bro14])

In [27]:
def measurement(mu, sigma, mu1, sigma1):
# product of two gaussians
mu_ = (sigma1** 2 * mu + sigma**2 * mu1)/ (sigma** 2 +sigma1** 2 )
sigma_ = np.sqrt( 1./( 1./sigma**2 + 1./sigma1**2) )
return mu_, sigma_

In [21]:
sigma_o = 3.
mu_o = -1.

In [22]:
o_pdf = scipy.stats.norm(loc=mu_o, scale=sigma_o).pdf(h_space)
mu, sigma = measurement(mu1, sigma1, mu_o, sigma_o)
h_pdf = scipy.stats.norm(loc=mu, scale=sigma).pdf(h_space)

In [23]:
plt.figure(figsize=(8,3))
plt.plot(h_space, h1_pdf,'r-', label="Prior")
plt.plot(h_space, o_pdf,'g-', label="Likelihood")
plt.plot(h_space, h_pdf,'b-', label="Posterior")
plt.ylabel("probability density")
plt.xlabel("h")
plt.legend()

Out[23]:
<matplotlib.legend.Legend at 0x1171b9da0>

### Example¶

In [24]:
sigma_o = 1.6
sigma_v = .9

In [25]:
observations = np.array([-2, -1.5, -0.4, 1.2, 2.1])
velocity = np.array([1, 1.1, 1.2, 1.2, 1.2])
h_space = np.arange(-6., 10., 0.05)

In [26]:
mu = 0. # initial position
sigma = 6. # initial standard deviation of the position
plt.figure(figsize=(10,12))

h_pdf = scipy.stats.norm(loc=mu, scale=sigma).pdf(h_space)
plt.subplot(6, 1, 1)
plt.plot(h_space, h_pdf,'b-', label="Position at t=0")
plt.ylim(0.,0.4)
plt.legend()
for i,(o, v) in enumerate(zip(observations, velocity)):
plt.subplot(6, 1, i+2)
plt.ylim(0.,0.4)
mu_, sigma_ = predict(mu, sigma, v, sigma_v)
h_pdf = scipy.stats.norm(loc=mu_, scale=sigma_).pdf(h_space)
plt.plot(h_space, h_pdf,'r-', label="Prediction at t=%i"%(i+1))

h_pdf = scipy.stats.norm(loc=o, scale=sigma_o).pdf(h_space)
plt.plot(h_space, h_pdf,'g-', label="Observation at t=%i"%(i+1))

mu, sigma = measurement(mu_, sigma_, o, sigma_o)
h_pdf = scipy.stats.norm(loc=mu, scale=sigma).pdf(h_space)
plt.plot(h_space, h_pdf,'b-', label="Position at t=%i"%(i+1))
plt.legend()


$$\hat x$$