diff --git a/AerialNavigation/rocket_powered_landing/rocket_powered_landing.py b/AerialNavigation/rocket_powered_landing/rocket_powered_landing.py index 1918dc1cee..e8ba8fa220 100644 --- a/AerialNavigation/rocket_powered_landing/rocket_powered_landing.py +++ b/AerialNavigation/rocket_powered_landing/rocket_powered_landing.py @@ -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. diff --git a/ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py b/ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py index c2227f18e3..09969c30be 100644 --- a/ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py +++ b/ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py @@ -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) diff --git a/Localization/ensemble_kalman_filter/ensemble_kalman_filter.py b/Localization/ensemble_kalman_filter/ensemble_kalman_filter.py index 2bab3b49c1..e8a988a270 100644 --- a/Localization/ensemble_kalman_filter/ensemble_kalman_filter.py +++ b/Localization/ensemble_kalman_filter/ensemble_kalman_filter.py @@ -4,7 +4,7 @@ author: Ryohei Sasaki(rsasaki0109) -Ref: +Reference: Ensemble Kalman filtering (https://rmets.onlinelibrary.wiley.com/doi/10.1256/qj.05.135) diff --git a/Mapping/DistanceMap/distance_map.py b/Mapping/DistanceMap/distance_map.py index 0dcc7380c5..0f96c9e8c6 100644 --- a/Mapping/DistanceMap/distance_map.py +++ b/Mapping/DistanceMap/distance_map.py @@ -3,7 +3,7 @@ author: Wang Zheng (@Aglargil) -Ref: +Reference: - [Distance Map] (https://cs.brown.edu/people/pfelzens/papers/dt-final.pdf) diff --git a/Mapping/rectangle_fitting/rectangle_fitting.py b/Mapping/rectangle_fitting/rectangle_fitting.py index 177f078871..7902619666 100644 --- a/Mapping/rectangle_fitting/rectangle_fitting.py +++ b/Mapping/rectangle_fitting/rectangle_fitting.py @@ -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/ diff --git a/MissionPlanning/BehaviorTree/behavior_tree.py b/MissionPlanning/BehaviorTree/behavior_tree.py index 59f4c713f1..9ad886aafb 100644 --- a/MissionPlanning/BehaviorTree/behavior_tree.py +++ b/MissionPlanning/BehaviorTree/behavior_tree.py @@ -3,7 +3,7 @@ author: Wang Zheng (@Aglargil) -Ref: +Reference: - [Behavior Tree](https://en.wikipedia.org/wiki/Behavior_tree_(artificial_intelligence,_robotics_and_control)) """ diff --git a/MissionPlanning/StateMachine/state_machine.py b/MissionPlanning/StateMachine/state_machine.py index de72f0f451..454759236e 100644 --- a/MissionPlanning/StateMachine/state_machine.py +++ b/MissionPlanning/StateMachine/state_machine.py @@ -3,7 +3,7 @@ author: Wang Zheng (@Aglargil) -Ref: +Reference: - [State Machine] (https://en.wikipedia.org/wiki/Finite-state_machine) @@ -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 + "-_" diff --git a/PathPlanning/ElasticBands/elastic_bands.py b/PathPlanning/ElasticBands/elastic_bands.py index 785f822d14..77d4e6e399 100644 --- a/PathPlanning/ElasticBands/elastic_bands.py +++ b/PathPlanning/ElasticBands/elastic_bands.py @@ -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) diff --git a/PathPlanning/Eta3SplinePath/eta3_spline_path.py b/PathPlanning/Eta3SplinePath/eta3_spline_path.py index dc07d3c84b..3f685e512f 100644 --- a/PathPlanning/Eta3SplinePath/eta3_spline_path.py +++ b/PathPlanning/Eta3SplinePath/eta3_spline_path.py @@ -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/) diff --git a/PathPlanning/FrenetOptimalTrajectory/cartesian_frenet_converter.py b/PathPlanning/FrenetOptimalTrajectory/cartesian_frenet_converter.py index 4cc8650c89..482712ceaf 100644 --- a/PathPlanning/FrenetOptimalTrajectory/cartesian_frenet_converter.py +++ b/PathPlanning/FrenetOptimalTrajectory/cartesian_frenet_converter.py @@ -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) diff --git a/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py b/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py index 248894c1c6..4b82fb70fd 100644 --- a/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py +++ b/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py @@ -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) diff --git a/PathPlanning/PotentialFieldPlanning/potential_field_planning.py b/PathPlanning/PotentialFieldPlanning/potential_field_planning.py index 8f136b5ee3..603a9d16cf 100644 --- a/PathPlanning/PotentialFieldPlanning/potential_field_planning.py +++ b/PathPlanning/PotentialFieldPlanning/potential_field_planning.py @@ -4,7 +4,7 @@ author: Atsushi Sakai (@Atsushi_twi) -Ref: +Reference: https://www.cs.cmu.edu/~motionplanning/lecture/Chap4-Potential-Field_howie.pdf """ diff --git a/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py b/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py index fdc181afab..86f9f662da 100644 --- a/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py +++ b/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py @@ -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/) diff --git a/PathPlanning/StateLatticePlanner/state_lattice_planner.py b/PathPlanning/StateLatticePlanner/state_lattice_planner.py index 7f8e725e0a..05f8df78f8 100644 --- a/PathPlanning/StateLatticePlanner/state_lattice_planner.py +++ b/PathPlanning/StateLatticePlanner/state_lattice_planner.py @@ -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 diff --git a/PathTracking/cgmres_nmpc/cgmres_nmpc.py b/PathTracking/cgmres_nmpc/cgmres_nmpc.py index a582c9da81..ee40e73504 100644 --- a/PathTracking/cgmres_nmpc/cgmres_nmpc.py +++ b/PathTracking/cgmres_nmpc/cgmres_nmpc.py @@ -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 diff --git a/PathTracking/move_to_pose/move_to_pose.py b/PathTracking/move_to_pose/move_to_pose.py index 34736a2e21..faf1264953 100644 --- a/PathTracking/move_to_pose/move_to_pose.py +++ b/PathTracking/move_to_pose/move_to_pose.py @@ -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 diff --git a/PathTracking/stanley_controller/stanley_controller.py b/PathTracking/stanley_controller/stanley_controller.py index bc98175f17..01c2ec0229 100644 --- a/PathTracking/stanley_controller/stanley_controller.py +++ b/PathTracking/stanley_controller/stanley_controller.py @@ -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) diff --git a/README.md b/README.md index 6d04c90bc8..05fd0262df 100644 --- a/README.md +++ b/README.md @@ -168,7 +168,7 @@ All animation gifs are stored here: [AtsushiSakai/PythonRoboticsGifs: Animation EKF pic -Ref: +Reference - [documentation](https://atsushisakai.github.io/PythonRobotics/modules/localization/extended_kalman_filter_localization_files/extended_kalman_filter_localization.html) @@ -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/) @@ -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/) @@ -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) @@ -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/) @@ -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*) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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/) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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/) @@ -539,7 +539,7 @@ Path tracking simulation with iterative linear model predictive speed and steeri 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) @@ -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) @@ -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) diff --git a/docs/modules/12_appendix/Kalmanfilter_basics_2_main.rst b/docs/modules/12_appendix/Kalmanfilter_basics_2_main.rst index 9ae6fc5bcb..b7ff84e6f6 100644 --- a/docs/modules/12_appendix/Kalmanfilter_basics_2_main.rst +++ b/docs/modules/12_appendix/Kalmanfilter_basics_2_main.rst @@ -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 diff --git a/docs/modules/12_appendix/Kalmanfilter_basics_main.rst b/docs/modules/12_appendix/Kalmanfilter_basics_main.rst index 6548377e07..a1d99d47ef 100644 --- a/docs/modules/12_appendix/Kalmanfilter_basics_main.rst +++ b/docs/modules/12_appendix/Kalmanfilter_basics_main.rst @@ -552,7 +552,7 @@ a given (X,Y) value. .. image:: Kalmanfilter_basics_files/Kalmanfilter_basics_28_1.png -References: +Reference ~~~~~~~~~~~ 1. Roger Labbe’s diff --git a/docs/modules/12_appendix/steering_motion_model_main.rst b/docs/modules/12_appendix/steering_motion_model_main.rst index 6e444b7909..c697123fa2 100644 --- a/docs/modules/12_appendix/steering_motion_model_main.rst +++ b/docs/modules/12_appendix/steering_motion_model_main.rst @@ -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 `_ diff --git a/docs/modules/2_localization/ensamble_kalman_filter_localization_files/ensamble_kalman_filter_localization_main.rst b/docs/modules/2_localization/ensamble_kalman_filter_localization_files/ensamble_kalman_filter_localization_main.rst index 2543d1186a..214e645d10 100644 --- a/docs/modules/2_localization/ensamble_kalman_filter_localization_files/ensamble_kalman_filter_localization_main.rst +++ b/docs/modules/2_localization/ensamble_kalman_filter_localization_files/ensamble_kalman_filter_localization_main.rst @@ -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 + diff --git a/docs/modules/2_localization/extended_kalman_filter_localization_files/extended_kalman_filter_localization_main.rst b/docs/modules/2_localization/extended_kalman_filter_localization_files/extended_kalman_filter_localization_main.rst index da214b6de5..adb41e5e40 100644 --- a/docs/modules/2_localization/extended_kalman_filter_localization_files/extended_kalman_filter_localization_main.rst +++ b/docs/modules/2_localization/extended_kalman_filter_localization_files/extended_kalman_filter_localization_main.rst @@ -23,28 +23,40 @@ is estimated trajectory with EKF. The red ellipse is estimated covariance ellipse with EKF. -Code: `PythonRobotics/extended_kalman_filter.py at master · -AtsushiSakai/PythonRobotics `__ +Code Link +~~~~~~~~~~~~~ -Kalman Filter with Speed Scale Factor Correction -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +.. autofunction:: Localization.extended_kalman_filter.extended_kalman_filter.ekf_estimation +Extended Kalman Filter algorithm +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ -.. image:: ekf_with_velocity_correction_1_0.png - :width: 600px +Localization process using Extended Kalman Filter:EKF is -This is a Extended kalman filter (EKF) localization with velocity correction. +=== Predict === -This is for correcting the vehicle speed measured with scale factor errors due to factors such as wheel wear. +:math:`x_{Pred} = Fx_t+Bu_t` -Code: `PythonRobotics/extended_kalman_ekf_with_velocity_correctionfilter.py -AtsushiSakai/PythonRobotics `__ +:math:`P_{Pred} = J_f P_t J_f^T + Q` -Filter design -~~~~~~~~~~~~~ +=== Update === -Position Estimation Kalman Filter -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +:math:`z_{Pred} = Hx_{Pred}` + +:math:`y = z - z_{Pred}` + +:math:`S = J_g P_{Pred}.J_g^T + R` + +:math:`K = P_{Pred}.J_g^T S^{-1}` + +:math:`x_{t+1} = x_{Pred} + Ky` + +:math:`P_{t+1} = ( I - K J_g) P_{Pred}` + + + +Filter design +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ In this simulation, the robot has a state vector includes 4 states at time :math:`t`. @@ -82,27 +94,9 @@ In the code, “observation” function generates the input and observation vector with noise `code `__ -Kalman Filter with Speed Scale Factor Correction -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -In this simulation, the robot has a state vector includes 5 states at -time :math:`t`. - -.. math:: \textbf{x}_t=[x_t, y_t, \phi_t, v_t, s_t] - -x, y are a 2D x-y position, :math:`\phi` is orientation, v is -velocity, and s is a scale factor of velocity. - -In the code, “xEst” means the state vector. -`code `__ - -The rest is the same as the Position Estimation Kalman Filter. Motion Model -~~~~~~~~~~~~ - -Position Estimation Kalman Filter -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +~~~~~~~~~~~~~~~~~ The robot model is @@ -137,9 +131,61 @@ Its Jacobian matrix is :math:`\begin{equation*} 　= \begin{bmatrix} 1& 0 & -v \sin(\phi) \Delta t & \cos(\phi) \Delta t\\ 0 & 1 & v \cos(\phi) \Delta t & \sin(\phi) \Delta t\\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \end{equation*}` +Observation Model +~~~~~~~~~~~~~~~~~ + +The robot can get x-y position information from GPS. + +So GPS Observation model is + +.. math:: \textbf{z}_{t} = g(\textbf{x}_t) = H \textbf{x}_t + +where + +:math:`\begin{equation*} H = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ \end{bmatrix} \end{equation*}` + +The observation function states that + +:math:`\begin{equation*} \begin{bmatrix} x' \\ y' \end{bmatrix} = g(\textbf{x}) = \begin{bmatrix} x \\ y \end{bmatrix} \end{equation*}` + +Its Jacobian matrix is + +:math:`\begin{equation*} J_g = \begin{bmatrix} \frac{\partial x'}{\partial x} & \frac{\partial x'}{\partial y} & \frac{\partial x'}{\partial \phi} & \frac{\partial x'}{\partial v}\\ \frac{\partial y'}{\partial x}& \frac{\partial y'}{\partial y} & \frac{\partial y'}{\partial \phi} & \frac{\partial y'}{ \partial v}\\ \end{bmatrix} \end{equation*}` + +:math:`\begin{equation*} 　= \begin{bmatrix} 1& 0 & 0 & 0\\ 0 & 1 & 0 & 0\\ \end{bmatrix} \end{equation*}` + Kalman Filter with Speed Scale Factor Correction ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +This is a Extended kalman filter (EKF) localization with velocity correction. + +This is for correcting the vehicle speed measured with scale factor errors due to factors such as wheel wear. + + +In this simulation, the robot has a state vector includes 5 states at +time :math:`t`. + +.. math:: \textbf{x}_t=[x_t, y_t, \phi_t, v_t, s_t] + +x, y are a 2D x-y position, :math:`\phi` is orientation, v is +velocity, and s is a scale factor of velocity. + +In the code, “xEst” means the state vector. +`code `__ + +The rest is the same as the Position Estimation Kalman Filter. + +.. image:: ekf_with_velocity_correction_1_0.png + :width: 600px + +Code Link +~~~~~~~~~~~~~ + +.. autofunction:: Localization.extended_kalman_filter.ekf_with_velocity_correction.ekf_estimation + + +Motion Model +~~~~~~~~~~~~ The robot model is @@ -178,33 +224,7 @@ Its Jacobian matrix is Observation Model ~~~~~~~~~~~~~~~~~ -Position Estimation Kalman Filter -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -The robot can get x-y position infomation from GPS. - -So GPS Observation model is - -.. math:: \textbf{z}_{t} = g(\textbf{x}_t) = H \textbf{x}_t - -where - -:math:`\begin{equation*} H = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ \end{bmatrix} \end{equation*}` - -The observation function states that - -:math:`\begin{equation*} \begin{bmatrix} x' \\ y' \end{bmatrix} = g(\textbf{x}) = \begin{bmatrix} x \\ y \end{bmatrix} \end{equation*}` - -Its Jacobian matrix is - -:math:`\begin{equation*} J_g = \begin{bmatrix} \frac{\partial x'}{\partial x} & \frac{\partial x'}{\partial y} & \frac{\partial x'}{\partial \phi} & \frac{\partial x'}{\partial v}\\ \frac{\partial y'}{\partial x}& \frac{\partial y'}{\partial y} & \frac{\partial y'}{\partial \phi} & \frac{\partial y'}{ \partial v}\\ \end{bmatrix} \end{equation*}` - -:math:`\begin{equation*} 　= \begin{bmatrix} 1& 0 & 0 & 0\\ 0 & 1 & 0 & 0\\ \end{bmatrix} \end{equation*}` - -Kalman Filter with Speed Scale Factor Correction -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -The robot can get x-y position infomation from GPS. +The robot can get x-y position information from GPS. So GPS Observation model is @@ -225,32 +245,8 @@ Its Jacobian matrix is :math:`\begin{equation*} 　= \begin{bmatrix} 1& 0 & 0 & 0 & 0\\ 0 & 1 & 0 & 0 & 0\\ \end{bmatrix} \end{equation*}` -Extended Kalman Filter -~~~~~~~~~~~~~~~~~~~~~~ - -Localization process using Extended Kalman Filter:EKF is - -=== Predict === - -:math:`x_{Pred} = Fx_t+Bu_t` - -:math:`P_{Pred} = J_f P_t J_f^T + Q` - -=== Update === - -:math:`z_{Pred} = Hx_{Pred}` - -:math:`y = z - z_{Pred}` - -:math:`S = J_g P_{Pred}.J_g^T + R` - -:math:`K = P_{Pred}.J_g^T S^{-1}` - -:math:`x_{t+1} = x_{Pred} + Ky` - -:math:`P_{t+1} = ( I - K J_g) P_{Pred}` -Ref: -~~~~ +Reference +^^^^^^^^^^^ - `PROBABILISTIC-ROBOTICS.ORG `__ diff --git a/docs/modules/2_localization/histogram_filter_localization/histogram_filter_localization_main.rst b/docs/modules/2_localization/histogram_filter_localization/histogram_filter_localization_main.rst index fafd578333..3a175b1316 100644 --- a/docs/modules/2_localization/histogram_filter_localization/histogram_filter_localization_main.rst +++ b/docs/modules/2_localization/histogram_filter_localization/histogram_filter_localization_main.rst @@ -16,6 +16,11 @@ The filter uses speed input and range observations from RFID for localization. Initial position information is not needed. +Code Link +~~~~~~~~~~~~~ + +.. autofunction:: Localization.histogram_filter.histogram_filter.histogram_filter_localization + Filtering algorithm ~~~~~~~~~~~~~~~~~~~~ @@ -107,7 +112,7 @@ There are two ways to calculate the final positions: -References: +Reference ~~~~~~~~~~~ - `_PROBABILISTIC ROBOTICS: `_ diff --git a/docs/modules/2_localization/particle_filter_localization/particle_filter_localization_main.rst b/docs/modules/2_localization/particle_filter_localization/particle_filter_localization_main.rst index 20a9eb58fc..d648d8e080 100644 --- a/docs/modules/2_localization/particle_filter_localization/particle_filter_localization_main.rst +++ b/docs/modules/2_localization/particle_filter_localization/particle_filter_localization_main.rst @@ -15,6 +15,12 @@ It is assumed that the robot can measure a distance from landmarks This measurements are used for PF localization. +Code Link +~~~~~~~~~~~~~ + +.. autofunction:: Localization.particle_filter.particle_filter.pf_localization + + How to calculate covariance matrix from particles ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ @@ -30,7 +36,7 @@ The covariance matrix :math:`\Xi` from particle information is calculated by the - :math:`\mu_j` is the :math:`j` th mean state of particles. -References: +Reference ~~~~~~~~~~~ - `_PROBABILISTIC ROBOTICS: `_ diff --git a/docs/modules/2_localization/unscented_kalman_filter_localization/unscented_kalman_filter_localization_main.rst b/docs/modules/2_localization/unscented_kalman_filter_localization/unscented_kalman_filter_localization_main.rst index bb6b5b664b..a7a5aab61b 100644 --- a/docs/modules/2_localization/unscented_kalman_filter_localization/unscented_kalman_filter_localization_main.rst +++ b/docs/modules/2_localization/unscented_kalman_filter_localization/unscented_kalman_filter_localization_main.rst @@ -7,7 +7,13 @@ This is a sensor fusion localization with Unscented Kalman Filter(UKF). The lines and points are same meaning of the EKF simulation. -References: +Code Link +~~~~~~~~~~~~~ + +.. autofunction:: Localization.unscented_kalman_filter.unscented_kalman_filter.ukf_estimation + + +Reference ~~~~~~~~~~~ - `Discriminatively Trained Unscented Kalman Filter for Mobile Robot Localization `_ diff --git a/docs/modules/4_slam/ekf_slam/ekf_slam_main.rst b/docs/modules/4_slam/ekf_slam/ekf_slam_main.rst index b27971225e..a1486ffe1e 100644 --- a/docs/modules/4_slam/ekf_slam/ekf_slam_main.rst +++ b/docs/modules/4_slam/ekf_slam/ekf_slam_main.rst @@ -578,7 +578,7 @@ reckoning and control functions are passed along here as well. .. image:: ekf_slam_1_0.png -References: +Reference ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - `PROBABILISTIC ROBOTICS `_ diff --git a/docs/modules/4_slam/graph_slam/graph_slam_main.rst b/docs/modules/4_slam/graph_slam/graph_slam_main.rst index 987eed385c..a2ef513527 100644 --- a/docs/modules/4_slam/graph_slam/graph_slam_main.rst +++ b/docs/modules/4_slam/graph_slam/graph_slam_main.rst @@ -17,7 +17,7 @@ The black stars are landmarks for graph edge generation. .. include:: graphSLAM_formulation.rst .. include:: graphSLAM_SE2_example.rst -References: +Reference ~~~~~~~~~~~ - `A Tutorial on Graph-Based SLAM `_ diff --git a/docs/modules/5_path_planning/bezier_path/bezier_path_main.rst b/docs/modules/5_path_planning/bezier_path/bezier_path_main.rst index d306a85352..9d9b3de709 100644 --- a/docs/modules/5_path_planning/bezier_path/bezier_path_main.rst +++ b/docs/modules/5_path_planning/bezier_path/bezier_path_main.rst @@ -13,7 +13,7 @@ You can get different Beizer course: .. image:: Figure_2.png -Ref: +Reference - `Continuous Curvature Path Generation Based on Bezier Curves for Autonomous diff --git a/docs/modules/5_path_planning/eta3_spline/eta3_spline_main.rst b/docs/modules/5_path_planning/eta3_spline/eta3_spline_main.rst index ffc3cc6451..82e0a71044 100644 --- a/docs/modules/5_path_planning/eta3_spline/eta3_spline_main.rst +++ b/docs/modules/5_path_planning/eta3_spline/eta3_spline_main.rst @@ -7,7 +7,7 @@ Eta^3 Spline path planning This is a path planning with Eta^3 spline. -Ref: +Reference - `\\eta^3-Splines for the Smooth Path Generation of Wheeled Mobile Robots `__ diff --git a/docs/modules/5_path_planning/frenet_frame_path/frenet_frame_path_main.rst b/docs/modules/5_path_planning/frenet_frame_path/frenet_frame_path_main.rst index 38efaf2b53..371d536e3f 100644 --- a/docs/modules/5_path_planning/frenet_frame_path/frenet_frame_path_main.rst +++ b/docs/modules/5_path_planning/frenet_frame_path/frenet_frame_path_main.rst @@ -35,7 +35,7 @@ Low Speed and Merging and Stopping Scenario This scenario illustrates the trajectory planning at low speeds with merging and stopping behaviors. -Ref: +Reference - `Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet diff --git a/docs/modules/5_path_planning/grid_base_search/grid_base_search_main.rst b/docs/modules/5_path_planning/grid_base_search/grid_base_search_main.rst index 1644ed00cc..bf82c9f391 100644 --- a/docs/modules/5_path_planning/grid_base_search/grid_base_search_main.rst +++ b/docs/modules/5_path_planning/grid_base_search/grid_base_search_main.rst @@ -63,7 +63,7 @@ This is a 2D grid based 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* search Wikipedia `__ @@ -74,7 +74,7 @@ This is a 2D grid based path planning and replanning with D star lite algorithm. .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/DStarLite/animation.gif -Ref: +Reference - `Improved Fast Replanning for Robot Navigation in Unknown Terrain `_ @@ -88,7 +88,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 `__ diff --git a/docs/modules/5_path_planning/model_predictive_trajectory_generator/model_predictive_trajectory_generator_main.rst b/docs/modules/5_path_planning/model_predictive_trajectory_generator/model_predictive_trajectory_generator_main.rst index 463363ddcf..1c5df1c9cc 100644 --- a/docs/modules/5_path_planning/model_predictive_trajectory_generator/model_predictive_trajectory_generator_main.rst +++ b/docs/modules/5_path_planning/model_predictive_trajectory_generator/model_predictive_trajectory_generator_main.rst @@ -16,7 +16,7 @@ Lookup table generation sample .. image:: lookup_table.png -Ref: +Reference - `Optimal rough terrain trajectory generation for wheeled mobile robots `__ diff --git a/docs/modules/5_path_planning/prm_planner/prm_planner_main.rst b/docs/modules/5_path_planning/prm_planner/prm_planner_main.rst index 0628719176..f816c5c1b9 100644 --- a/docs/modules/5_path_planning/prm_planner/prm_planner_main.rst +++ b/docs/modules/5_path_planning/prm_planner/prm_planner_main.rst @@ -13,7 +13,7 @@ Cyan crosses means searched points with Dijkstra method, The red line is the final path of PRM. -Ref: +Reference - `Probabilistic roadmap - Wikipedia `__ diff --git a/docs/modules/5_path_planning/quintic_polynomials_planner/quintic_polynomials_planner_main.rst b/docs/modules/5_path_planning/quintic_polynomials_planner/quintic_polynomials_planner_main.rst index 0412b3c9b3..66c3001c05 100644 --- a/docs/modules/5_path_planning/quintic_polynomials_planner/quintic_polynomials_planner_main.rst +++ b/docs/modules/5_path_planning/quintic_polynomials_planner/quintic_polynomials_planner_main.rst @@ -97,7 +97,7 @@ Each velocity and acceleration boundary condition can be calculated with each or :math:`v_{xe}=v_ecos(\theta_e), v_{ye}=v_esin(\theta_e)` -References: +Reference ~~~~~~~~~~~ - `Local Path Planning And Motion Control For Agv In diff --git a/docs/modules/5_path_planning/reeds_shepp_path/reeds_shepp_path_main.rst b/docs/modules/5_path_planning/reeds_shepp_path/reeds_shepp_path_main.rst index ff377eb91b..a4a5d28e01 100644 --- a/docs/modules/5_path_planning/reeds_shepp_path/reeds_shepp_path_main.rst +++ b/docs/modules/5_path_planning/reeds_shepp_path/reeds_shepp_path_main.rst @@ -380,7 +380,7 @@ Hence, we have: - :math:`v = (t - φ)` -Ref: +Reference - `15.3.2 Reeds-Shepp Curves `__ diff --git a/docs/modules/5_path_planning/rrt/rrt_main.rst b/docs/modules/5_path_planning/rrt/rrt_main.rst index e5f351a7ba..da3a4957a5 100644 --- a/docs/modules/5_path_planning/rrt/rrt_main.rst +++ b/docs/modules/5_path_planning/rrt/rrt_main.rst @@ -53,7 +53,7 @@ This is a path planning code with Informed RRT*. The cyan ellipse is the heuristic sampling domain of Informed RRT*. -Ref: +Reference - `Informed RRT\*: Optimal Sampling-based Path Planning Focused via Direct Sampling of an Admissible Ellipsoidal @@ -68,7 +68,7 @@ Batch Informed RRT\* This is a path planning code with Batch Informed RRT*. -Ref: +Reference - `Batch Informed Trees (BIT*): Sampling-based Optimal Planning via the Heuristically Guided Search of Implicit Random Geometric @@ -87,7 +87,7 @@ In this code, pure-pursuit algorithm is used for steering control, PID is used for speed control. -Ref: +Reference - `Motion Planning in Complex Environments using Closed-loop Prediction `__ @@ -109,7 +109,7 @@ A double integrator motion model is used for LQR local planner. .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/LQRRRTStar/animation.gif -Ref: +Reference - `LQR-RRT\*: Optimal Sampling-Based Motion Planning with Automatically Derived Extension diff --git a/docs/modules/5_path_planning/state_lattice_planner/state_lattice_planner_main.rst b/docs/modules/5_path_planning/state_lattice_planner/state_lattice_planner_main.rst index d5e7ed17d1..bf89ac11ae 100644 --- a/docs/modules/5_path_planning/state_lattice_planner/state_lattice_planner_main.rst +++ b/docs/modules/5_path_planning/state_lattice_planner/state_lattice_planner_main.rst @@ -22,7 +22,7 @@ Lane sampling .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/StateLatticePlanner/LaneSampling.gif -Ref: +Reference - `Optimal rough terrain trajectory generation for wheeled mobile robots `__ diff --git a/docs/modules/5_path_planning/vrm_planner/vrm_planner_main.rst b/docs/modules/5_path_planning/vrm_planner/vrm_planner_main.rst index 92e729ab29..65e0e53465 100644 --- a/docs/modules/5_path_planning/vrm_planner/vrm_planner_main.rst +++ b/docs/modules/5_path_planning/vrm_planner/vrm_planner_main.rst @@ -11,7 +11,7 @@ Cyan crosses mean searched points with Dijkstra method, The red line is the final path of Vornoi Road-Map. -Ref: +Reference - `Robotic Motion Planning `__ diff --git a/docs/modules/6_path_tracking/lqr_speed_and_steering_control/lqr_speed_and_steering_control_main.rst b/docs/modules/6_path_tracking/lqr_speed_and_steering_control/lqr_speed_and_steering_control_main.rst index ded187e972..568ef9a0df 100644 --- a/docs/modules/6_path_tracking/lqr_speed_and_steering_control/lqr_speed_and_steering_control_main.rst +++ b/docs/modules/6_path_tracking/lqr_speed_and_steering_control/lqr_speed_and_steering_control_main.rst @@ -134,7 +134,7 @@ Simulation results -References: +Reference ~~~~~~~~~~~ - `Towards fully autonomous driving: Systems and algorithms `__ diff --git a/docs/modules/6_path_tracking/lqr_steering_control/lqr_steering_control_main.rst b/docs/modules/6_path_tracking/lqr_steering_control/lqr_steering_control_main.rst index baca7a33fc..831423f364 100644 --- a/docs/modules/6_path_tracking/lqr_steering_control/lqr_steering_control_main.rst +++ b/docs/modules/6_path_tracking/lqr_steering_control/lqr_steering_control_main.rst @@ -113,7 +113,7 @@ The optimal control input `u` can be calculated as: where `K` is the feedback gain matrix obtained by solving the Riccati equation. -References: +Reference ~~~~~~~~~~~ - `ApolloAuto/apollo: An open autonomous driving platform `_ diff --git a/docs/modules/6_path_tracking/pure_pursuit_tracking/pure_pursuit_tracking_main.rst b/docs/modules/6_path_tracking/pure_pursuit_tracking/pure_pursuit_tracking_main.rst index 5c7bcef85f..d7354cf8fb 100644 --- a/docs/modules/6_path_tracking/pure_pursuit_tracking/pure_pursuit_tracking_main.rst +++ b/docs/modules/6_path_tracking/pure_pursuit_tracking/pure_pursuit_tracking_main.rst @@ -9,7 +9,7 @@ speed control. The red line is a target course, the green cross means the target point for pure pursuit control, the blue line is the tracking. -References: +Reference ~~~~~~~~~~~ - `A Survey of Motion Planning and Control Techniques for Self-driving diff --git a/docs/modules/6_path_tracking/rear_wheel_feedback_control/rear_wheel_feedback_control_main.rst b/docs/modules/6_path_tracking/rear_wheel_feedback_control/rear_wheel_feedback_control_main.rst index 70875fdc6c..d18cd6fbf7 100644 --- a/docs/modules/6_path_tracking/rear_wheel_feedback_control/rear_wheel_feedback_control_main.rst +++ b/docs/modules/6_path_tracking/rear_wheel_feedback_control/rear_wheel_feedback_control_main.rst @@ -6,7 +6,7 @@ PID speed control. .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/rear_wheel_feedback/animation.gif -References: +Reference ~~~~~~~~~~~ - `A Survey of Motion Planning and Control Techniques for Self-driving Urban Vehicles `__ diff --git a/docs/modules/6_path_tracking/stanley_control/stanley_control_main.rst b/docs/modules/6_path_tracking/stanley_control/stanley_control_main.rst index fe325b0102..69089ac33b 100644 --- a/docs/modules/6_path_tracking/stanley_control/stanley_control_main.rst +++ b/docs/modules/6_path_tracking/stanley_control/stanley_control_main.rst @@ -6,7 +6,7 @@ control. .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/stanley_controller/animation.gif -References: +Reference ~~~~~~~~~~~ - `Stanley: The robot that won the DARPA grand diff --git a/users_comments.md b/users_comments.md index a2f798eac4..1674f21377 100644 --- a/users_comments.md +++ b/users_comments.md @@ -10,7 +10,7 @@ This is an electric wheelchair control demo by [Katsushun89](https://github.com/ ![1](https://github.com/AtsushiSakai/PythonRoboticsGifs/blob/master/Users/WHILL_Model_CR_with_move_to_a_pose/WHLL_Model_CR_with_move_to_a_pose.gif) -Ref: +Reference: - [toioと同じように動くWHILL Model CR (in Japanese)](https://qiita.com/KatsuShun89/items/68fde7544ecfe36096d2)