Post on 14-Dec-2015
FastSLAM Algorithm
As interpreted by Aaron Lee
Figure 13.2
• For each particle:– “Retrieval”/”Prediction”• Move particle
– “Measurement update”• For each feature seen by the particle, update its mean
and covariance– “Importance weight”• Calculate how likely this particle is
• Resample• Duplicate and delete particles based on goodness
Get next particle
Get next particle
Move particle
Get next particle
Move particle
Loop through all features in theparticle and determine how welleach of them correlate with themeasurement.
Get next particle
Move particle
Loop through all features in theparticle and determine how welleach of them correlate with themeasurement.
Set default importance if new feature
Get next particle
Move particle
Loop through all features in theparticle and determine how welleach of them correlate with themeasurement.
Set default importance if new feature
Pick the feature that corresponds bestto the measurement
Get next particle
Move particle
Loop through all features in theparticle and determine how welleach of them correlate with themeasurement.
Set default importance if new feature
Pick the feature that corresponds bestto the measurement If is a new feature, increase number by 1
Get next particle
Move particle
Loop through all features in theparticle and determine how welleach of them correlate with themeasurement.
Set default importance if new feature
Pick the feature that corresponds bestto the measurement If is a new feature, increase number by 1
If new feature, set mean to what measurement said.Remember that we have seen this feature once
Get next particle
Move particle
Loop through all features in theparticle and determine how welleach of them correlate with themeasurement.
Set default importance if new feature
Pick the feature that corresponds bestto the measurement If is a new feature, increase number by 1
If new feature, set mean to what measurement said.Remember that we have seen this feature once
If we have seen this before, update mean and covariance.
We have seen this feature once more.
If we didn’t see the feature, don’t changeits properties
If we didn’t see the feature, don’t changeits properties
However, if we should have seen it, rememberthat we should have and didn’t
If we didn’t see the feature, don’t changeits properties
However, if we should have seen it, rememberthat we should have and didn’t
If we see a feature less than 50% of the timethen throw it away
If we didn’t see the feature, don’t changeits properties
However, if we should have seen it, rememberthat we should have and didn’t
If we see a feature less than 50% of the timethen throw it away
We are done with this particle, so move it totemporary storage place
If we didn’t see the feature, don’t changeits properties
However, if we should have seen it, rememberthat we should have and didn’t
If we see a feature less than 50% of the timethen throw it away
We are done with this particle, so move it totemporary storage place
Empty out main particle storage place
If we didn’t see the feature, don’t changeits properties
However, if we should have seen it, rememberthat we should have and didn’t
If we see a feature less than 50% of the timethen throw it away
We are done with this particle, so move it totemporary storage place
Empty out main particle storage place
Now we resample. This involves selecting M particles to copy from the temporary storage place to the main storage. The probability of any given particle of getting selected is based on its importance factor. w=target/proposal
• End.