Advertisement
Guest User

Untitled

a guest
Aug 20th, 2017
66
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 1.47 KB | None | 0 0
  1. void ParticleFilter::init(double x, double y, double theta, double std[]) {
  2.  
  3. // Set the number of particles
  4. num_particles = 100;
  5.  
  6. // Declare the random generator
  7. default_random_engine gen;
  8.  
  9. // Extract the standard deviations for x, y, and theta
  10. double std_x = std[0];
  11. double std_y = std[1];
  12. double std_theta = std[2];
  13.  
  14. // Creates normal distributions for x, y and theta.
  15. normal_distribution<double> dist_x(x, std_x);
  16. normal_distribution<double> dist_y(y, std_y);
  17. normal_distribution<double> dist_theta(theta, std_theta);
  18.  
  19. // Create the vector to contain the `num_particles` particles
  20. particles = vector<Particle>(num_particles);
  21.  
  22. // Create the vector to contain the weight for each particle
  23. weights = vector<double>(num_particles);
  24.  
  25. // Loop over the particle vector and initialize each particle with the initial (x,y,theta) position
  26. // passed in the arguments with added Gaussian noise and weight 1
  27. for (int i = 0; i < num_particles; i++) {
  28.  
  29. particles[i].id = i; // Set this particle's ID
  30. particles[i].x = dist_x(gen); // Generate a random value from `dist_x` to set as this particle's x position
  31. particles[i].y = dist_y(gen); // Generate a random value from `dist_y` to set as this particle's y position
  32. particles[i].theta = dist_theta(gen); // Generate a random value from `dist_theta` to set as this particle's orientation theta
  33. particles[i].weight = 1.0; // Set the initial weight for all particles to 1
  34.  
  35. }
  36.  
  37. is_initialized = true;
  38.  
  39. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement