!

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