Particle Filters

A Particle Filter is another implementation of the Bayes Filter.

In a Particle Filter, we create particles throughout the area defined by the GPS and we assign a weight to each particle.

The weight of a particle represents the probability that our vehicle is at the location of the particle.

Unlike the Kalman filter, we have our probabilities are not continuous values but discrete values, we talk about weights.

Kalman Filter vs Particle Filter (source)

Algorithm

The implementation of the algorithm is according to the following scheme. We distinguish four stages (Initialization, Prediction, Update, Sampling) realized with the help of several data (GPS, IMU, speeds, measurements of the landmarks).

Localization algorithm

Initialization — We use an initial estimate from the GPS and add noise (due to sensor inaccuracy) to initialize a chosen number of particles . Each particle has a position (x, y) and an orientation θ .

This gives us a particle distribution throughout the GPS area with equal weights.

— We use an from the GPS and add noise (due to sensor inaccuracy) to . . This gives us a particle distribution throughout the GPS area with equal weights. Prediction — Once our particles are initialized, we make a first prediction taking into account our speed and our rotations. In every prediction, our movements will be taken into account. We use equations describing x, y, θ (orientation) to describe the motion of a vehicle.

Motion equations

Update — In our update phase, we first realize a match between our measurements n and the map m.

Match between measurements and map

We use sensor fusion data to determine surrounding objects and then update our weights with the following equation :

Update

In this equation, for each particle:

- σx and σy are our uncertainties

- x and y are the observations of the landmarks

- μx and μy are the ground truth coordinates of the landmarks coming from the map.

In the case where the error is strong, the exponential term is 0, the weight of our particle is 0 as well. In the case where it is very low, the weight of the particle is 1 standardized by the term 2π.σx.σy.

Resampling — Finally, we have one last stage where we select the particles with the highest weights and destroy the least likely ones.

The higher the weight, the more likely the particle is to survive.

The cycle is then repeated with the most probable particles, we take into account our displacements since the last computation and realize a prediction then a correction according to our observations.

Particle filter in action

Particle filters are effective and can locate a vehicle very precisely. For each particle, we compare the measurements made by the particle with the measurements made by the vehicle and calculate a probability or weight. This calculation can makes the filter slow if we have a lot of particles. It also requires having the map of the environment where we drive permanently.

Results

My projects with Udacity taught me how to implement a Particle Filter in C ++. As in the algorithm described earlier, we implement localization by defining 100 particles and assigning a weight to each particle through measurements made by our sensors.

In the following video, we can see :

A green laser representing the measurements from the vehicle .

representing the . A blue laser representing the measurements from the nearest particle (blue circle).

representing the (blue circle). A particle locating the vehicle (blue circle).

locating the (blue circle). The black circles are our landmarks (traffic lights, signs, bushes, …) coming from the map.

Particle Filter Implementation

SLAM (Simultaneous Localization And Mapping)

Another very popular method is called SLAM, this technique makes it possible to estimate the map (the coordinates of the landmarks) in addition to estimating the coordinates of our vehicle.

To work, we can with the Lidar find walls, sidewalks and thus build a map. SLAM’s algorithms need to know how to recognize landmarks, then position them and add elements to the map.

SLAM created map (source)

Conclusion

Localization is an essential topic for any robot or autonomous vehicle. If we can locate our vehicle very precisely, we can drive independently. This subject is constantly evolving, the sensors are becoming more and more accurate and the algorithms are more and more efficient. SLAM techniques are very popular for outdoor and indoor navigation where GPS are not very effective. Cartography also has a very important role because without a map, we cannot know where we are.

Today, research is exploring localization using deep learning algorithms and cameras.

Now that we are localized and know our environment, we can discuss algorithms for creating trajectories and making decisions! Before I conclude, I would like to invite you to the private mailing list. Every day, I send an email where I share my experience on AI and Autonomous technology. Starting tomorrow, you will receive life-changing career advices, technical content like this one or discounts for my courses. Subscribe here, I hope to see you in tomorrow’s email. Jeremy Cohen.

Next on Autonomous Navigation, Controllers and Final Integration