Understand basic probability concepts from Probabilistic Robotics by Sebastian Thrun, Wolfram Burgard, Dieter Fox
Code, or find a library for, a basic Bayes Filter.
X = a random variable
x = a specific value that X might assume
$p(x)$ = probability of x
$p(x, y)$ = probability of x and probability of y
$p(x|y)$ = probability of x if y known
μ = mean
$σ^2$ = variance
∑ = covariance (little ∑)
η = Bayes rule normaliser (normalise to 1)
$E$ = expectation of a random variable
$x_t$ = state at time $t$ which might include state variables:
$z_t$ = measurement data at time t.
$z_{t_1 : t_2} = z_{t_1}, z_{t_1 + 1}, z_{t_1 + 2},..., z_{t_2}$ = the set of all measurement data from time $t_1$ to time $t_2$, for $t_1 ≤ t_2$.
$u_t$ = control data, ie information about change of state in the environment, in time interval $(t - 1 ; t)$.
$u_{t_1 : t_2} = u_{t_1}, u_{t_1 + 1}, u_{t_1 + 2},..., u_{t_2}$ = the set of all control data from time $t - 1$ to time $t$, for $t_1 ≤ t_2$.
$bel(x_t)$ = belief for state $x_t$
(If no MathJax installed, in ipnb:
from IPython.external.mathjax import install_mathjax
install_mathjax()
Example of MathJax and LaTeX usage from nbviewer markdown example:
$$e^x=\sum_{i=0}^\infty \frac{1}{i!}x^i$$And some more Markdown formatting.)
A probability density function (PDF) must sum to 1:
$$∫ p(x) dx = 1$$If independent variables:
$$p(x, y) = p(x) p(y)$$If conditional probability:
$$p(x | y) = \frac{p(x, y)}{p(y)}$$ie if know the probability of y, then know the probability of x.
Bayes rule:
$$p(x | y) = \frac{p (y | x) p(x)} {p(y)}$$In discrete space.
ie if x is a quantity we would like to infer from y, the probability p(x) is a prior probability distribution, and y is the data such as a sensor measurement.
$p(x | y)$ is called the posterior probability distribution.
Rewriting for the normaliser as $p(y)^{-1}$ will be the same for any value of x:
$$p(x | y) = η p(y | x) p(x)$$Expectation of a random variable:
$$E[X] = ∑_x x p(x)$$for discrete space.
Covariance (squared expected deviation from the mean) of X:
$$Cov[X] = E[X^2] - E[X]^2$$Entropy (the expected information that the value of x carries):
$$H_{p}(x) = - ∑_{x} p(x) log_{2} p(x)$$for discrete space.
A note on Markov Chains. The future may be stochastic (state is non-deterministic i.e. "random") but no variables prior to $x_t$ may influcence the stochastic evolution of future states, unless this dependence is mediated through the state $x_t$. Temporal processes that meet these conditions are known as Markov Chains.
A note on data. There is a clear distinction between measurement data and control data, although both may take place concurrently. Consider:
Probabalistic generative laws govern the evolution of state and measurements with state $x_t$ generated stochastically from $x_{t-1}$ so:
$$p(x_t | x_{0 : t - 1}, z_{1 : t - 1}, u_{1 : t}) = p(x_t | x_{t-1}, u_t)$$assuming robot undertakes control action $u_1$ first, then takes a measurement $z_1$.
Measurement probability can be defined as:
$$p(z_t | x_t)$$ie. knowing the state $x_t$ allows you to know the probability of measurement $z_t$. Consider measurements as noisy projections of the state!
In [62]:
# Image © Probabalistic Robotics figure 2.2.
from IPython.core.display import Image
Image(filename='dynamic_bayes_network_Image_Probabalistic_robotics.jpg')
Out[62]:
The dynamic Bayes network shows the evolution of states $x_t$ as control actions $u_t$ are applied and measurements $z_t$ are applied. Such a temporaral generative model is also known as a Hidden Markov Model (HMM).
Belief distributions reflect the robots internal knowledge about the state of the environment, nb state can not be measured directly, state must be infered from data.
$$bel(x_t) = p(x_t | z_{1:t}, u_{1:t})$$this posterior is the probability distribution over the state $x_t$ at time $t$, conditioned on all past measurements $z_{1:t}$ and all past controls $u_{1:t}$.
Sometimes the posterior will be calculated before incorporating measurement $z_t$ and after applying control $u_t$. This can be denoted:
$$\overline{bel}(x_t) = p(x_t | z_{1:t - 1}, u_{1:t})$$This probability distribution is refered to as prediction and can be used as part of correction.
algorithm_bayesfilter($bel(x{t-1}), u_t, z_t$):
for all $x_t$ do:
$$\overline{bel}(x_t) = ∫ p(x_t | u_t, x_{t-1}) . bel(x_{t-1})dx_{t-1}$$$$bel(x_t) = η . p(z_t | x_t) . \overline{bel}(x_t)$$endfor
return $bel(x_t)$
In discrete space $\overline{bel}(x_1) = ∫ p(x_1 | u_1, x_0) . bel(x_0)dx_0$ becomes:
$$\overline{bel}(x_1) = ∑_{x_0} p(x_1 | u_1, x_0) . bel(x_0)$$
In [2]:
%%file basic_bayes_filter_example.py
# -*- coding: ascii -*-
""" A basic Bayes Filter example.
A mobile robot estimates the state of a door.
First coding attempt, linear code.
"""
__author__ = "Mike McFarlane mike@mikemcfarlane.co.uk"
__version__ = "Revision: ??"
__date__ = "Date: 18-04-14"
__copyright__ = "Copyright (c)Mike McFarlane 2014"
__license__ = "TBC"
# Probabilities defining this example.
# p = probability, x = state, z = sensor measurement, u = manipulator action.
# Initial belief, no prior knowledge so assumes equal probability.
bel_X0_isOpen = 0.5
bel_X0_isClosed = 0.5
# Measurements z from sensor, assume noisy, so conditional probabilities,
# when door is open,
p_Zt_senseOpen_Xt_isOpen = 0.6
p_Zt_senseClosed_Xt_isOpen = 0.4
# and when door is closed,
p_Zt_senseOpen_Xt_isClosed = 0.2
p_Zt_senseClosed_Xt_isClosed = 0.8
# ie robot is good at detecting when door is closed, but not so good when door open.
# With actions u, if robot tries to open door,
p_Xt_isOpen_Ut_push_Xt_1_isOpen = 1
p_Xt_isClosed_Ut_push_Xt_1_isOpen = 0
p_Xt_isOpen_Ut_push_Xt_1_isClosed = 0.8
p_Xt_isClosed_Ut_push_Xt_1_isClosed = 0.2
# or does nothing,
p_Xt_isOpen_Ut_doNothing_Xt_1_isOpen = 1
p_Xt_isClosed_Ut_doNothing_Xt_1_isOpen = 0
p_Xt_isOpen_Ut_doNothing_Xt_1_isClosed = 0
p_Xt_isClosed_Ut_doNothing_Xt_1_isClosed = 1
# Initial value for normaliser,
n = 1.0
def main():
""" Main.
"""
# Declare n as global as needs modified.
global n
print "\nArrive at door, is it open or closed?\n"
# For hypothesis X1 = isOpen.
bel_prediction_X1_isOpen = (p_Xt_isOpen_Ut_doNothing_Xt_1_isOpen * bel_X0_isOpen) + \
(p_Xt_isOpen_Ut_doNothing_Xt_1_isClosed * bel_X0_isClosed)
print "bel_prediction_X1_isOpen: ", bel_prediction_X1_isOpen
# For hypothesis X1 = isClosed.
bel_prediction_X1_isClosed = (p_Xt_isClosed_Ut_doNothing_Xt_1_isOpen * bel_X0_isOpen) + \
(p_Xt_isClosed_Ut_doNothing_Xt_1_isClosed * bel_X0_isClosed)
print "bel_prediction_X1_isClosed: ", bel_prediction_X1_isClosed
# Incorporate the measurement data, with normaliser equal 1.
bel_X1_isOpen = n * p_Zt_senseOpen_Xt_isOpen * bel_prediction_X1_isOpen
print "bel_X1_isOpen (n=1): ", bel_X1_isOpen
bel_X1_isClosed = n * p_Zt_senseOpen_Xt_isClosed * bel_prediction_X1_isClosed
print "bel_X1_isClosed (n=1): ", bel_X1_isClosed
# Calculate the value of the normaliser
n = 1 / (bel_X1_isOpen + bel_X1_isClosed)
print "normaliser X1: ", n
# Recalculate incorporating measurement data, with normaliser.
bel_X1_isOpen = n * p_Zt_senseOpen_Xt_isOpen * bel_prediction_X1_isOpen
print "bel_X1_isOpen (normalised): ", bel_X1_isOpen
bel_X1_isClosed = n * p_Zt_senseOpen_Xt_isClosed * bel_prediction_X1_isClosed
print "bel_X1_isClosed (normalised): ", bel_X1_isClosed
# Give the door a push ie iterate
print "\nPush the door .....\n"
# For hypothesis X2 = isOpen.
bel_prediction_X2_isOpen = (p_Xt_isOpen_Ut_push_Xt_1_isOpen * bel_X1_isOpen) + \
(p_Xt_isOpen_Ut_push_Xt_1_isClosed * bel_X1_isClosed)
print "bel_prediction_X2_isOpen: ", bel_prediction_X2_isOpen
# For hypothesis X2 = isClosed.
bel_prediction_X2_isClosed = (p_Xt_isClosed_Ut_push_Xt_1_isOpen * bel_X1_isOpen) + \
(p_Xt_isClosed_Ut_push_Xt_1_isClosed * bel_X1_isClosed)
print "bel_prediction_X1_isClosed: ", bel_prediction_X2_isClosed
# Incorporate the measurement data, with normaliser = 1.
n = 1
bel_X2_isOpen = n * p_Zt_senseOpen_Xt_isOpen * bel_prediction_X2_isOpen
print "bel_X2_isOpen (n new): ", bel_X2_isOpen
bel_X2_isClosed = n * p_Zt_senseOpen_Xt_isClosed * bel_prediction_X2_isClosed
print "bel_X2_isClosed (n new): ", bel_X2_isClosed
# Calculate the value of the normaliser
n = 1 / (bel_X2_isOpen + bel_X2_isClosed)
print "normaliser X2: ", n
# Recalculate incorporating measurement data, with normaliser.
bel_X2_isOpen = n * p_Zt_senseOpen_Xt_isOpen * bel_prediction_X2_isOpen
print "bel_X2_isOpen (normalised): ", bel_X2_isOpen
bel_X2_isClosed = n * p_Zt_senseOpen_Xt_isClosed * bel_prediction_X2_isClosed
print "bel_X2_isClosed (normalised): ", bel_X2_isClosed
if __name__ == "__main__":
main()
In [3]:
!python basic_bayes_filter_example.py
In [5]:
%%file basic_bayes_filter_functions_example.py
# -*- coding: ascii -*-
""" A basic Bayes Filter example.
A mobile robot estimates the state of a door.
First coding attempt, break code out into functions, better data structure.
Not working, out of time.
"""
__author__ = "Mike McFarlane mike@mikemcfarlane.co.uk"
__version__ = "Revision: ??"
__date__ = "Date: 18-04-14"
__copyright__ = "Copyright (c)Mike McFarlane 2014"
__license__ = "TBC"
def bayes_filter(bel_xt_1, prob_distribution):
""" Simple Bayes filter.
"""
n = 1
bel_prediction_xt = 0
for p_xt_ut_xt_1 in prob_distribution:
bel_prediction_xt += p_xt_ut_xt_1 * bel_xt_1
bel_xt = n * p_zt_xt * bel_prediction_xt
n =
return bel_xt
def bayes_function_normaliser(*args):
"""Normalise the list of beliefs.
Return the Bayes Rule normaliser, n.
"""
n = 1 / sum(args)
return n
def main():
""" Main.
"""
# Probabilities defining this example.
# p = probability, x = state, z = sensor measurement, u = manipulator action.
# Initial belief, no prior knowledge so assumes equal probability.
bel_xt_1_isOpen = 0.5
bel_xt_1_isClosed = 0.5
# Measurements z from sensor, assume noisy, so conditional probabilities,
# when door is open,
p_Zt_senseOpen_Xt_isOpen = 0.6
p_Zt_senseClosed_Xt_isOpen = 0.4
# and when door is closed,
p_Zt_senseOpen_Xt_isClosed = 0.2
p_Zt_senseClosed_Xt_isClosed = 0.8
# ie robot is good at detecting when door is closed, but not so good when door open.
# With actions u, if robot tries to open door,
p_Xt_isOpen_Ut_push_Xt_1_isOpen = 1.0
p_Xt_isClosed_Ut_push_Xt_1_isOpen = 0.0
p_Xt_isOpen_Ut_push_Xt_1_isClosed = 0.8
p_Xt_isClosed_Ut_push_Xt_1_isClosed = 0.2
# or does nothing,
p_Xt_isOpen_Ut_doNothing_Xt_1_isOpen = 1.0
p_Xt_isOpen_Ut_doNothing_Xt_1_isClosed = 0.0
p_Xt_isClosed_Ut_doNothing_Xt_1_isOpen = 0.0
p_Xt_isClosed_Ut_doNothing_Xt_1_isClosed = 1.0
0,1,0,1.0
1,1,0,0.0
0,1,1,0.8
1,1,1,0.2
0,0,0,1.0
0,0,1,0.0
1,0,0,0.0
1,0,1,1.0
(0,1,0) : 1.0
# Initial value for normaliser,
n = 1.0
print "\nArrive at door, is it open or closed?\n"
# For hypothesis X1 = isOpen.
bel_prediction_Xt_isOpen = (p_Xt_isOpen_Ut_doNothing_Xt_1_isOpen * bel_xt_1_isOpen) + \
(p_Xt_isOpen_Ut_doNothing_Xt_1_isClosed * bel_xt_1_isClosed)
print "bel_prediction_X1_isOpen: ", bel_prediction_Xt_isOpen
# For hypothesis X1 = isClosed.
bel_prediction_Xt_isClosed = (p_Xt_isClosed_Ut_doNothing_Xt_1_isOpen * bel_xt_1_isOpen) + \
(p_Xt_isClosed_Ut_doNothing_Xt_1_isClosed * bel_xt_1_isClosed)
print "bel_prediction_X1_isClosed: ", bel_prediction_Xt_isClosed
# Incorporate the measurement data, with normaliser equal 1.
bel_Xt_isOpen = n * p_Zt_senseOpen_Xt_isOpen * bel_prediction_Xt_isOpen
print "bel_X1_isOpen (n=1): ", bel_Xt_isOpen
bel_Xt_isClosed = n * p_Zt_senseOpen_Xt_isClosed * bel_prediction_Xt_isClosed
print "bel_X1_isClosed (n=1): ", bel_Xt_isClosed
# Calculate the value of the normaliser
n = bayes_function_normaliser(bel_Xt_isOpen, bel_Xt_isClosed)
print "normaliser Xt: ", n
# Recalculate incorporating measurement data, with normaliser.
bel_Xt_isOpen = n * p_Zt_senseOpen_Xt_isOpen * bel_prediction_Xt_isOpen
print "bel_X1_isOpen (normalised): ", bel_Xt_isOpen
bel_Xt_isClosed = n * p_Zt_senseOpen_Xt_isClosed * bel_prediction_Xt_isClosed
print "bel_X1_isClosed (normalised): ", bel_Xt_isClosed
# Save bel_xt to bel_xt_1.
bel_prediction_Xt_1_isOpen = bel_prediction_Xt_isOpen
bel_prediction_Xt_1_isClosed = bel_prediction_Xt_isClosed
bel_Xt_1_isOpen = bel_Xt_isOpen
bel_Xt_1_isClosed = bel_Xt_isClosed
# Initial value for normaliser,
n = 1.0
# Give the door a push ie iterate
print "\nPush the door .....\n"
# For hypothesis X2 = isOpen.
bel_prediction_Xt_isOpen = (p_Xt_isOpen_Ut_push_Xt_1_isOpen * bel_Xt_1_isOpen) + \
(p_Xt_isOpen_Ut_push_Xt_1_isClosed * bel_Xt_1_isClosed)
print "bel_prediction_X2_isOpen: ", bel_prediction_Xt_isOpen
# For hypothesis X2 = isClosed.
bel_prediction_Xt_isClosed = (p_Xt_isClosed_Ut_push_Xt_1_isOpen * bel_Xt_1_isOpen) + \
(p_Xt_isClosed_Ut_push_Xt_1_isClosed * bel_Xt_1_isClosed)
print "bel_prediction_X2_isClosed: ", bel_prediction_Xt_isClosed
# Incorporate the measurement data, with normaliser = 1.
bel_Xt_isOpen = n * p_Zt_senseOpen_Xt_isOpen * bel_prediction_Xt_isOpen
print "bel_X2_isOpen (n new): ", bel_Xt_isOpen
bel_Xt_isClosed = n * p_Zt_senseOpen_Xt_isClosed * bel_prediction_Xt_isClosed
print "bel_X2_isClosed (n new): ", bel_Xt_isClosed
# Calculate the value of the normaliser
n = bayes_function_normaliser(bel_Xt_isOpen, bel_Xt_isClosed)
print "normaliser Xt: ", n
# Recalculate incorporating measurement data, with normaliser.
bel_Xt_isOpen = n * p_Zt_senseOpen_Xt_isOpen * bel_prediction_Xt_isOpen
print "bel_X2_isOpen (normalised): ", bel_Xt_isOpen
bel_Xt_isClosed = n * p_Zt_senseOpen_Xt_isClosed * bel_prediction_Xt_isClosed
print "bel_X2_isClosed (normalised): ", bel_Xt_isClosed
if __name__ == "__main__":
main()
In [96]:
!python basic_bayes_filter_functions_example.py
Probability distribution data structure ideas:
In [ ]: