Implements a particle filter for tracking multiple targets. More...

#include <particle_filter.hpp>

Public Member Functions
	MultiTargetParticleFilter (size_t num_particles=500, double initial_max_speed=20.0)
	Construct a new Multi Target Particle Filter object.

void	initialize (std::shared_ptr< grid_map::GridMap > map_ptr)
	Initialize multi-target particle filter.

void	predict (double dt)
	Predict the new (x,y) position and course of each particle.

void	updateWeights (std::shared_ptr< grid_map::GridMap > map_ptr, std::shared_ptr< grid_map::GridMap > stats_ptr, double dt)
	Update particle weights using the Euclidean distance transform, exponential decay, and density feedback.

void	addResampleNoise (Target &particle)
	Add Gaussian noise to a particle's position, yaw, yaw rate, and speed.

void	resample (std::shared_ptr< grid_map::GridMap > map_ptr, std::shared_ptr< grid_map::GridMap > stats_ptr, double dt)
	Perform systematic resampling of existing particles and inject new random particles.

std::vector< grid_map::Position >	getValidPositionsFromMap (const std::shared_ptr< grid_map::GridMap > &map_ptr)
	Get valid positions from the grid map.

void	seedUniform (const std::vector< grid_map::Position > &valid_positions, size_t n_seed, std::vector< Target > &output_particles)
	Seeds particles uniformly from a list of valid positions in the grid map.

void	seedWeighted (const std::vector< grid_map::Position > &valid_positions, size_t n_seed, const std::shared_ptr< grid_map::GridMap > &stats_ptr, std::vector< Target > &output_particles)
	Seed particles weighted by grid map particle density.

void	updateNoiseDistributions ()
	Update the noise distributions for particle motion used in the predict step.

const std::vector< Target > &	getParticles ()
	Get particles.

Public Attributes
double	observation_sigma_
	Standard deviation for Gaussian weight function.

double	weight_decay_half_life_
	Half-life for exponential decay of particle weights.

double	seed_fraction_
	Fraction of particles to be seeded with random positions.

double	noise_std_position_
	Standard deviation for position noise.

double	noise_std_yaw_
	Standard deviation for yaw noise.

double	noise_std_yaw_rate_
	Standard deviation for yaw rate noise.

double	noise_std_speed_
	Standard deviation for speed noise.

double	density_feedback_factor_
	Density (particles/m^2) at which the weight of a particle will be reduced by half.

Detailed Description

Implements a particle filter for tracking multiple targets.

Constructor & Destructor Documentation

◆ MultiTargetParticleFilter()

NS_HEAD MultiTargetParticleFilter::MultiTargetParticleFilter	(	size_t	num_particles = 500,
		double	initial_max_speed = 20.0 )

explicit

Construct a new Multi Target Particle Filter object.

Parameters

num_particles	Number of particles to use to initialize the particle filter (default: 500 particles).
initial_max_speed	Initial maximum speed of particles (default: 20.0 m/s).

Member Function Documentation

◆ addResampleNoise()

void MultiTargetParticleFilter::addResampleNoise ( Target & particle )

Add Gaussian noise to a particle's position, yaw, yaw rate, and speed.

This function modifies the particle in place by adding Gaussian noise to its position, yaw angle, yaw rate, and speed based on the defined noise distributions.

Parameters

particle Reference to the particle to which noise will be added.

◆ getParticles()

const std::vector< Target > & MultiTargetParticleFilter::getParticles ( )

Get particles.

Returns: const std::vector<Target>& Vector of particles on target.

◆ getValidPositionsFromMap()

std::vector< grid_map::Position > MultiTargetParticleFilter::getValidPositionsFromMap ( const std::shared_ptr< grid_map::GridMap > & map_ptr )

Get valid positions from the grid map.

Returns a vector of positions where the grid map has valid data (i.e., where radar intensity > 0). This is used for seeding new particles.

Parameters

map_ptr Shared pointer to GridMap with radar intensity-based targets to track.

Returns: std::vector<grid_map::Position> Vector of valid positions.

◆ initialize()

void MultiTargetParticleFilter::initialize ( std::shared_ptr< grid_map::GridMap > map_ptr )

Initialize multi-target particle filter.

Spawns particles with random positions and course angles around radar returns (i.e. anywhere in grid map where radar intensity > 0).

Parameters

map_ptr Shared pointer to GridMap with radar intensity-based targets to track.

◆ predict()

void MultiTargetParticleFilter::predict ( double dt )

Predict the new (x,y) position and course of each particle.

Parameters

dt	Time interval (delta t) from last particle filter update step.

◆ resample()

void MultiTargetParticleFilter::resample	(	std::shared_ptr< grid_map::GridMap >	map_ptr,
		std::shared_ptr< grid_map::GridMap >	stats_ptr,
		double	dt )

Perform systematic resampling of existing particles and inject new random particles.

This method replaces the current particle set by:

Systematic resampling of (N - M) particles, drawn proportional to their weights, adding Gaussian noise to position, heading and speed (via addResampleNoise), preserving each particle’s age, and resetting its weight to (1/N).
Injection of new particles at valid positions from map_ptr. Positions are chosen with probability proportional to the inverse local density (from the “particles_per_cell” layer of stats_ptr) to encourage seeding in less crowded areas i.e. new radar targets.
New particles receive:
- random speed ∈ [0, initial_max_speed_]
- random course ∈ [0, 2π)
- zero yaw_rate
- weight = (1/N)
- age = 0

Parameters

map_ptr	Shared pointer to GridMap with radar intensity-based targets to track.
stats_ptr	Shared pointer to GridMap with particle statistics.
dt	Time interval (delta t) from last particle filter update step.

◆ seedUniform()

void MultiTargetParticleFilter::seedUniform	(	const std::vector< grid_map::Position > &	valid_positions,
		size_t	n_seed,
		std::vector< Target > &	output_particles )

Seeds particles uniformly from a list of valid positions in the grid map.

Parameters

valid_positions	List of valid positions from the grid map where particles can be seeded.
n_seed	Number of particles to seed.
output_particles	Vector to store the seeded particles.

◆ seedWeighted()

void MultiTargetParticleFilter::seedWeighted	(	const std::vector< grid_map::Position > &	valid_positions,
		size_t	n_seed,
		const std::shared_ptr< grid_map::GridMap > &	stats_ptr,
		std::vector< Target > &	output_particles )

Seed particles weighted by grid map particle density.

Preferentially seeds particles in areas of lower density using an inverse weight function for particle density.

Parameters

valid_positions	List of valid positions from the grid map where particles can be seeded.
n_seed	Number of particles to seed.
stats_ptr	Shared pointer to the grid map containing statistics for particle density.
output_particles	Vector to store the seeded particles.

◆ updateNoiseDistributions()

void MultiTargetParticleFilter::updateNoiseDistributions ( )

Update the noise distributions for particle motion used in the predict step.

◆ updateWeights()

void MultiTargetParticleFilter::updateWeights	(	std::shared_ptr< grid_map::GridMap >	map_ptr,
		std::shared_ptr< grid_map::GridMap >	stats_ptr,
		double	dt )

Update particle weights using the Euclidean distance transform, exponential decay, and density feedback.

For each particle:

Computes a new observation likelihood from the "edt" layer of map_ptr: If outside the map or missing layer, a small baseline of 1e-6 is used.
Updates particle.obs_likelihood:
- If the new likelihood exceeds the previous, it is replaced.
- Otherwise, the previous likelihood is multiplied by the decay factor
Sets particle.weight = std::max(particle.obs_likelihood, 1e-8).
Retrieves local density from the "particles_per_cell" layer and applies a logistic penalty multiplying particle.weight by this factor. After processing all particles, normalizes their weights so that their sum equals 1.

Parameters

map_ptr	Shared pointer to GridMap with radar intensity-based targets to track.
stats_ptr	Shared pointer to GridMap with particle statistics.
dt	Time interval (delta t) from last particle filter update step.

Member Data Documentation

◆ density_feedback_factor_

double echoflow::MultiTargetParticleFilter::density_feedback_factor_

Density (particles/m^2) at which the weight of a particle will be reduced by half.

◆ noise_std_position_

double echoflow::MultiTargetParticleFilter::noise_std_position_

Standard deviation for position noise.

◆ noise_std_speed_

double echoflow::MultiTargetParticleFilter::noise_std_speed_

Standard deviation for speed noise.

◆ noise_std_yaw_

double echoflow::MultiTargetParticleFilter::noise_std_yaw_

Standard deviation for yaw noise.

◆ noise_std_yaw_rate_

double echoflow::MultiTargetParticleFilter::noise_std_yaw_rate_

Standard deviation for yaw rate noise.

◆ observation_sigma_

double echoflow::MultiTargetParticleFilter::observation_sigma_

Standard deviation for Gaussian weight function.

◆ seed_fraction_

double echoflow::MultiTargetParticleFilter::seed_fraction_

Fraction of particles to be seeded with random positions.

◆ weight_decay_half_life_

double echoflow::MultiTargetParticleFilter::weight_decay_half_life_

Half-life for exponential decay of particle weights.

The documentation for this class was generated from the following files:

echoflow/include/echoflow/particle_filter.hpp
echoflow/src/particle_filter.cpp

Public Member Functions

Public Attributes

Detailed Description

Constructor & Destructor Documentation

◆ MultiTargetParticleFilter()

Member Function Documentation

◆ addResampleNoise()

◆ getParticles()

◆ getValidPositionsFromMap()

◆ initialize()

◆ predict()

◆ resample()

◆ seedUniform()

◆ seedWeighted()

◆ updateNoiseDistributions()

◆ updateWeights()

Member Data Documentation

◆ density_feedback_factor_

◆ noise_std_position_

◆ noise_std_speed_

◆ noise_std_yaw_

◆ noise_std_yaw_rate_

◆ observation_sigma_

◆ seed_fraction_

◆ weight_decay_half_life_