Particle Filter - Kidnapped vehicle project

Github: https://github.com/williamhyin/CarND-Kidnapped-Vehicle

1. Definition of Particle Filter

Bayesian filter is a particle filter or implemented Markov localization filter. Particle filter based on the "principle of survival of the fittest" is mainly used to solve the location problem. Particle filter has the advantage of flexibility and ease of programming.

Performance comparison of three filters:

Compare	State space	Belief	Efficiency	In robotic
Histogram filter	Discrete	Multi-modal	Exponential	approximate
Kalman filter	Continuous	Unimodal	Quadratic	approximate
Particle filter	Continuous	Multi-modal	?	approximate

As you can see in the picture above, the red dot is discrete speculation on the possible location of the robot. Each has a red dot x coordinate, y coordinate, and direction. The particle filter is a rear robot posteriori reliability of thousands of such speculation thereof. Beginning, the particles are uniformly distributed, but allows the filter is proportional to the ratio of their existence and consistency of the particle measured by the sensor.

Weight (Weights):

The particle filter typically carries a number of discrete particles. Is each particle comprising a x-coordinate, y-coordinate and vector direction. Their survival depends on the consistency of the particles with the sensor measurements. Consistency is based on the degree of match between the actual measurement and the predicted measurement to measure, it referred to the weight of this matching.

Weight mean particle measuring how close the actual and predicted measurements. In the particle filter, the particle weight, the higher the probability of survival. In other words, the probability of survival and the right of each particle is proportional.

Resampling (Resampling)

Resampling technique is for new particles randomly selected from the N particles in the old, replaced and re-scaled according to the importance weights. After resampling, the weights larger particles may stay down, other particles may disappear. The posterior probability of particle aggregation in higher areas.

To perform resampling, using a resampling technique wheel.

Principle: the probability of each particle being selected and the particles are round proportional share of the perimeter, significant weight particles have more chance of being selected.

The initial index is 6, assuming a random beta = 0 + random weights> w6, the index + 1, beta = beta-w6. In this case beta <w7, No. 7 particles are added to the selected warehouse. After the next cycle, when the value of beta and the previous cycle index still reserved, beta = beta + random weights> w7 + w8, therefore, the index is incremented twice and arriving index = 1, this time w1> beta, w1 He was selected into the warehouse, followed by the next cycle.

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

Particle filter has four main steps:

Initialization steps:

Initialization step: We estimate our position from the GPS input. Next steps in the process will be complete this estimate to locate our vehicles
Predicting step: In the predicting step, we added a control input (velocity and yaw rate) of all particles
Particle weight update step: In the update step, we use the position measurement update token and particle characteristics Map of Heavy
Resampling steps of: during resampling, we resample m times (m is in the range of 0 to length_of_particleArray) plotted particles i (i is a particle index) is proportional to its weight. This step uses a resampling wheel technology.
It represents a new particle filter Bayesian posterior probability. We now have a precise estimate of the input position based on proof vehicle.

Fake code:

Initialization steps:

The first thing is to initialize all the particles particle filter. In this step, we must decide how many particles you want to use. In general, we have to come up with a good figure, because if not too small, the error-prone, if too much will slow down the speed of the Bayesian filter. The traditional way is to initialize the particle state space is divided into a grid, and a particle placed in each cell, but this approach is only suitable for small state space, the state space if the earth, it is not appropriate. So with GPS position input to our initial estimate of the particle distribution is the most practical. It is worth noting that the measurement results of all the sensors must be accompanied by noise, in order to simulate the real noise uncontrollable circumstances, consideration should be given to the initial GPS position and heading of this project add Gaussian noise.

The final project initialization steps Code:

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 particle

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

    // Normal distributions
    normal_distribution<double> dist_x(x, std_x);
    normal_distribution<double> dist_y(y, std_y);
    normal_distribution<double> dist_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;
}

Prediction step:

Now that we have initialized the particle, is predicted when the position of the vehicle. Here, we will use the following equation to predict the vehicle next time step, and by updating the yaw rate based on the speed, taking into account the sensor Gaussian noise.

The project final step of predicting Code:

void ParticleFilter::prediction(double delta_t, double std_pos[],
    double velocity, double yaw_rate) {
    /**
     * TODO: Add measurements to each particle and add random Gaussian noise.
     * NOTE: When adding noise you may find std::normal_distribution
     *   and std::default_random_engine useful.
     *  http://en.cppreference.com/w/cpp/numeric/random/normal_distribution
     *  http://www.cplusplus.com/reference/random/default_random_engine/
     */
    //Normal distributions for sensor noise
    normal_distribution<double> disX(0, std_pos[0]);
    normal_distribution<double> disY(0, std_pos[1]);
    normal_distribution<double> angle_theta(0, std_pos[2]);
    
    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);

    }

}

Update steps:

Now, we have measured the speed and yaw rate input into our filter, we must update the particle weight based radar and lidar landmarks weight readings.

Update procedure has three main steps:

Transformation
Association
Update Weights

Conversion (Transformation)

We first need to convert the measured data of the car from a local car coordinate system coordinates on the map.

By passing vehicle to the observation coordinates (xc and YC), map the particle coordinates (xp and YP) and our rotation angle (- 90 degrees), the homogeneous transformation matrix, observed value of the vehicle coordinate system can be converted to the map coordinates (xm and ym). The homogeneous transformation matrix, as shown, performs rotation and translation.

The results of 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, we need to update his weights (4,5) is its position in the map coordinates, it is heading (-90 degrees), since the measurement results of the sensor is based on waypoints vehicle coordinate itself, so we should observe the vehicle data is converted into map coordinates. L1 as signs of real map coordinates (5,3), vehicle sensors measured OBS2 vehicle coordinates (2,2), after the map coordinates of a homogeneous matrix transformation is (6,3), and now we can the measurement results of contact with the real results together, matching the real world landmarks, the right to update the black boxes of heavy particles.
Contact (Association)

Contact problem is in the real world landmarks measurement and object matching, such as maps landmarks. Our ultimate goal is to find a weight parameter for each particle, the weight parameter represents the particle and the actual car matching degree in the same location.

Has now been converted to observations map's coordinate space, the next step is to convert the value of each observation with an identifier to associate. In the above map exercise, a total of five landmarks, each of which is identified as L1, L2, L3, L4, L5, each with a known map position. We need to convert each observation TOBS1, TOBS2, TOBS3 linked to one of these five identifier. To do this, we must be the closest landmarks linked to observe every transformation.

TOBS1= (6,3), TOBS2= (2,2 &) and TOBS3= (0,5). OBS1 matching L1, OBS2 matching L2, OBS3 matching L2 or L5 (the same distance).

The following example to explain the issue of data associated with it.

In this case, we have two lidar measurements of rock. We need to find the two measured values, which correspond to the rocks. If we estimate that any measure to be true, the position of the car will be based on our choice of measurement are different. In other words, depending on the choice of road signs, and ultimately determine the location of the vehicle will be different.

Since we have multiple measurements of landmarks, we can use the nearest neighbor technique to find the right one.

In this approach, we will measure the closest as a correct measurement.

Close to most of the advantages and disadvantages of law:

Update weights (update weights)

We have now completed the conversion and associated measurement, we have calculated all parts of the final weight of particles required. The final weight of the particle will be calculated for each measurement polyhydric - Gaussian probability density of the product.

Pluralism - Gaussian probability density has two dimensions, x and y. Mean multivariate Gaussian distribution is measured in relation to the position of the landmark, standard deviation multivariate Gaussian distribution is determined by our initial uncertainty in the x and y ranges to describe. Polyhydric - Gaussian evaluation based on the measured position after the conversion. Multivariate Gaussian distribution formula as follows.

Note: x, y is the observed value of the map coordinate system, [mu] x , μy recent signs of map coordinates. As for OBS2 (X, Y) = (2,2 &), ( [mu] x , μy ) = (2,1)

Error formula:

Representative wherein x, y, theta best particle position, X MEAS , Y MEAS , Theta MEAS represent the true value

The final weight of the project to update the code:

