### Graph SLAM Imports and Setup

Source: https://github.com/atsushisakai/pythonrobotics/blob/master/docs/modules/4_slam/graph_slam/graph_slam.md

Imports necessary libraries and sets up initial parameters for the Graph SLAM example. Ensure these libraries are installed before running.

```ipython3
import copy
import math
import itertools
import numpy as np
import matplotlib.pyplot as plt
from graph_based_slam import calc_rotational_matrix, calc_jacobian, cal_observation_sigma, \
                             calc_input, observation, motion_model, Edge, pi_2_pi

%matplotlib inline
np.set_printoptions(precision=3, suppress=True)
np.random.seed(0)
```

--------------------------------

### 1D Graph SLAM Minimal Example Setup

Source: https://github.com/atsushisakai/pythonrobotics/blob/master/docs/modules/4_slam/graph_slam/graph_slam.md

Sets up parameters and simulates odometry and observation readings for a 1D robot and landmark scenario. This example visualizes the true path, dead reckoning, and observations.

```ipython3
R = 0.2
Q = 0.2
N = 3
graphics_radius = 0.1

odom = np.empty((N,1))
obs = np.empty((N,1))
x_true = np.empty((N,1))

landmark = 3
# Simulated readings of odometry and observations
x_true[0], odom[0], obs[0] = 0.0, 0.0, 2.9
x_true[1], odom[1], obs[1] = 1.0, 1.5, 2.0
x_true[2], odom[2], obs[2] = 2.0, 2.4, 1.0

hxDR = copy.deepcopy(odom)
# Visualization
plt.plot(landmark,0, '*k', markersize=30)
for i in range(N):
    plt.plot(odom[i], 0, '.', markersize=50, alpha=0.8, color='steelblue')
    plt.plot([odom[i], odom[i] + graphics_radius],
             [0,0], 'r')
    plt.text(odom[i], 0.02, "X_{}".format(i), fontsize=12)
    plt.plot(obs[i]+odom[i],0,'.', markersize=25, color='brown')
    plt.plot(x_true[i],0,'.g', markersize=20)
plt.grid()
plt.show()
```

--------------------------------

### Install Sphinx and Theme

Source: https://github.com/atsushisakai/pythonrobotics/blob/master/docs/README.md

Installs Sphinx and related themes for documentation generation. Ensure you have pip installed.

```bash
pip install sphinx sphinx-autobuild sphinx-rtd-theme sphinx_rtd_dark_mode sphinx_copybutton sphinx_rtd_dark_mode
```

--------------------------------

### FastSLAM1 Initialization and Update Example

Source: https://github.com/atsushisakai/pythonrobotics/blob/master/docs/modules/4_slam/FastSLAM1/FastSLAM1.md

Demonstrates the initialization of particles and landmarks, followed by sequential updates using simulated observations. This example shows how particle weights change as the algorithm processes data.

```python
# Setting up the landmarks
RFID = np.array([[10.0, -2.0],
                [15.0, 10.0]])
N_LM = RFID.shape[0]

# Initialize 1 particle
N_PARTICLE = 1
particles = [Particle(N_LM) for i in range(N_PARTICLE)]

xTrue = np.zeros((STATE_SIZE, 1))
xDR = np.zeros((STATE_SIZE, 1))

print("initial weight", particles[0].w)

xTrue, z, _, ud = observation(xTrue, xDR, u, RFID)
# Initialize landmarks
particles = update_with_observation(particles, z)
print("weight after landmark intialization", particles[0].w)
particles = update_with_observation(particles, z)
print("weight after update ", particles[0].w)

particles[0].x = -10
particles = update_with_observation(particles, z)
print("weight after wrong prediction", particles[0].w)
```

--------------------------------

### Unit Test Example Reference

Source: https://github.com/atsushisakai/pythonrobotics/blob/master/docs/modules/0_getting_started/3_how_to_contribute.md

When adding a new algorithm sample, include a unit test file in the tests directory. The provided link points to an example test file for the A* algorithm.

```text
This sample test code might help you : [test_a_star.py](https://github.com/AtsushiSakai/PythonRobotics/blob/master/tests/test_a_star.py).

At the least, try to run the example code without animation in the unit test.
```

--------------------------------

### Install Dependencies with Pip

Source: https://github.com/atsushisakai/pythonrobotics/blob/master/docs/modules/0_getting_started/2_how_to_run_sample_codes.md

Install all required libraries using pip and the provided requirements file.

```bash
>$ pip install -r requirements/requirements.txt
```

--------------------------------

### Install Dependencies with Pip

Source: https://github.com/atsushisakai/pythonrobotics/blob/master/README.md

Install the required libraries using pip by referencing the requirements.txt file.

```terminal
pip install -r requirements/requirements.txt
```

--------------------------------

### Install Dependencies with Conda

Source: https://github.com/atsushisakai/pythonrobotics/blob/master/docs/modules/0_getting_started/2_how_to_run_sample_codes.md

Create a conda environment and install all required libraries using the provided environment file.

```bash
>$ conda env create -f requirements/environment.yml
```

--------------------------------

### Graph SLAM Initialization and Setup

Source: https://github.com/atsushisakai/pythonrobotics/blob/master/docs/modules/4_slam/graph_slam/graphSLAM_doc.rst

Imports necessary libraries and sets up plotting options for Graph SLAM. Initializes random seed for reproducibility.

```ipython3
import copy
import math
import itertools
import numpy as np 
import matplotlib.pyplot as plt
from graph_based_slam import calc_rotational_matrix, calc_jacobian, cal_observation_sigma, \
                                 calc_input, observation, motion_model, Edge, pi_2_pi

%matplotlib inline
np.set_printoptions(precision=3, suppress=True)
np.random.seed(0)
```

--------------------------------

### Install Dependencies with Conda

Source: https://github.com/atsushisakai/pythonrobotics/blob/master/README.md

Create a conda environment using the provided yml file to install all required libraries.

```terminal
conda env create -f requirements/environment.yml
```

--------------------------------

### FastSLAM1 Initialization and Resampling Example

Source: https://github.com/atsushisakai/pythonrobotics/blob/master/docs/modules/4_slam/FastSLAM1/FastSLAM1_main.rst

Demonstrates the initialization of particles with Gaussian weights, normalization, resampling, and plotting the results. This code segment visualizes the effect of resampling on particle distribution.

```python
N_PARTICLE = 100
particles = [Particle(N_LM) for i in range(N_PARTICLE)]
x_pos = []
w = []
for i in range(N_PARTICLE):
    particles[i].x = np.linspace(-0.5,0.5,N_PARTICLE)[i]
    x_pos.append(particles[i].x)
    particles[i].w = gaussian(i, N_PARTICLE/2, N_PARTICLE/20)
    w.append(particles[i].w)
    

# Normalize weights
sw = sum(w)
for i in range(N_PARTICLE):
    w[i] /= sw

particles, new_indices = resampling(particles)
x_pos2 = []
for i in range(N_PARTICLE):
    x_pos2.append(particles[i].x)
    
# Plot results
fig, ((ax1,ax2,ax3)) = plt.subplots(nrows=3, ncols=1)
fig.tight_layout()
ax1.plot(x_pos,np.ones((N_PARTICLE,1)), '.r', markersize=2)
ax1.set_title("Particles before resampling")
ax1.axis((-1, 1, 0, 2))
ax2.plot(w)
ax2.set_title("Weights distribution")
ax3.plot(x_pos2,np.ones((N_PARTICLE,1)), '.r')
ax3.set_title("Particles after resampling")
ax3.axis((-1, 1, 0, 2))
fig.subplots_adjust(hspace=0.8)
plt.show()

plt.figure()
plt.hist(new_indices)
plt.xlabel("Particles indices to be resampled")
plt.ylabel("# of time index is used")
plt.show()
```

--------------------------------

### Initialize Graph SLAM Data

Source: https://github.com/atsushisakai/pythonrobotics/blob/master/docs/modules/4_slam/graph_slam/graphSLAM_doc.rst

Copies sensor data and pose estimates to prepare for graph construction. This is a setup step before processing observations.

```ipython3
zlist = copy.deepcopy(hz)
x_opt = copy.deepcopy(hxDR)
xlist = copy.deepcopy(hxDR)
number_of_nodes = x_opt.shape[1]
n = number_of_nodes * STATE_SIZE
```

--------------------------------

### Set Start and Target Poses

Source: https://github.com/atsushisakai/pythonrobotics/blob/master/docs/modules/6_path_tracking/move_to_a_pose/move_to_a_pose.md

Configures the robot's initial and final positions. Ensure the Pose objects are correctly defined before calling this function.

```ipython3
set_start_target_poses(pose_start, pose_target)
```

--------------------------------

### Bivariate Gaussian Distribution Setup

Source: https://github.com/atsushisakai/pythonrobotics/blob/master/docs/modules/12_appendix/Kalmanfilter_basics.md

Sets up parameters for a bivariate Gaussian distribution, including mean vector and covariance matrix. This code is a precursor to visualizing the distribution. Requires numpy and matplotlib.

```ipython3
#Example from:
#https://scipython.com/blog/visualizing-the-bivariate-gaussian-distribution/
import numpy as np
import matplotlib.pyplot as plt
from matplotlib import cm
from mpl_toolkits.mplot3d import Axes3D

# Our 2-dimensional distribution will be over variables X and Y
N = 60
X = np.linspace(-3, 3, N)
Y = np.linspace(-3, 4, N)
X, Y = np.meshgrid(X, Y)

# Mean vector and covariance matrix
mu = np.array([0., 1.])
Sigma = np.array([[ 1. , -0.5], [-0.5,  1.5]])

```

--------------------------------

### 1D Graph SLAM Minimal Example Simulation

Source: https://github.com/atsushisakai/pythonrobotics/blob/master/docs/modules/4_slam/graph_slam/graphSLAM_doc.rst

Simulates odometry and observation readings for a 1D robot and landmark. Visualizes the true path, odometry measurements, and observations.

```ipython3
R = 0.2
Q = 0.2
N = 3
graphics_radius = 0.1

odom = np.empty((N,1))
obs = np.empty((N,1))
x_true = np.empty((N,1))

landmark = 3
# Simulated readings of odometry and observations
x_true[0], odom[0], obs[0] = 0.0, 0.0, 2.9
x_true[1], odom[1], obs[1] = 1.0, 1.5, 2.0
x_true[2], odom[2], obs[2] = 2.0, 2.4, 1.0

hxDR = copy.deepcopy(odom)
# Visualization
plt.plot(landmark,0, '*k', markersize=30)
for i in range(N):
    plt.plot(odom[i], 0, '.', markersize=50, alpha=0.8, color='steelblue')
    plt.plot([odom[i], odom[i] + graphics_radius],
             [0,0], 'r')
    plt.text(odom[i], 0.02, "X_{}".format(i), fontsize=12)
    plt.plot(obs[i]+odom[i],0,'.', markersize=25, color='brown')
    plt.plot(x_true[i],0,'.g', markersize=20)
plt.grid()    
plt.show()
```

--------------------------------

### Forward Kinematics and Diagram Labeling Example

Source: https://github.com/atsushisakai/pythonrobotics/blob/master/docs/modules/7_arm_navigation/planar_two_link_ik_main.rst

Demonstrates the forward kinematics of a `TwoLinkArm` by updating its joint angles and plotting it. Includes a `label_diagram` function to annotate the arm with link lengths and joint angles, utilizing the `draw_angle` utility.

```ipython3
arm = TwoLinkArm()

theta0 = 0.5
theta1 = 1

arm.update_joints([theta0, theta1])
arm.plot()

def label_diagram():
    plt.plot([0, 0.5], [0, 0], 'k--')
    plt.plot([arm.elbow[0], arm.elbow[0]+0.5*cos(theta0)],
             [arm.elbow[1], arm.elbow[1]+0.5*sin(theta0)],
             'k--')
    
    draw_angle(theta0, r=0.25)
    draw_angle(theta1, offset=theta0, origin=[arm.elbow[0], arm.elbow[1]], r=0.25)
    
    plt.annotate("$l_0$", xy=(0.5, 0.4), size=15, color="r")
    plt.annotate("$l_1$", xy=(0.8, 1), size=15, color="r")
    
    plt.annotate(r"$\theta_0$", xy=(0.35, 0.05), size=15)
    plt.annotate(r"$\theta_1$", xy=(1, 0.8), size=15)

label_diagram()

plt.annotate("Shoulder", xy=(arm.shoulder[0], arm.shoulder[1]), xytext=(0.15, 0.5),

```

--------------------------------

### Bivariate Gaussian Distribution Setup

Source: https://github.com/atsushisakai/pythonrobotics/blob/master/docs/modules/12_appendix/Kalmanfilter_basics_main.rst

Initializes parameters for plotting a 2D Gaussian distribution. This code sets up the grid, mean vector, and covariance matrix for a bivariate normal distribution. Requires numpy and matplotlib.

```python
#Example from:
#https://scipython.com/blog/visualizing-the-bivariate-gaussian-distribution/
import numpy as np
import matplotlib.pyplot as plt
from matplotlib import cm
from mpl_toolkits.mplot3d import Axes3D

# Our 2-dimensional distribution will be over variables X and Y
N = 60
X = np.linspace(-3, 3, N)
Y = np.linspace(-3, 4, N)
X, Y = np.meshgrid(X, Y)

# Mean vector and covariance matrix
mu = np.array([0., 1.])
```

