From 2895d880b296b42470d95906d55cbbc0f6073383 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 14 Oct 2025 07:22:12 +0900 Subject: [PATCH 01/13] build(deps): bump github/codeql-action from 3 to 4 (#1287) Bumps [github/codeql-action](https://github.com/github/codeql-action) from 3 to 4. - [Release notes](https://github.com/github/codeql-action/releases) - [Changelog](https://github.com/github/codeql-action/blob/main/CHANGELOG.md) - [Commits](https://github.com/github/codeql-action/compare/v3...v4) --- updated-dependencies: - dependency-name: github/codeql-action dependency-version: '4' dependency-type: direct:production update-type: version-update:semver-major ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/codeql.yml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/codeql.yml b/.github/workflows/codeql.yml index 0ed1d7e90d..dcbcdbaa88 100644 --- a/.github/workflows/codeql.yml +++ b/.github/workflows/codeql.yml @@ -24,7 +24,7 @@ jobs: # Initializes the CodeQL tools for scanning. - name: Initialize CodeQL - uses: github/codeql-action/init@v3 + uses: github/codeql-action/init@v4 with: config-file: ./.github/codeql/codeql-config.yml # Override language selection by uncommenting this and choosing your languages @@ -34,7 +34,7 @@ jobs: # Autobuild attempts to build any compiled languages (C/C++, C#, or Java). # If this step fails, then you should remove it and run the build manually (see below) - name: Autobuild - uses: github/codeql-action/autobuild@v3 + uses: github/codeql-action/autobuild@v4 # ℹ️ Command-line programs to run using the OS shell. # 📚 https://git.io/JvXDl @@ -48,4 +48,4 @@ jobs: # make release - name: Perform CodeQL Analysis - uses: github/codeql-action/analyze@v3 + uses: github/codeql-action/analyze@v4 From 01a85c3c848975b5efc9b11b6a6a3be1e0a8b8dd Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 14 Oct 2025 07:23:37 +0900 Subject: [PATCH 02/13] build(deps): bump matplotlib from 3.10.6 to 3.10.7 in /requirements (#1289) Bumps [matplotlib](https://github.com/matplotlib/matplotlib) from 3.10.6 to 3.10.7. - [Release notes](https://github.com/matplotlib/matplotlib/releases) - [Commits](https://github.com/matplotlib/matplotlib/compare/v3.10.6...v3.10.7) --- updated-dependencies: - dependency-name: matplotlib dependency-version: 3.10.7 dependency-type: direct:production update-type: version-update:semver-patch ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- requirements/requirements.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/requirements/requirements.txt b/requirements/requirements.txt index 153add3786..166f4e2ddc 100644 --- a/requirements/requirements.txt +++ b/requirements/requirements.txt @@ -1,6 +1,6 @@ numpy == 2.3.3 scipy == 1.16.2 -matplotlib == 3.10.6 +matplotlib == 3.10.7 cvxpy == 1.7.3 ecos == 2.0.14 pytest == 8.4.2 # For unit test From b0c81aedbcfefe43fac7b298dd3b7664dcabd8a9 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 14 Oct 2025 19:30:32 +0900 Subject: [PATCH 03/13] build(deps): bump ruff from 0.13.3 to 0.14.0 in /requirements (#1288) Bumps [ruff](https://github.com/astral-sh/ruff) from 0.13.3 to 0.14.0. - [Release notes](https://github.com/astral-sh/ruff/releases) - [Changelog](https://github.com/astral-sh/ruff/blob/main/CHANGELOG.md) - [Commits](https://github.com/astral-sh/ruff/compare/0.13.3...0.14.0) --- updated-dependencies: - dependency-name: ruff dependency-version: 0.14.0 dependency-type: direct:production update-type: version-update:semver-minor ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- requirements/requirements.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/requirements/requirements.txt b/requirements/requirements.txt index 166f4e2ddc..c6223c01df 100644 --- a/requirements/requirements.txt +++ b/requirements/requirements.txt @@ -6,4 +6,4 @@ ecos == 2.0.14 pytest == 8.4.2 # For unit test pytest-xdist == 3.8.0 # For unit test mypy == 1.18.2 # For unit test -ruff == 0.13.3 # For unit test +ruff == 0.14.0 # For unit test From 39e29bbc15cddb86830b391e44f2edf819aa49b8 Mon Sep 17 00:00:00 2001 From: Copilot <198982749+Copilot@users.noreply.github.com> Date: Tue, 14 Oct 2025 20:23:16 +0900 Subject: [PATCH 04/13] Add comprehensive documentation for Ensemble Kalman Filter localization (#1278) * Initial plan * Add comprehensive documentation for Ensemble Kalman Filter localization Co-authored-by: AtsushiSakai <3813847+AtsushiSakai@users.noreply.github.com> --------- Co-authored-by: copilot-swe-agent[bot] <198982749+Copilot@users.noreply.github.com> Co-authored-by: AtsushiSakai <3813847+AtsushiSakai@users.noreply.github.com> --- ...samble_kalman_filter_localization_main.rst | 196 +++++++++++++++++- 1 file changed, 194 insertions(+), 2 deletions(-) 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 214e645d10..6274859002 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 @@ -1,12 +1,204 @@ -Ensamble Kalman Filter Localization +Ensemble Kalman Filter Localization ----------------------------------- .. figure:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Localization/ensamble_kalman_filter/animation.gif -This is a sensor fusion localization with Ensamble Kalman Filter(EnKF). +This is a sensor fusion localization with Ensemble Kalman Filter(EnKF). + +The blue line is true trajectory, the black line is dead reckoning trajectory, +and the red line is estimated trajectory with EnKF. + +The red ellipse is estimated covariance ellipse with EnKF. + +It is assumed that the robot can measure distance and bearing angle from landmarks (RFID). + +These measurements are used for EnKF localization. Code Link ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ .. autofunction:: Localization.ensemble_kalman_filter.ensemble_kalman_filter.enkf_localization + +Ensemble Kalman Filter Algorithm +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +The Ensemble Kalman Filter (EnKF) is a Monte Carlo approach to the Kalman filter that +uses an ensemble of state estimates to represent the probability distribution. Unlike +the traditional Kalman filter that propagates the mean and covariance analytically, +EnKF uses a collection of samples (ensemble members) to estimate the state and its uncertainty. + +The EnKF algorithm consists of two main steps: + +=== Predict === + +For each ensemble member :math:`i = 1, ..., N`: + +1. Add random noise to the control input: + + :math:`\mathbf{u}^i = \mathbf{u} + \epsilon_u`, where :math:`\epsilon_u \sim \mathcal{N}(0, \mathbf{R})` + +2. Predict the state: + + :math:`\mathbf{x}^i_{pred} = f(\mathbf{x}^i_t, \mathbf{u}^i)` + +3. Predict the observation: + + :math:`\mathbf{z}^i_{pred} = h(\mathbf{x}^i_{pred}) + \epsilon_z`, where :math:`\epsilon_z \sim \mathcal{N}(0, \mathbf{Q})` + +=== Update === + +1. Calculate ensemble mean for states: + + :math:`\bar{\mathbf{x}} = \frac{1}{N}\sum_{i=1}^N \mathbf{x}^i_{pred}` + +2. Calculate ensemble mean for observations: + + :math:`\bar{\mathbf{z}} = \frac{1}{N}\sum_{i=1}^N \mathbf{z}^i_{pred}` + +3. Calculate state deviation: + + :math:`\mathbf{X}' = \mathbf{x}^i_{pred} - \bar{\mathbf{x}}` + +4. Calculate observation deviation: + + :math:`\mathbf{Z}' = \mathbf{z}^i_{pred} - \bar{\mathbf{z}}` + +5. Calculate covariance matrices: + + :math:`\mathbf{U} = \frac{1}{N-1}\mathbf{X}'\mathbf{Z}'^T` + + :math:`\mathbf{V} = \frac{1}{N-1}\mathbf{Z}'\mathbf{Z}'^T` + +6. Calculate Kalman gain: + + :math:`\mathbf{K} = \mathbf{U}\mathbf{V}^{-1}` + +7. Update each ensemble member: + + :math:`\mathbf{x}^i_{t+1} = \mathbf{x}^i_{pred} + \mathbf{K}(\mathbf{z}_{obs} - \mathbf{z}^i_{pred})` + +8. Calculate final state estimate: + + :math:`\mathbf{x}_{est} = \frac{1}{N}\sum_{i=1}^N \mathbf{x}^i_{t+1}` + +9. Calculate covariance estimate: + + :math:`\mathbf{P}_{est} = \frac{1}{N}\sum_{i=1}^N (\mathbf{x}^i_{t+1} - \mathbf{x}_{est})(\mathbf{x}^i_{t+1} - \mathbf{x}_{est})^T` + + +Filter Design +~~~~~~~~~~~~~ + +In this simulation, the robot has a state vector with 4 states at time :math:`t`: + +.. math:: \mathbf{x}_t = [x_t, y_t, \phi_t, v_t]^T + +where: + +- :math:`x, y` are 2D position coordinates +- :math:`\phi` is orientation (yaw angle) +- :math:`v` is velocity + +The filter uses an ensemble of :math:`N=20` particles to represent the state distribution. + +Input Vector +^^^^^^^^^^^^ + +The robot has a velocity sensor and a gyro sensor, so the control input vector at each time step is: + +.. math:: \mathbf{u}_t = [v_t, \omega_t]^T + +where: + +- :math:`v_t` is linear velocity +- :math:`\omega_t` is angular velocity (yaw rate) + +The input vector includes sensor noise modeled by covariance matrix :math:`\mathbf{R}`: + +.. math:: \mathbf{R} = \begin{bmatrix} \sigma_v^2 & 0 \\ 0 & \sigma_\omega^2 \end{bmatrix} + + +Observation Vector +^^^^^^^^^^^^^^^^^^ + +The robot can observe landmarks (RFID tags) with distance and bearing measurements: + +.. math:: \mathbf{z}_t = [d_t, \theta_t, x_{lm}, y_{lm}] + +where: + +- :math:`d_t` is the distance to the landmark +- :math:`\theta_t` is the bearing angle to the landmark +- :math:`x_{lm}, y_{lm}` are the known landmark positions + +The observation noise is modeled by covariance matrix :math:`\mathbf{Q}`: + +.. math:: \mathbf{Q} = \begin{bmatrix} \sigma_d^2 & 0 \\ 0 & \sigma_\theta^2 \end{bmatrix} + + +Motion Model +~~~~~~~~~~~~ + +The robot motion model is: + +.. math:: \dot{x} = v \cos(\phi) + +.. math:: \dot{y} = v \sin(\phi) + +.. math:: \dot{\phi} = \omega + +.. math:: \dot{v} = 0 + +Discretized with time step :math:`\Delta t`, the motion model becomes: + +.. math:: \mathbf{x}_{t+1} = f(\mathbf{x}_t, \mathbf{u}_t) = \mathbf{F}\mathbf{x}_t + \mathbf{B}\mathbf{u}_t + +where: + +:math:`\begin{equation*} \mathbf{F} = \begin{bmatrix} 1 & 0 & 0 & 0\\ 0 & 1 & 0 & 0\\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 \\ \end{bmatrix} \end{equation*}` + +:math:`\begin{equation*} \mathbf{B} = \begin{bmatrix} \cos(\phi) \Delta t & 0\\ \sin(\phi) \Delta t & 0\\ 0 & \Delta t\\ 1 & 0\\ \end{bmatrix} \end{equation*}` + +The motion function expands to: + +:math:`\begin{equation*} \begin{bmatrix} x_{t+1} \\ y_{t+1} \\ \phi_{t+1} \\ v_{t+1} \end{bmatrix} = \begin{bmatrix} x_t + v_t\cos(\phi_t)\Delta t \\ y_t + v_t\sin(\phi_t)\Delta t \\ \phi_t + \omega_t \Delta t \\ v_t \end{bmatrix} \end{equation*}` + + +Observation Model +~~~~~~~~~~~~~~~~~ + +For each landmark at position :math:`(x_{lm}, y_{lm})`, the observation model calculates +the expected landmark position in the global frame: + +.. math:: + \begin{bmatrix} + x_{lm,obs} \\ + y_{lm,obs} + \end{bmatrix} = + \begin{bmatrix} + x + d \cos(\phi + \theta) \\ + y + d \sin(\phi + \theta) + \end{bmatrix} + +where :math:`d` and :math:`\theta` are the observed distance and bearing to the landmark. + +The observation function projects the robot state to expected landmark positions, +which are then compared with actual landmark positions for the update step. + + +Advantages of EnKF +~~~~~~~~~~~~~~~~~~ + +- **No Jacobian required**: Unlike EKF, EnKF does not need to compute Jacobian matrices, making it easier to implement for nonlinear systems +- **Handles non-Gaussian distributions**: The ensemble representation can capture non-Gaussian features of the state distribution +- **Computationally efficient**: For high-dimensional systems, EnKF can be more efficient than maintaining full covariance matrices +- **Easy to parallelize**: Each ensemble member can be propagated independently + + +Reference +~~~~~~~~~ + +- `Ensemble Kalman filtering `_ +- `PROBABILISTIC ROBOTICS `_ + From fc6b36e3092f0fefde02ad06a6a95406fd83579d Mon Sep 17 00:00:00 2001 From: Copilot <198982749+Copilot@users.noreply.github.com> Date: Sun, 19 Oct 2025 18:48:21 +0900 Subject: [PATCH 05/13] Add comprehensive documentation for Unscented Kalman Filter localization (#1290) * Initial plan * Add comprehensive UKF documentation with algorithm details Co-authored-by: AtsushiSakai <3813847+AtsushiSakai@users.noreply.github.com> * Add comprehensive docstrings to UKF core functions Co-authored-by: AtsushiSakai <3813847+AtsushiSakai@users.noreply.github.com> * Fix codestyle issues - remove trailing whitespace from docstrings Co-authored-by: AtsushiSakai <3813847+AtsushiSakai@users.noreply.github.com> --------- Co-authored-by: copilot-swe-agent[bot] <198982749+Copilot@users.noreply.github.com> Co-authored-by: AtsushiSakai <3813847+AtsushiSakai@users.noreply.github.com> --- .../unscented_kalman_filter.py | 77 ++++++ ...cented_kalman_filter_localization_main.rst | 240 +++++++++++++++++- 2 files changed, 316 insertions(+), 1 deletion(-) diff --git a/Localization/unscented_kalman_filter/unscented_kalman_filter.py b/Localization/unscented_kalman_filter/unscented_kalman_filter.py index 4af748ec71..2119e1eacc 100644 --- a/Localization/unscented_kalman_filter/unscented_kalman_filter.py +++ b/Localization/unscented_kalman_filter/unscented_kalman_filter.py @@ -91,6 +91,29 @@ def observation_model(x): def generate_sigma_points(xEst, PEst, gamma): + """ + Generate sigma points for UKF. + + Sigma points are deterministically sampled points used to capture + the mean and covariance of the state distribution. + + Parameters + ---------- + xEst : numpy.ndarray + Current state estimate (n x 1) + PEst : numpy.ndarray + Current state covariance estimate (n x n) + gamma : float + Scaling parameter sqrt(n + lambda) + + Returns + ------- + sigma : numpy.ndarray + Sigma points (n x 2n+1) + sigma[:, 0] = xEst + sigma[:, 1:n+1] = xEst + gamma * sqrt(P) + sigma[:, n+1:2n+1] = xEst - gamma * sqrt(P) + """ sigma = xEst Psqrt = scipy.linalg.sqrtm(PEst) n = len(xEst[:, 0]) @@ -149,6 +172,35 @@ def calc_pxz(sigma, x, z_sigma, zb, wc): def ukf_estimation(xEst, PEst, z, u, wm, wc, gamma): + """ + Unscented Kalman Filter estimation. + + Performs one iteration of UKF state estimation using the unscented transform. + + Parameters + ---------- + xEst : numpy.ndarray + Current state estimate [x, y, yaw, v]^T (4 x 1) + PEst : numpy.ndarray + Current state covariance estimate (4 x 4) + z : numpy.ndarray + Observation vector [x_obs, y_obs]^T (2 x 1) + u : numpy.ndarray + Control input [velocity, yaw_rate]^T (2 x 1) + wm : numpy.ndarray + Weights for calculating mean (1 x 2n+1) + wc : numpy.ndarray + Weights for calculating covariance (1 x 2n+1) + gamma : float + Sigma point scaling parameter sqrt(n + lambda) + + Returns + ------- + xEst : numpy.ndarray + Updated state estimate (4 x 1) + PEst : numpy.ndarray + Updated state covariance estimate (4 x 4) + """ # Predict sigma = generate_sigma_points(xEst, PEst, gamma) sigma = predict_sigma_motion(sigma, u) @@ -194,6 +246,31 @@ def plot_covariance_ellipse(xEst, PEst): # pragma: no cover def setup_ukf(nx): + """ + Setup UKF parameters and weights. + + Calculates the weights for mean and covariance computation, + and the scaling parameter gamma for sigma point generation. + + Parameters + ---------- + nx : int + Dimension of the state vector + + Returns + ------- + wm : numpy.ndarray + Weights for calculating mean (1 x 2n+1) + wm[0] = lambda / (n + lambda) + wm[i] = 1 / (2(n + lambda)) for i > 0 + wc : numpy.ndarray + Weights for calculating covariance (1 x 2n+1) + wc[0] = lambda / (n + lambda) + (1 - alpha^2 + beta) + wc[i] = 1 / (2(n + lambda)) for i > 0 + gamma : float + Sigma point scaling parameter sqrt(n + lambda) + where lambda = alpha^2 * (n + kappa) - n + """ lamb = ALPHA ** 2 * (nx + KAPPA) - nx # calculate weights wm = [lamb / (lamb + nx)] 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 a7a5aab61b..01d161b17e 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 @@ -5,7 +5,10 @@ Unscented Kalman Filter localization This is a sensor fusion localization with Unscented Kalman Filter(UKF). -The lines and points are same meaning of the EKF simulation. +The blue line is true trajectory, the black line is dead reckoning trajectory, +the green points are GPS observations, and the red line is estimated trajectory with UKF. + +The red ellipse is estimated covariance ellipse with UKF. Code Link ~~~~~~~~~~~~~ @@ -13,7 +16,242 @@ Code Link .. autofunction:: Localization.unscented_kalman_filter.unscented_kalman_filter.ukf_estimation +Unscented Kalman Filter Algorithm +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +The Unscented Kalman Filter (UKF) is a nonlinear state estimation technique that uses +the unscented transform to handle nonlinearities. Unlike the Extended Kalman Filter (EKF) +that linearizes the nonlinear functions using Jacobians, UKF uses a deterministic sampling +approach called sigma points to capture the mean and covariance of the state distribution. + +The UKF algorithm consists of two main steps: + +=== Predict === + +1. Generate sigma points around the current state estimate: + + :math:`\chi_0 = \mathbf{x}_{t}` + + :math:`\chi_i = \mathbf{x}_{t} + \gamma\sqrt{\mathbf{P}_t}_i` for :math:`i = 1, ..., n` + + :math:`\chi_i = \mathbf{x}_{t} - \gamma\sqrt{\mathbf{P}_t}_{i-n}` for :math:`i = n+1, ..., 2n` + + where :math:`\gamma = \sqrt{n + \lambda}` and :math:`\lambda = \alpha^2(n + \kappa) - n` + +2. Propagate sigma points through the motion model: + + :math:`\chi^-_i = f(\chi_i, \mathbf{u}_t)` + +3. Calculate predicted state mean: + + :math:`\mathbf{x}^-_{t+1} = \sum_{i=0}^{2n} w^m_i \chi^-_i` + +4. Calculate predicted state covariance: + + :math:`\mathbf{P}^-_{t+1} = \sum_{i=0}^{2n} w^c_i (\chi^-_i - \mathbf{x}^-_{t+1})(\chi^-_i - \mathbf{x}^-_{t+1})^T + \mathbf{Q}` + +=== Update === + +1. Generate sigma points around the predicted state: + + :math:`\chi_i = \text{generate\_sigma\_points}(\mathbf{x}^-_{t+1}, \mathbf{P}^-_{t+1})` + +2. Propagate sigma points through the observation model: + + :math:`\mathcal{Z}_i = h(\chi_i)` + +3. Calculate predicted observation mean: + + :math:`\mathbf{z}^-_{t+1} = \sum_{i=0}^{2n} w^m_i \mathcal{Z}_i` + +4. Calculate innovation covariance: + + :math:`\mathbf{S}_t = \sum_{i=0}^{2n} w^c_i (\mathcal{Z}_i - \mathbf{z}^-_{t+1})(\mathcal{Z}_i - \mathbf{z}^-_{t+1})^T + \mathbf{R}` + +5. Calculate cross-correlation matrix: + + :math:`\mathbf{P}_{xz} = \sum_{i=0}^{2n} w^c_i (\chi_i - \mathbf{x}^-_{t+1})(\mathcal{Z}_i - \mathbf{z}^-_{t+1})^T` + +6. Calculate Kalman gain: + + :math:`\mathbf{K} = \mathbf{P}_{xz}\mathbf{S}_t^{-1}` + +7. Update state estimate: + + :math:`\mathbf{x}_{t+1} = \mathbf{x}^-_{t+1} + \mathbf{K}(\mathbf{z}_t - \mathbf{z}^-_{t+1})` + +8. Update covariance estimate: + + :math:`\mathbf{P}_{t+1} = \mathbf{P}^-_{t+1} - \mathbf{K}\mathbf{S}_t\mathbf{K}^T` + + +Sigma Points and Weights +~~~~~~~~~~~~~~~~~~~~~~~~~ + +The UKF uses deterministic sigma points to represent the state distribution. The weights +for combining sigma points are calculated as: + +**Mean weights:** + +:math:`w^m_0 = \frac{\lambda}{n + \lambda}` + +:math:`w^m_i = \frac{1}{2(n + \lambda)}` for :math:`i = 1, ..., 2n` + +**Covariance weights:** + +:math:`w^c_0 = \frac{\lambda}{n + \lambda} + (1 - \alpha^2 + \beta)` + +:math:`w^c_i = \frac{1}{2(n + \lambda)}` for :math:`i = 1, ..., 2n` + +where: + +- :math:`\alpha` controls the spread of sigma points around the mean (typically :math:`0.001 \leq \alpha \leq 1`) +- :math:`\beta` incorporates prior knowledge of the distribution (:math:`\beta = 2` is optimal for Gaussian distributions) +- :math:`\kappa` is a secondary scaling parameter (typically :math:`\kappa = 0`) +- :math:`n` is the dimension of the state vector + + +Filter Design +~~~~~~~~~~~~~ + +In this simulation, the robot has a state vector with 4 states at time :math:`t`: + +.. math:: \mathbf{x}_t = [x_t, y_t, \phi_t, v_t]^T + +where: + +- :math:`x, y` are 2D position coordinates +- :math:`\phi` is orientation (yaw angle) +- :math:`v` is velocity + +In the code, "xEst" means the state vector. + +The covariance matrices are: + +- :math:`\mathbf{P}_t` is the state covariance matrix +- :math:`\mathbf{Q}` is the process noise covariance matrix +- :math:`\mathbf{R}` is the observation noise covariance matrix + +**Process Noise Covariance** :math:`\mathbf{Q}`: + +.. math:: + \mathbf{Q} = \begin{bmatrix} + 0.1^2 & 0 & 0 & 0 \\ + 0 & 0.1^2 & 0 & 0 \\ + 0 & 0 & (0.017)^2 & 0 \\ + 0 & 0 & 0 & 1.0^2 + \end{bmatrix} + +**Observation Noise Covariance** :math:`\mathbf{R}`: + +.. math:: + \mathbf{R} = \begin{bmatrix} + 1.0^2 & 0 \\ + 0 & 1.0^2 + \end{bmatrix} + + +Input Vector +^^^^^^^^^^^^ + +The robot has a velocity sensor and a gyro sensor, so the control input vector at each time step is: + +.. math:: \mathbf{u}_t = [v_t, \omega_t]^T + +where: + +- :math:`v_t` is linear velocity [m/s] +- :math:`\omega_t` is angular velocity (yaw rate) [rad/s] + +The input vector includes sensor noise. + + +Observation Vector +^^^^^^^^^^^^^^^^^^ + +The robot has a GNSS (GPS) sensor that can measure x-y position: + +.. math:: \mathbf{z}_t = [x_t, y_t]^T + +The observation includes GPS noise with covariance :math:`\mathbf{R}`. + + +Motion Model +~~~~~~~~~~~~ + +The robot motion model is: + +.. math:: \dot{x} = v \cos(\phi) + +.. math:: \dot{y} = v \sin(\phi) + +.. math:: \dot{\phi} = \omega + +.. math:: \dot{v} = 0 + +Discretized with time step :math:`\Delta t`, the motion model becomes: + +.. math:: \mathbf{x}_{t+1} = f(\mathbf{x}_t, \mathbf{u}_t) = \mathbf{F}\mathbf{x}_t + \mathbf{B}\mathbf{u}_t + +where: + +:math:`\begin{equation*} \mathbf{F} = \begin{bmatrix} 1 & 0 & 0 & 0\\ 0 & 1 & 0 & 0\\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 \\ \end{bmatrix} \end{equation*}` + +:math:`\begin{equation*} \mathbf{B} = \begin{bmatrix} \cos(\phi) \Delta t & 0\\ \sin(\phi) \Delta t & 0\\ 0 & \Delta t\\ 1 & 0\\ \end{bmatrix} \end{equation*}` + +The motion function expands to: + +:math:`\begin{equation*} \begin{bmatrix} x_{t+1} \\ y_{t+1} \\ \phi_{t+1} \\ v_{t+1} \end{bmatrix} = \begin{bmatrix} x_t + v_t\cos(\phi_t)\Delta t \\ y_t + v_t\sin(\phi_t)\Delta t \\ \phi_t + \omega_t \Delta t \\ v_t \end{bmatrix} \end{equation*}` + +:math:`\Delta t = 0.1` [s] is the time interval. + + +Observation Model +~~~~~~~~~~~~~~~~~ + +The robot can get x-y position information from GPS. + +The GPS observation model is: + +.. math:: \mathbf{z}_{t} = h(\mathbf{x}_t) = \mathbf{H} \mathbf{x}_t + +where: + +:math:`\begin{equation*} \mathbf{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_{obs} \\ y_{obs} \end{bmatrix} = h(\mathbf{x}) = \begin{bmatrix} x \\ y \end{bmatrix} \end{equation*}` + + +UKF Parameters +~~~~~~~~~~~~~~ + +The UKF uses three tuning parameters: + +- **ALPHA = 0.001**: Controls the spread of sigma points around the mean. Smaller values result in sigma points closer to the mean. +- **BETA = 2**: Incorporates prior knowledge about the distribution. For Gaussian distributions, the optimal value is 2. +- **KAPPA = 0**: Secondary scaling parameter. Usually set to 0 or 3-n where n is the state dimension. + +These parameters affect the calculation of :math:`\lambda = \alpha^2(n + \kappa) - n`, which determines +the sigma point spread and weights. + + +Advantages of UKF over EKF +~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +The Unscented Kalman Filter offers several advantages over the Extended Kalman Filter: + +- **No Jacobian required**: UKF does not need to compute Jacobian matrices, which can be error-prone and computationally expensive for complex nonlinear systems +- **Higher accuracy**: UKF captures the mean and covariance to the second order (third order for Gaussian distributions) while EKF only achieves first-order accuracy +- **Better handling of nonlinearities**: The unscented transform provides a better approximation of the probability distribution after nonlinear transformation +- **Easier implementation**: For highly nonlinear systems, UKF is often easier to implement since it doesn't require analytical derivatives +- **Numerical stability**: UKF can be more numerically stable than EKF for strongly nonlinear systems + + Reference ~~~~~~~~~~~ - `Discriminatively Trained Unscented Kalman Filter for Mobile Robot Localization `_ +- `The Unscented Kalman Filter for Nonlinear Estimation `_ +- `PROBABILISTIC ROBOTICS `_ From 1311b94d9b651da8bec85a47f0db2771d99e0c91 Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Mon, 20 Oct 2025 21:44:33 +0900 Subject: [PATCH 06/13] Potential fix for code scanning alert no. 328: Workflow does not contain permissions (#1294) Co-authored-by: Copilot Autofix powered by AI <62310815+github-advanced-security[bot]@users.noreply.github.com> --- .github/workflows/codeql.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/workflows/codeql.yml b/.github/workflows/codeql.yml index dcbcdbaa88..b402f989e8 100644 --- a/.github/workflows/codeql.yml +++ b/.github/workflows/codeql.yml @@ -8,6 +8,8 @@ on: schedule: - cron: '0 19 * * 0' +permissions: + contents: read jobs: CodeQL-Build: From 59d0cc7b4244a9e7fbe7b63ad14a70ff5db8c8c5 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 21 Oct 2025 07:56:16 +0900 Subject: [PATCH 07/13] build(deps): bump ruff from 0.14.0 to 0.14.1 in /requirements (#1296) Bumps [ruff](https://github.com/astral-sh/ruff) from 0.14.0 to 0.14.1. - [Release notes](https://github.com/astral-sh/ruff/releases) - [Changelog](https://github.com/astral-sh/ruff/blob/main/CHANGELOG.md) - [Commits](https://github.com/astral-sh/ruff/compare/0.14.0...0.14.1) --- updated-dependencies: - dependency-name: ruff dependency-version: 0.14.1 dependency-type: direct:production update-type: version-update:semver-patch ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- requirements/requirements.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/requirements/requirements.txt b/requirements/requirements.txt index c6223c01df..a2d843d7b7 100644 --- a/requirements/requirements.txt +++ b/requirements/requirements.txt @@ -6,4 +6,4 @@ ecos == 2.0.14 pytest == 8.4.2 # For unit test pytest-xdist == 3.8.0 # For unit test mypy == 1.18.2 # For unit test -ruff == 0.14.0 # For unit test +ruff == 0.14.1 # For unit test From 10764c2657ef36e9bcec6108020fd9c22563e93c Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 21 Oct 2025 20:05:29 +0900 Subject: [PATCH 08/13] build(deps): bump numpy from 2.3.3 to 2.3.4 in /requirements (#1297) Bumps [numpy](https://github.com/numpy/numpy) from 2.3.3 to 2.3.4. - [Release notes](https://github.com/numpy/numpy/releases) - [Changelog](https://github.com/numpy/numpy/blob/main/doc/RELEASE_WALKTHROUGH.rst) - [Commits](https://github.com/numpy/numpy/compare/v2.3.3...v2.3.4) --- updated-dependencies: - dependency-name: numpy dependency-version: 2.3.4 dependency-type: direct:production update-type: version-update:semver-patch ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- requirements/requirements.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/requirements/requirements.txt b/requirements/requirements.txt index a2d843d7b7..306d93b736 100644 --- a/requirements/requirements.txt +++ b/requirements/requirements.txt @@ -1,4 +1,4 @@ -numpy == 2.3.3 +numpy == 2.3.4 scipy == 1.16.2 matplotlib == 3.10.7 cvxpy == 1.7.3 From 2d3cb831c94230017ca5467a6ce2e25ec107f8c5 Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Tue, 21 Oct 2025 21:20:50 +0900 Subject: [PATCH 09/13] =?UTF-8?q?Revert=20"Potential=20fix=20for=20code=20?= =?UTF-8?q?scanning=20alert=20no.=20328:=20Workflow=20does=20not=20cont?= =?UTF-8?q?=E2=80=A6"=20(#1298)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit This reverts commit 1311b94d9b651da8bec85a47f0db2771d99e0c91. --- .github/workflows/codeql.yml | 2 -- 1 file changed, 2 deletions(-) diff --git a/.github/workflows/codeql.yml b/.github/workflows/codeql.yml index b402f989e8..dcbcdbaa88 100644 --- a/.github/workflows/codeql.yml +++ b/.github/workflows/codeql.yml @@ -8,8 +8,6 @@ on: schedule: - cron: '0 19 * * 0' -permissions: - contents: read jobs: CodeQL-Build: From 69d07fcc00dc0b8e7b673019f43bcec48fd4d7fb Mon Sep 17 00:00:00 2001 From: Anish Kumar Date: Tue, 21 Oct 2025 17:52:18 +0530 Subject: [PATCH 10/13] added pso algorithm (#1279) * added pso algorithm also known as bird flocking algorithm * fix: Resolve linting issues in PSO implementation * improved code formattings * added docstring and added pso in main rst file * fixed paths * fixed changes requested * fixed typo * improved readilbility --- .gitignore | 1 + .../particle_swarm_optimization.py | 335 ++++++++++++++++++ README.md | 19 + .../particleSwarmOptimization_main.rst | 153 ++++++++ .../5_path_planning/path_planning_main.rst | 1 + tests/test_particle_swarm_optimization.py | 11 + 6 files changed, 520 insertions(+) create mode 100755 PathPlanning/ParticleSwarmOptimization/particle_swarm_optimization.py create mode 100644 docs/modules/5_path_planning/particleSwarmOptimization/particleSwarmOptimization_main.rst create mode 100755 tests/test_particle_swarm_optimization.py diff --git a/.gitignore b/.gitignore index c971b8f9c5..8a0b1257b5 100644 --- a/.gitignore +++ b/.gitignore @@ -15,6 +15,7 @@ _build/ # Distribution / packaging .Python env/ +venv/ build/ develop-eggs/ dist/ diff --git a/PathPlanning/ParticleSwarmOptimization/particle_swarm_optimization.py b/PathPlanning/ParticleSwarmOptimization/particle_swarm_optimization.py new file mode 100755 index 0000000000..4d1df5e197 --- /dev/null +++ b/PathPlanning/ParticleSwarmOptimization/particle_swarm_optimization.py @@ -0,0 +1,335 @@ +""" +Particle Swarm Optimization (PSO) Path Planning + +author: Anish (@anishk85) + +See Wikipedia article (https://en.wikipedia.org/wiki/Particle_swarm_optimization) + +References: + - Kennedy, J.; Eberhart, R. (1995). "Particle Swarm Optimization" + - Shi, Y.; Eberhart, R. (1998). "A Modified Particle Swarm Optimizer" + - https://machinelearningmastery.com/a-gentle-introduction-to-particle-swarm-optimization/ + +This implementation uses PSO to find collision-free paths by treating +path planning as an optimization problem where particles explore the +search space to minimize distance to target while avoiding obstacles. +""" +import numpy as np +import matplotlib.pyplot as plt +import matplotlib.animation as animation +import matplotlib.patches as patches +import signal +import sys +# Add show_animation flag for consistency with other planners +show_animation = True + +def signal_handler(sig, frame): + print("\nExiting...") + plt.close("all") + sys.exit(0) + +signal.signal(signal.SIGINT, signal_handler) + +class Particle: + """Represents a single particle in the PSO swarm. + Each particle maintains its current position, velocity, and personal best + position discovered during the search. Particles explore the search space + by updating their velocity based on personal experience (cognitive component) + and swarm knowledge (social component). + Attributes: + search_bounds: List of tuples [(x_min, x_max), (y_min, y_max)] defining search space + max_velocity: Maximum velocity allowed in each dimension (5% of search space range) + position: Current 2D position [x, y] in search space + velocity: Current velocity vector [vx, vy] + personal_best_position: Personal best position found so far + personal_best_value: Fitness value at personal best position + path: List of all positions visited by this particle + """ + + def __init__(self, search_bounds, spawn_bounds): + self.search_bounds = search_bounds + self.max_velocity = np.array([(b[1] - b[0]) * 0.05 for b in search_bounds]) + self.position = np.array([np.random.uniform(b[0], b[1]) for b in spawn_bounds]) + self.velocity = np.random.randn(2) * 0.1 + self.personal_best_position = self.position.copy() + self.personal_best_value = np.inf + self.path = [self.position.copy()] + + def update_velocity(self, gbest_pos, w, c1, c2): + """Update particle velocity using PSO equation: + v = w*v + c1*r1*(personal_best - x) + c2*r2*(gbest - x) + """ + r1 = np.random.rand(2) + r2 = np.random.rand(2) + cognitive = c1 * r1 * (self.personal_best_position - self.position) + social = c2 * r2 * (gbest_pos - self.position) + self.velocity = w * self.velocity + cognitive + social + self.velocity = np.clip(self.velocity, -self.max_velocity, self.max_velocity) + + def update_position(self): + self.position = self.position + self.velocity + # Keep in bounds + for i in range(2): + self.position[i] = np.clip( + self.position[i], self.search_bounds[i][0], self.search_bounds[i][1] + ) + self.path.append(self.position.copy()) + +class PSOSwarm: + + def __init__( + self, n_particles, max_iter, target, search_bounds, spawn_bounds, obstacles + ): + self.n_particles = n_particles + self.max_iter = max_iter + self.target = np.array(target) + self.obstacles = obstacles + self.search_bounds = search_bounds + # PSO parameters + self.w_start = 0.9 # Initial inertia weight + self.w_end = 0.4 # Final inertia weight + self.c1 = 1.5 # Cognitive coefficient + self.c2 = 1.5 # Social coefficient + # Initialize particles + self.particles = [ + Particle(search_bounds, spawn_bounds) for _ in range(n_particles) + ] + self.gbest_position = None + self.gbest_value = np.inf + self.gbest_path = [] + self.iteration = 0 + + def fitness(self, pos): + """Calculate fitness - distance to target + obstacle penalty""" + dist = np.linalg.norm(pos - self.target) + # Obstacle penalty + penalty = 0 + for ox, oy, r in self.obstacles: + obs_dist = np.linalg.norm(pos - np.array([ox, oy])) + if obs_dist < r: + penalty += 1000 # Inside obstacle + elif obs_dist < r + 5: + penalty += 50 / (obs_dist - r + 0.1) # Too close + return dist + penalty + + def check_collision(self, start, end, obstacle): + """Check if path from start to end hits obstacle using line-circle intersection + Args: + start: Starting position (numpy array) + end: Ending position (numpy array) + obstacle: Tuple (ox, oy, r) representing obstacle center and radius + Returns: + bool: True if collision detected, False otherwise + """ + ox, oy, r = obstacle + center = np.array([ox, oy]) + # Vector math for line-circle intersection + d = end - start + f = start - center + a = np.dot(d, d) + # Guard against zero-length steps to prevent ZeroDivisionError + if a < 1e-10: # Near-zero length step + # Check if start point is inside obstacle + return np.linalg.norm(f) <= r + b = 2 * np.dot(f, d) + c = np.dot(f, f) - r * r + discriminant = b * b - 4 * a * c + if discriminant < 0: + return False + # Check if intersection on segment + sqrt_discriminant = np.sqrt(discriminant) + t1 = (-b - sqrt_discriminant) / (2 * a) + t2 = (-b + sqrt_discriminant) / (2 * a) + return (0 <= t1 <= 1) or (0 <= t2 <= 1) + + def step(self): + """Run one PSO iteration + Returns: + bool: True if algorithm should continue, False if completed + """ + if self.iteration >= self.max_iter: + return False + # Update inertia weight (linear decay) + w = self.w_start - (self.w_start - self.w_end) * ( + self.iteration / self.max_iter + ) + # Evaluate all particles + for particle in self.particles: + value = self.fitness(particle.position) + # Update personal best + if value < particle.personal_best_value: + particle.personal_best_value = value + particle.personal_best_position = particle.position.copy() + # Update global best + if value < self.gbest_value: + self.gbest_value = value + self.gbest_position = particle.position.copy() + if self.gbest_position is not None: + self.gbest_path.append(self.gbest_position.copy()) + # Update particles + for particle in self.particles: + particle.update_velocity(self.gbest_position, w, self.c1, self.c2) + # Predict next position + next_pos = particle.position + particle.velocity + # Check collision + collision = False + for obs in self.obstacles: + if self.check_collision(particle.position, next_pos, obs): + collision = True + break + if collision: + # Reduce velocity if collision detected + particle.velocity *= 0.2 + particle.update_position() + self.iteration += 1 + if show_animation and self.iteration % 20 == 0: + print( + f"Iteration {self.iteration}/{self.max_iter}, Best: {self.gbest_value:.2f}" + ) + return True + +def main(): + """Run PSO path planning algorithm demonstration. + This function demonstrates PSO-based path planning with the following setup: + - 15 particles exploring a (-50,50) x (-50,50) search space + - Start zone: (-45,-45) to (-35,-35) + - Target: (40, 35) + - 4 circular obstacles with collision avoidance + - Real-time visualization showing particle convergence (if show_animation=True) + - Headless mode support for testing (if show_animation=False) + The algorithm runs for up to 150 iterations, displaying particle movement, + personal/global best positions, and the evolving optimal path. + """ + print(__file__ + " start!!") + # Set matplotlib backend for headless environments + if not show_animation: + plt.switch_backend("Agg") # Use non-GUI backend for testing + # Setup parameters + N_PARTICLES = 15 + MAX_ITER = 150 + SEARCH_BOUNDS = [(-50, 50), (-50, 50)] + TARGET = [40, 35] + SPAWN_AREA = [(-45, -35), (-45, -35)] + OBSTACLES = [(10, 15, 8), (-20, 0, 12), (20, -25, 10), (-5, -30, 7)] + swarm = PSOSwarm( + n_particles=N_PARTICLES, + max_iter=MAX_ITER, + target=TARGET, + search_bounds=SEARCH_BOUNDS, + spawn_bounds=SPAWN_AREA, + obstacles=OBSTACLES, + ) + # pragma: no cover + if show_animation: + # Visualization setup + signal.signal(signal.SIGINT, signal_handler) + fig, ax = plt.subplots(figsize=(10, 10)) + ax.set_xlim(SEARCH_BOUNDS[0]) + ax.set_ylim(SEARCH_BOUNDS[1]) + ax.set_title("PSO Path Planning with Collision Avoidance", fontsize=14) + ax.grid(True, alpha=0.3) + # Draw obstacles + for ox, oy, r in OBSTACLES: + circle = patches.Circle((ox, oy), r, color="gray", alpha=0.7) + ax.add_patch(circle) + # Draw spawn area + spawn_rect = patches.Rectangle( + (SPAWN_AREA[0][0], SPAWN_AREA[1][0]), + SPAWN_AREA[0][1] - SPAWN_AREA[0][0], + SPAWN_AREA[1][1] - SPAWN_AREA[1][0], + linewidth=2, + edgecolor="green", + facecolor="green", + alpha=0.2, + label="Start Zone", + ) + ax.add_patch(spawn_rect) + # Draw target + ax.plot(TARGET[0], TARGET[1], "r*", markersize=20, label="Target") + # Initialize plot elements + particles_scatter = ax.scatter( + [], [], c="blue", s=50, alpha=0.6, label="Particles" + ) + gbest_scatter = ax.plot([], [], "yo", markersize=12, label="Best Position")[0] + particle_paths = [ + ax.plot([], [], "b-", lw=0.5, alpha=0.2)[0] for _ in range(N_PARTICLES) + ] + gbest_path_line = ax.plot([], [], "y--", lw=2, alpha=0.8, label="Best Path")[0] + iteration_text = ax.text( + 0.02, + 0.95, + "", + transform=ax.transAxes, + fontsize=12, + verticalalignment="top", + bbox=dict(boxstyle="round", facecolor="wheat", alpha=0.5), + ) + ax.legend(loc="upper right") + def animate(frame): + """Animation function for matplotlib FuncAnimation""" + if not swarm.step(): + return ( + particles_scatter, + gbest_scatter, + gbest_path_line, + iteration_text, + *particle_paths, + ) + # Update particle positions + positions = np.array([p.position for p in swarm.particles]) + particles_scatter.set_offsets(positions) + # Update particle paths + for i, particle in enumerate(swarm.particles): + if len(particle.path) > 1: + path = np.array(particle.path) + particle_paths[i].set_data(path[:, 0], path[:, 1]) + # Update global best + if swarm.gbest_position is not None: + gbest_scatter.set_data( + [swarm.gbest_position[0]], [swarm.gbest_position[1]] + ) + if len(swarm.gbest_path) > 1: + gbest = np.array(swarm.gbest_path) + gbest_path_line.set_data(gbest[:, 0], gbest[:, 1]) + # Update text + iteration_text.set_text( + f"Iteration: {swarm.iteration}/{MAX_ITER}\n" + f"Best Fitness: {swarm.gbest_value:.2f}" + ) + return ( + particles_scatter, + gbest_scatter, + gbest_path_line, + iteration_text, + *particle_paths, + ) + # Create animation and store reference to prevent garbage collection + animation_ref = animation.FuncAnimation( + fig, animate, frames=MAX_ITER, interval=100, blit=True, repeat=False + ) + plt.tight_layout() + plt.show() + # Keep reference to prevent garbage collection + return animation_ref + else: + # Run without animation for testing + print("Running PSO algorithm without animation...") + iteration_count = 0 + while swarm.step(): + iteration_count += 1 + if iteration_count >= MAX_ITER: + break + print(f"PSO completed after {iteration_count} iterations") + print(f"Best fitness: {swarm.gbest_value:.2f}") + if swarm.gbest_position is not None: + print( + f"Best position: [{swarm.gbest_position[0]:.2f}, {swarm.gbest_position[1]:.2f}]" + ) + return None +if __name__ == "__main__": + try: + main() + except KeyboardInterrupt: + print("\nProgram interrupted by user") + plt.close("all") + sys.exit(0) \ No newline at end of file diff --git a/README.md b/README.md index d1b801f219..65445fa4ce 100644 --- a/README.md +++ b/README.md @@ -36,6 +36,7 @@ Python codes and [textbook](https://atsushisakai.github.io/PythonRobotics/index. * [D* Lite algorithm](#d-lite-algorithm) * [Potential Field algorithm](#potential-field-algorithm) * [Grid based coverage path planning](#grid-based-coverage-path-planning) + * [Particle Swarm Optimization (PSO)](#particle-swarm-optimization-pso) * [State Lattice Planning](#state-lattice-planning) * [Biased polar sampling](#biased-polar-sampling) * [Lane sampling](#lane-sampling) @@ -356,6 +357,24 @@ This is a 2D grid based coverage path planning simulation. ![PotentialField](https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/GridBasedSweepCPP/animation.gif) +### Particle Swarm Optimization (PSO) + +This is a 2D path planning simulation using the Particle Swarm Optimization algorithm. + +![PSO](https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/ParticleSwarmOptimization/animation.gif) + +PSO is a metaheuristic optimization algorithm inspired by bird flocking behavior. In path planning, particles explore the search space to find collision-free paths while avoiding obstacles. + +The animation shows particles (blue dots) converging towards the optimal path (yellow line) from start (green area) to goal (red star). + +References + +- [Particle swarm optimization - Wikipedia](https://en.wikipedia.org/wiki/Particle_swarm_optimization) + +- [Kennedy, J.; Eberhart, R. (1995). "Particle Swarm Optimization"](https://ieeexplore.ieee.org/document/488968) + + + ## State Lattice Planning This script is a path planning code with state lattice planning. diff --git a/docs/modules/5_path_planning/particleSwarmOptimization/particleSwarmOptimization_main.rst b/docs/modules/5_path_planning/particleSwarmOptimization/particleSwarmOptimization_main.rst new file mode 100644 index 0000000000..b430faec1e --- /dev/null +++ b/docs/modules/5_path_planning/particleSwarmOptimization/particleSwarmOptimization_main.rst @@ -0,0 +1,153 @@ +.. _particle_swarm_optimization: + +Particle Swarm Optimization Path Planning +------------------------------------------ + +This is a 2D path planning simulation using the Particle Swarm Optimization algorithm. + +PSO is a metaheuristic optimization algorithm inspired by the social behavior of bird flocking or fish schooling. In path planning, particles represent potential solutions that explore the search space to find collision-free paths from start to goal. + +.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/ParticleSwarmOptimization/animation.gif + +Algorithm Overview +++++++++++++++++++ + +The PSO algorithm maintains a swarm of particles that move through the search space according to simple mathematical rules: + +1. **Initialization**: Particles are randomly distributed near the start position +2. **Evaluation**: Each particle's fitness is calculated based on distance to goal and obstacle penalties +3. **Update**: Particles adjust their velocities based on: + - Personal best position (cognitive component) + - Global best position (social component) + - Current velocity (inertia component) +4. **Movement**: Particles move to new positions and check for collisions +5. **Convergence**: Process repeats until maximum iterations or goal is reached + +Mathematical Foundation ++++++++++++++++++++++++ + +The core PSO velocity update equation is: + +.. math:: + + v_{i}(t+1) = w \cdot v_{i}(t) + c_1 \cdot r_1 \cdot (p_{i} - x_{i}(t)) + c_2 \cdot r_2 \cdot (g - x_{i}(t)) + +Where: +- :math:`v_{i}(t)` = velocity of particle i at time t +- :math:`x_{i}(t)` = position of particle i at time t +- :math:`w` = inertia weight (controls exploration vs exploitation) +- :math:`c_1` = cognitive coefficient (attraction to personal best) +- :math:`c_2` = social coefficient (attraction to global best) +- :math:`r_1, r_2` = random numbers in [0,1] +- :math:`p_{i}` = personal best position of particle i +- :math:`g` = global best position + +Position update: + +.. math:: + + x_{i}(t+1) = x_{i}(t) + v_{i}(t+1) + +Fitness Function +++++++++++++++++ + +The fitness function combines distance to target with obstacle penalties: + +.. math:: + + f(x) = ||x - x_{goal}|| + \sum_{j} P_{obs}(x, O_j) + +Where: +- :math:`||x - x_{goal}||` = Euclidean distance to goal +- :math:`P_{obs}(x, O_j)` = penalty for obstacle j +- :math:`O_j` = obstacle j with position and radius + +The obstacle penalty function is defined as: + +.. math:: + + P_{obs}(x, O_j) = \begin{cases} + 1000 & \text{if } ||x - O_j|| < r_j \text{ (inside obstacle)} \\ + \frac{50}{||x - O_j|| - r_j + 0.1} & \text{if } r_j \leq ||x - O_j|| < r_j + R_{influence} \text{ (near obstacle)} \\ + 0 & \text{if } ||x - O_j|| \geq r_j + R_{influence} \text{ (safe distance)} + \end{cases} + +Where: +- :math:`r_j` = radius of obstacle j +- :math:`R_{influence}` = influence radius (typically 5 units) + +Collision Detection ++++++++++++++++++++ + +Line-circle intersection is used to detect collisions between particle paths and circular obstacles: + +.. math:: + + ||P_0 + t \cdot \vec{d} - C|| = r + +Where: +- :math:`P_0` = start point of path segment +- :math:`\vec{d}` = direction vector of path +- :math:`C` = obstacle center +- :math:`r` = obstacle radius +- :math:`t \in [0,1]` = parameter along line segment + +Algorithm Parameters +++++++++++++++++++++ + +Key parameters affecting performance: + +- **Number of particles** (n_particles): More particles = better exploration but slower +- **Maximum iterations** (max_iter): More iterations = better convergence but slower +- **Inertia weight** (w): High = exploration, Low = exploitation +- **Cognitive coefficient** (c1): Attraction to personal best +- **Social coefficient** (c2): Attraction to global best + +Typical values: +- n_particles: 20-50 +- max_iter: 100-300 +- w: 0.9 → 0.4 (linearly decreasing) +- c1, c2: 1.5-2.0 + +Advantages +++++++++++ + +- **Global optimization**: Can escape local minima unlike gradient-based methods +- **No derivatives needed**: Works with non-differentiable fitness landscapes +- **Parallel exploration**: Multiple particles search simultaneously +- **Simple implementation**: Few parameters and straightforward logic +- **Flexible**: Easily adaptable to different environments and constraints + +Disadvantages ++++++++++++++ + +- **Stochastic**: Results may vary between runs +- **Parameter sensitive**: Performance heavily depends on parameter tuning +- **No optimality guarantee**: Metaheuristic without convergence proof +- **Computational cost**: Requires many fitness evaluations +- **Prone to stagnation**: Premature convergence where the entire swarm can get trapped in a local minimum if exploration is insufficient + +Code Link ++++++++++ + +.. autofunction:: PathPlanning.ParticleSwarmOptimization.particle_swarm_optimization.main + +Usage Example ++++++++++++++ + +.. code-block:: python + + import matplotlib.pyplot as plt + from PathPlanning.ParticleSwarmOptimization.particle_swarm_optimization import main + + # Run PSO path planning with visualization + main() + +References +++++++++++ + +- `Particle swarm optimization - Wikipedia `__ +- Kennedy, J.; Eberhart, R. (1995). "Particle Swarm Optimization". Proceedings of IEEE International Conference on Neural Networks. IV. pp. 1942–1948. +- Shi, Y.; Eberhart, R. (1998). "A Modified Particle Swarm Optimizer". IEEE International Conference on Evolutionary Computation. +- `A Gentle Introduction to Particle Swarm Optimization `__ +- Clerc, M.; Kennedy, J. (2002). "The particle swarm - explosion, stability, and convergence in a multidimensional complex space". IEEE Transactions on Evolutionary Computation. 6 (1): 58–73. \ No newline at end of file diff --git a/docs/modules/5_path_planning/path_planning_main.rst b/docs/modules/5_path_planning/path_planning_main.rst index 0c84a19c22..5132760dc5 100644 --- a/docs/modules/5_path_planning/path_planning_main.rst +++ b/docs/modules/5_path_planning/path_planning_main.rst @@ -19,6 +19,7 @@ Path planning is the ability of a robot to search feasible and efficient path to visibility_road_map_planner/visibility_road_map_planner vrm_planner/vrm_planner rrt/rrt + particleSwarmOptimization/particleSwarmOptimization cubic_spline/cubic_spline bspline_path/bspline_path catmull_rom_spline/catmull_rom_spline diff --git a/tests/test_particle_swarm_optimization.py b/tests/test_particle_swarm_optimization.py new file mode 100755 index 0000000000..95720022f6 --- /dev/null +++ b/tests/test_particle_swarm_optimization.py @@ -0,0 +1,11 @@ +import conftest # Add root path to sys.path +from PathPlanning.ParticleSwarmOptimization import particle_swarm_optimization as m + + +def test_1(): + m.show_animation = False + m.main() + + +if __name__ == '__main__': + conftest.run_this_test(__file__) \ No newline at end of file From 99af1f391686cc7fa10b14bc5ed667cbe7b3f5eb Mon Sep 17 00:00:00 2001 From: Musab Kasbati <43878981+Musab1Blaser@users.noreply.github.com> Date: Wed, 22 Oct 2025 15:03:13 +0500 Subject: [PATCH 11/13] Add theta star planner (#1293) * Theta Star path compression * add tests and documentation for theta star algorithm * remove erraneous nested loop * improve documentation for theta star planner * add reference for theta star algorithm * use built-in function for norm * Add Bresenham line algorithm description to docs --- PathPlanning/ThetaStar/theta_star.py | 345 ++++++++++++++++++ .../grid_base_search_main.rst | 29 ++ tests/test_theta_star.py | 11 + 3 files changed, 385 insertions(+) create mode 100644 PathPlanning/ThetaStar/theta_star.py create mode 100644 tests/test_theta_star.py diff --git a/PathPlanning/ThetaStar/theta_star.py b/PathPlanning/ThetaStar/theta_star.py new file mode 100644 index 0000000000..73a1448b28 --- /dev/null +++ b/PathPlanning/ThetaStar/theta_star.py @@ -0,0 +1,345 @@ +""" + +Theta* grid planning + +author: Musab Kasbati (@Musab1Blaser) + +See Wikipedia article (https://cdn.aaai.org/AAAI/2007/AAAI07-187.pdf) + +""" + +import math + +import matplotlib.pyplot as plt + +show_animation = True +use_theta_star = True + + +class ThetaStarPlanner: + + def __init__(self, ox, oy, resolution, rr): + """ + Initialize grid map for theta star planning + + ox: x position list of Obstacles [m] + oy: y position list of Obstacles [m] + resolution: grid resolution [m] + rr: robot radius[m] + """ + + self.resolution = resolution + self.rr = rr + self.min_x, self.min_y = 0, 0 + self.max_x, self.max_y = 0, 0 + self.obstacle_map = None + self.x_width, self.y_width = 0, 0 + self.motion = self.get_motion_model() + self.calc_obstacle_map(ox, oy) + + class Node: + def __init__(self, x, y, cost, parent_index): + self.x = x # index of grid + self.y = y # index of grid + self.cost = cost + self.parent_index = parent_index + + def __str__(self): + return str(self.x) + "," + str(self.y) + "," + str( + self.cost) + "," + str(self.parent_index) + + def planning(self, sx, sy, gx, gy): + """ + Theta star path search + + input: + s_x: start x position [m] + s_y: start y position [m] + gx: goal x position [m] + gy: goal y position [m] + + output: + rx: x position list of the final path + ry: y position list of the final path + """ + + start_node = self.Node(self.calc_xy_index(sx, self.min_x), + self.calc_xy_index(sy, self.min_y), 0.0, -1) + goal_node = self.Node(self.calc_xy_index(gx, self.min_x), + self.calc_xy_index(gy, self.min_y), 0.0, -1) + + open_set, closed_set = dict(), dict() + open_set[self.calc_grid_index(start_node)] = start_node + + while True: + if len(open_set) == 0: + print("Open set is empty..") + break + + c_id = min( + open_set, + key=lambda o: open_set[o].cost + self.calc_heuristic(goal_node, + open_set[ + o])) + current = open_set[c_id] + + # show graph + if show_animation: # pragma: no cover + x = self.calc_grid_position(current.x, self.min_x) + y = self.calc_grid_position(current.y, self.min_y) + + # Draw an arrow toward the parent + if current.parent_index != -1 and current.parent_index in closed_set: + parent = closed_set[current.parent_index] + px = self.calc_grid_position(parent.x, self.min_x) + py = self.calc_grid_position(parent.y, self.min_y) + + # Vector from current to parent + dx = px - x + dy = py - y + + # scale down vector for visibility + norm = math.hypot(dx, dy) + dx /= norm + dy /= norm + + # Draw a small arrow (scale it down for visibility) + plt.arrow(x, y, dx, dy, + head_width=0.5, head_length=0.5, + fc='c', ec='c', alpha=0.7) + + # For stopping simulation with the esc key + plt.gcf().canvas.mpl_connect( + 'key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None] + ) + + if len(closed_set.keys()) % 10 == 0: + plt.pause(0.001) + + if current.x == goal_node.x and current.y == goal_node.y: + print("Find goal") + goal_node.parent_index = current.parent_index + goal_node.cost = current.cost + break + + # Remove the item from the open set + del open_set[c_id] + + # Add it to the closed set + closed_set[c_id] = current + + # expand_grid search grid based on motion model + for i, _ in enumerate(self.motion): + node = self.Node(current.x + self.motion[i][0], + current.y + self.motion[i][1], + current.cost + self.motion[i][2], c_id) # cost may later be updated by theta star path compression + n_id = self.calc_grid_index(node) + + if not self.verify_node(node): + continue + + if n_id in closed_set: + continue + + # Theta* modification: + if use_theta_star and current.parent_index != -1 and current.parent_index in closed_set: + grandparent = closed_set[current.parent_index] + if self.line_of_sight(grandparent, node): + # If parent(current) has line of sight to neighbor + node.cost = grandparent.cost + math.hypot(node.x - grandparent.x, node.y - grandparent.y) + node.parent_index = current.parent_index # compress path directly to grandparent + + if n_id not in open_set: + open_set[n_id] = node + else: + if open_set[n_id].cost > node.cost: + # This path is the best until now. record it + open_set[n_id] = node + + + rx, ry = self.calc_final_path(goal_node, closed_set) + + return rx, ry + + def calc_final_path(self, goal_node, closed_set): + # generate final course + rx, ry = [self.calc_grid_position(goal_node.x, self.min_x)], [ + self.calc_grid_position(goal_node.y, self.min_y)] + parent_index = goal_node.parent_index + while parent_index != -1: + n = closed_set[parent_index] + rx.append(self.calc_grid_position(n.x, self.min_x)) + ry.append(self.calc_grid_position(n.y, self.min_y)) + parent_index = n.parent_index + + return rx, ry + + @staticmethod + def calc_heuristic(n1, n2): + w = 1.0 # weight of heuristic + d = w * math.hypot(n1.x - n2.x, n1.y - n2.y) + return d + + def calc_grid_position(self, index, min_position): + """ + calc grid position + + :param index: + :param min_position: + :return: + """ + pos = index * self.resolution + min_position + return pos + + def calc_xy_index(self, position, min_pos): + return round((position - min_pos) / self.resolution) + + def calc_grid_index(self, node): + return (node.y - self.min_y) * self.x_width + (node.x - self.min_x) + + def line_of_sight(self, node1, node2): + """ + Check if there is a direct line of sight between two nodes. + Uses Bresenham’s line algorithm for grid traversal. + """ + x0 = node1.x + y0 = node1.y + x1 = node2.x + y1 = node2.y + + dx = abs(x1 - x0) + dy = abs(y1 - y0) + sx = 1 if x0 < x1 else -1 + sy = 1 if y0 < y1 else -1 + + err = dx - dy + + while True: + if not self.verify_node(self.Node(x0, y0, 0, -1)): + return False + if x0 == x1 and y0 == y1: + break + e2 = 2 * err + if e2 > -dy: + err -= dy + x0 += sx + if e2 < dx: + err += dx + y0 += sy + return True + + + def verify_node(self, node): + px = self.calc_grid_position(node.x, self.min_x) + py = self.calc_grid_position(node.y, self.min_y) + + if px < self.min_x: + return False + elif py < self.min_y: + return False + elif px >= self.max_x: + return False + elif py >= self.max_y: + return False + + # collision check + if self.obstacle_map[node.x][node.y]: + return False + + return True + + def calc_obstacle_map(self, ox, oy): + + self.min_x = round(min(ox)) + self.min_y = round(min(oy)) + self.max_x = round(max(ox)) + self.max_y = round(max(oy)) + print("min_x:", self.min_x) + print("min_y:", self.min_y) + print("max_x:", self.max_x) + print("max_y:", self.max_y) + + self.x_width = round((self.max_x - self.min_x) / self.resolution) + self.y_width = round((self.max_y - self.min_y) / self.resolution) + print("x_width:", self.x_width) + print("y_width:", self.y_width) + + # obstacle map generation + self.obstacle_map = [[False for _ in range(self.y_width)] + for _ in range(self.x_width)] + for ix in range(self.x_width): + x = self.calc_grid_position(ix, self.min_x) + for iy in range(self.y_width): + y = self.calc_grid_position(iy, self.min_y) + for iox, ioy in zip(ox, oy): + d = math.hypot(iox - x, ioy - y) + if d <= self.rr: + self.obstacle_map[ix][iy] = True + break + + @staticmethod + def get_motion_model(): + # dx, dy, cost + motion = [[1, 0, 1], + [0, 1, 1], + [-1, 0, 1], + [0, -1, 1], + [-1, -1, math.sqrt(2)], + [-1, 1, math.sqrt(2)], + [1, -1, math.sqrt(2)], + [1, 1, math.sqrt(2)]] + + return motion + + +def main(): + print(__file__ + " start!!") + + # start and goal position + sx = 10.0 # [m] + sy = 10.0 # [m] + gx = 50.0 # [m] + gy = 50.0 # [m] + grid_size = 2.0 # [m] + robot_radius = 1.0 # [m] + + # set obstacle positions + ox, oy = [], [] + for i in range(-10, 60): + ox.append(i) + oy.append(-10.0) + for i in range(-10, 60): + ox.append(60.0) + oy.append(i) + for i in range(-10, 61): + ox.append(i) + oy.append(60.0) + for i in range(-10, 61): + ox.append(-10.0) + oy.append(i) + for i in range(-10, 40): + ox.append(20.0) + oy.append(i) + for i in range(0, 40): + ox.append(40.0) + oy.append(60.0 - i) + + if show_animation: # pragma: no cover + plt.plot(ox, oy, ".k") + plt.plot(sx, sy, "og") + plt.plot(gx, gy, "xb") + plt.grid(True) + plt.axis("equal") + + theta_star = ThetaStarPlanner(ox, oy, grid_size, robot_radius) + rx, ry = theta_star.planning(sx, sy, gx, gy) + + if show_animation: # pragma: no cover + plt.plot(rx, ry, "-r") + plt.pause(0.001) + plt.show() + + +if __name__ == '__main__': + main() 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 c4aa6882aa..e2f4d8832d 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 @@ -82,6 +82,35 @@ Code Link .. autofunction:: PathPlanning.BidirectionalAStar.bidirectional_a_star.BidirectionalAStarPlanner +.. _Theta*-algorithm: + +Theta\* algorithm +~~~~~~~~~~~~~~~~~ + +This is a 2D grid based shortest path planning with Theta star algorithm. + +It offers an optimization over the A star algorithm to generate any-angle paths. Unlike A star, which always assigns the current node as the parent of the successor, Theta star checks for a line-of-sight (unblocked path) from an earlier node (typically the parent of the current node) to the successor, and directly assigns it as a parent if visible. This reduces cost by replacing staggered segments with straight lines. + +Checking for line of sight involves verifying that no obstacles block the straight line between two nodes. On a grid, this means identifying all the discrete cells that the line passes through to determine if they are empty. Bresenham’s line algorithm is commonly used for this purpose. Starting from one endpoint, it incrementally steps along one axis, while considering the gradient to determine the position on the other axis. Because it relies only on integer addition and subtraction, it is both efficient and precise for grid-based visibility checks in Theta*. + +As a result, Theta star produces shorter, smoother paths than A star, ideal for ground or aerial robots operating in continuous environments where smoother motion enables higher acceleration and reduced travel time. + +.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/ThetaStar/animation.gif + +In the animation, each cyan arrow represents a node pointing to its parent. + +Reference +++++++++++++ + +- `Theta*: Any-Angle Path Planning on Grids `__ +- `Bresenham's line algorithm `__ + +Code Link ++++++++++++++ + +.. autofunction:: PathPlanning.ThetaStar.theta_star.ThetaStarPlanner + + .. _D*-algorithm: D\* algorithm diff --git a/tests/test_theta_star.py b/tests/test_theta_star.py new file mode 100644 index 0000000000..10ceae44e3 --- /dev/null +++ b/tests/test_theta_star.py @@ -0,0 +1,11 @@ +import conftest +from PathPlanning.ThetaStar import theta_star as m + + +def test_1(): + m.show_animation = False + m.main() + + +if __name__ == '__main__': + conftest.run_this_test(__file__) \ No newline at end of file From 2e6baeb2d326b152a28d185338935746468de151 Mon Sep 17 00:00:00 2001 From: Musab Kasbati <43878981+Musab1Blaser@users.noreply.github.com> Date: Sat, 25 Oct 2025 20:49:06 +0500 Subject: [PATCH 12/13] corrected obvious typos in text and mathematical notations as well as grammatical corrections (#1304) --- .../0_getting_started/2_how_to_run_sample_codes_main.rst | 2 +- .../0_getting_started/3_how_to_contribute_main.rst | 2 +- .../10_inverted_pendulum/inverted_pendulum_main.rst | 4 ++-- docs/modules/12_appendix/Kalmanfilter_basics_main.rst | 2 +- .../behavior_tree/behavior_tree_main.rst | 4 ++-- .../2_python_for_robotics/python_for_robotics_main.rst | 2 +- .../technologies_for_robotics_main.rst | 2 +- .../histogram_filter_localization_main.rst | 2 +- .../particle_filter_localization_main.rst | 2 +- .../lidar_to_grid_map_tutorial_main.rst | 2 +- docs/modules/4_slam/FastSLAM1/FastSLAM1_main.rst | 4 ++-- docs/modules/4_slam/ekf_slam/ekf_slam_main.rst | 2 +- docs/modules/4_slam/graph_slam/graphSLAM_doc.rst | 4 ++-- .../5_path_planning/dubins_path/dubins_path_main.rst | 2 +- .../5_path_planning/elastic_bands/elastic_bands_main.rst | 2 +- .../grid_base_search/grid_base_search_main.rst | 2 +- .../reeds_shepp_path/reeds_shepp_path_main.rst | 2 +- .../5_path_planning/vrm_planner/vrm_planner_main.rst | 2 +- .../model_predictive_speed_and_steering_control_main.rst | 8 ++++---- .../move_to_a_pose/move_to_a_pose_main.rst | 2 +- .../n_joint_arm_to_point_control_main.rst | 2 +- docs/modules/7_arm_navigation/planar_two_link_ik_main.rst | 2 +- 22 files changed, 29 insertions(+), 29 deletions(-) diff --git a/docs/modules/0_getting_started/2_how_to_run_sample_codes_main.rst b/docs/modules/0_getting_started/2_how_to_run_sample_codes_main.rst index b92fc9bde0..33440bb137 100644 --- a/docs/modules/0_getting_started/2_how_to_run_sample_codes_main.rst +++ b/docs/modules/0_getting_started/2_how_to_run_sample_codes_main.rst @@ -46,7 +46,7 @@ using pip : 5. Execute python script in each directory. -For example, to run the sample code of `Extented Kalman Filter` in the +For example, to run the sample code of `Extended Kalman Filter` in the `localization` directory, execute the following command: .. code-block:: diff --git a/docs/modules/0_getting_started/3_how_to_contribute_main.rst b/docs/modules/0_getting_started/3_how_to_contribute_main.rst index 9e773e930c..f975127107 100644 --- a/docs/modules/0_getting_started/3_how_to_contribute_main.rst +++ b/docs/modules/0_getting_started/3_how_to_contribute_main.rst @@ -132,7 +132,7 @@ At first, please fix all CI errors before code review. You can check your PR doc from the CI panel. After the "ci/circleci: build_doc" CI is succeeded, -you can access you PR doc with clicking the [Details] of the "ci/circleci: build_doc artifact" CI. +you can access your PR doc with clicking the [Details] of the "ci/circleci: build_doc artifact" CI. .. image:: /_static/img/doc_ci.png diff --git a/docs/modules/10_inverted_pendulum/inverted_pendulum_main.rst b/docs/modules/10_inverted_pendulum/inverted_pendulum_main.rst index 58dc0f2e57..47ae034195 100644 --- a/docs/modules/10_inverted_pendulum/inverted_pendulum_main.rst +++ b/docs/modules/10_inverted_pendulum/inverted_pendulum_main.rst @@ -4,7 +4,7 @@ Inverted Pendulum ------------------ An inverted pendulum on a cart consists of a mass :math:`m` at the top of a pole of length :math:`l` pivoted on a -horizontally moving base as shown in the adjacent. +horizontally moving base as shown in the adjacent figure. The objective of the control system is to balance the inverted pendulum by applying a force to the cart that the pendulum is attached to. @@ -97,7 +97,7 @@ Code Link MPC control ~~~~~~~~~~~~~~~~~~~~~~~~~~~ -The MPC controller minimize this cost function defined as: +The MPC controller minimizes this cost function defined as: .. math:: J = x^T Q x + u^T R u diff --git a/docs/modules/12_appendix/Kalmanfilter_basics_main.rst b/docs/modules/12_appendix/Kalmanfilter_basics_main.rst index a1d99d47ef..1ed5790d52 100644 --- a/docs/modules/12_appendix/Kalmanfilter_basics_main.rst +++ b/docs/modules/12_appendix/Kalmanfilter_basics_main.rst @@ -55,7 +55,7 @@ Variance, Covariance and Correlation Variance ^^^^^^^^ -Variance is the spread of the data. The mean does’nt tell much **about** +Variance is the spread of the data. The mean doesn’t tell much **about** the data. Therefore the variance tells us about the **story** about the data meaning the spread of the data. diff --git a/docs/modules/13_mission_planning/behavior_tree/behavior_tree_main.rst b/docs/modules/13_mission_planning/behavior_tree/behavior_tree_main.rst index 22849f7c54..4b0ef82c12 100644 --- a/docs/modules/13_mission_planning/behavior_tree/behavior_tree_main.rst +++ b/docs/modules/13_mission_planning/behavior_tree/behavior_tree_main.rst @@ -2,8 +2,8 @@ Behavior Tree ------------- Behavior Tree is a modular, hierarchical decision model that is widely used in robot control, and game development. -It present some similarities to hierarchical state machines with the key difference that the main building block of a behavior is a task rather than a state. -Behavior Tree have been shown to generalize several other control architectures (https://ieeexplore.ieee.org/document/7790863) +It presents some similarities to hierarchical state machines with the key difference that the main building block of a behavior is a task rather than a state. +Behavior Trees have been shown to generalize several other control architectures (https://ieeexplore.ieee.org/document/7790863) Code Link ~~~~~~~~~~~~~ diff --git a/docs/modules/1_introduction/2_python_for_robotics/python_for_robotics_main.rst b/docs/modules/1_introduction/2_python_for_robotics/python_for_robotics_main.rst index c47c122853..e3f38a55fd 100644 --- a/docs/modules/1_introduction/2_python_for_robotics/python_for_robotics_main.rst +++ b/docs/modules/1_introduction/2_python_for_robotics/python_for_robotics_main.rst @@ -1,7 +1,7 @@ Python for Robotics ---------------------- -A programing language, Python is used for this `PythonRobotics` project +A programming language, Python is used for this `PythonRobotics` project to achieve the purposes of this project described in the :ref:`What is PythonRobotics?`. This section explains the Python itself and features for science computing and robotics. diff --git a/docs/modules/1_introduction/3_technologies_for_robotics/technologies_for_robotics_main.rst b/docs/modules/1_introduction/3_technologies_for_robotics/technologies_for_robotics_main.rst index 0ed51e961b..dd3cd1d86c 100644 --- a/docs/modules/1_introduction/3_technologies_for_robotics/technologies_for_robotics_main.rst +++ b/docs/modules/1_introduction/3_technologies_for_robotics/technologies_for_robotics_main.rst @@ -3,7 +3,7 @@ Technologies for Robotics The field of robotics needs wide areas of technologies such as mechanical engineering, electrical engineering, computer science, and artificial intelligence (AI). -This project, `PythonRobotics`, only focus on computer science and artificial intelligence. +This project, `PythonRobotics`, only focuses on computer science and artificial intelligence. The technologies for robotics are categorized as following 3 categories: 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 3a175b1316..b5697d2dd9 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 @@ -68,7 +68,7 @@ But, the probability is getting uncertain without observations: The `gaussian filter `_ -is used in the simulation for adding noize. +is used in the simulation for adding noise. Step3: Update probability by observation ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 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 d648d8e080..8bf22a246b 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 @@ -13,7 +13,7 @@ and the red line is estimated trajectory with PF. It is assumed that the robot can measure a distance from landmarks (RFID). -This measurements are used for PF localization. +These measurements are used for PF localization. Code Link ~~~~~~~~~~~~~ diff --git a/docs/modules/3_mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_main.rst b/docs/modules/3_mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_main.rst index 29f5878e48..c97d204a82 100644 --- a/docs/modules/3_mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_main.rst +++ b/docs/modules/3_mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_main.rst @@ -19,7 +19,7 @@ unknown (unobserved) areas, which are close to 0.5. .. figure:: grid_map_example.png In order to construct the grid map from the measurement we need to -discretise the values. But, first let’s need to ``import`` some +discretise the values. But, first we need to ``import`` some necessary packages. .. code:: ipython3 diff --git a/docs/modules/4_slam/FastSLAM1/FastSLAM1_main.rst b/docs/modules/4_slam/FastSLAM1/FastSLAM1_main.rst index b6bafa0982..296f1766de 100644 --- a/docs/modules/4_slam/FastSLAM1/FastSLAM1_main.rst +++ b/docs/modules/4_slam/FastSLAM1/FastSLAM1_main.rst @@ -112,7 +112,7 @@ will converge to the correct estimate. MAX_RANGE = 20.0 # maximum observation range M_DIST_TH = 2.0 # Threshold of Mahalanobis distance for data association. STATE_SIZE = 3 # State size [x,y,yaw] - LM_SIZE = 2 # LM srate size [x,y] + LM_SIZE = 2 # LM state size [x,y] N_PARTICLE = 100 # number of particle NTH = N_PARTICLE / 1.5 # Number of particle for re-sampling @@ -409,7 +409,7 @@ probably will die out. 3- Resampling ~~~~~~~~~~~~~ -In the reseampling steps a new set of particles are chosen from the old +In the resampling steps a new set of particles are chosen from the old set. This is done according to the weight of each particle. The figure shows 100 particles distributed uniformly between [-0.5, 0.5] 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 3967a7ae4d..05bd636ef5 100644 --- a/docs/modules/4_slam/ekf_slam/ekf_slam_main.rst +++ b/docs/modules/4_slam/ekf_slam/ekf_slam_main.rst @@ -32,7 +32,7 @@ Introduction EKF SLAM models the SLAM problem in a single EKF where the modeled state is both the pose :math:`(x, y, \theta)` and an array of landmarks -:math:`[(x_1, y_1), (x_2, x_y), ... , (x_n, y_n)]` for :math:`n` +:math:`[(x_1, y_1), (x_2, y_2), ... , (x_n, y_n)]` for :math:`n` landmarks. The covariance between each of the positions and landmarks are also tracked. diff --git a/docs/modules/4_slam/graph_slam/graphSLAM_doc.rst b/docs/modules/4_slam/graph_slam/graphSLAM_doc.rst index 5297604809..3d329ae131 100644 --- a/docs/modules/4_slam/graph_slam/graphSLAM_doc.rst +++ b/docs/modules/4_slam/graph_slam/graphSLAM_doc.rst @@ -28,7 +28,7 @@ available that uses graph-based approaches to perform online estimation or to solve for a subset of the poses. GraphSLAM uses the motion information as well as the observations of the -environment to create least square problem that can be solved using +environment to create a least squares problem that can be solved using standard optimization techniques. Minimal Example @@ -331,7 +331,7 @@ between node 0 and 1 will be created. The equations for the error is as follows: :math:`e_{ij}^y = y_j + d_j sin(\psi_j + \theta_j) - y_i - d_i sin(\psi_i + \theta_i)` -:math:`e_{ij}^x = \psi_j + \theta_j - \psi_i - \theta_i` +:math:`e_{ij}^{\psi} = \psi_j + \theta_j - \psi_i - \theta_i` Where :math:`[x_i, y_i, \psi_i]` is the pose for node :math:`i` and similarly for node :math:`j`, :math:`d` is the measured distance at diff --git a/docs/modules/5_path_planning/dubins_path/dubins_path_main.rst b/docs/modules/5_path_planning/dubins_path/dubins_path_main.rst index 5a3d14464b..74cb757886 100644 --- a/docs/modules/5_path_planning/dubins_path/dubins_path_main.rst +++ b/docs/modules/5_path_planning/dubins_path/dubins_path_main.rst @@ -13,7 +13,7 @@ It can generates a shortest path between two 2D poses (x, y, yaw) with maximum c Generated paths consist of 3 segments of maximum curvature curves or a straight line segment. -Each segment type can is categorized by 3 type: 'Right turn (R)' , 'Left turn (L)', and 'Straight (S).' +Each segment type can be categorized by 3 types: 'Right turn (R)' , 'Left turn (L)', and 'Straight (S).' Possible path will be at least one of these six types: RSR, RSL, LSR, LSL, RLR, LRL. diff --git a/docs/modules/5_path_planning/elastic_bands/elastic_bands_main.rst b/docs/modules/5_path_planning/elastic_bands/elastic_bands_main.rst index d0109d4ec3..e37cbd4e77 100644 --- a/docs/modules/5_path_planning/elastic_bands/elastic_bands_main.rst +++ b/docs/modules/5_path_planning/elastic_bands/elastic_bands_main.rst @@ -67,7 +67,7 @@ Dynamic Path Maintenance .. math:: b_i^{\text{new}} = b_i^{\text{old}} + \alpha (f_c + f_r), - where :math:`\alpha` is a step-size parameter, which often proportional to :math:`\rho(b_i^{\text{old}})` + where :math:`\alpha` is a step-size parameter, which is often proportional to :math:`\rho(b_i^{\text{old}})` 2. **Overlap Enforcement**: - Insert new nodes if adjacent nodes are too far apart 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 e2f4d8832d..b7450107b6 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 @@ -91,7 +91,7 @@ This is a 2D grid based shortest path planning with Theta star algorithm. It offers an optimization over the A star algorithm to generate any-angle paths. Unlike A star, which always assigns the current node as the parent of the successor, Theta star checks for a line-of-sight (unblocked path) from an earlier node (typically the parent of the current node) to the successor, and directly assigns it as a parent if visible. This reduces cost by replacing staggered segments with straight lines. -Checking for line of sight involves verifying that no obstacles block the straight line between two nodes. On a grid, this means identifying all the discrete cells that the line passes through to determine if they are empty. Bresenham’s line algorithm is commonly used for this purpose. Starting from one endpoint, it incrementally steps along one axis, while considering the gradient to determine the position on the other axis. Because it relies only on integer addition and subtraction, it is both efficient and precise for grid-based visibility checks in Theta*. +Checking for line of sight involves verifying that no obstacles block the straight line between two nodes. On a grid, this means identifying all the discrete cells that the line passes through to determine if they are empty. Bresenham’s line algorithm is commonly used for this purpose. Starting from one endpoint, it incrementally steps along one axis, while considering the gradient to determine the position on the other axis. Because it relies only on integer addition and subtraction, it is both efficient and precise for grid-based visibility checks in Theta star. As a result, Theta star produces shorter, smoother paths than A star, ideal for ground or aerial robots operating in continuous environments where smoother motion enables higher acceleration and reduced travel time. 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 4dd54d7c97..bedcfc33d8 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 @@ -17,7 +17,7 @@ Here is an overview of mathematical derivations of formulae for individual path In all the derivations below, radius of curvature of the vehicle is assumed to be of unit length and start pose is considered to be at origin. (*In code we are removing the offset due to start position and normalising the lengths before passing the values to these functions.*) -Also, (t, u, v) respresent the measure of each motion requried. Thus, in case of a turning maneuver, they represent the angle inscribed at the centre of turning circle and in case of straight maneuver, they represent the distance to be travelled. +Also, (t, u, v) represent the measure of each motion required. Thus, in case of a turning maneuver, they represent the angle inscribed at the centre of turning circle and in case of straight maneuver, they represent the distance to be travelled. 1. **Left-Straight-Left** 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 a9a41aab74..46b86123f3 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 @@ -9,7 +9,7 @@ In the animation, blue points are Voronoi points, Cyan crosses mean searched points with Dijkstra method, -The red line is the final path of Vornoi Road-Map. +The red line is the final path of Voronoi Road-Map. Code Link ~~~~~~~~~~~~~~~ diff --git a/docs/modules/6_path_tracking/model_predictive_speed_and_steering_control/model_predictive_speed_and_steering_control_main.rst b/docs/modules/6_path_tracking/model_predictive_speed_and_steering_control/model_predictive_speed_and_steering_control_main.rst index 76a6819a46..cc5a6812ca 100644 --- a/docs/modules/6_path_tracking/model_predictive_speed_and_steering_control/model_predictive_speed_and_steering_control_main.rst +++ b/docs/modules/6_path_tracking/model_predictive_speed_and_steering_control/model_predictive_speed_and_steering_control_main.rst @@ -34,17 +34,17 @@ Input vector is: .. math:: u = [a, \delta] -a: accellation, δ: steering angle +a: acceleration, δ: steering angle -The MPC cotroller minimize this cost function for path tracking: +The MPC cotroller minimizes this cost function for path tracking: .. math:: min\ Q_f(z_{T,ref}-z_{T})^2+Q\Sigma({z_{t,ref}-z_{t}})^2+R\Sigma{u_t}^2+R_d\Sigma({u_{t+1}-u_{t}})^2 -z_ref come from target path and speed. +z_ref comes from target path and speed. subject to: -- Linearlied vehicle model +- Linearized vehicle model .. math:: z_{t+1}=Az_t+Bu+C diff --git a/docs/modules/6_path_tracking/move_to_a_pose/move_to_a_pose_main.rst b/docs/modules/6_path_tracking/move_to_a_pose/move_to_a_pose_main.rst index 19bb0e4c14..b3d23ab2be 100644 --- a/docs/modules/6_path_tracking/move_to_a_pose/move_to_a_pose_main.rst +++ b/docs/modules/6_path_tracking/move_to_a_pose/move_to_a_pose_main.rst @@ -32,7 +32,7 @@ Constructor PathFinderController(Kp_rho, Kp_alpha, Kp_beta) -Constructs an instantiate of the PathFinderController for navigating a 3-DOF wheeled robot on a 2D plane. +Constructs an instance of the PathFinderController for navigating a 3-DOF wheeled robot on a 2D plane. Parameters: diff --git a/docs/modules/7_arm_navigation/n_joint_arm_to_point_control_main.rst b/docs/modules/7_arm_navigation/n_joint_arm_to_point_control_main.rst index 56900acde1..6c1db7d2a5 100644 --- a/docs/modules/7_arm_navigation/n_joint_arm_to_point_control_main.rst +++ b/docs/modules/7_arm_navigation/n_joint_arm_to_point_control_main.rst @@ -3,7 +3,7 @@ N joint arm to point control N joint arm to a point control simulation. -This is a interactive simulation. +This is an interactive simulation. You can set the goal position of the end effector with left-click on the plotting area. diff --git a/docs/modules/7_arm_navigation/planar_two_link_ik_main.rst b/docs/modules/7_arm_navigation/planar_two_link_ik_main.rst index 5b2712eb48..8d3b709e47 100644 --- a/docs/modules/7_arm_navigation/planar_two_link_ik_main.rst +++ b/docs/modules/7_arm_navigation/planar_two_link_ik_main.rst @@ -7,7 +7,7 @@ Two joint arm to point control This is two joint arm to a point control simulation. -This is a interactive simulation. +This is an interactive simulation. You can set the goal position of the end effector with left-click on the plotting area. From 11a01f8beeccc5059788101ddd97f84d534a3bd5 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 28 Oct 2025 03:03:34 -0700 Subject: [PATCH 13/13] build(deps): bump ruff from 0.14.1 to 0.14.2 in /requirements (#1305) Bumps [ruff](https://github.com/astral-sh/ruff) from 0.14.1 to 0.14.2. - [Release notes](https://github.com/astral-sh/ruff/releases) - [Changelog](https://github.com/astral-sh/ruff/blob/main/CHANGELOG.md) - [Commits](https://github.com/astral-sh/ruff/compare/0.14.1...0.14.2) --- updated-dependencies: - dependency-name: ruff dependency-version: 0.14.2 dependency-type: direct:production update-type: version-update:semver-patch ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- requirements/requirements.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/requirements/requirements.txt b/requirements/requirements.txt index 306d93b736..bde51468ce 100644 --- a/requirements/requirements.txt +++ b/requirements/requirements.txt @@ -6,4 +6,4 @@ ecos == 2.0.14 pytest == 8.4.2 # For unit test pytest-xdist == 3.8.0 # For unit test mypy == 1.18.2 # For unit test -ruff == 0.14.1 # For unit test +ruff == 0.14.2 # For unit test