void ParticleFilter::updateWeights(double sensor_range, double std_landmark[],
                                   const vector<LandmarkObs> &observations,
                                   const Map &map_landmarks) {
    /**
     * TODO: Update the weights of each particle using a mult-variate Gaussian
     *   distribution. You can read more about this distribution here:
     *   https://en.wikipedia.org/wiki/Multivariate_normal_distribution
     * NOTE: The observations are given in the VEHICLE'S coordinate system.
     *   Your particles are located according to the MAP'S coordinate system.
     *   You will need to transform between the two systems. Keep in mind that
     *   this transformation requires both rotation AND translation (but no scaling).
     *   The following is a good resource for the theory:
     *   https://www.willamette.edu/~gorr/classes/GeneralGraphics/Transforms/transforms2d.htm
     *   and the following is a good resource for the actual equation to implement
     *   (look at equation 3.33) http://planning.cs.uiuc.edu/node99.html
     */
    double stdRange = std_landmark[0];
    double stdBearing = std_landmark[1];
    // Each particle for loop
    for (int i = 0; i < num_particles; i++) {
        double x = particles[i].x;
        double y = particles[i].y;
        double theta = particles[i].theta;
//        double sensor_range_2 = sensor_range * sensor_range;
        // Create a vector to hold the map landmark locations predicted to be within sensor range of the particle
        vector<LandmarkObs> validLandmarks;
        // Each map landmark for loop
        for (unsigned int j = 0; j < map_landmarks.landmark_list.size(); j++) {
            float landmarkX = map_landmarks.landmark_list[j].x_f;
            float landmarkY = map_landmarks.landmark_list[j].y_f;
            int id = map_landmarks.landmark_list[j].id_i;
            double dX = x - landmarkX;
            double dY = y - landmarkY;
            /*if (dX * dX + dY * dY <= sensor_range_2) {
                inRangeLandmarks.push_back(LandmarkObs{ id,landmarkX,landmarkY });
            }*/
            // Only consider landmarks within sensor range of the particle (rather than using the "dist" method considering a circular region around the particle, this considers a rectangular region but is computationally faster)
            if (fabs(dX) <= sensor_range && fabs(dY) <= sensor_range) {
                validLandmarks.push_back(LandmarkObs{id, landmarkX, landmarkY});
            }
        }
        // Create and populate a copy of the list of observations transformed from vehicle coordinates to map coordinates
        vector<LandmarkObs> transObs;
        for (int j = 0; j < observations.size(); j++) {
            double tx = x + cos(theta) * observations[j].x - sin(theta) * observations[j].y;
            double ty = y + sin(theta) * observations[j].x + cos(theta) * observations[j].y;
            transObs.push_back(LandmarkObs{observations[j].id, tx, ty});
        }

        // Data association for the predictions and transformed observations on current particle
        dataAssociation(validLandmarks, transObs);
        particles[i].weight = 1.0;

        for (unsigned int j = 0; j < transObs.size(); j++) {
            double observationX = transObs[j].x;
            double observationY = transObs[j].y;
            int landmarkId = transObs[j].id;

            double landmarkX, landmarkY;
            int k = 0;
            int nlandmarks = validLandmarks.size();
            bool found = false;
            // x,y coordinates of the prediction associated with the current observation
            while (!found && k < nlandmarks) {
                if (validLandmarks[k].id == landmarkId) {
                    found = true;
                    landmarkX = validLandmarks[k].x;
                    landmarkY = validLandmarks[k].y;
                }
                k++;
            }
            // Weight for this observation with multivariate Gaussian
            double dX = observationX - landmarkX;
            double dY = observationY - landmarkY;
            double weight = (1 / (2 * M_PI * stdRange * stdBearing)) *
                            exp(-(dX * dX / (2 * stdRange * stdRange) + (dY * dY / (2 * stdBearing * stdBearing))));
            // Product of this obersvation weight with total observations weight
            if (weight == 0) {
                particles[i].weight = particles[i].weight * 0.00001;

            } else {
                particles[i].weight = particles[i].weight * weight;
            }
        }
    }
}

Re-sampling step (Resample step)

Resampling technique is a new particle from the old random particles, and the weight proportion replaced according to importance weights. After resampling, the weights larger particles may stay down, other particles may disappear. This is the final step in the particle filter.

The final project resampling Code:

void ParticleFilter::resample() {
    /**
        * TODO: Resample particles with replacement with probability proportional
        *   to their weight.
        * NOTE: You may find std::discrete_distribution helpful here.
        *   http://en.cppreference.com/w/cpp/numeric/random/discrete_distribution
        */
    // Get weights and max weight.
    vector<double> weights;
    double maxWeight = numeric_limits<double>::min();
    for (int i = 0; i < num_particles; i++) {
        weights.push_back(particles[i].weight);
        if (particles[i].weight > maxWeight) {
            maxWeight = particles[i].weight;
        }
    }
    uniform_real_distribution<float> dist_float(0.0, maxWeight);
    uniform_real_distribution<float> dist_int(0.0, num_particles - 1);
    int index = dist_int(gen);
    double beta = 0.0;
    vector<Particle> resampledParticles;
    for (int i = 0; i < num_particles; i++) {
        beta += dist_float(gen) * 2.0;
        while (beta > weights[index]) {
            beta -= weights[index];
            index = (index + 1) % num_particles;
        }
        resampledParticles.push_back(particles[index]);
    }
    particles = resampledParticles;

}

3. Project Demo

Quote:

[Udacity]: www.udacity.com "Udacity"

[Medium]: https://medium.com/intro-to-artificial-intelligence/kidnapped-vehicle-project-using-particle-filters-udacitys-self-driving-car-nanodegree-aa1d37c40d49 "Dhanoop Karunakaran"

Linkedin: https://linkedin.com/in/williamhyin

williamhyin

Released seven original articles · won praise 3 · Views 553

Private letter concerns

Autopilot positioning technology - particle filter Practice

Particle Filter - Kidnapped vehicle project

1. Definition of Particle Filter

2. Particle Filters implementation

3. Project Demo

Guess you like