Localization

Particle Filter / Monte-Carlo Localization

We need to the particle filter to do Localization. It is a recursive Bayes Filter. Similar to Kalman Filter but relaxes some of the assumptions.

Resources

• 5 minute summary video
• CS287 Particle Filter Lecture, with slides
• F1TENTH Particle Filter Lecture, with slides

Algorithm

1. Sample the particles using the proposal distribution
2. Compute the importance weights
3. Resampling: Draw sample $i$ with probability $w_t^{[i]}$ and repeat $J$ times

TODO: Insert pseudocode

Running Particle Filter for F1TENTH

On the F1TENTH with an Nvidia Jetson Xavier NX, the pose is published at ~30-40Hz.

Clone the repository by running

git clone https://github.com/f1tenth/particle_filter.git

You will also need to sudo apt install ros-$ROS_DISTRO-tf-transformations

You will need to install the range_libc library, this is specified in the README.md.

Put the map files (image+.yaml) in particle_filter/maps. Change the map parameter inside particle_filter/config/localize.yaml to reflect the map you want to use.

To start the particle filter, run

ros2 launch particle_filter localize_launch.py

Warning

If your compute doesn’t have a GPU (ex: Raspberry Pi), then the default settings will not work. Change from rmgpu to rm in the localize.yaml config file.

You can then set an initial pose in rviz by clicking the “2D Pose Estimate” button.

nav2 AMCL

I tried the Nav2 version.

This comes part of the navigation stack, which you can run

sudo apt install ros-$ROS_DISTRO-navigation2

You can also try the nav2 version

ros2 launch nav2_bringup localization_launch.py

In another terminal, open rviz by running

rviz2

In another terminal, run the following command to load in the map

ros2 service call /map_server/load_map nav2_msgs/srv/LoadMap "{map_url: /home/ubuntu/sw_lab_robotics/maps/0.3x_gmapping.yaml}"

Then, open rviz and set the initial pose estimate.

Though you should specify the params file, which can be found

How fast do you need your particle filter

Depends on the use case. I don’t really have a choice on the f1tenth?

Calculation

Assuming the car goes 7m/s, at 30hz, you travel 7m / 30 = 0.233m. = 23cm

So between each pose, you might be off 20cm.

But I need to double check the Particle Filter.

Monte-Carlo Localization

This is a specific instance of Particle Filtering, where we are trying to localize ourselves in a known map.

Related

• Dirac Function
• Kullback-Leibler Divergence
• Localization Research