The Python implementation of the Particle Filter for robot localization is available in the Pybullet Particle Filter GitHub repository. This implementation uses the PyBullet physics engine and leverages NumPy and PyTorch for efficient computation. The core of the implementation is the ParticleFilter class, which encapsulates the initialization, motion update, measurement update, and resampling steps described in the tutorial. Below, we provide an overview of the code and highlight key functions with snippets that correspond to the core concepts.
Overview
The ParticleFilter class is designed to estimate the pose of a differential drive robot using LiDAR data and an occupancy grid map (OGM). It initializes a set of particles representing possible robot poses, updates their positions based on motion inputs (wheel encoders and IMU), adjusts particle weights using LiDAR measurements, and resamples particles to maintain a diverse and effective set of pose estimates. The implementation uses quaternions for orientation, handles noise via Gaussian distributions, and integrates with an OGM for map-based localization.
Key features of the implementation:
Modularity: The class separates the prediction, update, and resampling steps into distinct methods.
Efficiency: Uses NumPy for particle operations and PyTorch for GPU/CPU-accelerated computations in the measurement update.
Noise Modeling: Incorporates Gaussian noise for motion updates, as described in the tutorial, to ensure particle diversity.
Pose Estimation: Computes a weighted average of particle poses to estimate the robot’s position.
Below are the key components of the ParticleFilter class, with code snippets illustrating the core concepts from the tutorial.
Key Class and Functions
The initialization step creates a set of particles with uniform weights and assigns initial poses (x, y, theta). The particles are spread around the robot’s starting pose, with Gaussian noise applied to the x, y, and yaw components to account for uncertainty. The standard deviations for noise (sigma_x, sigma_y, sigma_yaw) are set to control the spread, and a covariance matrix is defined for motion noise, aligning with the tutorial’s description of noise modeling.
1 2 3 4 5 6 7 8 910111213141516
class ParticleFilter:
def __init__(self, initial_pose, OGM, numberofparticles=3):
self.numberofparticles = numberofparticles # Number of particles
self.particle_poses = np.tile(initial_pose, (self.numberofparticles, 1)).astype(np.float64) # Array of particle poses [x, y, theta]
self.particle_weights = np.ones(self.numberofparticles) / self.numberofparticles # Normalized weights
self.quaternions = np.repeat(np.array([[1.0, 0.0, 0.0, 0.0]]), self.numberofparticles, axis=0) # Quaternion representations
self.NumberEffective = numberofparticles # Effective number of particles
self.sigma_x = 0.01 # Stdev for linear velocity (x)
self.sigma_y = 0.01 # Stdev for linear velocity (y)
self.sigma_yaw = 0.01 # Stdev for yaw
self.lin_covariance = np.asarray([[self.sigma_x**2, 0], [0, self.sigma_y**2]]) # Linear covariance matrix
self.ang_covariance = np.zeros((3, 3)) # Angular covariance matrix
self.ang_covariance[2, 2] = self.sigma_yaw**2
self.xt = initial_pose
self.robotTosensor = np.array([OGM.sensor_x_r, OGM.sensor_y_r, OGM.sensor_yaw_r])
self.device = torch.device('cpu')
This code initializes numberofparticles particles with uniform weights (1/N) and sets up the covariance matrices for linear and angular noise, as described in the tutorial’s motion update section.
The motion update step propagates each particle’s pose based on the differential drive motion model, incorporating Gaussian noise to ensure diversity, as outlined in the tutorial. The method uses linear (linU) and angular (angU) velocities from wheel encoders and IMU, adds noise sampled from the covariance matrices, and updates particle positions and orientations (using quaternions for robustness).
1 2 3 4 5 6 7 8 910111213141516171819
def prediction_step(self, linU, angU, dt):
lin_noise = np.random.multivariate_normal([0, 0], self.lin_covariance, size=self.numberofparticles) # Linear noise
ang_noise = np.random.multivariate_normal([0, 0, 0], self.ang_covariance, size=self.numberofparticles) # Angular noise
noisy_linU = linU + lin_noise
noisy_angU = angU + ang_noise
dx = noisy_linU[:, 0] * dt # Linear displacement (x)
dy = noisy_linU[:, 1] * dt # Linear displacement (y)
theta = np.linalg.norm(noisy_angU, axis=1) * dt
axis = noisy_angU / np.linalg.norm(noisy_angU, axis=1)[:, None]
axis = np.nan_to_num(axis)
xyz = axis * np.sin(theta / 2)[:, None]
w = np.cos(theta / 2)[:, None]
dq = np.hstack([w, xyz])
self.quaternions = self.quaternion_multiply(dq, self.quaternions) # Update quaternions
self.quaternions /= np.linalg.norm(self.quaternions, axis=1)[:, None]
self.particle_poses[:, 0] += dx # Update x
self.particle_poses[:, 1] += dy # Update y
w, x, y, z = self.quaternions.T
self.particle_poses[:, 2] = np.arctan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z)) # Update theta
This snippet implements the motion update equation with noise, ensuring diverse particle guesses as described in the tutorial.
The method converts LiDAR scans to world coordinates, checks for map alignment, and updates weights based on scan-map matching scores, followed by normalization.
The resampling step regenerates particles based on their weights when the effective number of particles falls below a threshold (30% of total particles). It uses systematic resampling to select particles proportional to their weights and resets weights to 1/N.
The update_step method also computes the robot’s estimated pose as a weighted average of particle poses $$ X = \sum_{i=1}^{n} (a_i \times x_i) $$, as shown in the snippet above, where weighted_pose is calculated using the particle positions and weights.
1 2 3 4 5 6 7 8 91011121314151617
# updating the particles weights based on the score returned from matching the lidar scan and the OGM map
self.particle_weights*=np.exp(0.01*scores) # 0.01 is an arbritary scale
self.particle_weights /= np.sum(self.particle_weights)
weighted_x=np.sum(self.particle_poses[:, 0] * self.particle_weights)
weighted_y=np.sum(self.particle_poses[:, 1] * self.particle_weights)
weighted_sin=np.sum(np.sin(self.particle_poses[:, 2]) * self.particle_weights)
weighted_cos=np.sum(np.cos(self.particle_poses[:, 2]) * self.particle_weights)
weighted_angle=np.arctan2(weighted_sin, weighted_cos)
weighted_pose=np.array([weighted_x, weighted_y, weighted_angle])
weighted_pose[2]=np.degrees(weighted_pose[2])
weighted_pose[1]=-weighted_pose[1]
return weighted_pose
The full implementation, including dependencies and the OGM class, is available in the GitHub repository.
The code uses quaternions for robust orientation handling, which extends the tutorial’s differential drive model to 3D rotations.
Noise parameters (sigma_x, sigma_y, sigma_yaw) are tunable to balance localization accuracy and particle diversity, addressing the tutorial’s discussion on avoiding localization drift.
This implementation provides a practical realization of the particle filter concepts, suitable for real-time robot localization with LiDAR and wheel encoder data.