Mobile Robot Control 2024 Ultron:Solution 4

0. Assignment introduction

0.1 Explore the code framework

1.How is the code is structured?

• The code for this assignment is organized into multiple classes, with each object or component having its own class and functions that allow for modifying its specific aspects.

2.What is the difference between the ParticleFilter and ParticleFilterBase classes, and how are they related to each other?

• Differences:
  • ParticleFilterBase provides the basic functionalities and interfaces of the particle filter, including particle initialization, propagation, weight computation, and setting resampling strategies.
  • ParticleFilter inherits from ParticleFilterBase and implements specific update and resampling strategies. It extends the base class by providing concrete implementations for the update and configureResampler methods.
• Relationship:
  • ParticleFilter is a subclass of ParticleFilterBase, inheriting all its attributes and methods while overriding or extending some of them to provide additional functionalities.

3.How are the ParticleFilter and Particle class related to eachother?

• The ParticleFilter class manages and utilizes a set of Particle instances for filtering operations. It inherits from ParticleFilterBase and implements particle propagation, weight calculation, and resampling by invoking methods on Particle objects. ParticleFilter initializes particles, propagates their states based on odometry data, calculates weights from sensor measurements, and resamples according to the configured strategy. The Particle class encapsulates the attributes and behaviors of individual particles, such as state initialization, propagation, and likelihood computation. ParticleFilter coordinates the filtering process, while Particle represents and updates individual states.

4.Both the ParticleFilterBase and Particle classes implement a propagation method. What is the difference between the methods?

• The propagateSamples method in the ParticleFilterBase class propagates all particles, typically called in the main loop of the particle filter to update the state of all particles. In contrast, the propagateSample method in the Particle class propagates a single particle, usually invoked by the propagateSamples method in ParticleFilterBase, to update the state of a specific particle.

1. Assignments for the first week

1.1 Initialize the Particle Filter

1.1 Initialize the Particle Filter

1. What are the advantages/disadvantages of using the first constructor, what are the advantages/disadvantages of the second one?

• First Constructor (Uniform Distribution)
  • Advantages:
    • The wide coverage ensures that all possible states are taken into account and is therefore suitable for completely unknown initial states.
  • Disadvantages:
    • The efficiency is relatively low because in known environments it may be low due to the fact that many particles may be placed in irrelevant regions.
    • Convergence is slower and may take longer to converge to the true state, especially in large environments.
• Second Constructor (Gaussian Distribution)
  • Advantages:
    • The search is more focused because the particles are concentrated near the known mean value, thus speeding up convergence.
    • More efficient in scenarios with some prior knowledge of the initial state, placing particles where they are more likely to find the true state quickly.
  • Disadvantages:
    • Relies on having a reasonable estimate of the initial state. Incorrect initial mean can lead to incorrect convergence.
    • The coverage is limited because the true state may be missed if it is not within the initial Gaussian distribution.

2. In which cases would we use either of them?

• When choosing between constructors, it depends on the application's context and the knowledge of the initial state. Use the first constructor (uniform distribution) when the initial state is completely unknown, such as at system startup or in a complex, unknown environment, as it ensures broad coverage. Use the second constructor (Gaussian distribution) when there is prior knowledge of the initial state, such as in tracking known targets or when previous state estimates are available, as it allows for focused and efficient search, leading to faster convergence.

1.2 Calculate the pose estimate

1. Interpret the resulting filter average. What does it resemble? Is the estimated robot pose correct? Why?

• The filtered average is essentially a weighted average of the particles, providing a single estimate of the state of the system. This estimate is calculated based on the position and weight of each particle. The principle is that particles with higher weights contribute more to the average than particles with lower weights.

• The current filter average is the filter average of all particles under the same weights. The current filter average could not correctly estimate the robot's pose without additionally updating the weights based on additional information.

1.2 Calculate the pose estimate

2. Imagine a case in which the filter average is inadequate for determining the robot position.

If the environment has multiple similar features, the particles will cluster around these similar features, and the distributions will result in several peaks. The filter average may fall between these peaks, in a location that is not representative of the actual robot position.

1.3 Propagate the particles with odometry

The screen recodring of rviz can be seen in Propagate the particles with odometry

1. Why do we need to inject noise into the propagation when the received odometry infromation already has an unkown noise component?

The map given to the particle filter is not perfect. Adding noise ensures particles are not overly confident in map data and allows them to have more plausible locations. Also, the odometry information may not perfectly capture the actual motion of the robot for it contains uncertainties such as wheel slip. By adding noise, we include these uncertainties in the model and have a more realistic representation of the robot's state. Furthermore, by adding noise, we ensure that the particles are varied. It’s important to prevent particles converge rapidly to a small area, which may lead to an incorrect robot state.

2. What happens when we stop here, and do not incorporate a correction step?

Stopping at the propagation step without a correction step will lead to a particle filter becoming inaccurate and unreliable over time due to accumulated errors and drift in the odometry data. Without the correction step, the particles will start to diverge from the true state of the robot because of the noisy odometry data. The particles will not be able to provide a good representation of the posterior distribution of the state.

2. Assignment for the second week

2.1 Correct the particles with LiDAR

1. What does each of the component of the measurement model represent, and why is each necessary.

The measurement model consists of hit, short, maximum range, and random models. The hit model calculates the probability of the measured value being close to the predicted value, using a Gaussian distribution to handle sensor noise. The short model handles cases where the measured value is less than the predicted value, accounting for unexpected close-range obstacles. The maximum range model deals with measurements reaching the sensor's maximum range, often indicating no obstacle detection. The random model addresses random noise in the measurements, covering irregular measurement errors.

2. With each particle having N >> 1 rays, and each likelihood being ∈ [0, 1], where could you see an issue given our current implementation of the likelihood computation.

If the likelihood values are close to zero, it may lead to numerical underflow, resulting in difficulties in effectively distinguishing the weights of the particles.

2.2 Re-sample the particles

1. What are the benefits and disadvantages of both the multinomial resampling and the stratified resampling?

Multinomial resampling is straightforward but suffers from high variance and potential particle depletion, making it computationally inefficient for large particle sets. In contrast, stratified resampling reduces variance and maintains better particle diversity, ensuring a more representative set of particles, though it is slightly more complex to implement.

2. With each particle having N >> 1 rays, and each likelihood being ∈ [0, 1], where could you see an issue given our current implementation of the likelihood computation?

With N≫1 rays and each likelihood in the range [0,1], the main problem in the current likelihood computation is that when multiplying with many small likelihoods, it may result in very small numbers being rounded to zero.

2.3 Test on the physical setup

1. How you tuned the parameters.

2. How accurate your localization is, and whether this will be sufficient for the final challenge.

3. How your algorithm responds to unmodeled obstacles.

4. Whether your algorithm is sufficiently fast and accurate for the final challenge.