Probabilistic Robot Localization in 2D

A hands-on implementation of the classic probabilistic robot localization pipeline for a planar differential-drive robot: generate landmark maps and control sequences, simulate noisy actuation and sensing, then recover the robot's pose from sparse, noisy sensor data using dead reckoning, a particle filter (Monte-Carlo localization), and an extended Kalman filter.

The entire loop runs in a continuous 2 m × 2 m environment with point landmarks, so you can inspect every stage — control planning, noisy simulation, filtering, and measured error — without hardware. Everything runs on the standard numpy / scipy / matplotlib stack, organized into small, independently executable scripts.

---

Pipeline

The project mirrors the "simulate → sense → filter → evaluate" loop a robot experiences in the field:

1. Environment & controls — randomly placed landmark maps and piecewise-constant control sequences that keep the robot clear of the walls.
2. Simulation — integrate noisy dynamics into ground-truth poses and synthesize noisy odometry and landmark observations at every time step.
3. Dead reckoning — integrate raw odometry into an estimated pose (no global sensing); a useful upper bound on how fast error drifts.
4. Particle filter — Monte-Carlo localization (MCL): propagate a particle set through the motion model, re-weight by sensor likelihood, resample. Run from a known initial pose and from a kidnapped (uniform) prior.
5. Evaluation — compare estimates against ground truth and plot translational and rotational error over time.
6. Extended Kalman filter — a single-hypothesis alternative estimator (extra credit).

Stage	File	Purpose
Robot model	diff_drive.py	Differential-drive / unicycle dynamics (dt = 0.1 s).
Maps	gen_landmarks.py	Random point-landmark maps (5 / 11 / 12 landmarks).
Controls	controls.py	Random piecewise-constant control sequences that avoid the walls.
Simulation	simulate.py	Noisy dynamics → ground truth + odometry + landmark readings.
Dead reckoning	dead_reckoning.py	Odometry integration; animate estimate vs. ground truth.
Particle filter	particle_filter.py	MCL from a known initial pose.
Particle filter (kidnapped)	kidnapped.py	MCL from a uniform prior.
Evaluation	evaluation.py	Estimate-vs-ground-truth error plots over time steps.
Extended Kalman filter	kalman_filter.py	EKF alternative (extra credit).
Data patch helper	crawler.py	One-off script to clip a stray extra pose in estimate files.
Geometry / collision helpers	create_scene.py, collision_checking.py	Reused from earlier planning work.

---

Robot model

The robot is a 0.2 × 0.1 m rectangle with state q = (x, y, θ), heading in [−π, π), controlled by linear velocity v ∈ [−0.5, 0.5] and angular velocity ω ∈ [−0.9, 0.9], integrated at dt = 0.1 s:

q̇ = [ v·cos(θ) , v·sin(θ) , ω ]
q(t + dt) = q(t) + q̇(t)·dt

Noise enters as independent zero-mean Gaussians at each stage:

Quantity	Std. deviation
linear velocity (actuation)	0.075
angular velocity (actuation)	0.2
odometry, low noise	0.05 (linear) / 0.1 (ang)
odometry, high noise	0.10 (linear) / 0.3 (ang)
landmark distance / bearing	0.02 / 0.02

Ground truth, odometry, and landmark readings are stored in separate files so a filter only ever sees the sensory inputs and the landmark map, never the true pose.

File conventions

Experiment codes are X_Y_Z, where X = map id (0–4), Y = control-sequence id (0–1), Z = observation-noise level (L/H). For the filters an extra suffix N selects the particle-count bucket (L ≈ 500, H ≥ 1000).

Dataset	Files
Landmark maps	maps/landmark_X.npy (N×2 coordinates)
Control plans	controls/control_X_Y.npy (initial pose + 200 controls)
Ground truth poses	gts/gt_X_Y.npy (201 poses)
Odometry + landmark readings	readings/readings_X_Y_Z.npy
Estimated poses (known prior)	estim1/estim1_X_Y_Z_N.npy
Estimated poses (kidnapped)	estim2/estim2_X_Y_Z_N.npy
Animations	dr/, video1/, video2/, eval1/, eval2/ (.mp4)

Animations are emitted as .mp4 to keep them short and submission-friendly.

---

Getting started

Create the environment:

conda create -n cs460 python=3.10.12 numpy=1.25.2 matplotlib=3.7.2 scipy=1.11.2 -y
conda activate cs460
# or: pip install -r requirements.txt

Want the .mp4 outputs? Install ffmpeg (brew install ffmpeg).

Batch runs

python controls.py            # generate plans
./run_sim.sh                  # simulate → ground truth + readings
./run_pf.sh                   # particle filter, known prior
./run_pf_kidnapped.sh         # particle filter, kidnapped prior
./run_crawl.sh                # fix a stray extra pose in the estimate files

Single case (map 0, sequence 0, high noise)

python simulate.py --plan  controls/control_0_0.npy \
              --map  maps/landmarks_0.npy \
              --execution gts/gt_0_0.npy \
              --sensing  readings/readings_0_0_H.npy

python dead_reckoning.py --map  maps/landmarks_0.npy \
              --execution gts/gt_0_0.npy \
              --sensing  readings/readings_0_0_H.npy

python particle_filter.py --map  maps/landmarks_0.npy \
              --sensing readings/readings_0_0_H.npy \
              --num_particles 2000 \
              --estimates estim1/estim1_0_0_H_H.npy

python evaluation.py --map  maps/landmarks_0.npy \
              --execution gts/gt_0_0.npy \
              --estimates estim1/estim1_0_0_H_H.npy

python kalman_filter.py --map  maps/landmarks_0.npy \
              --sensing readings/readings_0_0_H.npy \
              --estimates estim1/kalman_estim1_0_0_H_H.npy

Note: the repo stores maps under maps/landmarks_*.npy (plural), as referenced by the batch harness scripts (run_*.sh) and the working examples above. The assignment spec labels them maps/landmark_*.npy (singular); make sure your invocations match the actual on-disk names when running.

---

Algorithms & references

• Particle filter / Monte-Carlo localization — Dellaert, Fox, Burgard & Thrun (1999), Monte Carlo Localization for Mobile Robots; and Thrun, Burgard & Fox, Probabilistic Robotics (2004), ch. 4 (motion models) and 8 (MCL, low-variance resampling).
• Extended Kalman filter — Thrun, Burgard & Fox, Probabilistic Robotics (2004), ch. 3 and 7, for the single-hypothesis estimator and uncertainty ellipses.
• Kinematic (unicycle) robot model — Thrun et al., ch. 3; diff_drive.py realizes the same θ-only motion approximation used throughout the experiments.

---

Notes

• All lengths in meters, angles in radians; environment is [0, 2] × [0, 2].
• A filter consumes odometry + landmark readings and the landmark map; the ground-truth files exist purely for evaluation and error plotting.
• The generated map/reading .npy artifacts are kept committed so every stage is reproducible from the staged data.
• This is an independent implementation; any external sources are credited in report.pdf.