TechUnited Robocup –

the object tracking problemRené van de Molengraft

MSL Workshop, Stuttgart, November 5/6th , 2009

2/26

Looking back• No joint paper

• two-day visits Stuttgart and Tribots

• Special issue IFAC Mechatronics on MSL

• EU-Project RoboEarth

3/26

Evolution and revolution

Devbot0 2005

Model 2006

Model 2007

Model 2008

Model 2009

4/25

5/26

• 5 against 5

• >30 kg, 4 m/s

• omni-vision: 30 x 640 x 480 x 3 bytes/s

• assume features (x, y, r) from omni-vision

• noise (shadow, motion blur, finite resolution) and multiple features/object

6/26

7/25

On the field

8/26

Object tracking

12

time 1

12

time 2

• data association

• find best state estimates given past position measurements

9/26

Combinatorial explosion

12

534

6

• at time t1: m1 measurements, n1 objects

• m1*n1 possible associations

• at next time t2: m1*n1*m2*n2

10/26

State estimation

• 2-D position measurements from vision

• constant velocity model

• stationary Kalman filter

11/26

Object model

Cqy

Aqq

01

00

00

01 ,

0000

1000

0000

0010

, CA

v

y

v

x

q

y

x

• zero-acceleration

12/26

Kalman filter

vyz

qCzKqAq

)ˆ(ˆ̂

• choose uncertainty weigths

• solve Riccati equation -> K

• variance equation is disregarded

13/26

Sequential clustering using Kalman filters

• inspired by Schubert and Sidenbladh, Sequential clustering with particle filters – estimating the number of clusters from data, 2005

• prune exploding tree by particle filtering

• static objects only

14/26

Sequential clustering using Kalman filters

• prune exploding tree by discrete filtering

• track dynamic objects

• incorporate a-priori knowledge

15/26

Sequential clustering using Kalman filters

• measurements are processed sequentially

• measurement can be associated with

• clutter

• new object

• existing object

16/26

Tree with all possible associations

[]

[0] [1]

[0,0] [0,1] [1,0] [1,1] [1,2]

measurement 1

measurement 2

• 1, 2, 5, 52, 203, 877, 4140, 21147, 115975, 678570

17/26

Hypothesis H

• e.g. H= [0, 1, 1, 2, 1, 2, 3, 0, 1, 1, 1, 4, 2]

• so, 4 objects -> 4 Kalman filters

• each hypothesis is assigned a probability

• propagate hypotheses via Bayesian filtering

)|()|(

)|()|()|( 11

1

1

kkkk

kkkkkk ZHP

ZzP

HHPHzPZHP

18/26

Prediction )|( 1kk HHP

02.0 ,01.0 ..

/)1(

cge

nPPP

PP

cP

objnewclutterexisting

existingnew

clutter

19/26

Correction )|( kk HzP

kqC ˆkz

corrP

• is the position of the object in assigned to measurement k

kHkqC ˆ

20/26

Pruning by discrete filter

• set of hypotheses form a discrete distribution

• select n hypotheses with highest probability

• throw away hypotheses with P<0.01*Pmax

• tree is pruned to at most n hypotheses

• n = 10 still works for 10 to 20 objects!

21/26

A-priori knowledge

• region of interest

• clip beyond boundary in state space

• measurements

• objects in hypotheses• maximum number of objects in hypothesis

22/26

• Maximum a Posteriori (MAP) estimate

• states

Best estimate

)(max PHH kP

MAPk

objn jkq ,ˆ

23/26

24/26

Implementation

• Simulink S-function in C

• cpu effort

• linear in #measurements

• linear in #hypotheses

• quadratic in #objects

• < 5 ms (10, 20, 10)

• add predictor for anticipation