Particle Filter Self Localization - northern-bites/nbites GitHub Wiki

We use a Particle Filter to determine the location of our robots on the field. If you have any interest in localization, email EJ Googins at [email protected]. He'd be happy to help.

System Architecture

The Localization Module uses a LocSystem class for most computations. The design is meant to make all functions the Localization Module needs part of the LocSystem class, thus any children of LocSystem can be plugged into the Localization Module. In reality, there are a few operations that depend on the Particle Filter, however these functions only serve debugging purposes. So creating a new Localization System should be very straightforward: Inherit from LocSystem, comment any dynamic casts to a Particle Filter in the Localization Module, and change the Localization Systems constructor to instantiate your new system.

The point of this architecture is that Localization is a hard problem, and any replacements or upgrades should be empirically proven more effective than the current iteration. It's not hard to imagine running automated tests with logged and ground truth data throughout the different filters to determine which one is best.

LocSystem

As mentioned above, the LocSystem is the abstract interface for different Localization Systems to be used by the Localization Module. It contains the core functions (updates, resets) as well as estimate getters.

Motion System

The Motion System class is responsible for handling odometry. Odometry is where the Motion Engine (courtesy of B-Human) believes the robot has moved. It is important to remember what odometry represents when working with this system: When odometry is reset, a query to odometry would give 0,0,0 (no movement). Any future query until odometry is reset again gives the expected pose of the robot relative to the location it was reset at. So walking 50 cm forward would give odometry of 50,0,0. Then spinning to the left and walking 40 cm forward would give a new query of 50,40,-90. NOTE: The actual value would be -1.57 (radians) instead of -90 (degrees) when in c++

Vision System

The Vision System class is responsible for interfacing with our Vision Module to score our pose estimate. With the particle filter, this involves comparing the known locations on the field of objects to their 'reconstructed' location. That is, where that object would be if the pose, distance, and bearing to the observed object where all perfect. The given error is the length of the minimal line to a possible concrete location. Since lines aren't confined to one point, the error is given by the minimal area from the projected end points to all possible lines on the field.

Particle Filter

The Particle Filter 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.

Noggin

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