diff --git a/Localization/histogram_filter/animation.gif b/Localization/histogram_filter/animation.gif new file mode 100644 index 00000000..d868c411 Binary files /dev/null and b/Localization/histogram_filter/animation.gif differ diff --git a/Localization/histogram_filter/histogram_filter.py b/Localization/histogram_filter/histogram_filter.py index f8a8be06..ed1f721a 100644 --- a/Localization/histogram_filter/histogram_filter.py +++ b/Localization/histogram_filter/histogram_filter.py @@ -33,7 +33,6 @@ MINY = -5.0 MAXX = 15.0 MAXY = 25.0 - # simulation paramters NOISE_RANGE = 2.0 # [m] 1σ range noise parameter NOISE_SPEED = 0.5 # [m/s] 1σ speed noise parameter diff --git a/README.md b/README.md index 24baf5d6..e2e4d83c 100644 --- a/README.md +++ b/README.md @@ -11,7 +11,8 @@ Python codes for robotics algorithm. * [Localization](#localization) * [Extended Kalman Filter localization](#extended-kalman-filter-localization) * [Unscented Kalman Filter localization](#unscented-kalman-filter-localization) - * [Particle Filter localization](#particle-filter-localization) + * [Particle filter localization](#particle-filter-localization) + * [Histogram filter localization](#histogram-filter-localization) * [Mapping](#mapping) * [Gaussian grid map](#gaussian-grid-map) * [Ray casting grid map](#ray-casting-grid-map) @@ -97,6 +98,10 @@ the green point is positioning observation (ex. GPS), and the red line is estima The red ellipse is estimated covariance ellipse with EKF. +Ref: + +- [PROBABILISTIC ROBOTICS](http://www.probabilistic-robotics.org/) + ## Unscented Kalman Filter localization ![2](https://github.com/AtsushiSakai/PythonRobotics/raw/master/Localization/unscented_kalman_filter/animation.gif) @@ -109,7 +114,8 @@ Ref: - [Discriminatively Trained Unscented Kalman Filter for Mobile Robot Localization](https://www.researchgate.net/publication/267963417_Discriminatively_Trained_Unscented_Kalman_Filter_for_Mobile_Robot_Localization) -## Particle Filter localization + +## Particle filter localization ![2](https://github.com/AtsushiSakai/PythonRobotics/raw/master/Localization/particle_filter/animation.gif) @@ -123,6 +129,27 @@ It is assumed that the robot can measure a distance from landmarks (RFID). This measurements are used for PF localization. +Ref: + +- [PROBABILISTIC ROBOTICS](http://www.probabilistic-robotics.org/) + + +## Histogram filter localization + +![3](https://github.com/AtsushiSakai/PythonRobotics/raw/master/Localization/histogram_filter/animation.gif) + +This is a 2D localization example with Histogram filter. + +In this simulation, x,y are unknown, yaw is known. + +The filter integrates speed input and range observations from RFID for localization. + +Initial position is not needed. + +Ref: + +- [PROBABILISTIC ROBOTICS](http://www.probabilistic-robotics.org/) + # Mapping ## Gaussian grid map @@ -575,3 +602,4 @@ Atsushi Sakai ([@Atsushi_twi](https://twitter.com/Atsushi_twi)) + diff --git a/tests/test_histogram_filter.py b/tests/test_histogram_filter.py new file mode 100644 index 00000000..f39995b2 --- /dev/null +++ b/tests/test_histogram_filter.py @@ -0,0 +1,12 @@ +from unittest import TestCase + +from Localization.histogram_filter import histogram_filter as m + +print(__file__) + + +class Test(TestCase): + + def test1(self): + m.show_animation = False + m.main()