Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- void ParticleFilter::init(double x, double y, double theta, double std[]) {
- // Set the number of particles
- num_particles = 100;
- // Declare the random generator
- default_random_engine gen;
- // Extract the standard deviations for x, y, and theta
- double std_x = std[0];
- double std_y = std[1];
- double std_theta = std[2];
- // Creates normal distributions for x, y and theta.
- normal_distribution<double> dist_x(x, std_x);
- normal_distribution<double> dist_y(y, std_y);
- normal_distribution<double> dist_theta(theta, std_theta);
- // Create the vector to contain the `num_particles` particles
- particles = vector<Particle>(num_particles);
- // Create the vector to contain the weight for each particle
- weights = vector<double>(num_particles);
- // Loop over the particle vector and initialize each particle with the initial (x,y,theta) position
- // passed in the arguments with added Gaussian noise and weight 1
- for (int i = 0; i < num_particles; i++) {
- particles[i].id = i; // Set this particle's ID
- particles[i].x = dist_x(gen); // Generate a random value from `dist_x` to set as this particle's x position
- particles[i].y = dist_y(gen); // Generate a random value from `dist_y` to set as this particle's y position
- particles[i].theta = dist_theta(gen); // Generate a random value from `dist_theta` to set as this particle's orientation theta
- particles[i].weight = 1.0; // Set the initial weight for all particles to 1
- }
- is_initialized = true;
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement