Particle Filter

The particle set is the core data structure, representing the estimated posterior probability distribution of the system's state. It consists of N discrete samples, or particles.

• Each particle is a complete hypothesis of the system's state (e.g., a robot's x, y, and yaw).
• The distribution of particles across the state space approximates the true probability density function.
• A larger number of particles provides a more accurate approximation but increases computational cost.

The motion model, also called the process model or transition model, predicts how each particle's state evolves over time based on control inputs.

• It accounts for the system's dynamics and inherent uncertainty.
• For a mobile robot, this often involves propagating a particle's pose using odometry data from wheel encoders or an IMU.
• The model adds process noise to each particle, which causes the particle cloud to disperse, representing increased uncertainty after movement.

The observation model predicts what sensor readings should be expected given a particle's hypothesized state. It is used to compute the particle's importance weight.

• The weight is proportional to the likelihood of the actual sensor measurement, given the particle's state.
• Particles whose predicted observations closely match the real sensor data receive high weights.
• This step focuses the particle distribution on regions of the state space that are consistent with the latest evidence.

Resampling is the critical step that mitigates particle degeneracy, where most particles have negligible weight.

• It creates a new particle set by drawing (with replacement) from the current set, with a probability proportional to each particle's weight.
• High-weight particles are likely to be duplicated; low-weight particles are discarded.
• The new set has uniform weights, concentrating computational resources on the most probable state hypotheses. Over-resampling can lead to particle impoverishment, where diversity is lost.

The final estimated state is derived from the weighted particle set. Common methods include:

• Weighted Mean: The most common estimate, calculated as the sum of each particle's state multiplied by its weight.
• Maximum A Posteriori (MAP): Using the state of the single particle with the highest weight.
• Robust Estimates: Using statistical measures like the weighted median to reduce the influence of outlier particles. This output provides the unified pose estimate for a single agent within the fleet.

The classic application, known as Monte Carlo Localization (MCL) or Adaptive Monte Carlo Localization (AMCL). A robot uses a particle filter to estimate its pose (position and orientation) within a known map.

• Particles represent hypotheses of the robot's location.
• Each particle is weighted by how well its predicted sensor observations (e.g., LiDAR scans) match the actual map.
• The filter naturally handles global localization (the 'kidnapped robot' problem) and position tracking.
• It is robust to multi-modal distributions, meaning it can represent multiple possible locations before converging.

Known as FastSLAM, this approach uses a Rao-Blackwellized particle filter to solve the SLAM problem.

• Each particle carries its own hypothesis of the robot's trajectory and the resulting map.
• The map for each particle is typically represented as a set of landmarks with associated Gaussian uncertainties, which are updated analytically.
• This elegantly handles the data association problem (matching observations to landmarks) within each particle's hypothesis.
• It is particularly effective for feature-based maps in environments with distinct landmarks.

Particle filters excel at tracking the state of other dynamic agents, a critical capability for collision avoidance and multi-agent coordination.

• The state vector for each tracked object can include position, velocity, acceleration, and even intent.
• The multi-modal nature of the filter allows it to represent multiple plausible future trajectories (e.g., at an intersection).
• This is used in autonomous vehicles to track pedestrians and other cars, and in robotics to track human collaborators.
• The observation model often fuses data from cameras, LiDAR, and radar.

When sensor fusion involves highly non-linear models where the Extended Kalman Filter (EKF) may fail, particle filters provide a robust alternative.

• They can seamlessly integrate heterogeneous sensors like visual-inertial odometry (VIO), wheel odometry, and GPS.
• The filter does not require linearization of the motion or observation models, avoiding linearization errors.
• This is crucial for systems with complex dynamics or bearing-only sensors (e.g., a camera observing angles to landmarks).
• It provides a full probability distribution, not just a mean and covariance, revealing uncertainty structure.

Particle filters can be used for model-based fault detection by estimating hidden parameters of a system.

• Particles can represent hypotheses about the internal state and parameters like sensor bias, actuator wear, or battery degradation.
• Significant deviations in estimated parameters from nominal values can signal a fault.
• This is applied in fleet health monitoring to predict maintenance needs for robots by tracking changes in their motion or power models.
• It enables predictive maintenance by identifying gradual performance degradation.

