Finite-Set Statistics and SLAM
Ronald Mahler
Lockheed Martin MS2, Eagan MN
651-456-4819 / [email protected]
Workshop on Stochastic Geometry in SLAM
IEEE International Confernce on Robotics and Automation
May 18, 2012, St. Paul MN
Purpose
Describe the elements of a new, practical, Bayes-optimal, and
theoretically unified foundation for multisensor-multitarget problems:
“Finite-Set Statistics” (FISST).
Finite-set statistics is the basis for a fundamentally new, Bayesoptimal, and theoretically unified approach to SLAM and related
robotics problems that is the focus of this workshop:
• Mullane, Vo, Adams, Vo: “A random-finite-set approach to
Bayesian SLAM, IEEE T-Robotics, (27)2: 268-282, 2011.
• Mullane, Vo, Adams, Vo: Random Finite Sets in Robotic
Map Building and SLAM, Springer, 2011.
My purpose here is contextual: to provide an overview of FISST and
to explain its pertinence for SLAM and similar applications
Simultaneous Localization and Mapping (SLAM)
• Multiple moving robots explore an unfamiliar environment without
access to GPS or a priori map (terrain, architectural) information
• Without human intervention and by employing only their oboard
sensors, the robots must detect and localize uknown stationary
landmarks (“features”)
• From these landmarks they must construct, on-the-fly, a local map
of the environment
• Then they must situate themselves within this map—along with any
unknown, moving, and possibly noncooperative targets
Important Points to Consider
• The landmarks will be unknown, and of unknown, varying number
• The robots will be unknown and of unknown, varying number
• The sensor measurements—whether generated by robots, targets,
landmarks, or clutter—will be varying and of varying number
• There is generally no a priori way to order the robots, the landmarks,
the targets, or the measurements
The Theoretical Challenge
• Vector representations of SLAM scenarios are problematic
• How can we measure the degree of deviation between between the
actual map and a SLAM algorithm’s estimate of it (which will differ
not only in estimates of individual landmarks, but in their number)?
• How can we claim that the algorithm’s estimate is “optimal” in a
Bayesian sense?
The Approach: Finite-Set Statistics
• Formulate SLAM problems in terms of random finite set (RFS) theory
• Generalize “Statistics 101” concepts to multitarget realm:
multitarget probability laws, multitarget integro-differential calculus
• From formal statistical models of sensors & targets, create RFS
multisensor-multitarget measurement models
• From formal statistical models of target motions (including
appearance & disappearance) create RFS multitarget motion models
• From the RFS motion & measurement models, construct “true”
multitarget Markov densities and likelihood functions
• From the Markov density & likelihood function, construct an optimal
solution: a multisensor-multitarget Bayes recursive filter
• Construct principled approximations of the optimal filter—e.g., PHD
filter, CPHD filter, multi-Bernoulli filter, etc.
Topics
• Overview
• Single-sensor, single-target Bayes filter
• RFS multi-object calculus
• RFS modeling of multisensor-multitarget systems
• Multisensor-multitarget recursive Bayes filter
• Approximate multitarget Bayes filters
• Conclusions
Topics
• Overview
• Single-sensor, single-target Bayes filter
• RFS multi-object calculus
• RFS modeling of multisensor-multitarget systems
• Multisensor-multitarget recursive Bayes filter
• Approximate multitarget Bayes filters
• Conclusions
Top-Down versus Bottom-Up Multitarget Data Fusion
usual “bottom-up” approach
to multitarget information fusion
multitarget
tracking
heuristically integrate a
patchwork of optimal or
heuristic algorithms that
address specific functions
“top-down,” system-level
approach
multiple sensors, multiple
targets = single system
specify statistical multitarget
measurement & motion models
detection
etc.
Bayesian-probabilistic
formulation of problem
requires random set theory

