Scribd
Upload
66 views26 pages

Bayes Filter Reminder: Bel (X P (X Bel (X

The document provides an overview of the Bayes filter and Kalman filter algorithms. 1. The Bayes filter uses probability distributions to represent beliefs about state over time, updating the belief based on new sensor measurements via Bayes' rule. 2. The Kalman filter is a specific implementation of the Bayes filter for linear systems with Gaussian noise, using efficient matrix calculations to perform the prediction and correction steps of the Bayes filter. 3. The Kalman filter represents beliefs as Gaussian distributions and performs an iterative prediction-correction cycle, using prior state estimates to predict future states and sensor measurements to correct predictions.

Uploaded by

Taimoor Shakeel
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as PDF, TXT or read online on Scribd
66 views26 pages

Bayes Filter Reminder: Bel (X P (X Bel (X

The document provides an overview of the Bayes filter and Kalman filter algorithms. 1. The Bayes filter uses probability distributions to represent beliefs about state over time, updating the belief based on new sensor measurements via Bayes' rule. 2. The Kalman filter is a specific implementation of the Bayes filter for linear systems with Gaussian noise, using efficient matrix calculations to perform the prediction and correction steps of the Bayes filter. 3. The Kalman filter represents beliefs as Gaussian distributions and performs an iterative prediction-correction cycle, using prior state estimates to predict future states and sensor measurements to correct predictions.

Uploaded by

Taimoor Shakeel
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as PDF, TXT or read online on Scribd
You are on page 1/ 26

!

Prediction

! Correction
Bayes Filter Reminder
bel(x
t
) = p(x
t
| u
t
, x
t!1
)
"
bel(x
t!1
) dx
t!1
) ( ) | ( ) (
t t t t
x bel x z p x bel ! =
bel(x
t
) =! p(z
t
| x
t
) p(x
t
| u
t
, x
t!1
)
"
bel(x
t!1
) dx
t!1
3
Bayes Filter Reminder
1. Algorithm Bayes_filter( Bel(x),d ):
2. !=0
3. If d is a perceptual data item z then
4. For all x do
5.
6.
7. For all x do
8.
9. Else if d is an action data item u then
10. For all x do
11.
12. Return Bel(x)
) ( ) | ( ) ( ' x Bel x z P x Bel =
) ( ' x Bel + =! !
) ( ' ) ( '
1
x Bel x Bel
!
="
' ) ' ( ) ' , | ( ) ( ' dx x Bel x u x P x Bel
!
=
1 1 1
) ( ) , | ( ) | ( ) (
! ! !
"
=
t t t t t t t t
dx x Bel x u x P x z P x Bel #
Kalman Filter
! Bayes filter with Gaussians
! Developed in the late 1950's
! Most relevant Bayes filter variant in practice
! Applications range from economics, weather
forecasting, satellite navigation to robotics
and many more.
! The Kalman filter "algorithm" is
a couple of matrix multiplications!
4
Gaussians
2
2
) (
2
1
2
2
1
) (
: ) , ( ~ ) (
!

! "
!
#
#
=
x
e x p
N x p
-" "

Univariate
p(x) ~ N(, !) :
p(x) =
1
(2!)
d/2
!
1/2
e
"
1
2
(x")
t
!
"1
(x")

Multivariate
Gaussians
1D
2D
3D
) , ( ~
) , ( ~
2 2
2
!
!
a b a N Y
b aX Y
N X
+ "
#
$
%
+ =
Properties of Gaussians
!
!
"
#
$
$
%
&
+ +
+
+
' (
)
*
+
, , 2
2
2
1
2
2
2
2
1
2
1
1
2
2
2
1
2
2
2 1
2
2 2 2
2
1 1 1
1
, ~ ) ( ) (
) , ( ~
) , ( ~
- -

- -
-

- -
-
-
-
N X p X p
N X
N X
! Univariate case
! Multivariate case

(where division "" denotes matrix inversion)
! We stay Gaussian as long as we start with
Gaussians and perform only linear
transformations
) , ( ~
) , ( ~
T
A A B A N Y
B AX Y
N X
! + "
#
$
%
+ =
!

Properties of Gaussians
!
!
"
#
$
$
%
&
' + ' ' + '
'
+
' + '
'
( )
*
+
,
'
'
- - 1
2
1
1
2
2 1
1
1
2 1
2
2 1
2 2 2
1 1 1
1
, ~ ) ( ) (
) , ( ~
) , ( ~

N X p X p
N X
N X
9
Discrete Kalman Filter
t t t t t t
u B x A x ! + + =
"1
t t t t
x C z ! + =
Estimates the state x of a discrete-time
controlled process that is governed by the
linear stochastic difference equation
with a measurement
10
Components of a Kalman Filter
t
!
Matrix (nxn) that describes how the state
evolves from t-1 to t without controls or
noise.
t
A
Matrix (nxl) that describes how the control u
t

changes the state from t-1 to t.
t
B
Matrix (kxn) that describes how to map the
state x
t
to an observation z
t
.
t
C
t
!
Random variables representing the process
and measurement noise that are assumed to
be independent and normally distributed
with covariance Q
t
and R
t
respectively.
! Prediction
! Correction
Bayes Filter Reminder
1 1 1
) ( ) , | ( ) (
! ! !
"
=
t t t t t t
dx x bel x u x p x bel
) ( ) | ( ) (
t t t t
x bel x z p x bel ! =
12
Kalman Filter Updates in 1D
prediction measurement
correction
It's a weighted mean!
13
Kalman Filter Updates in 1D
!
bel(x
t
) =

t
=
t
+ K
t
(z
t
"C
t

t
)
#
t
= (I "K
t
C
t
)#t
$
%
&
with K
t
= #tC
t
T
(C
t
#tC
t
T
+ R
t
)
"1
bel(x
t
) =

t
=
t
+ K
t
(z
t
!
t
)
!
t
2
= (1! K
t
)!
t
2
"
#
$
%
$
with K
t
=
!
t
2
!
t
2
+!
obs,t
2
How to get the blue one?
Kalman correction step
Kalman Filter Updates in 1D
!
bel(x
t
) =

t
= A
t

t"1
+ B
t
u
t
#t = A
t
#
t"1
A
t
T
+ Q
t
$
%
&
!
bel(x
t
) =

t
= a
t

t"1
+ b
t
u
t
#
t
2
= a
t
2
#
t
2
+#
act,t
2
$
%
&
How to get the
magenta one?
State prediction step
Kalman Filter Updates
prediction
correction
measurement
( )
0 0 0 0
, ; ) ( ! = x N x bel
Linear Gaussian Systems: Initialization
Initial belief is normally distributed:
Dynamics are linear functions of the state
and the control plus additive noise:
t t t t t t
u B x A x ! + + =
"1
Linear Gaussian Systems: Dynamics
!
p(x
t
| u
t
, x
t"1
) = N x
t
; A
t
x
t"1
+ B
t
u
t
,Q
t
( )
!
bel(x
t
) = p(x
t
| u
t
, x
t"1
)
#
bel(x
t"1
) dx
t"1
$ $
~ N x
t
; A
t
x
t"1
+ B
t
u
t
,Q
t
( )
~ N x
t"1
;
t"1
, %
t"1
( )
Linear Gaussian Systems: Dynamics
!
bel(x
t
) = p(x
t
| u
t
, x
t"1
)
#
bel(x
t"1
) dx
t"1
$ $
~ N x
t
; A
t
x
t"1
+ B
t
u
t
,Q
t
( )
~ N x
t"1
;
t"1
, %
t"1
( )
$
bel(x
t
) =& exp "
1
2
(x
t
" A
t
x
t"1
" B
t
u
t
)
T
Q
t
"1
(x
t
" A
t
x
t"1
" B
t
u
t
)
'
(
)
*
+
,
#
exp "
1
2
(x
t"1
"
t"1
)
T
%
t"1
"1
(x
t"1
"
t"1
)
'
(
)
*
+
,
dx
t"1
bel(x
t
) =

t
= A
t

t"1
+ B
t
u
t
%t = A
t
%
t"1
A
t
T
+ Q
t
'
(
)
Observations is a linear function of the state
plus additive noise:
t t t t
x C z ! + =
Linear Gaussian Systems: Observations
!
p(z
t
| x
t
) = N z
t
;C
t
x
t
, R
t
( )
!
bel(x
t
) = " p(z
t
| x
t
) bel(x
t
)
# #
~ N z
t
;C
t
x
t
, R
t
( )
~ N x
t
;
t
, $
t
( )
Linear Gaussian Systems: Observations
!
bel(x
t
) = " p(z
t
| x
t
) bel(x
t
)
# #
~ N z
t
;C
t
x
t
, R
t
( )
~ N x
t
;
t
, $t
( )
#
bel(x
t
) =" exp %
1
2
(z
t
%C
t
x
t
)
T
R
t
%1
(z
t
%C
t
x
t
)
&
'
(
)
*
+
exp %
1
2
(x
t
%
t
)
T
$
t
%1
(x
t
%
t
)
&
'
(
)
*
+
bel(x
t
) =

t
=
t
+ K
t
(z
t
%C
t

t
)
$
t
= (I %K
t
C
t
)$t
&
'
(
with K
t
= $tC
t
T
(C
t
$tC
t
T
+ R
t
)
%1
Kalman Filter Algorithm
1. Algorithm Kalman_filter(
t-1
,

#
t-1
, u
t
, z
t
):
2. Prediction:
3.
4.

5. Correction:
6.
7.
8.
9. Return
t
,

#
t

!

t
= A
t

t"1
+ B
t
u
t
!
"t = A
t
"
t#1
A
t
T
+ Q
t
!
K
t
= "tC
t
T
(C
t
"tC
t
T
+ R
t
)
#1
!

t
=
t
+ K
t
(z
t
"C
t

t
)
!
"
t
= (I #K
t
C
t
)"t
Kalman Filter Algorithm
Kalman Filter Algorithm
! Prediction
! Observation
! Matching
! Correction
24
The Prediction-Correction-Cycle
!
bel(x
t
) =

t
= A
t

t"1
+ B
t
u
t
#t = A
t
#
t"1
A
t
T
+ Q
t
$
%
&
!
bel(x
t
) =

t
= a
t

t"1
+ b
t
u
t
#
t
2
= a
t
2
#
t
2
+#
act,t
2
$
%
&
Prediction
25
The Prediction-Correction-Cycle
!
bel(x
t
) =

t
=
t
+ K
t
(z
t
"C
t

t
)
#
t
= (I "K
t
C
t
)#t
$
%
&
,K
t
= #tC
t
T
(C
t
#tC
t
T
+ R
t
)
"1
!
bel(x
t
) =

t
=
t
+ K
t
(z
t
"
t
)
#
t
2
= (1"K
t
)#
t
2
$
%
&
, K
t
=
#
t
2
#
t
2
+#
obs,t
2
Correction
26
The Prediction-Correction-Cycle
!
bel(x
t
) =

t
=
t
+ K
t
(z
t
"C
t

t
)
#
t
= (I "K
t
C
t
)#t
$
%
&
,K
t
= #tC
t
T
(C
t
#tC
t
T
+ R
t
)
"1
!
bel(x
t
) =

t
=
t
+ K
t
(z
t
"
t
)
#
t
2
= (1"K
t
)#
t
2
$
%
&
, K
t
=
#
t
2
#
t
2
+#
obs,t
2
!
bel(x
t
) =

t
= A
t

t"1
+ B
t
u
t
#t = A
t
#
t"1
A
t
T
+ Q
t
$
%
&
!
bel(x
t
) =

t
= a
t

t"1
+ b
t
u
t
#
t
2
= a
t
2
#
t
2
+#
act,t
2
$
%
&
Correction
Prediction
Kalman Filter Summary
! Only two parameters describe belief about
the state of the system
! Highly efficient: Polynomial in the
measurement dimensionality k and
state dimensionality n:
O(k
2.376
+ n
2
)
! Optimal for linear Gaussian systems!
! However: Most robotics systems are
nonlinear!
! Can only model unimodal beliefs

You might also like