In Partially Observable Markov Decision Processes (POMDPs), the agent must plan actions without full knowledge of the state. Particle filters are the standard tool for maintaining the belief state.

• The set of weighted particles is the belief state, representing the distribution over possible true states.
• This belief is updated after each action and observation.
• Planning algorithms then reason over this particle-based belief to choose optimal actions.
• This framework is fundamental for robust robotic decision-making in unpredictable environments.

State estimation is the overarching process of inferring the internal, hidden variables (the state) of a dynamic system from a sequence of noisy sensor observations and control inputs. For a mobile robot, the state typically includes its pose (position and orientation) and velocity.

• Core Problem: It bridges the gap between imperfect sensors (e.g., LiDAR, cameras, IMUs) and the need for a precise, real-time understanding of an agent's situation.
• Mathematical Foundation: Framed as a Bayesian inference problem, where the goal is to compute the posterior probability distribution of the state given all available data.
• Fleet Context: In heterogeneous fleets, each robot runs its own state estimator, the outputs of which are fused into a unified world model for orchestration.

Simultaneous Localization and Mapping (SLAM) is the coupled problem where a robot must build a map of an unknown environment while simultaneously using that map to localize itself within it. It's a canonical application for particle filters (e.g., FastSLAM).

• The Chicken-and-Egg Problem: Localization requires a map, but mapping requires knowledge of the robot's location. SLAM algorithms solve both concurrently.
• Particle Filter Solution: Algorithms like FastSLAM use a Rao-Blackwellized particle filter, where each particle carries its own trajectory hypothesis and an associated map.
• Fleet Extension: Multi-Robot SLAM extends this to teams of agents collaboratively building a shared map, requiring data association and communication.

Sensor fusion is the technique of combining data from multiple, often heterogeneous, sensors to produce a state estimate that is more accurate, complete, and reliable than could be obtained from any single sensor source.

• Redundancy & Complementarity: Fuses sensors with different failure modes (e.g., a camera fails in low light, but LiDAR does not) and characteristics (high-frequency IMU, absolute but low-frequency GPS).
• Architectures: Includes loose coupling (fusing state estimates) and tight coupling (fusing raw sensor measurements within a single estimation framework).
• Particle Filter Role: A particle filter is a natural sensor fusion engine, as its observation model can incorporate likelihoods from radically different sensor types (e.g., LiDAR scan matching and WiFi signal strength).

The Kalman Filter (KF) is an optimal, recursive estimator for linear dynamic systems with Gaussian noise. The Extended Kalman Filter (EKF) linearizes nonlinear systems around the current estimate, making it the historical benchmark for nonlinear state estimation.

• Key Difference from Particle Filters: KF/EKF represent the state distribution as a single Gaussian (mean and covariance matrix). Particle filters use a set of samples, enabling representation of arbitrary, multi-modal distributions.
• Computational Trade-off: KF/EKF are computationally efficient (O(n²)) but can diverge with severe nonlinearities or non-Gaussian noise. Particle filters are more flexible but computationally heavier (O(N), where N is the number of particles).
• Hybrid Approaches: In practice, systems often use an EKF for high-frequency odometry propagation, with a particle filter handling global localization or multi-hypothesis tracking.

These are the two fundamental mathematical models at the heart of any Bayesian filter, including the particle filter.

• Motion Model (Process Model): Predicts how the robot's state evolves from time k-1 to k based on control inputs (e.g., wheel velocities). It accounts for predictable motion and process noise (e.g., wheel slip). In a particle filter, this model is used in the prediction step to propagate each particle forward.
• Observation Model (Measurement Model): Predicts what sensor readings the robot should expect given a hypothesized state and a map of the environment. It is used in the update step to calculate the weight of each particle. For example, it might compute the likelihood of a LiDAR scan given a particle's proposed pose on an occupancy grid.
• Fidelity is Critical: The accuracy of the overall state estimate is bounded by the accuracy of these models.