I’ve written in past posts about designing a basic control system for a drone in a 2D, top-down environment. Fundamentally, we found that if we know our position, and we know a target position we are trying to track towards, we can often design a control law that accomplishes our goal even with random wind and step changes in a state.

There’s one fundamental assumption in designing these control systems however, the idea that we know with some certainty what our own position is, and what the position of our target is!

Sensing

Say we’ve got a series of sensors on the ground and we’re trying to track an EM emitting robot driving around. These are cheap passive sensors, meaning we only get a noisy bearing towards the target and not a distance.

Challenge

The real problem here is that our bearing measurements are noisy. Without noise, we could calculate the intersection between any two bearing lines and that would be the vehicle position.

With some noise, the intersection point between any two measurements is liable to be quite far away from the actual robot position, making this approach infeasible.

Particle Filtering

One big idea here is that while directly computing the robot position from several noisy bearings is hard, it’s pretty easy to evaluate candidate robot positions in light of a new bearing measurement. In other words, when picking where to eat, it’s easier to decide if a suggested restaurant is close to what you want than to directly pick the optimal restaurant from scratch.

Given a sensor position, a bearing measurement, and a candidate robot position, we can compute the expected bearing between the sensor and the candidate robot position first. This value can be compared to the actual bearing measurement to generate a likelihood that the sensor measurement was produced by a robot in the candidate position. A big difference between the expected and observed bearing means this candidate is unlikely and a small difference means the candidate is likely.

Knowing this, we use the difference between the expected and observed measurements for a given candidate position to compute a “score” for the measurement. One possible scoring function is simply 1.0 / sqrt(abs(angleDiff)). When a small angle difference is found, this candidate gets a large score. When a large angle difference is found, the score of this candidate (or “particle”) trends towards 0.

Resampling

Given this list of particles with scores, we now look to create a new list of particles that are generally closer to the target location than before. If we continue to build lists of particles that are closer to the target, eventually the particle cloud should converge to the solution position. In order to create this better list of particles, we can randomly resample particles (with replacement) using scores as a probability of selection values.

Just implementing this solution will cause a problem however. If we resample continuously we’ll eventually get a completely homogenous list of particles closest to the target. Once the diversity of the particles is down to 0, any change in the target position (or more generally the target model) would not be caught by the filter.

In order to solve this problem, we can add a small bit of gaussian noise to each particle per iteration. This generates more candidate solutions, but generally close to the existing solutions, allowing the filter to adapt to changes in the model while further exploring the statespace near existing candidate solutions.

Demo

Below you can see a simple particle filter essentially solving the SLAM problem for a small 2D world. Each sensor is randomly selected to generate a measurement and the particle list is then updated based on that measurement. You can see the basic filter operation by slowing down the simulation using the slider.

Make sure to also push the simulation speed to max and click and drag to see the filter working live!