A bunch of us at RTeam dream of the day when we will have our Mechs fully autonomous. I've also had it on my list of thing to do to figure out particle filters (PF). So as a bit of side diversion I've made a simulation of a Mech in a arena using PF localization (determining its location on a map). The simulation is written in the Matlab language.

The video on the left is the "Truth" model. Its a simulation of a Mech walking around the arena. It is assumed that the Mech has an angular rate sensor (gyro) in the yaw axis, two accelerometers in the x- and y-axes that are integrated to determine rates, a magnetometer to determine heading, as well as four sonar sensors to locate the buildings/arena walls.

The video on the right is the estimate of the mech pose (location and orientation). It uses the sensor data (with noise and bias sources) and a PF to determine its pose. If you read any papers on PFs they can seem quite daunting, but if you cut through it all they aren't actually that bad.

The way a particle filter works is that instead of running a single estimator for pose, it runs multiple estimators using Monte Carlo (randomizaton) methods. Each of these estimators are called "particles". At each time step in the estimate of the current pose, it uses the sonar sensor data and compares each estimator to a locally stored map of the arena to determine how well each is performing. For the next time step, it will randomly resample (select) each estimator based on how well its performing. Each individual estimator can be resampled zero, one, or multiple times. Higher performing estimators are more likely to be reselected. A bit of randomization is then added to each resampled estimator and the process continues. At any time step, the "output" estimate is just the average of all the estimators.

Its really quite a clever way of estimating pose but is very computationally intensive. May be a challenge implementing this on a small Mech microprocessor. I'll be working on this over the next year and (hopefully) will have something running by the 2017 Robogames. I'll also post the code once I get it cleaned up a bit if anyone wants to see it (and has access to Matlab).

## Bookmarks