How to track multiple robots with particle filter

I am using an IR camera to track N mobile robots driving about on the floor. Each robot has a few IR LEDs on its head in known locations, all at the same height above the floor. Each robot has 5 degrees of freedom, X, Y, theta, rotation rate, and velocity. All the camera sees is a bunch of blobs. I have a working blob detector, and can calculate the coordinates of visible blobs in world space. Now I would like to implement a particle filter.

I have two options:

1. Implement a single particle filter with a state space of 5xN dimensions.
2. Implement N particle filters with 5 dimensional state spaces.

My feeling is that 1. is the correct way to approach the problem, because otherwise each particle filter could easily get confused about which particle belongs to which robot. But, on the other hand, it seems like a lot of dimensions, and could be slow.

This is a good question, in that it's actually two good questions. The answer to which of those two options to use, is that they are the same. Your idea of "confusion" is intuitively correct, but is not a function of how you structure the state, but is instead about how you associate the measurements to part of the state (which robot was observed).

Problem 1: What red dot is attached to which robot. That's the data association problem.

Problem 2: How to use a particle filter to track $N$ robots.

Those problems are independent* of each other, and choosing a $5xN$ state versus five $N$ states doesn't help you resolve the "confusion" of which dot corresponds to which robot.

Update: How large of a state would you need to also estimate the association between measurements and robots as well as robot states?

Suppose you have 1 measurement, you need to estimate which robot it is associated with (let's call that $m_1$).

Say, $m_1=1$ with probability $p_1$, $m_2=2$ with probability $p_2$, etc. So, the state of the robot "fragments". Even for robot 1, you have its PDF as:

$$p(R_1) = p(R_1|m_1=1)p(m_1=1)+p(R_1|m_1=2)p(p_1=2)+p(R_1|m_1=3)p(m_1=3)...$$

or for a few robots:

$$p(R_j) = \sum p(R_j|m_1=i)p(m_1=i)$$

Now, what if you have two measurements:

\begin{align} p(R_1) &= p(R_1|m_1=1,m_2=1)p(m_1=1,m_2=1)\\ &+p(R_1|m_1=1,m_2=2)p(m_1=1,m_2=2) \\ ...\\ &+p(R_1|m_1=2,m_2=1)p(m_1=2,m_2=1)\\ &+p(R_1|m_1=2,m_2=2)p(m_1=2,m_2=2)\\ ...\\ &+p(R_1|m_1=3,m_2=1)p(m_1=3,m_2=1)\\ &+p(R_1|m_1=3,m_2=2)p(m_3=3,m_2=2)\\ ...\\ &+p(R_1|m_1=5,m_2=1)p(m_1=5,m_2=1)\\ &+p(R_1|m_1=5,m_2=2)p(m_3=5,m_2=2)\\ \end{align}

There are $2^K$ possible combinations / associations of measurements for each robot and $K$ measurements... and that's just one robot's state estimate. You need $N$ total.

In short, the state space explodes in size when you try to associate a PDF over combinations of measurements. You'll need a hugely increasing number of particles to even have a chance at approximating the real state.

There are very advanced filters that try to do this (see Probability Hypothesis Density Filter). But in general, estimating the mapping between measurements and robots is, as mentioned, the data association problem.

So, bringing it back down to earth, what you want to do is come up with a way to do good associations before you run your filter.

Some commentary:

Particle filters are not methods for tracking points that appear on robots. Not really. Particle filters sample the possible states for each robot, and assign a liklihood of that sample being correct.

What you could do, is run through possible associations of spots to robots and simultaneously estimate the states of the robots assuming that association is correct.

Here's a paper that might help. Particle filters and data association for multi-target tracking by Mats Ekman

It's just the first one that popped up in a google search, and talks about how to choose data association techniques while also using a particle filter to estimates state.

• This is an oversimplification. You can use a PF to solve both simultaneously by having a PDF over measurement associations and also sampling that. But I doubt that's what you're asking. Also, if the robot positions are correllated, then option 1 and 2 are not equivalent. However, this is not often assumed.
• Thanks for the answer. I've been trying to understand what you're saying. My thought was that a 5xN state space would be addressing the data association problem, because it would measuring the probability that certain associations were correct. – Rocketmagnet May 7 '17 at 10:42
• No, a 5xN state would still only estimate the 5 states for each N robots. I'll update the answer. – Josh Vander Hook May 7 '17 at 16:58