--------------------------------

### Selector Node Example

Source: https://github.com/atsushisakai/pythonrobotics/blob/master/docs/modules/13_mission_planning/behavior_tree/behavior_tree.md

Use Selector nodes to execute child tasks in order until one succeeds or all fail. Returns SUCCESS if any child succeeds, FAILURE if all fail, and RUNNING if a child is still executing.

```xml
<Selector>
    <Action1 />
    <Action2 />
</Selector>
```

--------------------------------

### Sequence Node Example

Source: https://github.com/atsushisakai/pythonrobotics/blob/master/docs/modules/13_mission_planning/behavior_tree/behavior_tree.md

Use Sequence nodes to execute child tasks in order until one fails or all succeed. Returns FAILURE if any child fails, SUCCESS if all succeed, and RUNNING if a child is still executing.

```xml
<Sequence>
    <Action1 />
    <Action2 />
</Sequence>
```

--------------------------------

### Potential Field Planning

Source: https://github.com/atsushisakai/pythonrobotics/blob/master/docs/modules/5_path_planning/grid_base_search/grid_base_search_main.rst

This function implements potential field planning on a grid. It calculates attractive and repulsive forces to guide a robot from a start to a goal position while avoiding obstacles.

```APIDOC
## potential_field_planning

### Description
Implements potential field planning on a grid.

### Parameters
- **start** (tuple): The starting position (row, col).
- **goal** (tuple): The goal position (row, col).
- **grid** (list of lists): A 2D list representing the grid, where 0 is traversable and 1 is an obstacle.
- **resolution** (float): The resolution of the grid.
- **max_force** (float): The maximum attractive force.
- **max_potential** (float): The maximum potential value.
- **obstacle_radius** (float): The radius around obstacles to consider for repulsion.
- **goal_radius** (float): The radius around the goal to consider for attraction.

### Returns
- **path** (list of tuples): A list of (row, col) tuples representing the planned path from start to goal.
- **potential_map** (list of lists): A 2D list representing the potential values on the grid.
```

--------------------------------

### Build Documentation

Source: https://github.com/atsushisakai/pythonrobotics/blob/master/docs/README.md

Builds the HTML documentation from the Sphinx source files. Navigate to the 'docs/' directory before running.

```bash
make html
```

--------------------------------

### Run Extended Kalman Filter Sample

Source: https://github.com/atsushisakai/pythonrobotics/blob/master/docs/modules/0_getting_started/2_how_to_run_sample_codes.md

Navigate to the Extended Kalman Filter sample directory and execute the Python script to visualize the algorithm.

```bash
>$ cd localization/extended_kalman_filter

    >$ python extended_kalman_filter.py
```

--------------------------------

### Run Extended Kalman Filter Sample

Source: https://github.com/atsushisakai/pythonrobotics/blob/master/docs/modules/0_getting_started/2_how_to_run_sample_codes_main.rst

Navigate to the Extended Kalman Filter sample directory and execute the Python script to visualize the EKF algorithm.

```bash
>$ cd localization/extended_kalman_filter

	>$ python extended_kalman_filter.py
```

--------------------------------

### Plotting a Gaussian Distribution

Source: https://github.com/atsushisakai/pythonrobotics/blob/master/docs/modules/12_appendix/Kalmanfilter_basics_main.rst

Visualizes a Gaussian (normal) distribution using matplotlib and scipy.stats.norm.pdf. Ensure numpy and matplotlib are installed.

```python
import matplotlib.mlab as mlab
import math
import scipy.stats

mu = 0
variance = 5
sigma = math.sqrt(variance)
x = np.linspace(mu - 5*sigma, mu + 5*sigma, 100)
plt.plot(x,scipy.stats.norm.pdf(x, mu, sigma))
plt.show()
```

--------------------------------

### Echo Node Example

Source: https://github.com/atsushisakai/pythonrobotics/blob/master/docs/modules/13_mission_planning/behavior_tree/behavior_tree.md

Use Echo nodes to print a message to the console. This node always returns SUCCESS after printing.

```xml
<Echo message="Hello, World!" />
```

--------------------------------

### Bidirectional A* Planner

Source: https://github.com/atsushisakai/pythonrobotics/blob/master/docs/modules/5_path_planning/grid_base_search/grid_base_search_main.rst

Implements a bidirectional A* algorithm for shortest path planning on a 2D grid, searching from both the start and goal simultaneously.

```APIDOC
## BidirectionalAStarPlanner

### Description
This function performs shortest path planning on a 2D grid using a bidirectional A* algorithm.

### Parameters
(No specific parameters are detailed in the source for direct user invocation, but the function is expected to operate on a grid structure.)

### Returns
(The return value is not explicitly detailed in the source.)

### Example
(No direct code example is provided in the source.)
```

--------------------------------

### Sleep Node Example

Source: https://github.com/atsushisakai/pythonrobotics/blob/master/docs/modules/13_mission_planning/behavior_tree/behavior_tree.md

Use Sleep nodes to pause execution for a specified duration. Returns SUCCESS once the duration has passed, or RUNNING if it has not.

```xml
<Sleep sec="1.5" />
```

--------------------------------

### Build Documentation Continuously

Source: https://github.com/atsushisakai/pythonrobotics/blob/master/docs/README.md

Builds the documentation and automatically rebuilds it whenever a file changes. This command should be run from the 'docs/' directory.

```bash
sphinx-autobuild . _build/html
```

--------------------------------

### calc_4points_bezier_path

Source: https://github.com/atsushisakai/pythonrobotics/blob/master/docs/modules/5_path_planning/bezier_path/bezier_path.md

Computes control points and the Bezier path given start and end positions, including yaw angles and an offset distance.

```APIDOC
## calc_4points_bezier_path

### Description
Compute control points and path given start and end position.

### Parameters
#### Path Parameters
- **sx** (float) - x-coordinate of the starting point
- **sy** (float) - y-coordinate of the starting point
- **syaw** (float) - yaw angle at start
- **ex** (float) - x-coordinate of the ending point
- **ey** (float) - y-coordinate of the ending point
- **eyaw** (float) - yaw angle at the end
- **offset** (float) - Offset distance from start and end points

### Returns
- (numpy array, numpy array) - Control points and the computed Bezier path.
```

--------------------------------

### Graph SLAM Simulation Parameters and Initialization

Source: https://github.com/atsushisakai/pythonrobotics/blob/master/docs/modules/4_slam/graph_slam/graphSLAM_doc.rst

Sets up simulation parameters, covariance matrices, and initial states for a 2D robot. It also simulates initial observations and stores historical data.

```python
#  Simulation parameter
Qsim = np.diag([0.01, np.deg2rad(0.010)])**2 # error added to range and bearing
Rsim = np.diag([0.1, np.deg2rad(1.0)])**2 # error added to [v, w]

DT = 2.0  # time tick [s]
SIM_TIME = 100.0  # simulation time [s]
MAX_RANGE = 30.0  # maximum observation range
STATE_SIZE = 3  # State size [x,y,yaw]

# TODO: Why not use Qsim ? 
# Covariance parameter of Graph Based SLAM
C_SIGMA1 = 0.1
C_SIGMA2 = 0.1
C_SIGMA3 = np.deg2rad(1.0)

MAX_ITR = 20  # Maximum iteration during optimization
timesteps = 1

# consider only 2 landmarks for simplicity
# RFID positions [x, y, yaw]
RFID = np.array([[10.0, -2.0, 0.0]])

# State Vector [x y yaw v]'
xTrue = np.zeros((STATE_SIZE, 1))
xDR = np.zeros((STATE_SIZE, 1))  # Dead reckoning
xTrue[2] = np.deg2rad(45)
xDR[2] = np.deg2rad(45)
# history initial values
hxTrue = xTrue
hxDR = xTrue
_, z, _, _ = observation(xTrue, xDR, np.array([[0,0]]).T, RFID)
hz = [z]

for i in range(timesteps):
    u = calc_input()
    xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFID)
    hxDR = np.hstack((hxDR, xDR))
    hxTrue = np.hstack((hxTrue, xTrue))
    hz.append(z)

# visualize
graphics_radius = 0.3
plt.plot(RFID[:, 0], RFID[:, 1], "*k", markersize=20)
plt.plot(hxDR[0, :], hxDR[1, :], '.', markersize=50, alpha=0.8, label='Odom')
plt.plot(hxTrue[0, :], hxTrue[1, :], '.', markersize=20, alpha=0.6, label='X_true')

for i in range(hxDR.shape[1]):
    x = hxDR[0, i]

```

--------------------------------

### prm_planning

Source: https://github.com/atsushisakai/pythonrobotics/blob/master/docs/modules/5_path_planning/prm_planner/prm_planner.md

Runs the Probabilistic Road Map planning algorithm to find a path from a start to a goal point, considering obstacles and robot radius.

