FastSLAM Algorithm As interpreted by Aaron Lee. Figure 13.2 For each particle: –...
-
Upload
raegan-brimm -
Category
Documents
-
view
215 -
download
0
Transcript of FastSLAM Algorithm As interpreted by Aaron Lee. Figure 13.2 For each particle: –...
![Page 1: FastSLAM Algorithm As interpreted by Aaron Lee. Figure 13.2 For each particle: – “Retrieval”/”Prediction” Move particle – “Measurement update” For each.](https://reader031.fdocuments.us/reader031/viewer/2022032516/56649c745503460f94926cef/html5/thumbnails/1.jpg)
FastSLAM Algorithm
As interpreted by Aaron Lee
![Page 2: FastSLAM Algorithm As interpreted by Aaron Lee. Figure 13.2 For each particle: – “Retrieval”/”Prediction” Move particle – “Measurement update” For each.](https://reader031.fdocuments.us/reader031/viewer/2022032516/56649c745503460f94926cef/html5/thumbnails/2.jpg)
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
![Page 3: FastSLAM Algorithm As interpreted by Aaron Lee. Figure 13.2 For each particle: – “Retrieval”/”Prediction” Move particle – “Measurement update” For each.](https://reader031.fdocuments.us/reader031/viewer/2022032516/56649c745503460f94926cef/html5/thumbnails/3.jpg)
![Page 4: FastSLAM Algorithm As interpreted by Aaron Lee. Figure 13.2 For each particle: – “Retrieval”/”Prediction” Move particle – “Measurement update” For each.](https://reader031.fdocuments.us/reader031/viewer/2022032516/56649c745503460f94926cef/html5/thumbnails/4.jpg)
Get next particle
![Page 5: FastSLAM Algorithm As interpreted by Aaron Lee. Figure 13.2 For each particle: – “Retrieval”/”Prediction” Move particle – “Measurement update” For each.](https://reader031.fdocuments.us/reader031/viewer/2022032516/56649c745503460f94926cef/html5/thumbnails/5.jpg)
Get next particle
Move particle
![Page 6: FastSLAM Algorithm As interpreted by Aaron Lee. Figure 13.2 For each particle: – “Retrieval”/”Prediction” Move particle – “Measurement update” For each.](https://reader031.fdocuments.us/reader031/viewer/2022032516/56649c745503460f94926cef/html5/thumbnails/6.jpg)
Get next particle
Move particle
Loop through all features in theparticle and determine how welleach of them correlate with themeasurement.
![Page 7: FastSLAM Algorithm As interpreted by Aaron Lee. Figure 13.2 For each particle: – “Retrieval”/”Prediction” Move particle – “Measurement update” For each.](https://reader031.fdocuments.us/reader031/viewer/2022032516/56649c745503460f94926cef/html5/thumbnails/7.jpg)
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
![Page 8: FastSLAM Algorithm As interpreted by Aaron Lee. Figure 13.2 For each particle: – “Retrieval”/”Prediction” Move particle – “Measurement update” For each.](https://reader031.fdocuments.us/reader031/viewer/2022032516/56649c745503460f94926cef/html5/thumbnails/8.jpg)
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
![Page 9: FastSLAM Algorithm As interpreted by Aaron Lee. Figure 13.2 For each particle: – “Retrieval”/”Prediction” Move particle – “Measurement update” For each.](https://reader031.fdocuments.us/reader031/viewer/2022032516/56649c745503460f94926cef/html5/thumbnails/9.jpg)
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
![Page 10: FastSLAM Algorithm As interpreted by Aaron Lee. Figure 13.2 For each particle: – “Retrieval”/”Prediction” Move particle – “Measurement update” For each.](https://reader031.fdocuments.us/reader031/viewer/2022032516/56649c745503460f94926cef/html5/thumbnails/10.jpg)
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
![Page 11: FastSLAM Algorithm As interpreted by Aaron Lee. Figure 13.2 For each particle: – “Retrieval”/”Prediction” Move particle – “Measurement update” For each.](https://reader031.fdocuments.us/reader031/viewer/2022032516/56649c745503460f94926cef/html5/thumbnails/11.jpg)
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.
![Page 12: FastSLAM Algorithm As interpreted by Aaron Lee. Figure 13.2 For each particle: – “Retrieval”/”Prediction” Move particle – “Measurement update” For each.](https://reader031.fdocuments.us/reader031/viewer/2022032516/56649c745503460f94926cef/html5/thumbnails/12.jpg)
If we didn’t see the feature, don’t changeits properties
![Page 13: FastSLAM Algorithm As interpreted by Aaron Lee. Figure 13.2 For each particle: – “Retrieval”/”Prediction” Move particle – “Measurement update” For each.](https://reader031.fdocuments.us/reader031/viewer/2022032516/56649c745503460f94926cef/html5/thumbnails/13.jpg)
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
![Page 14: FastSLAM Algorithm As interpreted by Aaron Lee. Figure 13.2 For each particle: – “Retrieval”/”Prediction” Move particle – “Measurement update” For each.](https://reader031.fdocuments.us/reader031/viewer/2022032516/56649c745503460f94926cef/html5/thumbnails/14.jpg)
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
![Page 15: FastSLAM Algorithm As interpreted by Aaron Lee. Figure 13.2 For each particle: – “Retrieval”/”Prediction” Move particle – “Measurement update” For each.](https://reader031.fdocuments.us/reader031/viewer/2022032516/56649c745503460f94926cef/html5/thumbnails/15.jpg)
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
![Page 16: FastSLAM Algorithm As interpreted by Aaron Lee. Figure 13.2 For each particle: – “Retrieval”/”Prediction” Move particle – “Measurement update” For each.](https://reader031.fdocuments.us/reader031/viewer/2022032516/56649c745503460f94926cef/html5/thumbnails/16.jpg)
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
![Page 17: FastSLAM Algorithm As interpreted by Aaron Lee. Figure 13.2 For each particle: – “Retrieval”/”Prediction” Move particle – “Measurement update” For each.](https://reader031.fdocuments.us/reader031/viewer/2022032516/56649c745503460f94926cef/html5/thumbnails/17.jpg)
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
![Page 18: FastSLAM Algorithm As interpreted by Aaron Lee. Figure 13.2 For each particle: – “Retrieval”/”Prediction” Move particle – “Measurement update” For each.](https://reader031.fdocuments.us/reader031/viewer/2022032516/56649c745503460f94926cef/html5/thumbnails/18.jpg)
• End.