Particle filter - implementation aspects

One interesting problem in the field of Autonomous Systems is the state estimation. An example of such problem is a robot localization - it is when the robot knows the map of the environment but has no idea where it currently is on this map and needs to use the information from the sensors to determine it’s current pose in the real world. The problem is complicated by imperfect (noisy) sensors and actuators and other effects of the environment which is difficult to predict. A popular framework for solving this problem is the Bayesian filter - a method that treats the system state as a probability distribution function and changes it based on the noisy measurements from the sensors and the commands executed by the robot. It’s theoretical base is built on the properties of the conditional probabilities and the Bayes' theorem. The particle filter - is a widely applied variant of the Bayesian filter, which imposes very little restrictions on the estimated system itself, (compared, for example to Kalman filter) but could be computationally very expensive, as it models the probability distribution function via a large set of independent hypotheses, called particles.

In this post I’d like to review the basic parts of this algorithm and pay close attention to the implementation of its main parts - the robot model and of the filter’s update step.

Robot model

The Bayesian filters work by continuously performing two steps - state prediction and state correction (update), doing so requires having a model of the estimated system. In the robot localization problem this model would consist of the two parts (each needed for the corresponding step of the filter):

1. Dynamics model - describes how the robot state changes when it executes a given control command.
2. Measurement model - describes the expected output of the robot’s sensor readings in a given state with a known map. (Such output could be in a form of a vector of real values).

The robot exists in the real world, where the environment affects it’s behavior non deterministically and the actuators do not always perform the commands perfectly. The true robot dynamics is essentially random. In the Bayesian Filters, we model the dynamics as a stochastic function which accounts for these inaccuracies by adding a random noise, which is called Processing noise.

The sensor readings are not accurate either and the State Estimation Filter should be aware of that when it uses the measurement results in it’s update step. The noise corresponding to the inaccuracies in the measurement output is called Observation Noise.

The parameters of the Processing and the Observation noises are both used by the filter, provided by the developer and determined empirically by observing the real world performance of the system. On practice they could often be modeled as a multivariate Gaussians around the zero vector with the covariance matrices having some reasonably chosen values (in the most simple case it could be just a diagonal matrix, containing independent variances for each vector component).

Robot model implementation

The Particle filter applies the described above functions to a large set of different particles (hypothetical states in which the robot could be in reality) - which in the essence is a Single Instruction, Multiple Data condition. The implementation should take advantage of it (via either vectorization on the CPU or parallelization on the GPU if the particles are stored in VRAM). On practice, it means that the signatures of the methods used by the Particle filter, including the robot model functions should take multiple states corresponding to multiple particles, and take advantage of the data uniformity in the function implementation. The signatures could look like this:

  // Change the robot pose(s) according to the dynamics model by one time step
  void dynamics_step(States &states, const Control u, const bool noisy = true);
  // Receive relative landmark positions for each robot pose
  Measurements measure(const States &robot_states, const Landmarks &landmarks);

The memory layout of “States” and “Measurements” objects should be chosen accordingly. For example, if a single robot state consist of 3 variables - X, Y, Z - then placing all X’s from the different particles continuously first, then all Y’s, and then all Z’s (a different layout from the one which we’d get with the standard array of structs)- allows performing vectorized computations on different particles simultaneously using long registers - easily giving ~4x or more performance gain on the CPU.

Particle filter - predict and update steps

Every time the robot makes a move - our certainty about it’s position decreases, as a result of accumulating the processing noise. Every time we use the sensor readings to update the estimate - our certainty about the current robot state could increase (there is the observation noise, but the repeated measurements would drive the noise component to it’s mean (zero), at the same time we get a potentially insightful information about the current robot surroundings which we use to improve the estimate).

At the high level the particle filter algorithm looks the following way:

0. Initialize: We generate a large uniformly distributed set of states (particles), representing different assumptions about the true robot state.
1. Predict: When the robot executes a command - we predict each particle’s new state, conditioned on it’s current state by applying the dynamics model with the processing noise.
2. Update: When the robot receives the measurements from it’s sensors: We evaluate the likelihood of receiving such measurements conditioned on the state represented by each particle. (We use the measurement model and the parameters of the observation noise to compute it) We assign each particle a weight equals to this likelihood, and normalize it so that the sum of all weights is equal to 1. These weights are interpreted as a discrete probability distribution function, and a new set of particles of the same size is sampled from it with replacement, so that the particles with higher weights are more likely to be chosen (potentially multiple times) to the new set. We replace our particle set with this new one.
3. Estimate: To estimate the current robot state - we compute the mean value across the current particle set.

As a result of the update step many particles could collapse to the same position. This is what we want for the state estimation close to the correct one and it is, generally speaking safe, as after the next robot move the identical particles will disperse again due to the presence of the random processing noise. However if we are unlucky with the initial distribution, the particles could collapse to the states that have no chance to converge to the true robot position. To prevent this scenario we could replace a small number of particles with randomly generated positions after each update step. Another aspect is that the speed of convergence is being dependent on the processing noise. So in the default implementation, having a very accurate dynamics model with low processing noise could penalize the state estimation filter convergence. A way to improve it could be adjusting the processing noise level, depending on how rich and insightful were the last measurements. (To disperse the particles more when there is a way to compensate this inaccuracy by the information from the sensor readings).

Particle filter update step implementation

The first problem in applying the algorithm “as is” - tiny likelihoods associated with the each particle. Even before normalizing the weights they all will simply underflow to zero. The common solution to this problems is to use the logarithm of probability in all intermediate computations. Let’s take a look at how can we use it in the particle filter.

If our Observation noise is Gaussian - finding the logarithm of the likelihood of a certain measurement for each particle is not a difficult task. Let’s look at the probability distribution function of a multivariate Gaussian:

p(x) = exp(−1/2(x − µ)T Σ^(−1)(x − µ)) / ((2π)^(n/2)|Σ|^(1/2))

In our problem, µ and x are the output vector of the measurement model for a single particle, and the measurements vector received from the real robot (the values could be swapped), Σ is the observation noise covariance matrix. Assuming that the parameters (covariance matrix) of the observation noise do not change the denominator of the expression is constant. Moreover, as later in the course of the algorithm we normalise the likelihoods (dividing each value by the sum of all likelihoods) this term will be completely canceled out.

We are left with just exp(−1/2(x − µ)T Σ^(−1)(x − µ)), the logarithm of this expression is (−1/2(x − µ)T Σ^(−1)(x − µ)), how can we efficiently compute it for a large number of particles? The answer is by rewriting the problem from the vector format for a single particle to a matrix format for multiple particles and leveraging optimized linear algebra routines. An example using Eigen library on C++ could look like this:

// Return a vector of likelihoods for the "measurements",
// against a true measurement
Eigen::VectorXd ParticleFilter::ln_unnorm_likelihood_of_measurement(
    const Measurements &measurements, const Measurement &expected)
  // NxM matrix, where N is the number of measurements and M is the size of
  // one measurement
  auto errors = (measurements.rowwise() - expected.transpose());
  // MxM * MxN - > MxN, multiplication by the inverse covariance matrix
  auto tmp = r_inv * errors.transpose();
  // A scaled by -1/2 dot product of each row from the errors matrix
  // with each column of the tmp matrix. Computed via
  // element-wise multiplication of NxM * NxM matrices and sum of the columns.
  return -0.5 * (errors.array() * tmp.transpose().array()).rowwise().sum();
}

Ok, we got the log probs - now, how do we get the normalized likelihoods? Just taking the exponent directly would result in the same underflow for all particles. To address this we can use the Log-Sum-Exp trick: Get the largest log prob value across all particles. Subtract this value from all log probs. Compute the exponents and normalized the weights. Mathematically speaking, the Log-Sum-Exp does not change the outcome (it’s easy to prove that the subtracted constant from all log probs will be cancelled out in the final expression for the normalized value) at the same time this operation shifts our negative log prob values to the right, increasing the exponent values for them and ensures that the non normalized likelihood of the most probable particle will never underflow (as the exponent of zero is 1).

Finally, I would like to note that the dynamics and the measurement models do not necessary need to account for all complexities of the real world. Even if in the reality the robot can not pass through obstacles - doing the collision detection for each particle could be computationally very expensive. For the state estimation purposes it’s fine to allow the particle to travel through walls. The same thing goes for the measurement model - if the robot measurements depend on the presence of the landmarks which could be not always visible from all positions due to obstacles, we do not necessary need to model this complexity in the Robot measurement model. We can use the visibility information from the real robot measurements and take it into account in our likelihood calculations (in the simplest form we can zero out the vector components corresponding to the currently non visible landmarks in all measurements before computing the error) - I like the classic robotic tasks, because they always involve this blend of science and making the practical choices.

If you liked this post and would like to see the full implementation, I made a small C++ project with the simulation environment and the particle filter implementation for a simple differential drive robot operating in a 2d environment with obstacles and landmarks: github link