Autonomous Driving Positioning Technology - Particle Filter Practical Case Analysis

Particle Filter - Kidnapped vehicle project

1. Definition of Particle Filter

Particle filter is the implementation of Bayesian filter or Markov positioning filter. Particle filter is mainly used to solve positioning problems based on the "survival of the fittest principle". The advantage of particle filter is that it is easy to program and flexible. Performance comparison of three filters:

As you can see in the image above, the red dots are discrete guesses about where the robot might be. Each red dot has an x-coordinate, a y-coordinate, and a direction. The particle filter is a representation of the robot's posterior confidence that is composed of thousands of these guesses. At first, the particles are uniformly distributed, but the filter makes them live in proportion to how well the particle agrees with the sensor measurement.

Weights:

Particle filters usually carry a discrete number of particles. Each particle is a vector containing an x-coordinate, a y-coordinate, and a direction. The survival of particles depends on their consistency with the sensor measurements. The consistency is measured based on the match between the actual measurement and the predicted measurement, which is called a weight.

The weight means how close the actual measurement of the particle is to the predicted measurement. In a particle filter, the larger the particle weight, the higher the survival probability. In other words, the survival probability of each particle is proportional to the weight.

Resampling

Resampling technique is used to randomly extract N new particles from old particles and replace them in proportion according to the importance weights. After resampling, particles with larger weights may stay and other particles may disappear. Particles gather in areas with higher posterior probability. For resampling, resampling wheel technique is used.

Principle: The probability of each particle being selected is proportional to the circumference of this particle wheel. Particles with large weights have more chances to be selected. The initial index is 6. Assuming the random beta = 0 + random weight > w6, then index +1, beta = beta-w6. At this time, beta < w7, particle 7 is selected and added to the warehouse. Then the next cycle is carried out. At this time, beta and index still retain the values ​​of the previous cycle, beta = beta + random weight > w7 + w8, so the index is incremented twice to index = 1. At this time, w1 > beta, w1 is selected and put into the warehouse, and then the next cycle is carried out. Resampling code:

p3 = []
index = int(random.random()*N)
beta=0.0
mw=max(w)
for i in range(N):
    beta +=random.random()*2.0*mw
    while beta>w[index]:
        beta-=w[index]
        index=(index+1)%N
    p3.append(p[index])
p=p3

2. Particle Filters implementation

There are four main steps in particle filtering:

Initialization step: We estimate our location from GPS input. Subsequent steps in the process will refine this estimate to localize our vehicle

Prediction step: In the prediction step, we add the control inputs (yaw rate and velocity) of all particles

Particle weight update step: In the update step, we update the particle weights using measurements of map landmark positions and features

Resampling step: During resampling, we will resample m times (m is the range of 0 to length_of_particleArray) to draw particle i (i is the particle index) proportional to its weight. This step uses the resampling wheel technique.

The new particle represents the Bayesian filtered posterior probability. We now have an accurate estimate of the vehicle's position based on the input proof.

pseudocode:

1. Initialization steps:

The first thing a particle filter does is initialize all the particles. In this step, we have to decide how many particles to use. In general, we have to come up with a good number because if it is not too small, it will be prone to errors, and if it is too many, it will slow down the Bayesian filter. The traditional way to initialize particles is to divide the state space into a grid and place a particle in each cell, but this method is only suitable for small state spaces. If the state space is the earth, this is not appropriate. Therefore, it is most practical to use GPS position input to initially estimate our particle distribution. It is worth noting that the measurements of all sensors are inevitably accompanied by noise. In order to simulate the real uncontrollable noise situation, you should consider adding Gaussian noise to the initial GPS position and heading of this project. The final initialization step code of the project:
void ParticleFilter::init(double x, double y, double theta, double std[]) {

    /**
     * TODO: Set the number of particles. Initialize all particles to
     * first position (based on estimates of x, y, theta and their uncertainties
     * from GPS) and all weights to 1.
     * TODO: Add random Gaussian noise to each particle.
     * NOTE: Consult particle_filter.h for more information about this method
     * (and others in this file).
     */

    if (is_initialized) {
        return;
    }
    num_particles = 100; // TODO: Set the number of particles

    double std_x = std[0];
    double std_y = std[1];
    double std_theta = std[2];

    // Normal distributions
    normal_distributiondist_x(x, std_x);
    normal_distributiondist_y(y, std_y);
    normal_distributiondist_theta(theta, std_theta);

    // Generate particles with normal distribution with mean on GPS values.
    for (int i = 0; i < num_particles; ++i) {
        Particle pe;
        pe.id = i;
        pe.x = dist_x(gen);
        pe.y = dist_y(gen);
        pe.theta = dist_theta(gen);
        pe.weight = 1.0;
        particles.push_back(pe);
        
    }
    is_initialized = true;
}

2. Prediction Step: Now that we have initialized the particles, it is time to predict the position of the vehicle. Here we will use the following formula to predict where the vehicle will be at the next time step, by updating based on the yaw rate and velocity while taking into account the Gaussian sensor noise.

The final prediction step code for the project:

for (int i = 0; i < num_particles; i++) { if (fabs(yaw_rate) >= 0.00001) { particles[i].x += (velocity / yaw_rate) * (sin(particles[i].theta + yaw_rate * delta_t) - sin(particles[i].theta)); particles[i].y += (velocity / yaw_rate) * (cos(particles[i].theta) - cos(particles[i].theta + yaw_rate * delta_t)); particles[i].theta += yaw_rate * delta_t; } else { particles[i].x += velocity * delta_t * cos(particles[i].theta); particles[i].y += velocity * delta_t * sin(particles[i].theta); } // Add noise particles[i].x += disX(gen); particles[i].y += disY(gen); particles[i].theta += angle_theta(gen); }

3. Update steps:

Now that we have incorporated velocity and yaw rate measurement inputs into our filter, we must update the particle weights based on the lidar and radar landmark readings. The update step has three main steps:

Transformation

Association

Update Weights

Transformation We first need to transform the car's measurements from the local car coordinate system to the coordinate system on the map.

By passing the vehicle observation coordinates (xc and yc), the map particle coordinates (xp and yp), and our rotation angle (-90 degrees), the observations in the vehicle coordinate system can be transformed into map coordinates (xm and ym) using a homogeneous transformation matrix. This homogeneous transformation matrix, shown below, performs both rotation and translation.

The result of the matrix multiplication is:

Code

 double x_part, y_part, x_obs, y_obs, theta;
  double x_map;
 x_map = x_part + (cos(theta) * x_obs) - (sin(theta) * y_obs);
  double y_map;
 y_map = y_part + (sin(theta) * x_obs) + (cos(theta) * y_obs);

Note: The black box is a particle, and we need to update its weight. (4,5) is its position in the map coordinates, and its heading is (-90 degrees). Since the sensor's measurement of the road sign is based on the vehicle's own coordinates, we need to convert the vehicle's observation data into map coordinates. For example, the real map coordinates of the L1 road sign are (5,3), and the vehicle coordinates of OBS2 measured by the vehicle sensor are (2,2). The map coordinates after the homogeneous matrix conversion are (6,3). Now we can associate the measurement results with the real results, match the landmarks in the real world, and update the weight of the black box particle.
Association

The connection problem is the problem of matching landmark measurements with objects in the real world, such as map landmarks. Our ultimate goal is to find a weight parameter for each particle that represents how closely this particle matches the actual car at the same location.

Now that the observations have been transformed into the coordinate space of the map, the next step is to associate each transformed observation with a landmark identifier. In the map exercise above, we have a total of 5 landmarks, each identified as L1, L2, L3, L4, L5, each with a known map location. We need to associate each transformed observation TOBS1, TOBS2, TOBS3 with one of these 5 identifiers. In order to do this, we must associate the closest landmark to each transformed observation.
TOBS1 = (6,3), TOBS2 = (2,2) and TOBS3 = (0,5). OBS1 matches L1, OBS2 matches L2, and OBS3 matches either L2 or L5 (same distance).
The following example explains the problem of data association.

In this case, we have two LiDAR measurements of a rock. We need to find out which of the two measurements corresponds to the rock. If we estimate that any measurement is true, the position of the car will be different depending on which measurement we choose. That is, depending on which landmark we choose, the final determined position of the vehicle will also be different.
Since we have multiple measured landmarks, we can use the nearest neighbor technique to find the correct one.