CSE-571 Robotics Extremely noisy (nonlinear) motion of observer - - PowerPoint PPT Presentation

SLIDE 1

1

CSE-571 Robotics

Rao-Blackwelized Particle Filters for State Estimation

Ball Tracking in RoboCup

§ Extremely noisy (nonlinear) motion of

• bserver

§ Inaccurate sensing, limited processing

power

§ Interactions between target and

environment

§ Interactions between robot(s) and target

Goal: Unified framework for modeling the ball and its interactions.

Dieter Fox 2 CSE-571: Probabilistic Robotics

Tracking Techniques

§ Kalman Filter

§ Highly efficient, robust (even for nonlinear) § Uni-modal, limited handling of nonlinearities

§ Particle Filter

§ Less efficient, highly robust § Multi-modal, nonlinear, non-Gaussian

§ Rao-Blackwellised Particle Filter, MHT

§ Combines PF with KF § Multi-modal, highly efficient

Dieter Fox 3 CSE-571: Probabilistic Robotics

Dynamic Bayes Network for Ball Tracking

k-2

b

k-1

b

k

b r k-2 r k-1 r k z k-2 zk-1 z k u k-2 u k-1 zk-1 zk z k-2

k

m

k-1

m

k-2

m

Ball observation Ball location and velocity Ball motion mode Map and robot location Robot control Landmark detection

Ball tracking Robot localization

l l l b b b Dieter Fox 4 CSE-571: Probabilistic Robotics

SLIDE 2

2 Robot Location

k-2

b

k-1

b

k

b r k-2 r k-1 r k z k-2 zk-1 z k u k-2 u k-1 zk-1 zk z k-2

k

m

k-1

m

k-2

m

Ball observation Ball location and velocity Ball motion mode Map and robot location Robot control Landmark detection

Ball tracking Robot localization

l l l b b b Dieter Fox 5 CSE-571: Probabilistic Robotics

Robot and Ball Location (and velocity)

k-2

b

k-1

b

k

b r k-2 r k-1 r k z k-2 zk-1 z k u k-2 u k-1 zk-1 zk z k-2

k

m

k-1

m

k-2

m

Ball observation Ball location and velocity Ball motion mode Map and robot location Robot control Landmark detection

Ball tracking Robot localization

l l l b b b Dieter Fox 6 CSE-571: Probabilistic Robotics

Ball-Environment Interactions

None Grabbed Bounced Kicked Deflected Dieter Fox 7 CSE-571: Probabilistic Robotics

Ball-Environment Interactions

None Grabbed Bounced Kicked (0.8) Robot loses grab (0.2) Robot kicks ball (0.9) Within grab range and robot grabs (prob. from model ) Kick fails (0.1) Deflected (residual prob .) Dieter Fox 8 CSE-571: Probabilistic Robotics

SLIDE 3

3 Integrating Discrete Ball Motion Mode

k-2

b

k-1

b

k

b r k-2 r k-1 r k z k-2 zk-1 z k u k-2 u k-1 zk-1 zk z k-2

k

m

k-1

m

k-2

m

Ball observation Ball location and velocity Ball motion mode Map and robot location Robot control Landmark detection

Ball tracking Robot localization

l l l b b b Dieter Fox 9 CSE-571: Probabilistic Robotics

Grab Example (1)

k-2

b

k-1

b

k

b r k-2 r k-1 r k z k-2 zk-1 z k u k-2 u k-1 zk-1 zk z k-2

k

m

k-1

m

k-2

m

Ball observation Ball location and velocity Ball motion mode Map and robot location Robot control Landmark detection

Ball tracking Robot localization

l l l b b b Dieter Fox 10 CSE-571: Probabilistic Robotics

Grab Example (2)

k-2

b

k-1

b

k

b r k-2 r k-1 r k z k-2 zk-1 z k u k-2 u k-1 zk-1 zk z k-2

k

m

k-1

m

k-2

m

Ball observation Ball location and velocity Ball motion mode Map and robot location Robot control Landmark detection

Ball tracking Robot localization

l l l b b b Dieter Fox 11 CSE-571: Probabilistic Robotics

Inference: Posterior Estimation

k-2

b

k-1

b

k

b r k-2 r k-1 r k z k-2 zk-1 z k u k-2 u k-1 zk-1 zk z k-2

k

m

k-1

m

k-2

m

Ball observation Ball location and velocity Ball motion mode Map and robot location Robot control Landmark detection

Ball tracking Robot localization

