Particle Filter Self Localization

We use a Particle Filter to determine the location of our robots on the field.

Particle Filter

The Particle Filter class inherits from LocSystem and is responsible for maintaining a swarm of LocalizationParticle's. Each LocalizationParticle consists of a Location (x,y,h) and a weight and represents a single localization hypothesis. The Particle Filter class has two important methods, run and resample, as well as maintains a MotionModel and a SensorModel. The Particle Filter class also takes in a struct which contains the Particle Filter Params. Additionally, the Particle Filter has a python wrapper as a result from inheriting from LocSystem. Current research includes experimentation with Random Particle Injection, Augmented Monte Carlo Localization algorithms as a partial solution to the symmetric field problem, and a jiggle wiggle algorithm for robust localization against uncertainty.

run(bool motionUpdate, bool sensorUpdate)

run consists of three main steps:

1. Update the Motion Model in accordance to the particles if there has been a change in odometry. This corresponds to moving the particles

2. Update the Sensor Model if there has been a new observation. This corresponds to updating the weights of the particles.

3. If the Sensor Model was updated then the particles are then resampled

resample()

resample consist of two main steps:

1. Normalizing the partial weights. This allows the weights to be considered as probabilities. If the weights are all 0, then the particles are reset since the swarm is clearly terribly positioned.

2. Resample the particles by replacement to construct a new particle set "belief." The probability of a particle being selected is proportional to it's weight, and if the particle is selected it still remains in the set of possible selections

• The Particle Filter class also contains functions to reset the localization and reset the localization to a given location.

Motion Model

The MotionModel used by the Particle Filter is MotionSystem (which inherits from MotionModel). It simply updates particles based on odometry measurements from the motion system. It updates the particles throughout a call to update() which takes in a ParticleSet.

Sensor Model

The Sensor Model used by the Particle Filter is VisionSystem (which inherits from SensorModel). It implements an interface between the localization and the vision system and apply's visual landmark measurements to the particles as observations. Ambiguous Landmarks are put into a set of possibilities which are then compared to hypothesis vectors constructed from each particle. The closest landmark to the hypothesis vector is used when calculating that particles weights. This allows the swarm to entertain multiple hypotheses simultaneously in specific ambiguous situations.

Noggin

In Noggin, updateLocalization is responsible for constructing landmarks from VisualFieldObjects and passing these observations to the particle filter.

"This is where the magic happens" - Ellis Ratner