localization
motion models
multisensor-multitarget
recursive Bayes filter
models & functions subsumed
in optimally integrated algorithm
tracking
sensor models
I.D.
data association
The FISST Research Program
Advance 1:
unification of
expert systems
theory
Advance 2:
unification of
Level 1 fusion
fuzzy logic
unified
expert
systems
DempsterShafer
rules
detection
tracking
unified
Bayes
filter
Bayes
unified
Bayes
filter
identification
Advance 3:
unification of
Level 1 sensor
mgmt
Advance 4?
beginnings of a
foundation for
Levels 2/3?
unified
Level 1
fusion
unified
Level 1
sensor
mgmt
objective
functions
mission
goals
representation group target
of relationships
filtering
unified
Bayes
filter
principled
approximation
principled
approximation
?
unified
Bayes
filter?
Unified Information Fusion
α
φ
z
Q
traditional data
quantized data
operator-extracted
attributes
DSP-extracted
features
andom set
generalized
likelihood
function
random set
generalized
likelihood
function
random set
generalized
likelihood
function
random set
generalized
likelihood
function
unified algorithms:
- multitarget tracking
- target ID
- sensor mgmt
- etc.
S1 ⇒ S2
S
natural-language
statements
random set
generalized
likelihood
function
inference rules
(from knowledge-base)
random set
generalized
likelihood
function
also unifies much of expertsystem theory: Bayes,
fuzzy, Dempster-Shafer,
rule-based
Approximate
Multitarget
Filters:
MHT,
PHD,
CPHD
MHT VERSUS CPHD FILTER
given time-sequence of measurement-sets: Z(k) : Z1,…, Zk
multihypothesis
tracker (MHT)
PHD filter
(introduced 2000)
track
table
hypothesis
list
time
prediction
predicted
tracks
predicted
hypotheses
update using
new data
MEASUREMENTTO-TRACK ASSN
REQUIRED
updated
tracks
updated
hypotheses
filter on probability hypothesis densities (PHDs)
… → Dk|k(x|Z(k)) → Dk+1|k(x|Z(k)) → Dk+1|k+1(x|Z(k+1)) →
⋅⋅⋅
…
no measurement-to-track association required
filter on PHDs
… → Dk|k(x|Z(k)) → Dk+1|k(x|Z(k)) → Dk+1|k+1(x|Z(k+1)) →
…
CPHD filter
(introduced 2006)
… → pk|k(n|Z(k)) → pk+1|k(n|Z(k))
filter on target-number distributions
→ pk+1|k+1(n|Z(k+1)) → …
no measurement-to-track association required
Algorithms Derived Using Finite-Set Statistics
“classical”
PHD filter
? PHD/CPHD filters
for superpositional
sensors (w/ McGill)
unified tracking
and sensor-bias
estimation
“classical”
CPHD filter
finiteset
statistics
PHD/CPHD filters for
unknown clutter
and prob. detection
unified Bayes filters
for nontraditional data
multiple motion model
PHD/CPHD filters
multisensor PHD
and CPHD filters
PHD/CPHD
smoothers
PHD filter for statedependent clutter
Selected Applications
ground moving target
indicator (GMTI)
radar multitarget
tracking
direction-of-arrival (DOA)
tracking of dynamic
signal sources
(22 km, 6 hr, real-time pipeline
tracking with a UUV,
SeeByte and British Petroleum)
(real-time “Bold Avenger”
NATO exercise, 2007,
FGAN/FKIE and EADS)
simultaneous
localization and
mapping (SLAM)
PHD
and
CPHD
filters
tracking multiple moving
targets in terrain
tracking in
video images
group-target tracking
multitarget tracking
in distributed networks
underwater activeacoustic tracking
passive coherent
localization
(PCL) tracking
multisensormultitarget
sensor
management
Primary References
Overview: Chapter 16
Level 1 Fusion Textbook:
Sensor Mgmt: Chapter 12
CRC Press 2008
Artech House 2007
World Scientific 2004
2007
Mignogna
Data
Fusion
Award
“Statistics 101” for MultisensorMultitarget Data Fusion
invited
tutorial:
IEEE AES
Mag. 2004
2005
IEEE AESS
Mimno Award
Multitarget Filtering Via
First-Order Multitarget Moments
PHD
Filter
Theory:
IEEE Trans.
AES 2003
PHD Filters of Higher Order
in Target Number
CPHD Filter
Theory:
IEEE Trans.
AES 2007
2007
IEEE AESS
Carlton Award
The Random Set Filtering Website
RFS Filtering Website
•
United Kingdom mirror Prof. Daniel Clark, [email protected]
– http://randomsets.eps.hw.ac.uk/index.html
•
Australian mirror Prof. Ba-Ngu Vo, [email protected]
– http://randomsets.ee.unimelb.edu.au/index.html
RFS-SLAM Website
•
Prof. Martin Adams, [email protected]
– http://www.cec.uchile.cl/~martin/Martin_research_18_8_11.html
Topics
• Overview
• Single-sensor, single-target Bayes filter
• RFS multi-object calculus
• RFS modeling of multisensor-multitarget systems
• Multisensor-multitarget recursive Bayes filter
• Approximate multitarget Bayes filters
• Conclusions
Foundation: The Single-Target Bayes Filter
observation space
measurement
collected by sensor
single-target state space
Xk|k
Xk+1|k
Xk+1|k+1
evolving random
state-vector
⋅⋅⋅
fk|k(x|Zk)
Bayes
probability
distribution
on target state
x
fk+1|k(x|Zk)
time-update
(predictor)
fk+1|k+1(x|Zk+))
measurementupdate
(corrector)
⋅⋅⋅
Foundation: The Single-Target Bayes Filter, 2
target
state
accumulated
measurements
at time-step k
^xk+1|k+1
Bayes-optimal
state estimation
⋅⋅⋅ → fk|k(x|Zk) → fk+1|k(x|Zk) → fk+1|k+1(x|Zk+1) → ⋅⋅⋅
predictor
formal Markov
density
corrector
new
measurement
fk+1|k(x|x′)
calculus
formal statistical
model of the motion
of the target
fk+1(zk+1|x) = L zk+1(x)
calculus
Xk+1 = ϕk(x) + Vk
deterministic
motion
model
zk+1
formal
likelihood
function
motion
noise
Zk+1 = ηk+1(x)
deterministic
measurement
model
formal Bayes modeling
formal
+ Wk+1 statistical
measurement
sensor
model of the
noise
sensor
Special Case: The Kalman Filter
→ NPk|k(x−xk|k) → NPk+1|k(x−xk+1|k) → NPk+1|k+1(x −xk+1|k+1) →
NQ (x−Fkx′)
k
xk+1 = Fkx + Vk
NR (zk+1−Hk+1x)
k+1
zk+1 = Hk+1x + Wk+1
Topics
• Overview
• Single-sensor, single-target Bayes filter
• RFS multi-object calculus
• RFS modeling of multisensor-multitarget systems
• Multisensor-multitarget recursive Bayes filter
• Approximate multitarget Bayes filters
• Conclusions
Mathematical Core of Single-Target Bayes Filter
formal Bayes
modeling
pX(S)
(prevent modelmismatch due to a
heuristics-generated
fictitious sensor!)
single-target
filtering
probability measures / probabilitymass functions of random vector X
δpX
(S)
δx
fX(x)
integrals
& derivatives
∫
fX(x)dx
probability density
functions of random vector X
ordinary calculus permits derivation of concrete algorithm formulas
Mathematical Core of Multitarget Bayes Filter
formal Bayes
modeling
β Ψ(S)
(prevent modelmismatch due to a
heuristics-generated
fictitious sensor!)
multitarget
filtering
belief-mass functions of
random finite set Ψ
δβΨ
(S)
δY
fΨ(Y)
∫
fΨ(Y)δY
multi-object probability density
functions of random finite set Ψ
δGΨ
[h]
δY
principled
approximation
set integrals
& derivatives
GΨ[h]
set integrals &
functional derivatives
∫
fΨ(Y)δY
probability generating functionals
(p.g.fl.’s) of random finite set Ψ
Multi-Object Calculus, 1
∞
set
integrals
∫ fΨ(Y)δY = Σ ∫ fΨ({y1,…,yn})dy1⋅⋅⋅dyn
n=0
1
n!
functional power
hY =
probability generating
functional (p.g.fl.)
GΨ[h] = ∫ hY ⋅ fΨ(Y)δY
Π y∈Y h(y)
set
integral
Dirac delta function
functional
derivatives
δGΨ
GΨ[h+ε⋅δy] − GΨ[h]
[h] = lim
ε
δy
ε→0
δGΨ
δnGΨ
[h] =
[h]
δy1⋅⋅⋅δyn
δY
Multi-Object Calculus, 2
multitarget
distribution
probability
hypothesis
density (PHD)
δGΨ
fΨ(Y) = δY [0]
δGΨ
DΨ(y) = δy [1] =
functional
derivative
∫
fΨ({y}∪Y)δY
multitarget calculus permits derivation of concrete algorithm formulas
Topics
• Overview
• Single-sensor, single-target Bayes filter
• RFS multi-object calculus
• RFS modeling of multisensor-multitarget systems
• Multisensor-multitarget recursive Bayes filter
• Approximate multitarget Bayes filters
• Conclusions
Multisensor-Multitarget Statistics
sensors
single-sensor
random measurement-vectors
z2
z1
z3 ,…
sensor
measurement
spaces
UNIFIED MULTISENSOR MEASUREMENT SPACE
Σ = {z1,…,zm}
…,zn-1
zn
random finite set (RFS) of
measurements ( m is also random)
single-target
state space
x1
single-target
random state-vectors
UNIFIED MULTITARGET STATE SPACE
x3 ,…
x2
…,xn-1
xn
Ξ = {x1,…,xn}
random finite set (RFS)
of targets ( n is also random)
multisensor, multitarget transformed to single-sensor, single-target
Statistical Representation of a Multitarget System
equivalent notations for a (multidimensional) simple point process
sum the Dirac deltas
concentrated at the
elements of Ξ
single-target state space
Ξ
δ Ξ ( x) = ∑ δ y ( x)
y∈Ξ
random state-set
a particular formulation
of a random point process
(stochastic geometry
formulation
random density/
random measure
(point process theory
formulation)
Systematic Multitarget Modeling & Approximation
1.
2.
formal multitarget
statistical model of the
motions of all targets
multitarget calculus
3.
formal multisensor-multitarget
statistical measurement model of
ACTUAL INFORMATION SOUCES
multitarget calculus
“true” multitarget Markov density
fk+1|k(X|X′)
4.
sensors, attributes,
features, naturallanguage statements,
inference rules
− no information lost
− no extraneous information added
“true” multitarget likelihood function
fk+1(Z|X)
− no information lost
− no extraneous information added
5.
“true” multitarget Bayes filter
(Bayes-optimal multitarget detection & tracking)
multitarget calculus
6.
fk|k(X|Z(k))
− no information lost
− no extraneous information added
p.g.fl. multitarget Bayes filter
(less complex formulas)
Gk|k[h|Z(k)]
i.i.d. cluster process approximation
7.
approximate p.g.fl. multitarget Bayes filter G [h|Z(k)]
k|k
(algebraically tractable formulas)
multitarget calculus
8.
PHD & CPHD filters
(approximate multitarget detection & tracking)
Dk|k(x|Z(k)), pk|k(n|Z(k))
Systematic Multitarget Modeling & Approximation, 2
PHD approximation of multitarget Bayes filter (OR OTHER APPROXIMATE FILTERS)
(sub-optimal)
⋅⋅⋅ → Dk|k(x)
→
Dk+1|k(x)
→
Dk+1|k+1(x) → ⋅⋅⋅
→
Gk+1|k+1[h] → ⋅⋅⋅
p.g.fl. form of multitarget Bayes filter
(optimal, intractable, but algebraically simpler)
⋅⋅⋅ → Gk|k[h]
→
Gk+1|k[h]
⋅⋅⋅ → fk|k(X|Z(k)) → fk+1|k(X|Z(k)) → fk+1|k+1(X|Z(k+1)) → ⋅⋅⋅
multitarget Bayes filter
(optimal but usually intractable solution)
true multitarget
Markov density
fk+1|k(X|X′)
multitarget
calculus
multitarget
motion model
Xk+1 = Φ k(X) ∪ Bk
fk+1(Z|X)
true multitarget
likelihood
function
multitarget
calculus
Zk+1 = Tk+1(X) ∪ Ck+1
multitarget
measurement
model
Topics
• Overview
• Single-sensor, single-target Bayes filter
• RFS multi-object calculus
• RFS modeling of multisensor-multitarget systems
• Multisensor-multitarget recursive Bayes filter
• Approximate multitarget Bayes filters
• Conclusions
The Multitarget Bayes Filter
observation space
random
observationsets Z produced
by targets
state space
multi-target motion
three targets
five targets
multitarget
Bayes filter
fk|k(X|Z(k))
multitarget
probability
density
function
time
prediction
fk+1|k(X|Z(k))
update using
new data
fk+1|k+1(X|Z(k+1))
⋅⋅⋅
fk|k(∅|Z(k))
(probability that there are no targets present)
fk|k({x1}|Z(k)
(probability of one target with state x1)
(k)
fk|k({x1 ,x2}|Z ) (probability of two targets with states x1 ,x2)
…
fk|k({x1 ,..., xn}|Z(k)) (probability of n targets with states x1 ,..., xn)
The Multitarget Bayes Filter, 2
^
multitarget
state
Xk+1|k+1
accumulated multisensor-multitarget
measurements at time-step k
Bayes-optimal multitarget
state estimation
⋅⋅⋅ → fk|k(X|Z(k)) predictor
→ fk+1|k(X|Z(k)) corrector
→ fk+1|k+1(X|Z(k+1)) → ⋅⋅⋅
formal multitarget
Markov density
fk+1|k(X|X′)
multitarget calculus
formal multitarget
model of the
motions of all targets
fk+1(Zk+1|X) = L Zk+1(X)
multitarget calculus
Xk+1 = Φk(X) ∪ Bk
persisting
targets
new
targets
Zk+1 = Tk+1(X) ∪ Ck+1
targetgenerated
observations
formal Bayes modeling
clutter
observations
formal
multisensormultitarget
likelihood
function
formal
multisensormultitarget
measurement
model of all
info sources
Conventional Multitarget Filtering
(multi-hypothesis correlation trackers)
measurements
targets
predicted multitarget
motion
multitarget
Bayes filter
(optimal)
fk|k(X|Z(k))
multihypothesis
correlator
tracker
track
table
hypothesis
list
time
prediction
approximation
time
prediction
fk+1|k(X|Z(k))
update using
new data
fk+1|k+1(X|Z(k+1))
approximation
predicted
tracks
predicted
hypotheses
update using
new data
⋅⋅⋅
approximation
updated
tracks
updated
hypotheses
⋅⋅⋅
Conventional Multitarget Tracking
o
simultaneous
estimates of
positions of targets
1 and 2, via data
association and
parallel Kalman
filters
Topics
• Overview
• Single-sensor, single-target Bayes filter
• RFS multi-object calculus
• RFS modeling of multisensor-multitarget systems
• Multisensor-multitarget recursive Bayes filter
• Approximate multitarget Bayes filters
• Conclusions
Probability Hypothesis Density (PHD) Filter
observation space
single-target state space
multitarget
⋅⋅⋅
Bayes filter
(optimal)
1st-moment
⋅⋅⋅
(PHD) filter
fk|k(X|Z(k))
fk+1|k(X|Z(k))
compress to multitarget first moment
Dk|k(x|Z(k))
compress to multitarget first moment
Dk+1|k(x|Z(k))
time-update
step (predictor)
⋅⋅⋅
fk+1|k+1(X|Z(k+1))
compress to multitarget first moment
Dk+1|k+1(x|Z(k+1))
data-update
step (corrector)
⋅⋅⋅
computational complexity O(mn) , n = no. targets, m = no. measurements
Example of a PHD
5 peaks (greatest target densities)
correspond to locations of 7
partially resolved, closely spaced targets
(but target number correctly estimated as 7)
target
density,
D(x)
7 targets moving
abreast in parallel
y
x
PHD represents targets first as a group, then as individual targets
Probability Hypothesis Density: Picture
DΞ(x0) = Pr(x0 ∈ Ξ)
= p1 + p6 + p9 + p11 + p16
p17
p11
p8
p4
x0“
four-state instantiations
p16
p15
p13
= probability of the
hypothesis: “the
multitarget system
contains a target with state
{x, x2, x3, x4}
p14
p12
of
Ξ
three-state instantiations
{x, x2, x3}
p10
p9
of
Ξ
two-state instantiations
p7
p5
p2
p6
{x, x2}
p3
p1
x0
of
Ξ
one-state instantiations
{x1} of Ξ
state space (discrete)
The Cardinalized PHD (CPHD) Filter
observation space
single-target state space
multitarget
⋅⋅⋅
Bayes filter
⋅⋅⋅
fk|k(X|Z(k))
compress
fk+1|k(X|Z(k))
compress
compress
Dk|k(x|Z(k))
Dk+1|k(x|Z(k))
Dk+1|k+1(x|Z(k+1))
⋅⋅⋅
pk|k(n|Z(k))
pk+1|k(n|Z(k))
pk+1|k+1(n|Z(k+1))
target-no.
⋅⋅⋅ distribution
CPHD filter
⋅⋅⋅
⋅⋅⋅
fk+1|k+1(X|Z(k+1))
PHD
computational complexity O(m3n) , n = no. targets, m = no. measurements
The PHD/CPHD Filters and Closely-Spaced Targets
PHD / CPHD filters permit detection and tracking of multiple targets
when conventional approaches begin to perform poorly
“tracking barrier”
targets are sufficiently separated
w/r/t sensor resolution that they
can be tracked individually
using conventional multitarget
filters or by CPHD filter
targets are so closely spaced
w/r/t sensor resolution that
conventional filters
begin to break down
CPHD filter tracks targets individually
CPHD filter tracks closely-spaced
targets as a group, while accurately
estimating no. of targets in group
as targets begin to separate,
CPHD filter begins to track
them individually
The PHD/CPHD Filter and Large Target Clusters
PHD / CPHD filter permits tracking of dense target clusters when
conventional approaches begin to perform poorly
group targets
group targets
so many targets are present, and
relatively closely spaces, that conventional
multitarget filters begin experiencing
computational difficulties
group targets
group targets
“tracking barrier”
CPHD filter tracks target-cluster
targets as a group, while accurately
estimating no. of targets in group
group targets
Conclusions
• Finite-set statistics is the basis for a new, Bayesoptimal, and theoretically unified approach to SLAM
• Permits a more principled way of approaching SLAM
• Promising new SLAM algorithms
• For more details on finite-set statistics
–
–
–
–
Handbook of Multisensor Data Fusion, 2nd Ed., Chapter 16
Statistical Multisource-Multitarget Information Fusion
“Statistics ‘101’ for multisensor, multitarget data fusion”
papers listed in bibliography of the workshop paper
Thank You!