自动驾驶定位算法(十三)-粒子滤波(Particle Filter)

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.
  // Add random Gaussian noise to each particle.
  // NOTE: Consult particle_filter.h for more information about this method (and
  // others in this file).
  // TODO Complete

  /*Number is particles are chosen in order to run the algorithm in almost real
  time and introduce lowest possible error in localization. This is a tunable
  parameter.*/
  num_particles = 20;
  default_random_engine gen;

  normal_distribution<double> dist_x(x, std[0]);
  normal_distribution<double> dist_y(y, std[1]);
  normal_distribution<double> dist_theta(theta, std[2]);

  int i;
  for (i = 0; i < num_particles; i++) {
    Particle current_particle;
    current_particle.id = i;
    current_particle.x = dist_x(gen);
    current_particle.y = dist_y(gen);
    current_particle.theta = dist_theta(gen);
    current_particle.weight = 1.0;

    particles.push_back(current_particle);
    weights.push_back(current_particle.weight);
  }
  is_initialized = true;
}

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/
  // TODO complete

  default_random_engine gen;
  int i;
  for (i = 0; i < num_particles; i++) {
    double particle_x = particles[i].x;
    double particle_y = particles[i].y;
    double particle_theta = particles[i].theta;

    double pred_x;
    double pred_y;
    double pred_theta;
    // Instead of a hard check of 0, adding a check for very low value of
    // yaw_rate
    if (fabs(yaw_rate) < 0.0001) {
      pred_x = particle_x + velocity * cos(particle_theta) * delta_t;
      pred_y = particle_y + velocity * sin(particle_theta) * delta_t;
      pred_theta = particle_theta;
    } else {
      pred_x = particle_x + (velocity / yaw_rate) *
                                (sin(particle_theta + (yaw_rate * delta_t)) -
                                 sin(particle_theta));
      pred_y = particle_y + (velocity / yaw_rate) *
                                (cos(particle_theta) -
                                 cos(particle_theta + (yaw_rate * delta_t)));
      pred_theta = particle_theta + (yaw_rate * delta_t);
    }

    normal_distribution<double> dist_x(pred_x, std_pos[0]);
    normal_distribution<double> dist_y(pred_y, std_pos[1]);
    normal_distribution<double> dist_theta(pred_theta, std_pos[2]);

    particles[i].x = dist_x(gen);
    particles[i].y = dist_y(gen);
    particles[i].theta = dist_theta(gen);
  }
}

void ParticleFilter::dataAssociation(std::vector<LandmarkObs> predicted,
                                     std::vector<LandmarkObs> &observations,
                                     double sensor_range) {
  // TODO: Find the predicted measurement that is closest to each observed
  // measurement and assign the
  //   observed measurement to this particular landmark.
  // NOTE: this method will NOT be called by the grading code. But you will
  // probably find it useful to
  //   implement this method and use it as a helper during the updateWeights
  //   phase.
  // TODO complete

  /*Associate observations in map co-ordinates to predicted landmarks using
nearest neighbor algorithm. Here, the number of observations may be less than
the total number of landmarks as some of the landmarks may be outside the range
of vehicle's sensor.*/
  int i, j;
  for (i = 0; i < observations.size(); i++) {
    // Maximum distance can be square root of 2 times the range of sensor.
    double lowest_dist = sensor_range * sqrt(2);
    int closest_landmark_id = -1;
    double obs_x = observations[i].x;
    double obs_y = observations[i].y;

    for (j = 0; j < predicted.size(); j++) {
      double pred_x = predicted[j].x;
      double pred_y = predicted[j].y;
      int pred_id = predicted[j].id;
      double current_dist = dist(obs_x, obs_y, pred_x, pred_y);

      if (current_dist < lowest_dist) {
        lowest_dist = current_dist;
        closest_landmark_id = pred_id;
      }
    }
    observations[i].id = closest_landmark_id;
  }
}