```APIDOC
## prm_planning(start_x, start_y, goal_x, goal_y, obstacle_x_list, obstacle_y_list, robot_radius, rng=None)

### Description
Run probabilistic road map planning.

### Parameters
#### Path Parameters
- **start_x** (float) - start x position
- **start_y** (float) - start y position
- **goal_x** (float) - goal x position
- **goal_y** (float) - goal y position
- **obstacle_x_list** (list[float]) - obstacle x positions
- **obstacle_y_list** (list[float]) - obstacle y positions
- **robot_radius** (float) - robot radius
- **rng** (Optional[Any]) - Random generator

### Returns
(list[tuple[float, float]]) - The planned path as a list of (x, y) coordinates.
```

--------------------------------

### plan_dubins_path

Source: https://github.com/atsushisakai/pythonrobotics/blob/master/docs/modules/5_path_planning/dubins_path/dubins_path.md

Plans a Dubins path between a start and goal pose with a specified curvature constraint. It can optionally consider only a subset of the possible path types.

```APIDOC
## plan_dubins_path

### Description
Plans a Dubins path between a start and goal pose with a specified curvature constraint. It can optionally consider only a subset of the possible path types.

### Parameters
#### Path Parameters
- **s_x** (float) - Required - x position of the start point [m]
- **s_y** (float) - Required - y position of the start point [m]
- **s_yaw** (float) - Required - yaw angle of the start point [rad]
- **g_x** (float) - Required - x position of the goal point [m]
- **g_y** (float) - Required - y position of the end point [m]
- **g_yaw** (float) - Required - yaw angle of the end point [rad]
- **curvature** (float) - Required - curvature for curve [1/m]
- **step_size** (float) - Optional - step size between two path points [m]. Default is 0.1
- **selected_types** (list of string or None) - Optional - selected path planning types. If None, all types are used for path planning, and minimum path length result is returned. You can select used path plannings types by a string list. e.g.: ["RSL", "RSR"]

### Returns
- **x_list** (array) - x positions of the path
- **y_list** (array) - y positions of the path
- **yaw_list** (array) - yaw angles of the path
- **modes** (array) - mode list of the path
- **lengths** (array) - arrow_length list of the path segments.

### Example
```python
>>> import numpy as np
>>> import matplotlib.pyplot as plt
>>> from path_planning.dubins_path import plan_dubins_path
>>> # Assuming plot_arrow is defined elsewhere
>>> # from visualization import plot_arrow
>>> 
>>> start_x = 1.0  # [m]
>>> start_y = 1.0  # [m]
>>> start_yaw = np.deg2rad(45.0)  # [rad]
>>> end_x = -3.0  # [m]
>>> end_y = -3.0  # [m]
>>> end_yaw = np.deg2rad(-45.0)  # [rad]
>>> curvature = 1.0
>>> path_x, path_y, path_yaw, mode, _ = plan_dubins_path(
            start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature)
>>> plt.plot(path_x, path_y, label="final course " + "".join(mode))
>>> # plot_arrow(start_x, start_y, start_yaw)
>>> # plot_arrow(end_x, end_y, end_yaw)
>>> plt.legend()
>>> plt.grid(True)
>>> plt.axis("equal")
>>> plt.show()
```
```

--------------------------------

### Clone Repository and Navigate

Source: https://github.com/atsushisakai/pythonrobotics/blob/master/docs/modules/0_getting_started/2_how_to_run_sample_codes.md

Clone the PythonRobotics repository from GitHub and navigate into the project directory.

```bash
>$ git clone https://github.com/AtsushiSakai/PythonRobotics.git

>$ cd PythonRobotics
```

--------------------------------

### Initialize SymPy and NumPy for Equation Generation

Source: https://github.com/atsushisakai/pythonrobotics/blob/master/docs/modules/8_aerial_navigation/rocket_powered_landing/rocket_powered_landing_main.rst

Sets up SymPy for symbolic mathematics and NumPy for numerical operations, initializing SymPy's LaTeX printing.

```python
import sympy as sp
import numpy as np
from IPython.display import display
sp.init_printing(use_latex='mathjax')
```

--------------------------------

### Get Landmark Position from State

Source: https://github.com/atsushisakai/pythonrobotics/blob/master/docs/modules/4_slam/ekf_slam/ekf_slam_main.rst

Retrieves the position of a specific landmark from the augmented state vector. This is used to access individual landmark coordinates.