l l l b b b

) , , | , , (

1 : 1 : 1 : 1

• k

l k b k k k k

u z z r m b p

Dieter Fox 12

SLIDE 4

4 Rao-Blackwellised PF for Inference

§ Represent posterior by random samples § Each sample

contains robot location, ball mode, ball Kalman filter

§ Generate individual components of a particle

stepwise using the factorization

i i i i i i i

m y x b m r s S = = , , , , , , , µ q

) , | ( ) , , | ( ) , , , | ( ) , | , , (

1 : 1 : 1 : 1 1 : 1 : 1 : 1 : 1 1 : 1 : 1 : 1 : 1 1 : 1 : 1 : 1 : 1

• ×

=

k k k k k k k k k k k k k k k k k

u z r p u z r m p u z r m b p u z r m b p

Dieter Fox 13 CSE-571: Probabilistic Robotics

Rao-Blackwellised Particle Filter for Inference

k-1

b

k

b r k-1 r k

k

m

k-1

m

Ball location and velocity Ball motion mode Map and robot location

Ball tracking Robot localization

) ( 1 ) ( 1 ) ( 1

, ,

i k i k i k

b m r

• § Draw a sample from the previous sample set:

Dieter Fox 14 CSE-571: Probabilistic Robotics

Generate Robot Location

r k-1 r k u k-1 zk

Map and robot location Robot control Landmark detection

Robot localization

l k-1

b

k-1

m

Ball location and velocity Ball motion mode

Ball tracking

k

b

k

m

_ _, , ) , , , , | ( ~

) ( 1 ) ( 1 ) ( 1 ) ( 1 ) ( i k k k i k i k i k k i k

r u z b m r r p r Þ

• Dieter Fox

15 CSE-571: Probabilistic Robotics

Generate Ball Motion Model

k-1

b r k-1 r k u k-1 zk

k

m

k-1

m

Ball location and velocity Ball motion mode Map and robot location Robot control Landmark detection

Ball tracking Robot localization

l k

b

_ , , ) , , , , | ( ~

) ( ) ( 1 ) ( 1 ) ( 1 ) ( ) ( i k i k k k i k i k i k k i k

m r u z b m r m p m Þ

• Dieter Fox

16 CSE-571: Probabilistic Robotics

SLIDE 5

5 Update Ball Location and Velocity

k-1

b r k-1 r k u k-1 zk

k

m

k-1

m

Ball location and velocity Ball motion mode Map and robot location Robot control Landmark detection

Ball tracking Robot localization

l k

b z k

b

) ( ) ( ) ( ) ( 1 ) ( ) ( ) (

, , ) , , , | ( ~

i k i k i k k i k i k i k k i k

b m r z b m r b p b Þ

• Dieter Fox

17 CSE-571: Probabilistic Robotics

Importance Resampling

§ Weight sample by if observation is landmark detection and by if observation is ball detection. § Resample ) | (

) ( ) ( i k l k i k

r z p w µ

k i k i k i k i k i k i k i k b k i k i k i k b k i k

b b r m b p b r m z p b r m z p w d ) , , | ( ) , , | ( ) , , | (

) ( 1 ) ( ) ( ) ( ) ( ) ( ) ( ) ( 1 ) ( ) ( ) (

• ò

= µ

Dieter Fox 18 CSE-571: Probabilistic Robotics

Ball-Environment Interaction

Dieter Fox 19 CSE473: Introduction to AI

Ball-Environment Interaction

Dieter Fox 20 CSE473: Introduction to AI

SLIDE 6

6 Tracking and Finding the Ball

§ Cluster ball samples by discretizing

pan / tilt angles

§ Uses negative information

Dieter Fox 21 CSE-571: Probabilistic Robotics

Experiment: Real Robot

0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5 5 10 15 20 25 30 35 40 45 50 Percentage of ball lost Number of ball samples

With Map Without Map

§ Robot kicks ball 100 times, tries to find it

afterwards

§ Finds ball in 1.5 seconds on average

Dieter Fox 22

Reference * Observations

Simulation Runs

Dieter Fox 23 CSE-571: Probabilistic Robotics

Comparison to KF* (optimized for straight

motion)

RBPF KF* Reference * Observations

Dieter Fox 24 CSE-571: Probabilistic Robotics

SLIDE 7

7

RBPF KF’ Reference * Observations

Comparison to KF’ (inflated prediction noise)

Dieter Fox 25 CSE-571: Probabilistic Robotics

Error vs. Prediction Time

10 20 30 40 50 60 70 80 0.5 1 1.5 2 RMS Error [cm] Prediction time [sec]

RBPF KF' KF* Dieter Fox 26 CSE-571: Probabilistic Robotics

Orientation Errors

20 40 60 80 100 120 140 160 180 2 3 4 5 6 7 8 9 10 11 Orientation Error [degrees] Time [sec]

RBPF KF* KF'

Dieter Fox 27 CSE-571: Probabilistic Robotics

Goalie

CSE-571: Probabilistic Robotics Dieter Fox 28

SLIDE 8

8 Discussion

§ Particle filters are intuitive and simple

§ Support point-wise thinking (reduced uncertainty) § Good for test implementation if system behavior is not well known

§ Inefficient compared to Kalman filter § Rao-Blackwellization

§ Only sample discrete / highly non-linear parts of state space § Solve remaining part analytically (KF,discrete)

Dieter Fox 29 CSE-571: Probabilistic Robotics