void ParticleFilter::updateWeights(double sensor_range, double std_landmark[],
                                   std::vector<LandmarkObs> observations,
                                   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
  //   TODO complete

  int i, j;
  /*This variable is used for normalizing weights of all particles and bring
    them in the range of [0, 1]*/
  double weight_normalizer = 0.0;

  for (i = 0; i < num_particles; i++) {
    double particle_x = particles[i].x;
    double particle_y = particles[i].y;
    double particle_theta = particles[i].theta;

    /*Step 1: Transform observations from vehicle co-ordinates to map
     * co-ordinates.*/
    // Vector containing observations transformed to map co-ordinates w.r.t.
    // current particle.
    vector<LandmarkObs> transformed_observations;

    // Transform observations from vehicle's co-ordinates to map co-ordinates.
    for (j = 0; j < observations.size(); j++) {
      LandmarkObs transformed_obs;
      transformed_obs.id = j;
      transformed_obs.x = particle_x +
                          (cos(particle_theta) * observations[j].x) -
                          (sin(particle_theta) * observations[j].y);
      transformed_obs.y = particle_y +
                          (sin(particle_theta) * observations[j].x) +
                          (cos(particle_theta) * observations[j].y);
      transformed_observations.push_back(transformed_obs);
    }

    /*Step 2: Filter map landmarks to keep only those which are in the
     sensor_range of current particle. Push them to predictions vector.*/
    vector<LandmarkObs> predicted_landmarks;
    for (j = 0; j < map_landmarks.landmark_list.size(); j++) {
      Map::single_landmark_s current_landmark = map_landmarks.landmark_list[j];
      if ((fabs((particle_x - current_landmark.x_f)) <= sensor_range) &&
          (fabs((particle_y - current_landmark.y_f)) <= sensor_range)) {
        predicted_landmarks.push_back(LandmarkObs{
            current_landmark.id_i, current_landmark.x_f, current_landmark.y_f});
      }
    }

    /*Step 3: Associate observations to lpredicted andmarks using nearest
     * neighbor algorithm.*/
    // Associate observations with predicted landmarks
    dataAssociation(predicted_landmarks, transformed_observations,
                    sensor_range);

    /*Step 4: Calculate the weight of each particle using Multivariate Gaussian
     * distribution.*/
    // Reset the weight of particle to 1.0
    particles[i].weight = 1.0;

    double sigma_x = std_landmark[0];
    double sigma_y = std_landmark[1];
    double sigma_x_2 = pow(sigma_x, 2);
    double sigma_y_2 = pow(sigma_y, 2);
    double normalizer = (1.0 / (2.0 * M_PI * sigma_x * sigma_y));
    int k, l;

    /*Calculate the weight of particle based on the multivariate Gaussian
     * probability function*/
    for (k = 0; k < transformed_observations.size(); k++) {
      double trans_obs_x = transformed_observations[k].x;
      double trans_obs_y = transformed_observations[k].y;
      double trans_obs_id = transformed_observations[k].id;
      double multi_prob = 1.0;

      for (l = 0; l < predicted_landmarks.size(); l++) {
        double pred_landmark_x = predicted_landmarks[l].x;
        double pred_landmark_y = predicted_landmarks[l].y;
        double pred_landmark_id = predicted_landmarks[l].id;

        if (trans_obs_id == pred_landmark_id) {
          multi_prob = normalizer *
                       exp(-1.0 * ((pow((trans_obs_x - pred_landmark_x), 2) /
                                    (2.0 * sigma_x_2)) +
                                   (pow((trans_obs_y - pred_landmark_y), 2) /
                                    (2.0 * sigma_y_2))));
          particles[i].weight *= multi_prob;
        }
      }
    }
    weight_normalizer += particles[i].weight;
  }

  /*Step 5: Normalize the weights of all particles since resmapling using
   * probabilistic approach.*/
  for (int i = 0; i < particles.size(); i++) {
    particles[i].weight /= weight_normalizer;
    weights[i] = particles[i].weight;
  }
}

