exact particle flow for nonlinear filters
TRANSCRIPT
exact particle flow
for
nonlinear filters
Fred Daum & Jim Huang
14 February 2012
Copyright © 2012 Raytheon Company. All rights reserved.
Customer Success Is Our Mission is a trademark of Raytheon Company.
1
new nonlinear filter: particle flow
new particle flow filter standard particle filters
roughly 6 to 8 orders of magnitude faster
than standard particle filters
suffers from curse of dimensionality
3 to 4 orders of magnitude faster
computation per particle
suffers from “particle degeneracy”
3 to 4 orders of magnitude fewer particles
required to achieve optimal accuracy
requires millions or billions of particles
for high dimensional problems
Bayes’ rule is computed using particle
flow (like physics)
Bayes’ rule is computed using a pointwise
multiplication of two functions
no proposal density depends on proposal density (e.g.,
Gaussian from EKF or UKF)
no resampling of particles resampling is needed to repair the damage
done by Bayes’ rule
embarrassingly parallelizable suffers from bottleneck due to resampling
computes log of unnormalized density suffers from severe numerical problems
due to computation of normalized density2
discrete time nonlinear filter problem
k
k
k
kkkk
z
w
tx
wttxFtx
tat timet vector measuremen
tat time vector noise process
tat time vectorstate)(
),),(()(
k
k
k
1
=
=
=
=+estimate x
given noisy
measurements
{ }k
k
kk
k
kkkk
k
zzz
Z
Ztxp
v
vttxHz
,...,,Z
tsmeasuremen all ofset
given Z tat time x ofdensity y probabilit),(
tat time vector noiset measuremen
),),((
21k
kk
k
k
=
=
=
=
=
3
nonlinear filtering problem
x = d-dimensional state vector
t = time
w(t) = process noise vector
dx = F(x, t)dt + G(x, t) dwcontinuous
time
dynamicsdiscrete
time
measurements
z(tk) = m-dimensional measurement vector
tk = time of kth measurement
vk = measurement noise vector
p(x, t | Zk) = probability density of x at time t given Zk
Zk = set of all measurements up to & including time tk
z(tk) = H(x(tk), tk, vk)
measurements
4
curse of dimensionality for
classic particle filter
optimal
accuracy:
r = 1.0
5
prediction of
conditional
probability
density from
tk-1 to tk
nonlinear filter
measurements
:rule Bayes'
solution of
Fokker-Planck
equation
),(),(),(
:rule Bayes'
1 kkkkkk txzpZtxpZtxp −=
6
particle degeneracy*
likelihood of
measurementprior
density
particles to represent the prior
7
*Daum & Huang, “Particle degeneracy: root cause & solution,” SPIE Proceedings 2011.
induced flow of particles
for Bayes’ rule
pdf pdfflow of density
prior posterior
)(log)(log),(log xhxgxp λλ +=
particles particles
flow of particles
sample from
density
sample from
density
λ=0 λ=1
)(log)(log),(log xhxgxp λλ +=
),( λλ
xfd
dx=
8
derivation of PDE for exact particle flow:
∂
∂−=
∂
∂
∂
∂−=
∂
∂
=
λλ
λ
λ
λ
λλ
x
pfTrxp
xp
x
pfTr
xp
xfd
dx
)(),(
),(log
)(),(
),( Fokker-Planck
equation with zero
diffusion
definition
of p(x,λ)
−−=
=
−=
∂
∂−
−+=
∂∂
λ
λλη
η
λλ
λ
λλλ
λ
d
Kdxhxp
pfdiv
pfdivxpK
xh
Kxhxgxp
x
)(log)(log),(
)(
)(),()(log
)(log
)(log)(log)(log),(log
9
definition
of η
PDE for
f given p
linear first order highly underdetermined PDE:
d
d
x
q
x
q
x
q
d
Kdxhxpx
xxqdiv
∂
∂++
∂
∂+
∂
∂=
−−=
=
...
)(log)(log),(),(
),()),((
2
2
1
1η
λ
λλλη
ληλ
like Gauss’ divergence
law in electromagneticsdxxx ∂∂∂ 21
function values are
only known at
random points in d-
dimensional space
10
define: q = pf
f = unknown function
p & η = known at random points
We want dx/dλ = f(x,λ)
to be a stable dynamical
system.
law in electromagnetics
method to solve PDE how to pick unique solution computation
1. generalized inverse of linear differential
operator
minimum norm* Coulomb’s law or fast Poisson solver
(e.g., fast Ewald method)
2. Poisson’s equation assume irrotational flow (i.e., velocity
vector is the gradient of a potential)
Coulomb’s law or fast Poisson solver
(e.g., fast Ewald method)
3. generalized inverse of gradient of log-
homotopy
assume incompressible flow (i.e.,
divergence free flow)
fast (but need to compute the gradient
from random points)
4. most general solution most robustly stable filter or random
pick, etc.
fast (but need to compute the gradient
from random points)
5. separation of variables (Gaussian) pick solution of specific form
(polynomial)
extremely fast (formula)
6. separation of variables
(exponential family)
pick solution of specific form (finite basis
functions)
very fast (formula)
7. variational formulation (Gauss & Hertz) convex function minimization ODEs
8. optimal control formulation convex functional minimization (e.g.,
least action like Monge-Kantorovich)
HJB PDE or Euler-Lagrange PDEs
(or maybe ODES for nice problem)
9. direct integration (of first order linear PDE in
divergence form)
choice of d-1 arbitrary functions one-dimensional integral
10. generalized method of characteristics more conditions (e.g., curl = given or
compute gradient to get d equations)
ODEs from chain rule
11. another homotopy (inspired by Gromov’s
h-principle) like Feynman’s QED perturbation
initial condition of ODE &
uniqueness of sol. to ODE
ODEs from homotopy
12. finite dimensional parametric flow
(e.g., f = Ax+b with A & b parameters)
non-singular matrix to invert d³ or d^6 (least squares for d or d²
parameters, i.e., A & b)
13. Fourier transform of PDE (divergence form
of linear PDE has constant coefficients)
minimum norm* or most stable flow Coulomb’s law or inverse Fourier
transform, etc.
11
Fred Daum, Jim Huang & Arjang Noushin, “exact particle flow for nonlinear filters,” Proceedings of SPIE Conference, Orlando Florida,
April 2010.
Fred Daum & Jim Huang, “particle degeneracy: root cause & solution,” Proceedings of SPIE Conference, Orlando Florida, April 2011.
Fred Daum & Jim Huang “numerical experiments for nonlinear filters with exact particle flow induced by log-homotopy,” Proceedings of SPIE Conference, Orlando Florida, April 2010.Conference, Orlando Florida, April 2010.
Fred Daum & Jim Huang, “exact particle flow for nonlinear filters: seventeen dubious solutions to a linear first order underdetermined PDE,” Proceedings of IEEE Conference, Asilomar California, November 2010.
Fred Daum, Jim Huang & Arjang Noushin, “Coulomb’s law particle flow for nonlinear filters,” Proceedings of SPIE Conference, San Diego California, August 2011.
12
exact particle flow for Gaussian densities:
fx
pfdivh
xfd
dx
∂
∂−−=
=
:exactly ffor solvecan weGaussian,h & gfor
log)()log(
),( λλ
[ ]
( ) ( )[ ]xAzRPHAIAIb
HRHPHPHA
bAxf
T
TT
+++=
+−=
+=
−
−
1
1
2
2
1
:exactly ffor solvecan weGaussian,h & gfor
λλ
λ
13
automatically stable
under very mild
conditions &
extremely fast
incompressible particle flow
∂
∂
∂
−=
gradient zero-nonfor
),(log
))(log(dx
2
x
xp
xh
T
λ
λ
∂
∂−
=
otherwise 0
gradient zero-nonfor ),(log
))(log(
d
dx 2
x
xpxh
λλ
14
irrotational particle flow:
∫
∫ −
∂
=
≥−
−=
=
∂
∂
∂
∂==
d
T
dyc
dyyx
cyxV
xx
xVTr
xpx
xVxf
d
dx
)logK(
-logh(y))p(y,)V(x,
3 dfor ),(),(
),(),(
),(/),(
),(
2
2
2
λλλ
ληλ
ληλ
λλ
λλ Poisson’s
equation
∑
∫
∫
∈
−
−
−−
∂
∂−≈
∂
∂
−
−−
∂
∂−=
∂
∂
−
−−
∂
∂−=
∂
∂
− ∂
=
iSjd
ji
T
ji
ji
d
T
d
T
d
xx
xxdcKxh
Mx
xV
yx
yxdcKyhE
x
xV
dyyx
yxdcKyhyp
x
xV
dyyx
))(2()
)(log)((log
1),(
))(2()
)(log)((log
),(
))(2()(log)(log),(
),(
-logh(y))p(y,)V(x,2
λ
λλ
λ
λλ
λ
λλ
λ
λλλ
15
like
Coulomb’s
law
0 5 10 15 20 25 30 35 400
100
200
300N = 1000
An
gle
Err
or
(de
g)
EKF
PF
0 5 10 15 20 25 30 35 400
5
10
15
Time
An
gle
Ra
te E
rro
r (d
eg
/se
c)
EKF
PF
16
100
200
300
400Time = 1, Frame 1
An
gle
Ra
te (
de
g/s
ec)
-200 -150 -100 -50 0 50 100 150 200-300
-200
-100
0
Angle (deg)
An
gle
Ra
te (
de
g/s
ec)
17
100
200
300
400Time = 1, Frame 2
An
gle
Ra
te (
de
g/s
ec)
-200 -150 -100 -50 0 50 100 150 200-300
-200
-100
0
Angle (deg)
An
gle
Ra
te (
de
g/s
ec)
18
100
200
300
400Time = 1, Frame 3
An
gle
Ra
te (
de
g/s
ec)
-200 -150 -100 -50 0 50 100 150 200-300
-200
-100
0
Angle (deg)
An
gle
Ra
te (
de
g/s
ec)
19
100
200
300
400Time = 1, Frame 4
An
gle
Ra
te (
de
g/s
ec)
-200 -150 -100 -50 0 50 100 150 200-300
-200
-100
0
Angle (deg)
An
gle
Ra
te (
de
g/s
ec)
20
100
200
300
400Time = 1, Frame 5
An
gle
Ra
te (
de
g/s
ec)
-200 -150 -100 -50 0 50 100 150 200-300
-200
-100
0
Angle (deg)
An
gle
Ra
te (
de
g/s
ec)
21
100
200
300
400Time = 1, Frame 6
An
gle
Ra
te (
de
g/s
ec)
-200 -150 -100 -50 0 50 100 150 200-300
-200
-100
0
Angle (deg)
An
gle
Ra
te (
de
g/s
ec)
22
100
200
300
400Time = 1, Frame 7
An
gle
Ra
te (
de
g/s
ec)
-200 -150 -100 -50 0 50 100 150 200-300
-200
-100
0
Angle (deg)
An
gle
Ra
te (
de
g/s
ec)
23
100
200
300
400Time = 1, Frame 8
An
gle
Ra
te (
de
g/s
ec)
-200 -150 -100 -50 0 50 100 150 200-300
-200
-100
0
Angle (deg)
An
gle
Ra
te (
de
g/s
ec)
24
100
200
300
400Time = 1, Frame 9
An
gle
Ra
te (
de
g/s
ec)
-200 -150 -100 -50 0 50 100 150 200-300
-200
-100
0
Angle (deg)
An
gle
Ra
te (
de
g/s
ec)
25
100
200
300
400Time = 1, Frame 10
An
gle
Ra
te (
de
g/s
ec)
-200 -150 -100 -50 0 50 100 150 200-300
-200
-100
0
Angle (deg)
An
gle
Ra
te (
de
g/s
ec)
26
103
104
d = 12, ny = 3, y = x2, SNR = 20dB
Dim
en
sio
nle
ss E
rro
r
quadratic measurement
nonlinearity
exact flow
beats EKF by
102
103
104
105
102
Dim
en
sio
nle
ss E
rro
r
Number of Particles
EKF
PF
27
beats EKF by
2 orders of
magnitude
0
0.2
0.4
0.6
0.8
1x 10
5 Time = 1, Magenta: truth, Green: PF estimate, Black: KF
-1 -0.5 0 0.5 1 1.5
x 105
-1
-0.8
-0.6
-0.4
-0.2
28
0
0.2
0.4
0.6
0.8
1x 10
5 Time = 1, Magenta: truth, Green: PF estimate, Black: KF
-1 -0.5 0 0.5 1 1.5
x 105
-1
-0.8
-0.6
-0.4
-0.2
29
0
0.2
0.4
0.6
0.8
1x 10
5 Time = 1, Magenta: truth, Green: PF estimate, Black: KF
-1 -0.5 0 0.5 1 1.5
x 105
-1
-0.8
-0.6
-0.4
-0.2
30
0
0.2
0.4
0.6
0.8
1x 10
5 Time = 1, Magenta: truth, Green: PF estimate, Black: KF
-1 -0.5 0 0.5 1 1.5
x 105
-1
-0.8
-0.6
-0.4
-0.2
31
0
0.2
0.4
0.6
0.8
1x 10
5 Time = 1, Magenta: truth, Green: PF estimate, Black: KF
-1 -0.5 0 0.5 1 1.5
x 105
-1
-0.8
-0.6
-0.4
-0.2
32
0
0.2
0.4
0.6
0.8
1x 10
5 Time = 1, Magenta: truth, Green: PF estimate, Black: KF
-1 -0.5 0 0.5 1 1.5
x 105
-1
-0.8
-0.6
-0.4
-0.2
33
0
0.2
0.4
0.6
0.8
1x 10
5 Time = 1, Magenta: truth, Green: PF estimate, Black: KF
-1 -0.5 0 0.5 1 1.5
x 105
-1
-0.8
-0.6
-0.4
-0.2
34
0
0.2
0.4
0.6
0.8
1x 10
5 Time = 1, Magenta: truth, Green: PF estimate, Black: KF
-1 -0.5 0 0.5 1 1.5
x 105
-1
-0.8
-0.6
-0.4
-0.2
35
0
0.2
0.4
0.6
0.8
1x 10
5 Time = 1, Magenta: truth, Green: PF estimate, Black: KF
-1 -0.5 0 0.5 1 1.5
x 105
-1
-0.8
-0.6
-0.4
-0.2
36
0
0.2
0.4
0.6
0.8
1x 10
5 Time = 1, Magenta: truth, Green: PF estimate, Black: KF
-1 -0.5 0 0.5 1 1.5
x 105
-1
-0.8
-0.6
-0.4
-0.2
37
105
106
107
d = 12, ny = 3, y = x
3, SNR = 20dB
Dim
en
sio
nle
ss
Err
or exact flow
beats EKF by
2 orders of
magnitude
cubic measurement nonlinearity
102
103
104
105
103
104
Dim
en
sio
nle
ss
Err
or
Number of Particles
EKF
PF
38
magnitude
nonlinear filter performance (accuracy wrt
optimal & computational complexity)
DIMENSIONprocess noise
initial uncertainty
of state vector
measurementexploit
smoothness
sparseness
measurement
noise
stability
& mixing
of dynamics
multi-modal
nonlinearityill-conditioning
quality of
proposal density
concentration
of measure
exploit
structure (e.g.
exact filters)
39
variation in initial uncertainty of x
1010
1015
1020
Dim
en
sio
nle
ss E
rro
r
N = 1000, Stable, d = 10, Quadratic
Huge Initial Uncertainty
Large Initial Uncertainty
Medium Initial Uncertainty
Small Initial Uncertainty
, λλλλ = 0.6
25 Monte Carlo Trials
0 5 10 15 20 25 3010
-5
100
105
Time
Dim
en
sio
nle
ss E
rro
r
40
variation in eigenvalues of the plant (λ)
106
108
1010
1012
Dim
en
sio
nle
ss E
rro
r
N = 1000, d = 10, Cubic
λλλλ = 0.1
0.5
1.0
1.1
1.2
25 Monte Carlo Trials
0 5 10 15 20 25 3010
-2
100
102
104
Time
Dim
en
sio
nle
ss E
rro
r
41
variation in dimension of x
106
108
1010
1012
Dim
en
sio
nle
ss E
rro
r
N = 1000, λλλλ = 1.0, Cubic
Dimension = 5
10
15
20
25 Monte Carlo Trials
0 5 10 15 20 25 3010
-2
100
102
104
Time
Dim
en
sio
nle
ss E
rro
r
42
exact flow filter is many orders of magnitude faster per
particle than standard particle filters
- - - - -
bootstrap
EKF proposal
incomp flow
exact flow
3
104
105
106
107
Med
ian
Co
mp
uta
tio
n T
ime
fo
r 30 U
pd
ate
s (
sec)
d = 30
d = 20
d = 10
d = 5 bootstrap
particle filter
EKF proposal
* Intel Corel 2 CPU, 1.86GHz, 0.98GB of RAM, PC-MATLAB version 7.7
25 Monte Carlo trials10
210
310
410
510
-1
100
101
102
103
Number of Particles
Med
ian
Co
mp
uta
tio
n T
ime
fo
r 30 U
pd
ate
s (
sec)
43
exact flow
EKF proposal
incompressible
flow
particle flow filter is many orders of magnitude faster
real time computation (for the same or better
estimation accuracy)
3 or 4 orders of
3 or 4 orders of magnitude faster
per particle
avoids bottleneck in
many orders of
magnitude faster
3 or 4 orders of magnitude
fewer particles
bottleneck in parallel
processing due to resampling
44
new nonlinear filter: particle flow
new particle flow filter standard particle filters
roughly 6 to 8 orders of magnitude faster
than standard particle filters
suffers from curse of dimensionality
3 to 4 orders of magnitude faster
computation per particle
suffers from “particle degeneracy”
3 to 4 orders of magnitude fewer particles
required to achieve optimal accuracy
requires millions or billions of particles
for high dimensional problems
Bayes’ rule is computed using particle
flow (like physics)
Bayes’ rule is computed using a pointwise
multiplication of two functions
no proposal density depends on proposal density (e.g.,
Gaussian from EKF or UKF)
no resampling of particles resampling is needed to repair the damage
done by Bayes’ rule
embarrassingly parallelizable suffers from bottleneck due to resampling
computes log of unnormalized density suffers from severe numerical problems
due to computation of normalized density45
MOVIES of
particle flowparticle flow
46
exact particle flow for Gaussian densities:
fx
pfdivh
xfd
dx
∂
∂−−=
=
:exactly ffor solvecan weGaussian,h & gfor
log)()log(
),( λλ
[ ]
( ) ( )[ ]xAzRPHAIAIb
HRHPHPHA
bAxf
T
TT
+++=
+−=
+=
−
−
1
1
2
2
1
:exactly ffor solvecan weGaussian,h & gfor
λλ
λ
47
automatically stable
under very mild
conditions &
extremely fast
0
0.2
0.4
0.6
0.8Inside = 5 percent, Magenta: truth, Green: PF estimate, Black: KF
Ax+
b
-1 -0.8 -0.6 -0.4 -0.2 0 0.2 0.4 0.6-0.8
-0.6
-0.4
-0.2
Ax+
b
48
0
0.2
0.4
0.6
0.8Inside = 10.6 percent, Magenta: truth, Green: PF estimate, Black: KF
Ax+
b
-1 -0.8 -0.6 -0.4 -0.2 0 0.2 0.4 0.6-0.8
-0.6
-0.4
-0.2
Ax+
b
49
0
0.2
0.4
0.6
0.8Inside = 13.8 percent, Magenta: truth, Green: PF estimate, Black: KF
Ax+
b
-1 -0.8 -0.6 -0.4 -0.2 0 0.2 0.4 0.6-0.8
-0.6
-0.4
-0.2
Ax+
b
50
0
0.2
0.4
0.6
0.8Inside = 16.6 percent, Magenta: truth, Green: PF estimate, Black: KF
Ax+
b
-1 -0.8 -0.6 -0.4 -0.2 0 0.2 0.4 0.6-0.8
-0.6
-0.4
-0.2
Ax+
b
51
0
0.2
0.4
0.6
0.8Inside = 17.6 percent, Magenta: truth, Green: PF estimate, Black: KF
Ax+
b
-1 -0.8 -0.6 -0.4 -0.2 0 0.2 0.4 0.6-0.8
-0.6
-0.4
-0.2
Ax+
b
52
0
0.2
0.4
0.6
0.8Inside = 21 percent, Magenta: truth, Green: PF estimate, Black: KF
Ax+
b
-1 -0.8 -0.6 -0.4 -0.2 0 0.2 0.4 0.6-0.8
-0.6
-0.4
-0.2
Ax+
b
53
0
0.2
0.4
0.6
0.8Inside = 21 percent, Magenta: truth, Green: PF estimate, Black: KF
Ax+
b
-1 -0.8 -0.6 -0.4 -0.2 0 0.2 0.4 0.6-0.8
-0.6
-0.4
-0.2
Ax+
b
54
0
0.2
0.4
0.6
0.8Inside = 23.8 percent, Magenta: truth, Green: PF estimate, Black: KF
Ax+
b
-1 -0.8 -0.6 -0.4 -0.2 0 0.2 0.4 0.6-0.8
-0.6
-0.4
-0.2
Ax+
b
55
0
0.2
0.4
0.6
0.8Inside = 26.4 percent, Magenta: truth, Green: PF estimate, Black: KF
Ax+
b
-1 -0.8 -0.6 -0.4 -0.2 0 0.2 0.4 0.6-0.8
-0.6
-0.4
-0.2
Ax+
b
56
0
0.2
0.4
0.6
0.8Inside = 27.6 percent, Magenta: truth, Green: PF estimate, Black: KF
Ax+
b
-1 -0.8 -0.6 -0.4 -0.2 0 0.2 0.4 0.6-0.8
-0.6
-0.4
-0.2
Ax+
b
57
incompressible particle flow
2
log)log(
dx
∂
∂
∂−
= x
ph
T
gradient zerofor 0
2logd
=
∂
∂ ∂=
λ
λ
d
dx
x
px
58
0
0.2
0.4
0.6
0.8Inside = 6 percent, Magenta: truth, Green: PF estimate, Black: KF
Hessia
n
-1 -0.8 -0.6 -0.4 -0.2 0 0.2 0.4 0.6-0.8
-0.6
-0.4
-0.2
Hessia
n
59
0
0.2
0.4
0.6
0.8Inside = 5.8 percent, Magenta: truth, Green: PF estimate, Black: KF
Hessia
n
-1 -0.8 -0.6 -0.4 -0.2 0 0.2 0.4 0.6-0.8
-0.6
-0.4
-0.2
Hessia
n
60
0
0.2
0.4
0.6
0.8Inside = 8.2 percent, Magenta: truth, Green: PF estimate, Black: KF
Hessia
n
-1 -0.8 -0.6 -0.4 -0.2 0 0.2 0.4 0.6-0.8
-0.6
-0.4
-0.2
Hessia
n
61
0
0.2
0.4
0.6
0.8Inside = 9.2 percent, Magenta: truth, Green: PF estimate, Black: KF
Hessia
n
-1 -0.8 -0.6 -0.4 -0.2 0 0.2 0.4 0.6-0.8
-0.6
-0.4
-0.2
Hessia
n
62
0
0.2
0.4
0.6
0.8Inside = 11.2 percent, Magenta: truth, Green: PF estimate, Black: KF
Hessia
n
-1 -0.8 -0.6 -0.4 -0.2 0 0.2 0.4 0.6-0.8
-0.6
-0.4
-0.2
Hessia
n
63
0
0.2
0.4
0.6
0.8Inside = 11.8 percent, Magenta: truth, Green: PF estimate, Black: KF
Hessia
n
-1 -0.8 -0.6 -0.4 -0.2 0 0.2 0.4 0.6-0.8
-0.6
-0.4
-0.2
Hessia
n
64
0
0.2
0.4
0.6
0.8Inside = 12.8 percent, Magenta: truth, Green: PF estimate, Black: KF
Hessia
n
-1 -0.8 -0.6 -0.4 -0.2 0 0.2 0.4 0.6-0.8
-0.6
-0.4
-0.2
Hessia
n
65
0
0.2
0.4
0.6
0.8Inside = 12 percent, Magenta: truth, Green: PF estimate, Black: KF
Hessia
n
-1 -0.8 -0.6 -0.4 -0.2 0 0.2 0.4 0.6-0.8
-0.6
-0.4
-0.2
Hessia
n
66
0
0.2
0.4
0.6
0.8Inside = 11.6 percent, Magenta: truth, Green: PF estimate, Black: KF
Hessia
n
-1 -0.8 -0.6 -0.4 -0.2 0 0.2 0.4 0.6-0.8
-0.6
-0.4
-0.2
Hessia
n
67