Kalman  Filters:  Examples  

CS  4758  Ashutosh  Saxena  

Methods  

Bayes  Filter  

Par@cle  Filter  

Unscented  Kalman  Filter  

Kalman  Filter  

Extended  Kalman  Filter  

PR2  localiza@on:  Door  Detector  

Example  2:    Simple  1D  Linear  System  with  Erroneous  Start  

Given:  F=G=H=1,  u=cos(t/5)  Ini@al  state  es@mate  =  20    Linear  system:  

xt+1 = xt + cos(t / 5)+wt

zt+1 = xt+1 + nt+1Propaga@on:   Update:  (no  change)  

xt+1 = xt + cos(t / 5)Pt+1− = Pt +Qt

Unknown  noise  parameters  

zt+1 = xt+1Kt+1 = P

−t+1(P

−t+1 + Rt+1)

−1

xt+1 = x−t+1 +Kt+1(zt+1 − x

−t+1)

Pt+1 = (I −Kt+1)P−t+1

State  Es@mate  

State  Es@ma@on  Error  vs  3σ  Region  of  Confidence  

Sensor  Residual  vs  3σ  Region  of  Confidence  

Kalman  Gain  and  State  Covariance  

Approxima@ng  Robot  Mo@on  Uncertainty  with  a  Gaussian  

Robot  Mo@ons  are  not  linear  

•  Extended  Kalman  Filter  

Linearized  Mo@on  Model    for  PR2  /  aerial  robot.  

X  

Y  

ω

x  y  

G  

v  

tt

t

tt

yVx

ωφ =

=

=

0From  a  robot-­‐centric  perspec@ve,  the  veloci@es  look  like  this:  

From  the  global  perspec@ve,  the  veloci@es  look  like  this:  

tt

ttt

ttt

VyVx

ωφ

φ

φ

=

=

=

sincos

The  discrete  @me  state  es@mate  (including  noise)  looks  like  this:  

tw

twVyy

twVxx

t

t

t

ttt

tVttt

tVttt

δωφφ

φδ

φδ

ω )(ˆˆ

ˆsin)(ˆˆ

ˆcos)(ˆˆ

1

1

1

++=

++=

++=

+

+

+ Problem!    We  don’t  know  linear  and  rota@onal  velocity  errors.    The  state  es@mate  will  rapidly  diverge  if  this  is  the  only  source  of  informa@on!  

Linearized  Mo@on  Model    for  PR2/aerial  robot  

111

111

111

~ˆ

~ˆ

~ˆ

+++

+++

+++

=−

=−

=−

ttt

ttt

ttt

yyyxxx

φφφ

The  indirect  Kalman  filter  derives  the  pose  equa@ons  from  the  es@mated  error  of  the  state:  

In  order  to  linearize  the  system,  the  following  small-­‐angle    assump@ons  are  made:  

φφ

φ~~sin

1~cos

≅

≅

Now,  we  have  to  compute  the  covariance  matrix  propaga@on  equa@ons.  

Linearized  Mo@on  Model    for  PR2/aerial  robot  

xt+1yt+1ϕ t+1

!

"

####

$

%

&&&&

=

1 0 −Vmδt sinϕ0 1 Vmδt cosϕ0 0 1

!

"

####

$

%

&&&&

xtytϕ t

!

"

####

$

%

&&&&

+

−δt cosϕR 0−δt sinϕR 0

0 −δt

!

"

####

$

%

&&&&

wVt

wωt

!

"

##

$

%

&&

Xt+1 = At Xt +GtWt

From  the  error-­‐state  propaga@on  equa@on,  we  can  obtain  the  State  propaga@on  and  noise  input  func@ons  F  and  G  :  

From  these  values,  we  can  easily  compute  the  standard  covariance  propaga@on  equa@on:  

Pt+1− = AtPtAt

T +GtQtGtT

Sensor  Model  for  a  Robot  with  a  Perfect  Map  

X  

Y  

x  y  

G  

L  

z  

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

+⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

=

+

+

+

+

φφ nnn

yx

z y

x

L

L

L

t

t

t

t

1

1

1

1

From  the  robot,  the  measurement  looks  like  this:  

From  a  global  perspec@ve,  the  measurement  looks  like:  

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

+⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

−

−

−

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡ −

=

+

+

+

++

++

+

+

+

+

φφφ

φφ

φφ

nnn

yyxx

z y

x

tL

tL

tL

tt

tt

t

t

t

t

1

1

1

11

11

1

1

1

1

1000cossin0sincos

The  measurement  equa@on  is  nonlinear  and  must  also  be  linearized!  

Sensor  Model  for  a  Robot  with  a  Perfect  Map  

Now,  we  have  to  compute  the  linearized  sensor  func@on.    Once  again,  we  make  use  of  the  indirect  Kalman  filter  where  the  error  in  the  reading  must  be  es@mated.  

In  order  to  linearize  the  system,  the  following  small-­‐angle    assump@ons  are  made:  

φφ

φ~~sin

1~cos

≅

≅

The  final  expression  for  the  error  in  the  sensor  reading  is:  

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

+

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

−

−−−−−

−+−−−−

=⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

+

+

+

+++++

++++ +

+

+

+

φφ

φφφφ

φφφφ

φ nnn

yx

yyxxyyxx

yx

y

x

t

t

t

tLttLttt

tLttLtt

L

L

L t

t

t

t

1

1

1

11111

1111

~~~

100)ˆ(ˆsin)ˆ(ˆcosˆcosˆsin)ˆ(ˆcos)ˆ(ˆsinˆsinˆcos

~~~

1

1

1

1

Upda@ng  the  State  Vector  

Propaga@on  only   Propaga@on  and  update