```python
    def get_LM_Pos_from_state(x, ind):
        """
        Returns the position of a given landmark

        :param x:   The state containing all landmark positions
        :param ind: landmark id
        :returns:   The position of the landmark
        """
        lm = x[STATE_SIZE + LM_SIZE * ind: STATE_SIZE + LM_SIZE * (ind + 1), :]

        return lm
```

--------------------------------

### Initialize and Populate Information Matrix and Vector

Source: https://github.com/atsushisakai/pythonrobotics/blob/master/docs/modules/4_slam/graph_slam/graphSLAM_doc.rst

Initializes the system's information matrix (H) and information vector (b), then iterates through edges to calculate Jacobians (A, B) and update H and b based on edge constraints and their associated uncertainties. Visualizes the matrix and vector before and after applying an origin constraint.

```python
import numpy as np
import copy
import matplotlib.pyplot as plt
import math

# Assuming STATE_SIZE, edges, hxDR, C_SIGMA1, C_SIGMA2, C_SIGMA3, calc_rotational_matrix, tangle1, tangle2, edge.yaw1, edge.angle1, edge.d1, edge.yaw2, edge.angle2, edge.omega, edge.e, n, hxTrue are defined elsewhere.

# Initialize the system information matrix and information vector
H = np.zeros((n, n))
b = np.zeros((n, 1))
x_opt = copy.deepcopy(hxDR)

for edge in edges:
    id1 = edge.id1 * STATE_SIZE
    id2 = edge.id2 * STATE_SIZE
    
    t1 = edge.yaw1 + edge.angle1
    A = np.array([[-1.0, 0, edge.d1 * math.sin(t1)],
                  [0, -1.0, -edge.d1 * math.cos(t1)],
                  [0, 0, -1.0]])

    t2 = edge.yaw2 + edge.angle2
    B = np.array([[1.0, 0, -edge.d2 * math.sin(t2)],
                  [0, 1.0, edge.d2 * math.cos(t2)],
                  [0, 0, 1.0]])
    
    # TODO: use Qsim instead of sigma
    sigma = np.diag([C_SIGMA1, C_SIGMA2, C_SIGMA3])
    Rt1 = calc_rotational_matrix(tangle1)
    Rt2 = calc_rotational_matrix(tangle2)
    edge.omega = np.linalg.inv(Rt1 @ sigma @ Rt1.T + Rt2 @ sigma @ Rt2.T)

    # Fill in entries in H and b
    H[id1:id1 + STATE_SIZE, id1:id1 + STATE_SIZE] += A.T @ edge.omega @ A
    H[id1:id1 + STATE_SIZE, id2:id2 + STATE_SIZE] += A.T @ edge.omega @ B
    H[id2:id2 + STATE_SIZE, id1:id1 + STATE_SIZE] += B.T @ edge.omega @ A
    H[id2:id2 + STATE_SIZE, id2:id2 + STATE_SIZE] += B.T @ edge.omega @ B

    b[id1:id1 + STATE_SIZE] += (A.T @ edge.omega @ edge.e)
    b[id2:id2 + STATE_SIZE] += (B.T @ edge.omega @ edge.e)
   

print("The determinant of H: ", np.linalg.det(H))
plt.figure()
plt.subplot(1,2,1)
plt.imshow(H, extent=[0, n, 0, n])
plt.subplot(1,2,2)
plt.imshow(b, extent=[0, 1, 0, n])
plt.show()

# Fix the origin, multiply by large number gives same results but better visualization
H[0:STATE_SIZE, 0:STATE_SIZE] += np.identity(STATE_SIZE)
print("The determinant of H after origin constraint: ", np.linalg.det(H))    
plt.figure()
plt.subplot(1,2,1)
plt.imshow(H, extent=[0, n, 0, n])
plt.subplot(1,2,2)
plt.imshow(b, extent=[0, 1, 0, n])
plt.show()
```

--------------------------------

### EKF SLAM Initialization and Parameters

Source: https://github.com/atsushisakai/pythonrobotics/blob/master/docs/modules/4_slam/ekf_slam/ekf_slam_main.rst

Initializes EKF state covariance, simulation parameters, and constants for EKF SLAM. Sets up simulation time, sensor noise, process noise, and state/landmark sizes.

```python
"""
    Extended Kalman Filter SLAM example
    original author: Atsushi Sakai (@Atsushi_twi)
    notebook author: Andrew Tu (drewtu2)
    """

    import math
    import numpy as np
    %matplotlib notebook
    import matplotlib.pyplot as plt


    # EKF state covariance
    Cx = np.diag([0.5, 0.5, np.deg2rad(30.0)])**2 # Change in covariance

    #  Simulation parameter
    Qsim = np.diag([0.2, np.deg2rad(1.0)])**2  # Sensor Noise
    Rsim = np.diag([1.0, np.deg2rad(10.0)])**2 # Process Noise

    DT = 0.1  # time tick [s]
    SIM_TIME = 50.0  # simulation time [s]
    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 state size [x,y]

    show_animation = True
```