p3 = []
index = int(random.random()*N)
beta = 0.0
mw = max(w)
for i in range(N):
  beta += random.random()*2*mw
  while w[index] < beta:
     beta = beta - w[index]
     index = index + 1

  p3.append(p[index])

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
  //   TODO complete
  
  vector<Particle> resampled_particles;

  // Create a generator to be used for generating random particle index and beta
  // value
  default_random_engine gen;

  // Generate random particle index
  uniform_int_distribution<int> particle_index(0, num_particles - 1);

  int current_index = particle_index(gen);

  double beta = 0.0;

  double max_weight_2 = 2.0 * *max_element(weights.begin(), weights.end());

  for (int i = 0; i < particles.size(); i++) {
    uniform_real_distribution<double> random_weight(0.0, max_weight_2);
    beta += random_weight(gen);

    while (beta > weights[current_index]) {
      beta -= weights[current_index];
      current_index = (current_index + 1) % num_particles;
    }
    resampled_particles.push_back(particles[current_index]);
  }
  particles = resampled_particles;
}

double delta_t = 0.1;     // Time elapsed between measurements [sec]
double sensor_range = 50; // Sensor range [m]

double sigma_pos[3] = {
    0.3, 0.3, 0.01}; // GPS measurement uncertainty [x [m], y [m], theta [rad]]
double sigma_landmark[2] = {
    0.3, 0.3}; // Landmark measurement uncertainty [x [m], y [m]]

// Read map data
Map map;
if (!read_map_data("../data/map_data.txt", map)) {
  cout << "Error: Could not open map file" << endl;
  return -1;
}

// Create particle filter
ParticleFilter pf;

while (循环接收车辆的控制信息和Sense信息) {

  if (!pf.initialized()) {

    // Sense noisy position data from the simulator
    double sense_x = std::stod(j[1]["sense_x"].get<std::string>());
    double sense_y = std::stod(j[1]["sense_y"].get<std::string>());
    double sense_theta = std::stod(j[1]["sense_theta"].get<std::string>());

    pf.init(sense_x, sense_y, sense_theta, sigma_pos);
  } else {
    // Predict the vehicle's next state from previous (noiseless
    // control) data.
    double previous_velocity =
        std::stod(j[1]["previous_velocity"].get<std::string>());
    double previous_yawrate =
        std::stod(j[1]["previous_yawrate"].get<std::string>());

    pf.prediction(delta_t, sigma_pos, previous_velocity, previous_yawrate);
  }

  // receive noisy observation data from the simulator
  // sense_observations in JSON format
  // [{obs_x,obs_y},{obs_x,obs_y},...{obs_x,obs_y}]
  vector<LandmarkObs> noisy_observations;
  string sense_observations_x = j[1]["sense_observations_x"];
  string sense_observations_y = j[1]["sense_observations_y"];

  std::vector<float> x_sense;
  std::istringstream iss_x(sense_observations_x);

  std::copy(std::istream_iterator<float>(iss_x), std::istream_iterator<float>(),
            std::back_inserter(x_sense));

  std::vector<float> y_sense;
  std::istringstream iss_y(sense_observations_y);

  std::copy(std::istream_iterator<float>(iss_y), std::istream_iterator<float>(),
            std::back_inserter(y_sense));

  for (int i = 0; i < x_sense.size(); i++) {
    LandmarkObs obs;
    obs.x = x_sense[i];
    obs.y = y_sense[i];
    noisy_observations.push_back(obs);
  }

  // Update the weights and resample
  pf.updateWeights(sensor_range, sigma_landmark, noisy_observations, map);
  pf.resample();

  // Calculate and output the average weighted error of the particle
  // filter over all time steps so far.
  vector<Particle> particles = pf.particles;
  int num_particles = particles.size();
  double highest_weight = -1.0;
  Particle best_particle;
  double weight_sum = 0.0;
  for (int i = 0; i < num_particles; ++i) {
    if (particles[i].weight > highest_weight) {
      highest_weight = particles[i].weight;
      best_particle = particles[i];
    }
    weight_sum += particles[i].weight;
  }
}