Particle FiltersA Collaborative Explanation

By:Neeti Wagle

&Mikael Pryor

Dynamic system

• We want to estimate the state of some system : x• Problem : x is hidden and cannot be directly observed• Instead, we can observe y

x1 x2 x3

y1 y2 y3

Filtering

• Use the posterior distribution to estimate from sequential observations.

)|()|(

)|()|( 1:1

1:1:1

tttt

tttt P

P

PP yx

yy

xyyx

11:1111:1 )|()|()|( ttttttt dppP xyxxxyx

tx

Dynamic systemKey properties• Markov property : Future is independent of past given

current state i.e. • Observation at time t depends only on state at time t

)|()|( 1:1 tttt PP xxxx

):1 |()|( tttt PP xyxy

x1 x2 x3

y1 y2 y3

Kalman Filtering Approach

• xt is a linear function of xt-1

• A is the state transition matrix• zt is linear function of xt

• B is the measurement matrix• wt is process noise

• vt is the measurement noise

2

2

1

~,

~,

t

t

t

t

tttt

tttt

y

x

σ0Nv

σ0Nw

vxBy

wxAx

Drawbacks : Susceptible to poor measurements and incorrect updates; may never recover after divergence Assume the state has unimodal distribution

Monte Carlo Approach

1. Simulate M random variables from a Gaussian distribution

2. Compute the average

3. Similar idea for particle filter…

),0(~ 2)( Nx m

M

m

mxM

v1

2)( )(1

Miodrag Bolic, Lecture for the School of Information Technology and Engineering at the University of Ottawa, mbolic@site.uottawa.ca

Particle Filter

Miodrag Bolic, Lecture for the School of Information Technology and Engineering at the University of Ottawa, mbolic@site.uottawa.ca

• Let the proposal density be equal to the prior• Particle filtering steps for m=1,…,M:

1. Particle generation

2a. Weight computation

2b. Weight normalization

3. Estimate computation

)|(~ 1)(

ttmt px xx

)|( )()*(1

)*( mtt

mt

mt xpww y

M

m

mt

mtm

t

w

ww

1

)*(

)*()(

M

m

mt

mttt wxggE

1

)()(:1 )())|(( zx

Examples

Sample space

Posterior density

Miodrag Bolic, Lecture for the School of Information Technology and Engineering at the University of Ottawa, mbolic@site.uottawa.caHaris Baltzakis, November 2004, Kalman/Particle Filters Tutorial

Examples

Source : Robotics and State Estimation Lab, University of Washington

Two Robots Play “Catch”

By: Mikael Pryorpryorm@colorado.edu

Pong

Tracking The Ball

• Where:

• The depends on which paddle is tracking the ball.• Counterclockwise convention• + for the left paddle• - for the right paddle

tttt ytxtt vyvx:1:1

x

1

1

,

,

:1

:1

sin

cos

tty

ttx

paddletttt

paddletttt

yyv

xxv

ydy

xdx

tt

tt

Predicting Where The Ball Will Be

• Prediction based on paddle position:

• If the predicted position of the ball is above

lower wall theofposition y the

upper wall theofposition y the

arena theofdimension y the

min

max

y

y

ydist

distdist

t

dist

t

dist

t

distdist

t

dist

t

dist

tpaddlet

yy

yy

y

yyy

y

yyeven

yyy

yy

y

yy

y

yyoddy

max1max1max

max1

minmax1max1max1

,1

intint

intint

maxy

Predicting Where The Ball Will Be (Cont’d)

• If the predicted position of the ball is below

distdist

t

dist

t

dist

t

distdist

t

dist

t

dist

tpaddlet

yy

yy

y

yyy

y

yyodd

yyy

yy

y

yy

y

yyeveny

1min1minmax

1min

min1min1min1min

,1

intint

intint

miny

Random Error

• Random Noise in how the ball moves• Random Error in the laser scanner

• The scanner has a resolution of 0.36 degrees.• The scanner is only accurate to within + 0.01 meters

• Random Error in odometry for the robot• The odometry could be off by as much as 36% of the distance traveled• Compensated for by doing relative odometry using laser scanner.

– Only 0.01 m error as opposed to 0.36 m error– Uses the walls to localize the robot.

tttttty

tttttx

t

dδdd

δdd

t

t

sincos

cossin

meters 01.0d

radians 0063.0

t

The Particle Filter

• Random error can be modeled with a Gaussian Probability Density Function with the following properties:

tpaddlett

tttt

tttt

σ

x

y

t

meastmeastt

tθ

t

xxxx

yyyy

yx

eσ

w

yx

σ

t

tmeast

meast

t

t

1

11

2

arctan

2

,,

,

2

1Re

,

radians 0063.0

2

2

,

,2

wy

y

The Final Product

Adding Excess Noise in X-Direction

• Using a camera instead of a laser scanner.• The y-direction can be estimated accurately.• The x-direction is harder to estimate with one camera.

• No depth perception• Size of the ball

• The particle filter still works robustly in this scenario.

The X-Noise Final Product

OBJECT LOCALIZATION USING A PHYSICS SIMULATOR

Neeti WagleUniversity of Colorado at Boulder

INTRODUCTION

Based on these particle filters, how does the world appear to the robot?

Consider object localization using particle filters

INTRODUCTION

• Norm : Localization of objects assumes that we know nothing about the environment

• What if, the environment was “intelligent”?• What if, objects could communicate with the

robot?– Add an RFID tag to each object

• What if, objects could provide information about their geometries?– Store object geometry in the tag

INTRODUCTION

• What if, the robot had conceptual knowledge about the world ?– Use a physics simulator

• Question : How can the robot use – object geometries– what it sees– world knowledge (how the world behaves)to have a better understanding of the world?

HYPOTHESIS

Using • object geometry, and • simulated physics to localize objects with particle filters

OPEN DYNAMICS ENGINE (ODE)

• C++ physics engine with– Spaces, geometries, rigid bodies, joints– Real time simulator– Collision detection system

• Products that use ODE– Video games– Robotics simulators : Gazebo, Webots, OpenRAVE

• Developed by Russell Smith

PHYSICS SIMULATOR

PHYSICS SIMULATOR

OBJECT GEOMETRIES

PARTICLE FILTERS

MOTIVATION - 1

PARTICLE FILTER VIEW OBJECT VIEW

This configuration is not valid in the real world!

MOTIVATION – 2

PARTICLE FILTER VIEW OBJECT VIEW

This configuration is not valid in the real world!

KEY IDEA

• Use the physics simulator to reject scenarios (particle filter hypotheses) where– Objects are overlapping/embedded in each other– Objects fall off when the simulation engine is run

• Goal : Find a world model which adheres to– the particle filter hypotheses– laws of physics

ALGORITHM

Consider JN possible configurations , whereN : number of objects M : number of particles in each particle filter J <M

• Step 1 : Use ODE collision detection to check if any objects are overlapping or embedded in each other– If yes, reject this configuration

• Step 2 : Run the ODE simulation engine by advancing time. Stop the engine when all objects are stable– Calculate the mean squared distance of final configuration from starting

configuration– If distance below acceptable threshold, accept the configuration

Output : Configuration with least mean squared distance -> valid world model

EXAMPLE 1

EXAMPLE 1

EXAMPLE 1

EXAMPLE 2

EXAMPLE 2

EXAMPLE 2

EXAMPLE 3

EXAMPLE 3

EXAMPLE 3

ADVANTAGES

• Drops the “know-nothing-about-the-world” constraint which is too restrictive and unnecessary in intelligent environments

• Takes a multi-object view of the environment– Significant in applications like grasping objects

using a robotics arm• Can be integrated with OpenRAVE to plan

grasping motion using inverse kinematics solvers

SHORTCOMING

Testing all hypotheses is exponential in the number of objects– Need a way to test only hypotheses with high

likelihood– Goal : make this an “anytime” algorithm

IN THE PIPELINE

• Developing a branch-and-bound method for testing the hypotheses in decreasing order of likelihood

• Approach : Variation of the knapsack problem based on entropy

• When time runs out it returns the best hypotheses found so far

FUTURE WORK

• Visual validation :– Idea :

• Save the N best world models’ as snapshots• Compare snapshots to a camera image to select the

best hypotheses

– Middle ground approach

• Model using SIFT features instead of particle filters

SUMMARY

• Aims to construct a object view of the world from individual location estimates

• Uses a simulator to reject invalid object configurations – serves as robot’s conceptual knowledge of the world

• Constructs 3D models of the world

QUESTIONS?