--------------------------------

### Numpy Einsum Examples

Source: https://github.com/atsushisakai/pythonrobotics/blob/master/docs/modules/12_appendix/Kalmanfilter_basics.md

Demonstrates various uses of numpy's einsum function for array manipulation, including diagonal extraction and element-wise multiplication with summation.

```python
a = np.arange(25).reshape(5,5)
b = np.arange(5)
c = np.arange(6).reshape(2,3)
print(a)
print(b)
print(c)
```

```default
[[ 0  1  2  3  4]
 [ 5  6  7  8  9]
 [10 11 12 13 14]
 [15 16 17 18 19]
 [20 21 22 23 24]]
[0 1 2 3 4]
[[0 1 2]
 [3 4 5]]
```

```python
#this is the diagonal sum, i repeated means the diagonal
np.einsum('ij', a)
#this takes the output ii which is the diagonal and outputs to a
np.einsum('ii->i',a)
#this takes in the array A represented by their axes 'ij' and  B by its only axes'j'
#and multiples them element wise
np.einsum('ij,j',a, b)
```

```default
array([ 30,  80, 130, 180, 230])
```

```python
A = np.arange(3).reshape(3,1)
B = np.array([[ 0,  1,  2,  3],
              [ 4,  5,  6,  7],
              [ 8,  9, 10, 11]])
C=np.multiply(A,B)
np.sum(C,axis=1)
```

```default
array([ 0, 22, 76])
```

```python
D = np.array([0,1,2])
E = np.array([[ 0,  1,  2,  3],
              [ 4,  5,  6,  7],
              [ 8,  9, 10, 11]])

np.einsum('i,ij->i',D,E)
```

```default
array([ 0, 22, 76])
```

--------------------------------

### Load and Inspect Dataset

Source: https://github.com/atsushisakai/pythonrobotics/blob/master/docs/modules/4_slam/graph_slam/graphSLAM_SE2_example.rst

Loads the SE(2) dataset from a G2O file and prints the number of edges and vertices. This is the initial step before any processing.

```python
g = load_g2o_se2("data/input_INTEL.g2o")

print("Number of edges:    {}".format(len(g._edges)))
print("Number of vertices: {}".format(len(g._vertices)))
```

--------------------------------

### generate_clothoid_path

Source: https://github.com/atsushisakai/pythonrobotics/blob/master/docs/modules/5_path_planning/clothoid_path/clothoid_path.md

Generates a clothoid path between a start and goal pose with specified yaw angles. This function is useful for creating smooth, continuous paths for robotic applications.

```APIDOC
## generate_clothoid_path(start_point, start_yaw, goal_point, goal_yaw, n_path_points)

### Description
Generates a clothoid path list between a start and goal pose.

### Parameters
- **start_point** (tuple or list) - Start point of the path (x, y).
- **start_yaw** (float) - Orientation at the start point in radians.
- **goal_point** (tuple or list) - Goal point of the path (x, y).
- **goal_yaw** (float) - Orientation at the goal point in radians.
- **n_path_points** (int) - The desired number of points in the generated path.

### Returns
- list - A list of points representing the clothoid path.
```

--------------------------------

### Run PSO Path Planning with Visualization

Source: https://github.com/atsushisakai/pythonrobotics/blob/master/docs/modules/5_path_planning/particleSwarmOptimization/particleSwarmOptimization_main.rst

Executes the Particle Swarm Optimization path planning algorithm and displays the results visually. Ensure matplotlib is installed for visualization.

```python
import matplotlib.pyplot as plt
from PathPlanning.ParticleSwarmOptimization.particle_swarm_optimization import main

# Run PSO path planning with visualization
main()
```

--------------------------------

### Numpy Array Initialization

Source: https://github.com/atsushisakai/pythonrobotics/blob/master/docs/modules/12_appendix/Kalmanfilter_basics_main.rst

Initializes and prints three numpy arrays: a 5x5 array 'a', a 1D array 'b', and a 2x3 array 'c'. Used for demonstrating einsum operations.

```python
a = np.arange(25).reshape(5,5)
    b = np.arange(5)
    c = np.arange(6).reshape(2,3)
    print(a)
    print(b)
    print(c)
```