1. Probabilistic State Estimation
With Application To Vehicle Navigation
Matthew Kirchner
Naval Air Warfare Center – Weapons Division
Department of ECEE – University of Colorado at Boulder
November 1, 2010
3. Why Probability?
• Real sensors have
uncertainty
• May have multiple
sensors
• Some states are not
directly observable
• Ambiguous sensor
observations
4. Importance of Bayes Rule
Bayes Rule
Recursive
Bayesian
Estimation
Linear Kalman
Filter
Unscented
Kalman Filter
Extended Kalman
Filter
EKF SLAM
FastSLAMParticle Filter FastSLAM
7. Bayes Rule Example
• School: 60% boys and 40% girls
• All boys wear pants
• Half of girls wear skirts, half wear pants
• You see a random student and can only tell
they are wearing pants.
• Based on your observation, what is the
probability the student you saw is a girl?
8. Bayes Rule Example
• School: 60% boys and
40% girls
• All boys wear pants
• Half of girls wear skirts,
half wear pants
• You see a random student
and can only tell they are
wearing pants.
• Based on your
observation, what is the
probability the student
you saw is a girl?
• We want to find:
– P(Student=Girl | Clothes=Pants)
• Prior?
– P(Student=Girl) = 0.4
• Likelihood?
– P(Clothes=Pants | Student=Girl)
= 0.5
• Normalizing Constant?
– P(Clothes=Pants) = 0.8
• Bayes Rule!
– P(Student=Girl | Clothes=Pants)
= (0.5)*(0.4)/(0.8) = 0.25
11. Gaussian
• 2D Probability density
function described by
mean vector and
covariance matrix
• 1D Probability density
function described by
mean and variance
),(~ 2
Nx
2
221
12
2
1
2
1
2
1
),(~
NX
x
x
X
13. Functions of Random Variables
• Linear function
• Mean:
• Covariance:
• Linear functions only!
xFy
T
xy FF
xFy
14. Kalman Filter
• Introduced in 1960 by Rudolf Kalman
• Many applications:
– Vehicle guidance systems
– Control systems
– Radar tracking
– Object tracking in video
– Atmospheric models
17. Kalman Filter
• Process Model:
– Deterministic:
– Probabilistic:
• Used to calculate prior distribution
• Observation Model:
– Deterministic:
– Probabilistic:
• Used to calculate likelihood distribution
),( 1 ttt uxfx
)( tt xfz
),,(),|( 11 tttttt wuxfuxxP
),()|( tttt vxfxzP
18. Kalman Filter: Assumptions
• Underlying system is modeled as Markov
–
• All beliefs are Gaussian distributions
– Additive zero mean Gaussian noise
• Linear process and observation models
– Process Model:
•
– Observation Model:
•
),,,|()|( 3211 tttttt xxxxPxxP
ttt vHxz
tttt wBuFxx 1
),0(~ QNwt
),0(~ RNvt
19. Kalman Filter Steps
1. Using process model, previous state, and
controls, find prior
– Sometimes called ‘predict’ step, or ‘a priori’
2. Using prior and observation model, find sensor
likelihood
– If I knew state, what should sensors read?
3. Find observation residual
– Difference between actual sensor values and what
was calculated in step 2. Also sometimes called
‘innovation’.
20. Kalman Filter Steps
4. Compute Kalman gain matrix
5. Using Kalman gain and prior, calculate
posterior
– Sometimes called ‘correct’ step or ‘a posteriori’
27. Kalman Filter
• O(k^2.4+n^2)
• Many real systems are non linear
– Extended Kalman filter
– Unscented Kalman filter
– Particle filter
• Some systems are non-Gaussian
– Particle filter
28. Extended Kalman Filter
• Linearize process and observation models
– By finding Jacobian matrices
– Analytically or numerically
• Then use regular Kalman filter algorithm
• Sub-optimal
34. Particle Filter
• Represent distribution as set of randomly
generated samples, called ‘particles’.
• Functions can be nonlinear and non-gaussian
• Multi-hypothesis belief propagation
35. Particle Filter
• Sample the prior
• Compute likelihood of particles given
measurement
– Also called particle ‘weights’
• Sample posterior: Sample from particles
proportional to particle weights
– Also called ‘resampling’ or ‘importance sampling’
36. Simultaneous Localization and
Mapping
• SLAM Problem:
– Need map to localize
– Need location to make map
• Brainstorming: How can we solve this problem?
– Map could be locations of landmarks or occupancy
grid
• Kalman filter based: landmark positions part of
state variables
• Particle filter based: landmark positions or
occupancy map included in each particle
),|,( ttt uzmxP