Probabilistic State EstimationWith Application To Vehicle Navigation

Matthew KirchnerNaval Air Warfare Center – Weapons Division

Department of ECEE – University of Colorado at BoulderNovember 1, 2010

Topics

• Why?• Review• Background• Kalman Filter• Particle Filter• SLAM

Why Probability?

• Real sensors have uncertainty

• May have multiple sensors

• Some states are not directly observable

• Ambiguous sensor observations

Importance of Bayes Rule

Bayes RuleRecursive Bayesian

Estimation

Linear Kalman Filter

Unscented Kalman Filter

Extended Kalman Filter

EKF SLAM

FastSLAMParticle Filter FastSLAM

Bayes Rule

)(

)()|()|(

BP

APABPBAP

PriorLikelihood

Posterior

Normalizing Constant

Equivalent Bayes Rule

)()|()|( APABPBAP

)()|()|( APABPBAP

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?

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

Gaussian

1D 2D

Gaussian

1D 2D

Gaussian

• 2D Probability density function described by mean vector and covariance matrix

• 1D Probability density function described by mean and variance

),(~ 2Nx

2221

1221

2

1

2

1

),(~

NX

x

xX

Functions of Random Variables

Functions of Random Variables

• Linear function

• Mean:

• Covariance:

• Linear functions only!

xFy

Txy FF

xFy

Kalman Filter

• Introduced in 1960 by Rudolf Kalman• Many applications:– Vehicle guidance systems– Control systems– Radar tracking– Object tracking in video– Atmospheric models

Kalman Filter

),|( ttt uzxPWe want to find:

Kalman Filter

ProcessModel

ProcessModel

ObservationModel

ObservationModel

ObservationModel

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

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

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’.

Kalman Filter Steps

4. Compute Kalman gain matrix5. Using Kalman gain and prior, calculate

posterior– Sometimes called ‘correct’ step or ‘a posteriori’

Kalman Filter Steps

• Algorithm Kalman_Filter( )– 1a:– 1b:– 2:– 3:– 4:– 5a:– 5b:

• Return( )

ttttt uBxFx 1ˆ

tTtttt QFF 1

ˆ

ttt xHz ˆˆ

ttt zzz ˆ1)ˆ(ˆ t

Tttt

Tttt RHHHK

tttt zKxx ˆ

tttt HKI ˆ)(

tttt zux ,,, 11

ttx ,

Kalman Filter

Kalman Filter Example

1txtx

Kalman Filter Example

Root Mean Squared Error (RMSE)KF 1.0030m

GPS Only 3.6840m

Kalman Filter Example

GPS Lost

GPS Reacquired

Kalman Filter Example

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

Extended Kalman Filter

• Linearize process and observation models– By finding Jacobian matrices– Analytically or numerically

• Then use regular Kalman filter algorithm• Sub-optimal

29

EKF Linearization

EKF Linearization

Unscented Kalman Filter

Unscented Kalman Filter

EKF UKF

Particle Filter

Particle Filter

• Represent distribution as set of randomly generated samples, called ‘particles’.

• Functions can be nonlinear and non-gaussian• Multi-hypothesis belief propagation

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’

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

Feature-based SLAM

FastSLAM - Example

Other Probabilistic Applications

Other Probabilistic Applications