Skip to content

Add code links for localization module #1210

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 1 commit into from
May 2, 2025
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,7 @@
author: Sven Niederberger
Atsushi Sakai

Ref:
Reference:
- Python implementation of 'Successive Convexification for 6-DoF Mars Rocket Powered Landing with Free-Final-Time' paper
by Michael Szmuk and Behcet Acıkmese.

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,7 @@
Author: Daniel Ingram (daniel-s-ingram)
Atsushi Sakai (@Atsushi_twi)

Ref: P. I. Corke, "Robotics, Vision & Control", Springer 2017,
Reference: P. I. Corke, "Robotics, Vision & Control", Springer 2017,
ISBN 978-3-319-54413-7 p102
- [Robotics, Vision and Control]
(https://link.springer.com/book/10.1007/978-3-642-20144-8)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@

author: Ryohei Sasaki(rsasaki0109)

Ref:
Reference:
Ensemble Kalman filtering
(https://rmets.onlinelibrary.wiley.com/doi/10.1256/qj.05.135)

Expand Down
2 changes: 1 addition & 1 deletion Mapping/DistanceMap/distance_map.py
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@

author: Wang Zheng (@Aglargil)

Ref:
Reference:

- [Distance Map]
(https://cs.brown.edu/people/pfelzens/papers/dt-final.pdf)
Expand Down
2 changes: 1 addition & 1 deletion Mapping/rectangle_fitting/rectangle_fitting.py
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@

author: Atsushi Sakai (@Atsushi_twi)

Ref:
Reference:
- Efficient L-Shape Fitting for Vehicle Detection Using Laser Scanners -
The Robotics Institute Carnegie Mellon University
https://www.ri.cmu.edu/publications/
Expand Down
2 changes: 1 addition & 1 deletion MissionPlanning/BehaviorTree/behavior_tree.py
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@

author: Wang Zheng (@Aglargil)

Ref:
Reference:

- [Behavior Tree](https://en.wikipedia.org/wiki/Behavior_tree_(artificial_intelligence,_robotics_and_control))
"""
Expand Down
4 changes: 2 additions & 2 deletions MissionPlanning/StateMachine/state_machine.py
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@

author: Wang Zheng (@Aglargil)

Ref:
Reference:

- [State Machine]
(https://en.wikipedia.org/wiki/Finite-state_machine)
Expand All @@ -23,7 +23,7 @@ def deflate_and_encode(plantuml_text):
"""
zlib compress the plantuml text and encode it for the plantuml server.

Ref: https://plantuml.com/en/text-encoding
Reference https://plantuml.com/en/text-encoding
"""
plantuml_alphabet = (
string.digits + string.ascii_uppercase + string.ascii_lowercase + "-_"
Expand Down
2 changes: 1 addition & 1 deletion PathPlanning/ElasticBands/elastic_bands.py
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@

author: Wang Zheng (@Aglargil)

Ref:
Reference:

- [Elastic Bands: Connecting Path Planning and Control]
(http://www8.cs.umu.se/research/ifor/dl/Control/elastic%20bands.pdf)
Expand Down
2 changes: 1 addition & 1 deletion PathPlanning/Eta3SplinePath/eta3_spline_path.py
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,7 @@
author: Joe Dinius, Ph.D (https://jwdinius.github.io)
Atsushi Sakai (@Atsushi_twi)

Ref:
Reference:
- [eta^3-Splines for the Smooth Path Generation of Wheeled Mobile Robots]
(https://ieeexplore.ieee.org/document/4339545/)

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@

author: Wang Zheng (@Aglargil)

Ref:
Reference:

- [Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame]
(https://www.researchgate.net/profile/Moritz_Werling/publication/224156269_Optimal_Trajectory_Generation_for_Dynamic_Street_Scenarios_in_a_Frenet_Frame/links/54f749df0cf210398e9277af.pdf)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@

author: Atsushi Sakai (@Atsushi_twi)

Ref:
Reference:

- [Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame]
(https://www.researchgate.net/profile/Moritz_Werling/publication/224156269_Optimal_Trajectory_Generation_for_Dynamic_Street_Scenarios_in_a_Frenet_Frame/links/54f749df0cf210398e9277af.pdf)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@

author: Atsushi Sakai (@Atsushi_twi)

Ref:
Reference:
https://www.cs.cmu.edu/~motionplanning/lecture/Chap4-Potential-Field_howie.pdf

"""
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@

author: Atsushi Sakai (@Atsushi_twi)

Ref:
Reference:

- [Local Path planning And Motion Control For Agv In Positioning](https://ieeexplore.ieee.org/document/637936/)

Expand Down
2 changes: 1 addition & 1 deletion PathPlanning/StateLatticePlanner/state_lattice_planner.py
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,7 @@
https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning
/ModelPredictiveTrajectoryGenerator/lookup_table_generator.py

Ref:
Reference:

- State Space Sampling of Feasible Motions for High-Performance Mobile Robot
Navigation in Complex Environments
Expand Down
2 changes: 1 addition & 1 deletion PathTracking/cgmres_nmpc/cgmres_nmpc.py
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@

author Atsushi Sakai (@Atsushi_twi)

Ref:
Reference:
Shunichi09/nonlinear_control: Implementing the nonlinear model predictive
control, sliding mode control https://github.com/Shunichi09/PythonLinearNonlinearControl

Expand Down
2 changes: 1 addition & 1 deletion PathTracking/move_to_pose/move_to_pose.py
Original file line number Diff line number Diff line change
Expand Up @@ -76,7 +76,7 @@ def calc_control_command(self, x_diff, y_diff, theta, theta_goal):
# [-pi, pi] to prevent unstable behavior e.g. difference going
# from 0 rad to 2*pi rad with slight turn

# Ref: The velocity v always has a constant sign which depends on the initial value of α.
# The velocity v always has a constant sign which depends on the initial value of α.
rho = np.hypot(x_diff, y_diff)
v = self.Kp_rho * rho

Expand Down
2 changes: 1 addition & 1 deletion PathTracking/stanley_controller/stanley_controller.py
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@

author: Atsushi Sakai (@Atsushi_twi)

Ref:
Reference:
- [Stanley: The robot that won the DARPA grand challenge](http://isl.ecst.csuchico.edu/DOCS/darpa2005/DARPA%202005%20Stanley.pdf)
- [Autonomous Automobile Path Tracking](https://www.ri.cmu.edu/pub_files/2009/2/Automatic_Steering_Methods_for_Autonomous_Automobile_Path_Tracking.pdf)

Expand Down
42 changes: 21 additions & 21 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -168,7 +168,7 @@ All animation gifs are stored here: [AtsushiSakai/PythonRoboticsGifs: Animation

<img src="https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Localization/extended_kalman_filter/animation.gif" width="640" alt="EKF pic">

Ref:
Reference

- [documentation](https://atsushisakai.github.io/PythonRobotics/modules/localization/extended_kalman_filter_localization_files/extended_kalman_filter_localization.html)

Expand All @@ -186,7 +186,7 @@ It is assumed that the robot can measure a distance from landmarks (RFID).

These measurements are used for PF localization.

Ref:
Reference

- [PROBABILISTIC ROBOTICS](http://www.probabilistic-robotics.org/)

Expand All @@ -207,7 +207,7 @@ The filter integrates speed input and range observations from RFID for localizat

Initial position is not needed.

Ref:
Reference

- [PROBABILISTIC ROBOTICS](http://www.probabilistic-robotics.org/)

Expand Down Expand Up @@ -256,7 +256,7 @@ It can calculate a rotation matrix, and a translation vector between points and

![3](https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/SLAM/iterative_closest_point/animation.gif)

Ref:
Reference

- [Introduction to Mobile Robotics: Iterative Closest Point Algorithm](https://cs.gmu.edu/~kosecka/cs685/cs685-icp.pdf)

Expand All @@ -275,7 +275,7 @@ Black points are landmarks, blue crosses are estimated landmark positions by Fas
![3](https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/SLAM/FastSLAM1/animation.gif)


Ref:
Reference

- [PROBABILISTIC ROBOTICS](http://www.probabilistic-robotics.org/)

Expand Down Expand Up @@ -321,7 +321,7 @@ This is a 2D grid based the shortest path planning with D star algorithm.

The animation shows a robot finding its path avoiding an obstacle using the D* search algorithm.

Ref:
Reference

- [D* Algorithm Wikipedia](https://en.wikipedia.org/wiki/D*)

Expand All @@ -346,7 +346,7 @@ This is a 2D grid based path planning with Potential Field algorithm.

In the animation, the blue heat map shows potential value on each grid.

Ref:
Reference

- [Robotic Motion Planning:Potential Functions](https://www.cs.cmu.edu/~motionplanning/lecture/Chap4-Potential-Field_howie.pdf)

Expand All @@ -362,7 +362,7 @@ This script is a path planning code with state lattice planning.

This code uses the model predictive trajectory generator to solve boundary problem.

Ref:
Reference

- [Optimal rough terrain trajectory generation for wheeled mobile robots](https://journals.sagepub.com/doi/pdf/10.1177/0278364906075328)

Expand Down Expand Up @@ -390,7 +390,7 @@ Cyan crosses means searched points with Dijkstra method,

The red line is the final path of PRM.

Ref:
Reference

- [Probabilistic roadmap \- Wikipedia](https://en.wikipedia.org/wiki/Probabilistic_roadmap)

Expand All @@ -406,7 +406,7 @@ This is a path planning code with RRT\*

Black circles are obstacles, green line is a searched tree, red crosses are start and goal positions.

Ref:
Reference

- [Incremental Sampling-based Algorithms for Optimal Motion Planning](https://arxiv.org/abs/1005.0416)

Expand All @@ -426,7 +426,7 @@ A double integrator motion model is used for LQR local planner.

![LQR_RRT](https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/LQRRRTStar/animation.gif)

Ref:
Reference

- [LQR\-RRT\*: Optimal Sampling\-Based Motion Planning with Automatically Derived Extension Heuristics](https://lis.csail.mit.edu/pubs/perez-icra12.pdf)

Expand All @@ -441,7 +441,7 @@ Motion planning with quintic polynomials.

It can calculate a 2D path, velocity, and acceleration profile based on quintic polynomials.

Ref:
Reference

- [Local Path Planning And Motion Control For Agv In Positioning](https://ieeexplore.ieee.org/document/637936/)

Expand All @@ -451,7 +451,7 @@ A sample code with Reeds Shepp path planning.

![RSPlanning](https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/ReedsSheppPath/animation.gif?raw=true)

Ref:
Reference

- [15.3.2 Reeds\-Shepp Curves](http://planning.cs.uiuc.edu/node822.html)

Expand All @@ -477,7 +477,7 @@ The cyan line is the target course and black crosses are obstacles.

The red line is the predicted path.

Ref:
Reference

- [Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame](https://www.researchgate.net/profile/Moritz_Werling/publication/224156269_Optimal_Trajectory_Generation_for_Dynamic_Street_Scenarios_in_a_Frenet_Frame/links/54f749df0cf210398e9277af.pdf)

Expand All @@ -492,7 +492,7 @@ This is a simulation of moving to a pose control

![2](https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/move_to_pose/animation.gif)

Ref:
Reference

- [P. I. Corke, "Robotics, Vision and Control" \| SpringerLink p102](https://link.springer.com/book/10.1007/978-3-642-20144-8)

Expand All @@ -503,7 +503,7 @@ Path tracking simulation with Stanley steering control and PID speed control.

![2](https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/stanley_controller/animation.gif)

Ref:
Reference

- [Stanley: The robot that won the DARPA grand challenge](http://robots.stanford.edu/papers/thrun.stanley05.pdf)

Expand All @@ -517,7 +517,7 @@ Path tracking simulation with rear wheel feedback steering control and PID speed

![PythonRobotics/figure_1.png at master · AtsushiSakai/PythonRobotics](https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/rear_wheel_feedback/animation.gif)

Ref:
Reference

- [A Survey of Motion Planning and Control Techniques for Self-driving Urban Vehicles](https://arxiv.org/abs/1604.07446)

Expand All @@ -528,7 +528,7 @@ Path tracking simulation with LQR speed and steering control.

![3](https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/lqr_speed_steer_control/animation.gif)

Ref:
Reference

- [Towards fully autonomous driving: Systems and algorithms \- IEEE Conference Publication](https://ieeexplore.ieee.org/document/5940562/)

Expand All @@ -539,7 +539,7 @@ Path tracking simulation with iterative linear model predictive speed and steeri

<img src="https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/model_predictive_speed_and_steer_control/animation.gif" width="640" alt="MPC pic">

Ref:
Reference

- [documentation](https://atsushisakai.github.io/PythonRobotics/modules/path_tracking/model_predictive_speed_and_steering_control/model_predictive_speed_and_steering_control.html)

Expand All @@ -551,7 +551,7 @@ A motion planning and path tracking simulation with NMPC of C-GMRES

![3](https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/cgmres_nmpc/animation.gif)

Ref:
Reference

- [documentation](https://atsushisakai.github.io/PythonRobotics/modules/path_tracking/cgmres_nmpc/cgmres_nmpc.html)

Expand Down Expand Up @@ -591,7 +591,7 @@ This is a 3d trajectory generation simulation for a rocket powered landing.

![3](https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/AerialNavigation/rocket_powered_landing/animation.gif)

Ref:
Reference

- [documentation](https://atsushisakai.github.io/PythonRobotics/modules/aerial_navigation/rocket_powered_landing/rocket_powered_landing.html)

Expand Down
2 changes: 1 addition & 1 deletion docs/modules/12_appendix/Kalmanfilter_basics_2_main.rst
Original file line number Diff line number Diff line change
Expand Up @@ -331,7 +331,7 @@ are vectors and matrices, but the concepts are exactly the same:
- Use the process model to predict the next state (the prior)
- Form an estimate part way between the measurement and the prior

References:
Reference
~~~~~~~~~~~

1. Roger Labbe’s
Expand Down
2 changes: 1 addition & 1 deletion docs/modules/12_appendix/Kalmanfilter_basics_main.rst
Original file line number Diff line number Diff line change
Expand Up @@ -552,7 +552,7 @@ a given (X,Y) value.
.. image:: Kalmanfilter_basics_files/Kalmanfilter_basics_28_1.png


References:
Reference
~~~~~~~~~~~

1. Roger Labbe’s
Expand Down
2 changes: 1 addition & 1 deletion docs/modules/12_appendix/steering_motion_model_main.rst
Original file line number Diff line number Diff line change
Expand Up @@ -91,7 +91,7 @@ the target minimum velocity :math:`v_{min}` can be calculated as follows:
:math:`v_{min} = \frac{d_{t+1}+d_{t}}{\Delta t} = \frac{d_{t+1}+d_{t}}{(\kappa_{t+1}-\kappa_{t})}\frac{tan\dot{\delta}_{max}}{WB}`


References:
Reference
~~~~~~~~~~~

- `Vehicle Dynamics and Control <https://link.springer.com/book/10.1007/978-1-4614-1433-9>`_
Original file line number Diff line number Diff line change
Expand Up @@ -5,3 +5,8 @@ Ensamble Kalman Filter Localization

This is a sensor fusion localization with Ensamble Kalman Filter(EnKF).

Code Link
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^

.. autofunction:: Localization.ensemble_kalman_filter.ensemble_kalman_filter.enkf_localization

Loading