SLAM and localization

Two components work out where the car is on a map. slam_toolbox builds the map while driving and keeps correcting the car’s position against it. The particle filter localizes the car on a map that already exists. Mapping runs use SLAM. A race on a saved map uses the particle filter. Both are taught in Lecture 8 (notes), Lecture 9 (notes) and Tutorial 5 (notes), and the particle filter in Lecture 7 (notes).

Topics

slam_toolbox reads odometry from the transform tree, not from the /odom topic. With map_course gyro_odom:=true it reads the odom_imu frame instead of odom, so its odometry has a gyro heading; see IMU fusion. The particle filter reads the /odom topic.

SLAM

SLAM, simultaneous localization and mapping, builds an occupancy grid and locates the car in it at the same time, from the LiDAR scans and the odometry. Each cell of the grid is free, occupied or unknown. slam_toolbox keeps a pose graph: each node is a scan at a pose, and each edge is a constraint between two poses from scan matching. When the car returns to a place it has seen before, a loop closure adds a constraint between the new scan and the old one, and the whole graph is re-optimized to remove the drift accumulated in between.

SLAM publishes the correction as the map to odom transform. It updates only when its gates allow, and each update arrives as a jump. See Coordinate frames.

The workspace’s settings

slam_launch.py loads the upstream F1TENTH configuration, then the workspace’s own slam_tracking.yamlGitHub on top. The overrides live in a separate file so that a sync of the vendored f1tenth_system cannot remove them.

On the club’s 126 m hallway loop, the upstream settings recorded 106 pose jumps over 0.12 m in one lap. After 126 m of dead reckoning, the estimate was unlikely to be within 3 m of the start when the car got back there, so the upstream 3 m search found no loop-closure candidate. The chain size doubled with the halved travel distance to keep the evidence for a loop closure at 5 m of travel. Change the two together.

To judge a change to these settings, check a recorded mapping run with the SLAM report, then replay the run through SLAM with the new settings, with the car parked. On car 1 the exact replay reproduces the car’s own SLAM poses, so a difference in the replayed map comes from the changed settings rather than from replay timing.

The particle filter

The particle filter, Monte Carlo localization, tracks thousands of weighted guesses of the car’s pose, called particles. It is the upstream F1TENTH implementation, included as the src/particle_filter submodule from f1tenth/particle_filter.

Why particles. The car’s pose could be anywhere the odometry error allows, and near a long featureless wall several poses can explain a scan equally well. A single best guess with an uncertainty, as a Kalman filter keeps, cannot represent “here or 2 m further down the corridor”. A cloud of particles can: each particle is one possible pose, and where the cloud is dense the car is likely to be. Each update moves every particle the way the car moved, then asks how well the scan fits the map from each particle’s pose, and keeps more copies of the particles that fit best.

The figure shows the cycle starting from Predict. The code runs the same cycle starting from Resample: each update first resamples using the weights the previous scan produced, then predicts and weighs with the new scan.

Resample. A new set of 4,000 particles is drawn from the old set, with replacement, each with probability equal to its weight. A particle with twice the weight of another is drawn twice as often on average, and particles with near-zero weight disappear. The cloud therefore concentrates where the scans have fitted best.

Predict. Each particle $i$ is moved by the odometry change $(\Delta x,\Delta y,\Delta \theta )$ since the last update, rotated into that particle’s heading, plus Gaussian noise that spreads the cloud to cover the odometry’s error:

Weight. For each particle, range_libc ray-casts what each LiDAR beam would measure on the map from that pose, $d_k$. Each real reading $r_k$ is scored with a beam model that mixes four cases: a hit near the expected range, an early return from something not on the map, a maximum-range reading, and random noise:

The particle’s weight is the product over beams, because the beams are treated as independent evidence. A product of 61 small numbers makes a few particles’ weights millions of times larger than the rest, so the weight is raised to $1/2.2$ (squash_factor), which flattens the differences and keeps the cloud from collapsing onto a handful of particles. The model is precomputed as a table indexed by $r$ and $d$, so scoring a beam is one table lookup. The weights are then divided by their sum, so they add up to 1.

Estimate. The published pose is the weighted mean of the particles, on /pf/viz/inferred_pose. It is the LiDAR’s position, not the rear axle’s, because each guess is scored as if the scan were taken from it.

The upstream code is short and vectorized: every step works on all 4,000 particles at once as NumPy arrays.

def MCL(self, a, o):
    proposal_indices = np.random.choice(self.particle_indices, self.MAX_PARTICLES, p=self.weights)
    proposal_distribution = self.particles[proposal_indices,:]
    self.motion_model(proposal_distribution, a)
    self.sensor_model(proposal_distribution, o, self.weights)
    self.weights /= np.sum(self.weights)
    self.particles = proposal_distribution

def expected_pose(self):
    return np.dot(self.particles.transpose(), self.weights)

np.random.choice(..., p=self.weights) is the resampling step. The motion model rotates the odometry step into each particle’s own heading before adding it, then adds the noise:

cosines = np.cos(proposal_dist[:,2])
sines = np.sin(proposal_dist[:,2])
self.local_deltas[:,0] = cosines*action[0] - sines*action[1]
self.local_deltas[:,1] = sines*action[0] + cosines*action[1]
self.local_deltas[:,2] = action[2]
proposal_dist[:,:] += self.local_deltas
proposal_dist[:,0] += np.random.normal(loc=0.0,scale=self.MOTION_DISPERSION_X,size=self.MAX_PARTICLES)
proposal_dist[:,1] += np.random.normal(loc=0.0,scale=self.MOTION_DISPERSION_Y,size=self.MAX_PARTICLES)
proposal_dist[:,2] += np.random.normal(loc=0.0,scale=self.MOTION_DISPERSION_THETA,size=self.MAX_PARTICLES)

action is the odometry step in the car’s own frame, so a step of 0.1 m forward moves a particle facing north 0.1 m north and a particle facing east 0.1 m east.

Parameters

In src/particle_filter/config/localize.yaml, from the upstream f1tenth/particle_filter repository that the submodule tracks:

GPU ray casting

4,000 particles with 61 rays each is 244,000 ray casts per scan. On the CPU that took 28.9 ms, longer than the 25 ms between scans. On the GPU it takes 6.0 ms, 4.8 times faster, with identical results. range_libc must be built with CUDA, as in Set up the workspace, step 4.

Run them

When the particle filter is launched without a run folder, it needs a starting pose from RViz’s 2D Pose Estimate tool. Without one it does not know which part of the map it is on. With run_dir:=, race_launch.py seeds it from the run’s start_pose.yaml.

Check it

ros2 topic hz /pf/viz/inferred_pose      # one estimate per scan when it keeps up
ros2 run tf2_ros tf2_echo map base_link  # while SLAM runs

Troubleshooting

Limitations

• The particle filter’s estimate averages the particles’ headings as plain numbers. Near a heading of ±180°, a cloud split across the wrap averages to a heading near 0°.
• The motion noise is fixed, not scaled to how far the car moved, so a fast car spreads the cloud by the same amount as a slow one.
• SLAM corrections arrive as jumps of the map → odom transform, not as a smooth correction.