I have been trying to implement FastSlam 1.0. To implement this, I need to create particles. Now my confusion is how to create Particles? I have some odometry and Measurement data. Using those data values, how could I generate the Particles?
$\begingroup$
$\endgroup$
Add a comment
|
$\begingroup$
$\endgroup$
At $t_{0}$ your particles should have some initial distribution. To simplify you can assume that all particles are aligned at the same pose $x_{0}$.
For subsequent timesteps, let's say you have all the particles at time $t-1$ and you want to get a new particle set for time $t$. FastSLAM 1.0 uses only the motion model to propagate the particles, so the new particles at time $t$ are sampled according to:
Now to translate this equation into code you need to define a motion model for your robot. These slides introduce several probabilistic motion models depending on the type of robot you are using (odometry-based, velocity-based).
Let's assume we take the following odometry-based motion model:
Now to propagate particle $k$ from time $t-1$ to time $t$, you take the pose $x, y, \theta$ of particle $k$ at time $t-1$ + your odometry measurements $u$ and you feed them to the sample_motion_model(u,x) algorithm. The output will be $x', y', \theta'$, the pose of particle $k$ at time $t$.
