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

2. Topics
• Why?
• Review
• Background
• Kalman Filter
• Particle Filter
• SLAM

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

5. Bayes Rule
)(
)()|(
)|(
BP
APABP
BAP 
Prior
Likelihood
Posterior
Normalizing
Constant

6. Equivalent Bayes Rule
)()|()|( APABPBAP 
)()|()|( APABPBAP 

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

9. Gaussian
1D 2D

10. Gaussian
1D 2D

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

12. Functions of Random Variables

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

15. Kalman Filter
),|( ttt uzxPWe want to find:

16. Kalman Filter
Process
Model
Process
Model
Observation
Model
Observation
Model
Observation
Model

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’

21. Kalman Filter Steps
• Algorithm Kalman_Filter( )
– 1a:
– 1b:
– 2:
– 3:
– 4:
– 5a:
– 5b:
• Return( )
ttttt uBxFx  1
ˆ
t
T
tttt QFF  1
ˆ
ttt xHz ˆˆ 
ttt zzz ˆ
1
)ˆ(ˆ 
 t
T
ttt
T
ttt RHHHK
tttt zKxx  ˆ
tttt HKI  ˆ)(
tttt zux ,,, 11  
ttx ,

22. Kalman Filter

23. Kalman Filter Example
1txtx

24. Kalman Filter Example
Root Mean Squared Error (RMSE)
KF 1.0030m
GPS Only 3.6840m

25. Kalman Filter Example
GPS Lost
GPS
Reacquired

26. Kalman Filter Example

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

29. 29
EKF Linearization

30. EKF Linearization

31. Unscented Kalman Filter

32. Unscented Kalman Filter
EKF UKF

33. Particle Filter

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

37. Feature-based SLAM

38. FastSLAM - Example

39. Other Probabilistic Applications

40. Other Probabilistic Applications