Project: Particle Filter$-$Based Robot Localization

November $12,2023$

Abstract

Based on the information from the $$Particle Filter$$ slides, here is a project idea for students

involving the implementation of a Particle Filter for localization and navigation using Python. The

project is designed to be straightforward enough for students with some programming experience,

yet challenging enough to provide a comprehensive understanding of Particle Filters in a practical

scenario.

$1$ Project Description

In this project, students will implement a Particle Filter to estimate the position of a robot moving in

a two$-$dimensional space. The robot$$s environment will be represented as a grid, where each cell can

be either an obstacle or free space. The robot will have access to a simple sensor that provides noisy

measurements of its distance to the nearest obstacle in its front, left, right, and back directions.

$1\mathrm{.}1$ Objectives

$$ Implement a Particle Filter: Students will develop a Particle Filter to estimate the robot$$s

location based on sensor readings and a map of the environment.

$$ Simulate Robot Movement: Create a simulation where the robot moves a certain number of

steps in the environment, making random turns and moves.

$$ Sensor Data Simulation: Generate simulated sensor data based on the robot$$s actual position

and the map.

$$ Visualization: Implement real$-$time visualization of the particle cloud and the estimated position of the robot in comparison to its actual position.

$1\mathrm{.}2$ Implementation Approaches

Basic Python Implementation: $-$ Use standard Python libraries $($numpy$,$matplotlib$$ for visualization$).-$ Represent the map as a $2$D array, the robot$$s position as coordinates, and particles as

objects with position and weight attributes. $-$ Implement particle resampling, motion update, and

measurement update functions.

Object$-$Oriented Approach: $-$ Define classes for the Robot, Particle, and Map. $-$ Implement

methods for movement, sensing$,$ and updating in each class. $-$ Use inheritance to showcase different

types of particles or robots, if desired.

Advanced Visualization with Pygame: $-$ Utilize the $$pygame$$ library for more interactive

and sophisticated visualization. $-$ Allow real$-$time interaction, e$.$g$.,$ manually controlling the robot$$s

movement or altering the environment.

$2$ Example Template

Import Necessary Libraries

$1$ import numpy as np

$2$ import matplotlib $.$ pyplot as plt

$3$ from matplotlib $.$ animation import FuncAnimation

$1$

Define the Robot and Particle Classes

$1$ class Robot :

$2$ def $\_\_$init$\_\_($ self $,$ x $,$ y $,$ orientation $)$ :

$3$ self $.$x $=$ x

$4$ self $.$y $=$ y

$5$ self $.$ orientation $=$ orientation # in degrees

$6$

$7$ def move $($ self $,$ delta$\_$x $,$ delta$\_$y $,$ delta$\_$orientation $)$ :

$8$ self $.$x $+=$ delta$\_$x

$9$ self $.$y $+=$ delta$\_$y

$10$ self $.$ orientation $=($ self $.$ orientation $+$ delta$\_$orientation $)\%360$

$11$

$12$ # Simulate sensor reading based on robot $$s position

$13$ def sense $($ self $,$ environment$\_$map $)$:

$14$ # Implement sensor reading logic here

$15$ pass

$16$

$17$ class Particle :

$18$ def $\_\_$init$\_\_($ self $,$ x $,$ y $,$ orientation $,$ weight $)$ :

$19$ self $.$x $=$ x

$20$ self $.$y $=$ y

$21$ self $.$ orientation $=$ orientation

$22$ self $.$ weight $=$ weight

$23$

$24$ def move $($ self $,$ delta$\_$x $,$ delta$\_$y $,$ delta$\_$orientation $)$ :

$25$ # Add noise to movement

$26$ self $.$x $+=$ delta$\_$x $+$ np $.$ random $.$ normal $(0,0\mathrm{.}1)$

$27$ self $.$y $+=$ delta$\_$y $+$ np $.$ random $.$ normal $(0,0\mathrm{.}1)$

$28$ self $.$ orientation $=($ self $.$ orientation $+$ delta$\_$orientation $)\%360+$ np $.$ random $.$

normal $(0,5)$

$29$

$30$ # Update weight based on measurement

$31$ def update$\_$weight $($ self $,$ measurement $,$ robot$\_$measurement $)$:

$32$ # Implement weight updating logic here

$33$ pass

Initialize Robot and Particles

$1$ robot $=$ Robot $(50,50,0)$

$2$ particles $=[$ Particle $($ np $.$ random $.$ randint $(100),$ np $.$ random $.$ randint $(100),$ np $.$ random $.$

randint $(360),1\mathrm{.}0)$ for $\_$ in range $(1000)]$

Particle Filter Algorithm

$1$ def particle$\_$filter $($ particles $,$ robot $,$ environment$\_$map $,$ move$\_$command $)$:

$2$ # Move the robot and particles

$3$ robot $.$ move $(*$ move$\_$command $)$

$4$ for particle in particles :

$5$ particle $.$ move $(*$ move$\_$command $)$

$6$

$7$ # Update particles $$ weights based on sensor reading

$8$ robot$\_$measurement $=$ robot $.$ sense $($ environment$\_$map $)$

$9$ for particle in particles :

$10$ particle$\_$measurement $=$ particle $.$ sense $($ environment$\_$map $)$ # Particle $$s sense

method not shown

$11$ particle $.$ update$\_$weight $($ particle$\_$measurement $,$ robot$\_$measurement $)$

$12$

$13$ # Resampling

$14$ weights $=$ np $.$ array $([$ particle $.$ weight for particle in particles $])$

$15$ weights $/=$ np $.$sum$($ weights $)$ # Normalize weights

$16$ indices $=$ np $.$ random $.$ choice $($ range $($len$($ particles $)),$ size $=$len$($ particles $),$ p$=$ weights $)$

$17$ resampled$\_$particles $=[$ particles $[$i$]$ for i in indices $]$

$18$

$19$ return resampled$\_$particles

Visualization using Matplotlib

$1$ def update $($ frame$\_$number $)$:

$2$ global particles $,$ robot

$3$ move$\_$command $=(1,0,10)$ # Example move command

$4$ particles $=$ particle$\_$filter $($ particles $,$ robot $,$ environment