# Welcome to PythonRobotics’s documentation!¶

Python codes for robotics algorithm. The project is on GitHub.

This is a Python code collection of robotics algorithms.

Features:

1. Easy to read for understanding each algorithm’s basic idea.
2. Widely used and practical algorithms are selected.
3. Minimum dependency.

See this paper for more details:

## Getting Started¶

### What is this?¶

This is an Open Source Software (OSS) project: PythonRobotics, which is a Python code collection of robotics algorithms.

The focus of the project is on autonomous navigation, and the goal is for beginners in robotics to understand the basic ideas behind each algorithm.

In this project, the algorithms which are practical and widely used in both academia and industry are selected.

Each sample code is written in Python3 and only depends on some standard modules for readability and ease of use.

It includes intuitive animations to understand the behavior of the simulation.

See this paper for more details:

• Python 3.6.x
• numpy
• scipy
• matplotlib
• pandas
• cvxpy

### How to use¶

1. Install the required libraries. You can use environment.yml with conda command.
2. Clone this repo.
3. Execute python script in each directory.
4. Add star to this repo if you like it 😃.

## Localization¶

### Extended Kalman Filter Localization¶

from IPython.display import Image
Image(filename="ekf.png",width=600)  EKF

This is a sensor fusion localization with Extended Kalman Filter(EKF).

The blue line is true trajectory, the black line is dead reckoning trajectory,

the green point is positioning observation (ex. GPS), and the red line is estimated trajectory with EKF.

The red ellipse is estimated covariance ellipse with EKF.

#### Filter design¶

In this simulation, the robot has a state vector includes 4 states at time $$t$$.

$\textbf{x}_t=[x_t, y_t, \phi_t, v_t]$

x, y are a 2D x-y position, $$\phi$$ is orientation, and v is velocity.

In the code, “xEst” means the state vector. code

And, $$P_t$$ is covariace matrix of the state,

$$Q$$ is covariance matrix of process noise,

$$R$$ is covariance matrix of observation noise at time $$t$$

The robot has a speed sensor and a gyro sensor.

So, the input vecor can be used as each time step

$\textbf{u}_t=[v_t, \omega_t]$

Also, the robot has a GNSS sensor, it means that the robot can observe x-y position at each time.

$\textbf{z}_t=[x_t,y_t]$

The input and observation vector includes sensor noise.

In the code, “observation” function generates the input and observation vector with noise code

#### Motion Model¶

The robot model is

$\dot{x} = vcos(\phi)$
$\dot{y} = vsin((\phi)$
$\dot{\phi} = \omega$

So, the motion model is

$\textbf{x}_{t+1} = F\textbf{x}_t+B\textbf{u}_t$

where

$$\begin{equation*} F= \begin{bmatrix} 1 & 0 & 0 & 0\\ 0 & 1 & 0 & 0\\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 \\ \end{bmatrix} \end{equation*}$$

$$\begin{equation*} B= \begin{bmatrix} cos(\phi)dt & 0\\ sin(\phi)dt & 0\\ 0 & dt\\ 1 & 0\\ \end{bmatrix} \end{equation*}$$

$$dt$$ is a time interval.

This is implemented at code

Its Jacobian matrix is

$$\begin{equation*} J_F= \begin{bmatrix} \frac{dx}{dx}& \frac{dx}{dy} & \frac{dx}{d\phi} & \frac{dx}{dv}\\ \frac{dy}{dx}& \frac{dy}{dy} & \frac{dy}{d\phi} & \frac{dy}{dv}\\ \frac{d\phi}{dx}& \frac{d\phi}{dy} & \frac{d\phi}{d\phi} & \frac{d\phi}{dv}\\ \frac{dv}{dx}& \frac{dv}{dy} & \frac{dv}{d\phi} & \frac{dv}{dv}\\ \end{bmatrix} \end{equation*}$$

$$\begin{equation*} = \begin{bmatrix} 1& 0 & -v sin(\phi)dt & cos(\phi)dt\\ 0 & 1 & v cos(\phi)dt & sin(\phi) dt\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1\\ \end{bmatrix} \end{equation*}$$

#### Observation Model¶

The robot can get x-y position infomation from GPS.

So GPS Observation model is

$\textbf{z}_{t} = H\textbf{x}_t$

where

$$\begin{equation*} B= \begin{bmatrix} 1 & 0 & 0& 0\\ 0 & 1 & 0& 0\\ \end{bmatrix} \end{equation*}$$

Its Jacobian matrix is

$$\begin{equation*} J_H= \begin{bmatrix} \frac{dx}{dx}& \frac{dx}{dy} & \frac{dx}{d\phi} & \frac{dx}{dv}\\ \frac{dy}{dx}& \frac{dy}{dy} & \frac{dy}{d\phi} & \frac{dy}{dv}\\ \end{bmatrix} \end{equation*}$$

$$\begin{equation*} = \begin{bmatrix} 1& 0 & 0 & 0\\ 0 & 1 & 0 & 0\\ \end{bmatrix} \end{equation*}$$

#### Extented Kalman Filter¶

Localization process using Extendted Kalman Filter:EKF is

=== Predict ===

$$x_{Pred} = Fx_t+Bu_t$$

$$P_{Pred} = J_FP_t J_F^T + Q$$

=== Update ===

$$z_{Pred} = Hx_{Pred}$$

$$y = z - z_{Pred}$$

$$S = J_H P_{Pred}.J_H^T + R$$

$$K = P_{Pred}.J_H^T S^{-1}$$

$$x_{t+1} = x_{Pred} + Ky$$

$$P_{t+1} = ( I - K J_H) P_{Pred}$$

### 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.

Ref:

### Particle filter localization¶ This is a sensor fusion localization with Particle Filter(PF).

The blue line is true trajectory, the black line is dead reckoning trajectory,

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.

Ref:

### Histogram filter localization¶ This is a 2D localization example with Histogram filter.

The red cross is true position, black points are RFID positions.

The blue grid shows a position probability of histogram filter.

In this simulation, x,y are unknown, yaw is known.

The filter integrates speed input and range observations from RFID for localization.

Initial position is not needed.

Ref:

## Mapping¶

### Gaussian grid map¶

This is a 2D Gaussian grid mapping example. ### Ray casting grid map¶

This is a 2D ray casting grid mapping example. ### k-means object clustering¶

This is a 2D object clustering with k-means algorithm. ### Object shape recognition using circle fitting¶

This is an object shape recognition using circle fitting. The blue circle is the true object shape.

The red crosses are observations from a ranging sensor.

The red circle is the estimated object shape using circle fitting.

## SLAM¶

Simultaneous Localization and Mapping(SLAM) examples

### Iterative Closest Point (ICP) Matching¶

This is a 2D ICP matching example with singular value decomposition.

It can calculate a rotation matrix and a translation vector between points to points. Ref:

### EKF SLAM¶

This is an Extended Kalman Filter based SLAM example.

The blue line is ground truth, the black line is dead reckoning, the red line is the estimated trajectory with EKF SLAM.

The green crosses are estimated landmarks. Ref:

### FastSLAM1.0¶

from IPython.display import Image
Image(filename="animation.png",width=600) #### Simulation¶

This is a feature based SLAM example using FastSLAM 1.0.

The blue line is ground truth, the black line is dead reckoning, the red line is the estimated trajectory with FastSLAM.

The red points are particles of FastSLAM.

Black points are landmarks, blue crosses are estimated landmark positions by FastSLAM. gif

#### Introduction¶

FastSLAM algorithm implementation is based on particle filters and belongs to the family of probabilistic SLAM approaches. It is used with feature-based maps (see gif above) or with occupancy grid maps.

As it is shown, the particle filter differs from EKF by representing the robot’s estimation through a set of particles. Each single particle has an independent belief, as it holds the pose $$(x, y, \theta)$$ and an array of landmark locations $$[(x_1, y_1), (x_2, y_2), ... (x_n, y_n)]$$ for n landmarks.

• The blue line is the true trajectory
• The red line is the estimated trajectory
• The red dots represent the distribution of particles
• The black line represent dead reckoning tracjectory
• The blue x is the observed and estimated landmarks
• The black x is the true landmark

I.e. Each particle maintains a deterministic pose and n-EKFs for each landmark and update it with each measurement.

#### Algorithm walkthrough¶

The particles are initially drawn from a uniform distribution the represent the initial uncertainty. At each time step we do:

• Predict the pose for each particle by using $$u$$ and the motion model (the landmarks are not updated).
• Update the particles with observations $$z$$, where the weights are adjusted based on how likely the particle to have the correct pose given the sensor measurement
• Resampling such that the particles with the largest weights survive and the unlikely ones with the lowest weights die out.

#### 1- Predict¶

The following equations and code snippets we can see how the particles distribution evolves in case we provide only the control $$(v,w)$$, which are the linear and angular velocity repsectively.

$$\begin{equation*} F= \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{bmatrix} \end{equation*}$$

$$\begin{equation*} B= \begin{bmatrix} \Delta t cos(\theta) & 0\\ \Delta t sin(\theta) & 0\\ 0 & \Delta t \end{bmatrix} \end{equation*}$$

$$\begin{equation*} X = FX + BU \end{equation*}$$

$$\begin{equation*} \begin{bmatrix} x_{t+1} \\ y_{t+1} \\ \theta_{t+1} \end{bmatrix}= \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{bmatrix}\begin{bmatrix} x_{t} \\ y_{t} \\ \theta_{t} \end{bmatrix}+ \begin{bmatrix} \Delta t cos(\theta) & 0\\ \Delta t sin(\theta) & 0\\ 0 & \Delta t \end{bmatrix} \begin{bmatrix} v_{t} + \sigma_v\\ w_{t} + \sigma_w\\ \end{bmatrix} \end{equation*}$$

The following snippets playsback the recorded trajectory of each particle.

To get the insight of the motion model change the value of $$R$$ and re-run the cells again. As R is the parameters that indicates how much we trust that the robot executed the motion commands.

It is interesting to notice also that only motion will increase the uncertainty in the system as the particles start to spread out more. If observations are included the uncertainty will decrease and particles will converge to the correct estimate.

# CODE SNIPPET #
import numpy as np
import math
from copy import deepcopy
# Fast SLAM covariance

#  Simulation parameter
OFFSET_YAWRATE_NOISE = 0.01

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 srate size [x,y]
N_PARTICLE = 100  # number of particle
NTH = N_PARTICLE / 1.5  # Number of particle for re-sampling

class Particle:

def __init__(self, N_LM):
self.w = 1.0 / N_PARTICLE
self.x = 0.0
self.y = 0.0
self.yaw = 0.0
# landmark x-y positions
self.lm = np.zeros((N_LM, LM_SIZE))
# landmark position covariance
self.lmP = np.zeros((N_LM * LM_SIZE, LM_SIZE))

def motion_model(x, u):
F = np.array([[1.0, 0, 0],
[0, 1.0, 0],
[0, 0, 1.0]])

B = np.array([[DT * math.cos(x[2, 0]), 0],
[DT * math.sin(x[2, 0]), 0],
[0.0, DT]])
x = F @ x + B @ u

x[2, 0] = pi_2_pi(x[2, 0])
return x

def predict_particles(particles, u):
for i in range(N_PARTICLE):
px = np.zeros((STATE_SIZE, 1))
px[0, 0] = particles[i].x
px[1, 0] = particles[i].y
px[2, 0] = particles[i].yaw
ud = u + (np.random.randn(1, 2) @ R).T  # add noise
px = motion_model(px, ud)
particles[i].x = px[0, 0]
particles[i].y = px[1, 0]
particles[i].yaw = px[2, 0]

return particles

def pi_2_pi(angle):
return (angle + math.pi) % (2 * math.pi) - math.pi

# END OF SNIPPET

N_LM = 0
particles = [Particle(N_LM) for i in range(N_PARTICLE)]
time= 0.0
v = 1.0  # [m/s]
u = np.array([v, yawrate]).reshape(2, 1)
history = []
while SIM_TIME >= time:
time += DT
particles = predict_particles(particles, u)
history.append(deepcopy(particles))

# from IPython.html.widgets import *
from ipywidgets import *
import numpy as np
import matplotlib.pyplot as plt
%matplotlib inline

# playback the recorded motion of the particles
def plot_particles(t=0):
x = []
y = []
for i in range(len(history[t])):
x.append(history[t][i].x)
y.append(history[t][i].y)
plt.figtext(0.15,0.82,'t = ' + str(t))
plt.plot(x, y, '.r')
plt.axis([-20,20, -5,25])

interact(plot_particles, t=(0,len(history)-1,1));

interactive(children=(IntSlider(value=0, description='t', max=499), Output()), _dom_classes=('widget-interact'…


#### 2- Update¶

For the update step it is useful to observe a single particle and the effect of getting a new measurements on the weight of the particle.

As mentioned earlier, each particle maintains $$N$$ $$2x2$$ EKFs to estimate the landmarks, which includes the EKF process described in the EKF notebook. The difference is the change in the weight of the particle according to how likely the measurement is.

The weight is updated according to the following equation:

$$\begin{equation*} w_i = |2\pi Q|^{\frac{-1}{2}} exp\{\frac{-1}{2}(z_t - \hat z_i)^T Q^{-1}(z_t-\hat z_i)\} \end{equation*}$$

Where, $$w_i$$ is the computed weight, $$Q$$ is the measurement covariance, $$z_t$$ is the actual measurment and $$\hat z_i$$ is the predicted measurement of particle $$i$$.

To experiment this, a single particle is initialized then passed an initial measurement, which results in a relatively average weight. However, setting the particle coordinate to a wrong value to simulate wrong estimation will result in a very low weight. The lower the weight the less likely that this particle will be drawn during resampling and probably will die out.

# CODE SNIPPET #
def observation(xTrue, xd, u, RFID):

# calc true state
xTrue = motion_model(xTrue, u)

# add noise to range observation
z = np.zeros((3, 0))
for i in range(len(RFID[:, 0])):

dx = RFID[i, 0] - xTrue[0, 0]
dy = RFID[i, 1] - xTrue[1, 0]
d = math.sqrt(dx**2 + dy**2)
angle = pi_2_pi(math.atan2(dy, dx) - xTrue[2, 0])
if d <= MAX_RANGE:
dn = d + np.random.randn() * Qsim[0, 0]  # add noise
anglen = angle + np.random.randn() * Qsim[1, 1]  # add noise
zi = np.array([dn, pi_2_pi(anglen), i]).reshape(3, 1)
z = np.hstack((z, zi))

ud1 = u[0, 0] + np.random.randn() * Rsim[0, 0]
ud2 = u[1, 0] + np.random.randn() * Rsim[1, 1] + OFFSET_YAWRATE_NOISE
ud = np.array([ud1, ud2]).reshape(2, 1)

xd = motion_model(xd, ud)

return xTrue, z, xd, ud

def update_with_observation(particles, z):
for iz in range(len(z[0, :])):

lmid = int(z[2, iz])

for ip in range(N_PARTICLE):
# new landmark
if abs(particles[ip].lm[lmid, 0]) <= 0.01:
particles[ip] = add_new_lm(particles[ip], z[:, iz], Q)
# known landmark
else:
w = compute_weight(particles[ip], z[:, iz], Q)
particles[ip].w *= w
particles[ip] = update_landmark(particles[ip], z[:, iz], Q)

return particles

def compute_weight(particle, z, Q):
lm_id = int(z)
xf = np.array(particle.lm[lm_id, :]).reshape(2, 1)
Pf = np.array(particle.lmP[2 * lm_id:2 * lm_id + 2])
zp, Hv, Hf, Sf = compute_jacobians(particle, xf, Pf, Q)
dx = z[0:2].reshape(2, 1) - zp
dx[1, 0] = pi_2_pi(dx[1, 0])

try:
invS = np.linalg.inv(Sf)
except np.linalg.linalg.LinAlgError:
print("singuler")
return 1.0

num = math.exp(-0.5 * dx.T @ invS @ dx)
den = 2.0 * math.pi * math.sqrt(np.linalg.det(Sf))
w = num / den

return w

def compute_jacobians(particle, xf, Pf, Q):
dx = xf[0, 0] - particle.x
dy = xf[1, 0] - particle.y
d2 = dx**2 + dy**2
d = math.sqrt(d2)

zp = np.array(
[d, pi_2_pi(math.atan2(dy, dx) - particle.yaw)]).reshape(2, 1)

Hv = np.array([[-dx / d, -dy / d, 0.0],
[dy / d2, -dx / d2, -1.0]])

Hf = np.array([[dx / d, dy / d],
[-dy / d2, dx / d2]])

Sf = Hf @ Pf @ Hf.T + Q

return zp, Hv, Hf, Sf

r = z
b = z
lm_id = int(z)

s = math.sin(pi_2_pi(particle.yaw + b))
c = math.cos(pi_2_pi(particle.yaw + b))

particle.lm[lm_id, 0] = particle.x + r * c
particle.lm[lm_id, 1] = particle.y + r * s

# covariance
Gz = np.array([[c, -r * s],
[s, r * c]])

particle.lmP[2 * lm_id:2 * lm_id + 2] = Gz @ Q @ Gz.T

return particle

def update_KF_with_cholesky(xf, Pf, v, Q, Hf):
PHt = Pf @ Hf.T
S = Hf @ PHt + Q

S = (S + S.T) * 0.5
SChol = np.linalg.cholesky(S).T
SCholInv = np.linalg.inv(SChol)
W1 = PHt @ SCholInv
W = W1 @ SCholInv.T

x = xf + W @ v
P = Pf - W1 @ W1.T

return x, P

def update_landmark(particle, z, Q):

lm_id = int(z)
xf = np.array(particle.lm[lm_id, :]).reshape(2, 1)
Pf = np.array(particle.lmP[2 * lm_id:2 * lm_id + 2, :])

zp, Hv, Hf, Sf = compute_jacobians(particle, xf, Pf, Q)

dz = z[0:2].reshape(2, 1) - zp
dz[1, 0] = pi_2_pi(dz[1, 0])

xf, Pf = update_KF_with_cholesky(xf, Pf, dz, Q, Hf)

particle.lm[lm_id, :] = xf.T
particle.lmP[2 * lm_id:2 * lm_id + 2, :] = Pf

return particle

# END OF CODE SNIPPET #

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

# 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.w)

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

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

initial weight 1.0
weight after landmark intialization 1.0
weight after update  0.023098460073039763
weight after wrong prediction 7.951154575772496e-07


#### 3- Resampling¶

In the reseampling 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] with the weights of each particle distributed according to a Gaussian funciton.

The resampling picks

$$i \in 1,...,N$$ particles with probability to pick particle with index $$i ∝ \omega_i$$, where $$\omega_i$$ is the weight of that particle

To get the intuition of the resampling step we will look at a set of particles which are initialized with a given x location and weight. After the resampling the particles are more concetrated in the location where they had the highest weights. This is also indicated by the indices

# CODE SNIPPET #
def normalize_weight(particles):

sumw = sum([p.w for p in particles])

try:
for i in range(N_PARTICLE):
particles[i].w /= sumw
except ZeroDivisionError:
for i in range(N_PARTICLE):
particles[i].w = 1.0 / N_PARTICLE

return particles

return particles

def resampling(particles):
"""
low variance re-sampling
"""

particles = normalize_weight(particles)

pw = []
for i in range(N_PARTICLE):
pw.append(particles[i].w)

pw = np.array(pw)

Neff = 1.0 / (pw @ pw.T)  # Effective particle number
# print(Neff)

if Neff < NTH:  # resampling
wcum = np.cumsum(pw)
base = np.cumsum(pw * 0.0 + 1 / N_PARTICLE) - 1 / N_PARTICLE
resampleid = base + np.random.rand(base.shape) / N_PARTICLE

inds = []
ind = 0
for ip in range(N_PARTICLE):
while ((ind < wcum.shape - 1) and (resampleid[ip] > wcum[ind])):
ind += 1
inds.append(ind)

tparticles = particles[:]
for i in range(len(inds)):
particles[i].x = tparticles[inds[i]].x
particles[i].y = tparticles[inds[i]].y
particles[i].yaw = tparticles[inds[i]].yaw
particles[i].w = 1.0 / N_PARTICLE

return particles, inds
# END OF SNIPPET #

def gaussian(x, mu, sig):
return np.exp(-np.power(x - mu, 2.) / (2 * np.power(sig, 2.)))
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))
plt.show()

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

### Dynamic Window Approach¶

This is a 2D navigation sample code with Dynamic Window Approach. ### Model Predictive Trajectory Generator¶

This is a path optimization sample on model predictive trajectory generator.

This algorithm is used for state lattice planner.

#### Path optimization sample¶ #### Lookup table generation sample¶ Ref:

### State Lattice Planning¶

This script is a path planning code with state lattice planning.

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

Ref:

#### Uniform polar sampling¶ #### Biased polar sampling¶ #### Lane sampling¶  This PRM planner uses Dijkstra method for graph search.

In the animation, blue points are sampled points,

Cyan crosses means searched points with Dijkstra method,

The red line is the final path of PRM.

Ref: This Voronoi road-map planner uses Dijkstra method for graph search.

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.

Ref:

### Rapidly-Exploring Random Trees (RRT)¶

#### Basic RRT¶ This is a simple path planning code with Rapidly-Exploring Random Trees (RRT)

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

#### RRT*¶ This is a path planning code with RRT*

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

##### Simulation¶
from IPython.display import Image
Image(filename="Figure_1.png",width=600)  gif

#### RRT with dubins path¶ Path planning for a car robot with RRT and dubins path planner.

#### RRT* with dubins path¶ Path planning for a car robot with RRT* and dubins path planner.

#### RRT* with reeds-sheep path¶ Path planning for a car robot with RRT* and reeds sheep path planner.

#### Informed RRT*¶ This is a path planning code with Informed RRT*.

The cyan ellipse is the heuristic sampling domain of Informed RRT*.

Ref:

#### Batch Informed RRT*¶ This is a path planning code with Batch Informed RRT*.

Ref:

#### Closed Loop RRT*¶

A vehicle model based path planning with closed loop RRT*. In this code, pure-pursuit algorithm is used for steering control,

PID is used for speed control.

Ref:

#### LQR-RRT*¶

This is a path planning simulation with LQR-RRT*.

A double integrator motion model is used for LQR local planner. Ref:

### Cubic spline planning¶

A sample code for cubic path planning.

This code generates a curvature continuous path based on x-y waypoints with cubic spline.

Heading angle of each point can be also calculated analytically.   ### B-Spline planning¶ This is a path planning with B-Spline curse.

If you input waypoints, it generates a smooth path with B-Spline curve.

The final course should be on the first and last waypoints.

Ref:

### Eta^3 Spline path planning¶ This is a path planning with Eta^3 spline.

Ref:

### Bezier path planning¶

A sample code of Bezier path planning.

It is based on 4 control points Beier path. If you change the offset distance from start and end point,

You can get different Beizer course: Ref:

### Quintic polynomials planning¶

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

Ref:

### Dubins path planning¶

A sample code for Dubins path planning. Ref:

### Reeds Shepp planning¶

A sample code with Reeds Shepp path planning. Ref:

### LQR based path planning¶

A sample code using LQR based path planning for double integrator model. ### Optimal Trajectory in a Frenet Frame¶ This is optimal trajectory generation in a Frenet Frame.

The cyan line is the target course and black crosses are obstacles.

The red line is predicted path.

Ref:

## Path tracking¶

### move to a pose control¶

This is a simulation of moving to a pose control Ref:

### Pure pursuit tracking¶

Path tracking simulation with pure pursuit steering control and PID speed control. The red line is a target course, the green cross means the target point for pure pursuit control, the blue line is the tracking.

Ref:

### Stanley control¶

Path tracking simulation with Stanley steering control and PID speed control. Ref:

### Rear wheel feedback control¶

Path tracking simulation with rear wheel feedback steering control and PID speed control. Ref:

### Linear–quadratic regulator (LQR) steering control¶

Path tracking simulation with LQR steering control and PID speed control. Ref:

### Linear–quadratic regulator (LQR) speed and steering control¶

Path tracking simulation with LQR speed and steering control. Ref:

### Model predictive speed and steering control¶ Model predictive speed and steering control

code:

PythonRobotics/model_predictive_speed_and_steer_control.py at master · AtsushiSakai/PythonRobotics

This is a path tracking simulation using model predictive control (MPC).

The MPC controller controls vehicle speed and steering base on linealized model.

This code uses cvxpy as an optimization modeling tool.

#### MPC modeling¶

State vector is:

$z = [x, y, v,\phi]$

x: x-position, y:y-position, v:velocity, φ: yaw angle

Input vector is:

$u = [a, \delta]$

a: accellation, δ: steering angle

The MPC cotroller minimize this cost function for path tracking:

$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.

subject to:

• Linearlied vehicle model
$z_{t+1}=Az_t+Bu+C$
• Maximum steering speed
$|u_{t+1}-u_{t}|<du_{max}$
• Maximum steering angle
$|u_{t}|<u_{max}$
• Initial state
$z_0 = z_{0,ob}$
• Maximum and minimum speed
$v_{min} < v_t < v_{max}$
• Maximum and minimum input
$u_{min} < u_t < u_{max}$

This is implemented at

PythonRobotics/model_predictive_speed_and_steer_control.py at f51a73f47cb922a12659f8ce2d544c347a2a8156 · AtsushiSakai/PythonRobotics

#### Vehicle model linearization¶

Vehicle model is

$\dot{x} = vcos(\phi)$
$\dot{y} = vsin((\phi)$
$\dot{v} = a$
$\dot{\phi} = \frac{vtan(\delta)}{L}$

ODE is

$\dot{z} =\frac{\partial }{\partial z} z = f(z, u) = A'z+B'u$

where

$$\begin{equation*} A' = \begin{bmatrix} \frac{\partial }{\partial x}vcos(\phi) & \frac{\partial }{\partial y}vcos(\phi) & \frac{\partial }{\partial v}vcos(\phi) & \frac{\partial }{\partial \phi}vcos(\phi)\\ \frac{\partial }{\partial x}vsin(\phi) & \frac{\partial }{\partial y}vsin(\phi) & \frac{\partial }{\partial v}vsin(\phi) & \frac{\partial }{\partial \phi}vsin(\phi)\\ \frac{\partial }{\partial x}a& \frac{\partial }{\partial y}a& \frac{\partial }{\partial v}a& \frac{\partial }{\partial \phi}a\\ \frac{\partial }{\partial x}\frac{vtan(\delta)}{L}& \frac{\partial }{\partial y}\frac{vtan(\delta)}{L}& \frac{\partial }{\partial v}\frac{vtan(\delta)}{L}& \frac{\partial }{\partial \phi}\frac{vtan(\delta)}{L}\\ \end{bmatrix} \\ = \begin{bmatrix} 0 & 0 & cos(\bar{\phi}) & -\bar{v}sin(\bar{\phi})\\ 0 & 0 & sin(\bar{\phi}) & \bar{v}cos(\bar{\phi}) \\ 0 & 0 & 0 & 0 \\ 0 & 0 &\frac{tan(\bar{\delta})}{L} & 0 \\ \end{bmatrix} \end{equation*}$$

$$\begin{equation*} B' = \begin{bmatrix} \frac{\partial }{\partial a}vcos(\phi) & \frac{\partial }{\partial \delta}vcos(\phi)\\ \frac{\partial }{\partial a}vsin(\phi) & \frac{\partial }{\partial \delta}vsin(\phi)\\ \frac{\partial }{\partial a}a & \frac{\partial }{\partial \delta}a\\ \frac{\partial }{\partial a}\frac{vtan(\delta)}{L} & \frac{\partial }{\partial \delta}\frac{vtan(\delta)}{L}\\ \end{bmatrix} \\ = \begin{bmatrix} 0 & 0 \\ 0 & 0 \\ 1 & 0 \\ 0 & \frac{\bar{v}}{Lcos^2(\bar{\delta})} \\ \end{bmatrix} \end{equation*}$$

You can get a discrete-time mode with Forward Euler Discretization with sampling time dt.

$z_{k+1}=z_k+f(z_k,u_k)dt$

Using first degree Tayer expantion around zbar and ubar

$z_{k+1}=z_k+(f(\bar{z},\bar{u})+A'z_k+B'u_k-A'\bar{z}-B'\bar{u})dt$
$z_{k+1}=(I + dtA')z_k+(dtB')u_k + (f(\bar{z},\bar{u})-A'\bar{z}-B'\bar{u})dt$

So,

$z_{k+1}=Az_k+Bu_k +C$

where,

$$\begin{equation*} A = (I + dtA')\\ = \begin{bmatrix} 1 & 0 & cos(\bar{\phi})dt & -\bar{v}sin(\bar{\phi})dt\\ 0 & 1 & sin(\bar{\phi})dt & \bar{v}cos(\bar{\phi})dt \\ 0 & 0 & 1 & 0 \\ 0 & 0 &\frac{tan(\bar{\delta})}{L}dt & 1 \\ \end{bmatrix} \end{equation*}$$

$$\begin{equation*} B = dtB'\\ = \begin{bmatrix} 0 & 0 \\ 0 & 0 \\ dt & 0 \\ 0 & \frac{\bar{v}}{Lcos^2(\bar{\delta})}dt \\ \end{bmatrix} \end{equation*}$$

$$\begin{equation*} C = (f(\bar{z},\bar{u})-A'\bar{z}-B'\bar{u})dt\\ = dt( \begin{bmatrix} \bar{v}cos(\bar{\phi})\\ \bar{v}sin(\bar{\phi}) \\ \bar{a}\\ \frac{\bar{v}tan(\bar{\delta})}{L}\\ \end{bmatrix} - \begin{bmatrix} \bar{v}cos(\bar{\phi})-\bar{v}sin(\bar{\phi})\bar{\phi}\\ \bar{v}sin(\bar{\phi})+\bar{v}cos(\bar{\phi})\bar{\phi}\\ 0\\ \frac{\bar{v}tan(\bar{\delta})}{L}\\ \end{bmatrix} - \begin{bmatrix} 0\\ 0 \\ \bar{a}\\ \frac{\bar{v}\bar{\delta}}{Lcos^2(\bar{\delta})}\\ \end{bmatrix} )\\ = \begin{bmatrix} \bar{v}sin(\bar{\phi})\bar{\phi}dt\\ -\bar{v}cos(\bar{\phi})\bar{\phi}dt\\ 0\\ -\frac{\bar{v}\bar{\delta}}{Lcos^2(\bar{\delta})}dt\\ \end{bmatrix} \end{equation*}$$

This equation is implemented at

PythonRobotics/model_predictive_speed_and_steer_control.py at eb6d1cbe6fc90c7be9210bf153b3a04f177cc138 · AtsushiSakai/PythonRobotics

Ref:

### Nonlinear Model Predictive Control with C-GMRES¶

from IPython.display import Image
Image(filename="Figure_4.png",width=600) Image(filename="Figure_1.png",width=600) Image(filename="Figure_2.png",width=600) Image(filename="Figure_3.png",width=600)  gif

#### Mathematical Formulation¶

Motion model is

$\dot{x}=vcos\theta$
$\dot{y}=vsin\theta$
$\dot{\theta}=\frac{v}{WB}sin(u_{\delta})$

(tan is not good for optimization)

$\dot{v}=u_a$

Cost function is

$J=\frac{1}{2}(u_a^2+u_{\delta}^2)-\phi_a d_a-\phi_\delta d_\delta$

Input constraints are

$|u_a| \leq u_{a,max}$
$|u_\delta| \leq u_{\delta,max}$

So, Hamiltonian　is

$\begin{split}J=\frac{1}{2}(u_a^2+u_{\delta}^2)-\phi_a d_a-\phi_\delta d_\delta\\ +\lambda_1vcos\theta+\lambda_2vsin\theta+\lambda_3\frac{v}{WB}sin(u_{\delta})+\lambda_4u_a\\ +\rho_1(u_a^2+d_a^2+u_{a,max}^2)+\rho_2(u_\delta^2+d_\delta^2+u_{\delta,max}^2)\end{split}$

Partial differential equations of the Hamiltonian are:

$$\begin{equation*} \frac{\partial H}{\partial \bf{x}}=\\ \begin{bmatrix} \frac{\partial H}{\partial x}= 0&\\ \frac{\partial H}{\partial y}= 0&\\ \frac{\partial H}{\partial \theta}= -\lambda_1vsin\theta+\lambda_2vcos\theta&\\ \frac{\partial H}{\partial v}=-\lambda_1cos\theta+\lambda_2sin\theta+\lambda_3\frac{1}{WB}sin(u_{\delta})&\\ \end{bmatrix} \\ \end{equation*}$$

$$\begin{equation*} \frac{\partial H}{\partial \bf{u}}=\\ \begin{bmatrix} \frac{\partial H}{\partial u_a}= u_a+\lambda_4+2\rho_1u_a&\\ \frac{\partial H}{\partial u_\delta}= u_\delta+\lambda_3\frac{v}{WB}cos(u_{\delta})+2\rho_2u_\delta&\\ \frac{\partial H}{\partial d_a}= -\phi_a+2\rho_1d_a&\\ \frac{\partial H}{\partial d_\delta}=-\phi_\delta+2\rho_2d_\delta&\\ \end{bmatrix} \\ \end{equation*}$$

### Two joint arm to point control¶ TwoJointArmToPointControl

This is two joint arm to a point control simulation.

This is a interactive simulation.

You can set the goal position of the end effector with left-click on the ploting area.

### N joint arm to point control¶

N joint arm to a point control simulation.

This is a interactive simulation.

You can set the goal position of the end effector with left-click on the ploting area. In this simulation N = 10, however, you can change it.

### Arm navigation with obstacle avoidance¶

Arm navigation with obstacle avoidance simulation. ### Drone 3d trajectory following¶

This is a 3d trajectory following simulation for a quadrotor. ### rocket powered landing¶

#### Simulation¶

from IPython.display import Image
Image(filename="figure.png",width=600)
from IPython.display import display, HTML

display(HTML(data="""
<style>
div#notebook-container    { width: 95%; }
div#maintoolbar-container { width: 99%; }
</style>
""")) gif

#### Equation generation¶

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

# parameters
# Angular moment of inertia
J_B = 1e-2 * np.diag([1., 1., 1.])

# Gravity
g_I = np.array((-1, 0., 0.))

# Fuel consumption
alpha_m = 0.01

# Vector from thrust point to CoM
r_T_B = np.array([-1e-2, 0., 0.])

def dir_cosine(q):
return np.matrix([
[1 - 2 * (q ** 2 + q ** 2), 2 * (q * q +
q * q), 2 * (q * q - q * q)],
[2 * (q * q - q * q), 1 - 2 *
(q ** 2 + q ** 2), 2 * (q * q + q * q)],
[2 * (q * q + q * q), 2 * (q * q -
q * q), 1 - 2 * (q ** 2 + q ** 2)]
])

def omega(w):
return np.matrix([
[0, -w, -w, -w],
[w, 0, w, -w],
[w, -w, 0, w],
[w, w, -w, 0],
])

def skew(v):
return np.matrix([
[0, -v, v],
[v, 0, -v],
[-v, v, 0]
])

f = sp.zeros(14, 1)

x = sp.Matrix(sp.symbols(
'm rx ry rz vx vy vz q0 q1 q2 q3 wx wy wz', real=True))
u = sp.Matrix(sp.symbols('ux uy uz', real=True))

g_I = sp.Matrix(g_I)
r_T_B = sp.Matrix(r_T_B)
J_B = sp.Matrix(J_B)

C_B_I = dir_cosine(x[7:11, 0])
C_I_B = C_B_I.transpose()

f[0, 0] = - alpha_m * u.norm()
f[1:4, 0] = x[4:7, 0]
f[4:7, 0] = 1 / x[0, 0] * C_I_B * u + g_I
f[7:11, 0] = 1 / 2 * omega(x[11:14, 0]) * x[7: 11, 0]
f[11:14, 0] = J_B ** -1 * \
(skew(r_T_B) * u - skew(x[11:14, 0]) * J_B * x[11:14, 0])

display(sp.simplify(f)) # f

$\begin{split}\left[\begin{matrix}- 0.01 \sqrt{ux^{2} + uy^{2} + uz^{2}}\\vx\\vy\\vz\\\frac{- 1.0 m - ux \left(2 q_{2}^{2} + 2 q_{3}^{2} - 1\right) - 2 uy \left(q_{0} q_{3} - q_{1} q_{2}\right) + 2 uz \left(q_{0} q_{2} + q_{1} q_{3}\right)}{m}\\\frac{2 ux \left(q_{0} q_{3} + q_{1} q_{2}\right) - uy \left(2 q_{1}^{2} + 2 q_{3}^{2} - 1\right) - 2 uz \left(q_{0} q_{1} - q_{2} q_{3}\right)}{m}\\\frac{- 2 ux \left(q_{0} q_{2} - q_{1} q_{3}\right) + 2 uy \left(q_{0} q_{1} + q_{2} q_{3}\right) - uz \left(2 q_{1}^{2} + 2 q_{2}^{2} - 1\right)}{m}\\- 0.5 q_{1} wx - 0.5 q_{2} wy - 0.5 q_{3} wz\\0.5 q_{0} wx + 0.5 q_{2} wz - 0.5 q_{3} wy\\0.5 q_{0} wy - 0.5 q_{1} wz + 0.5 q_{3} wx\\0.5 q_{0} wz + 0.5 q_{1} wy - 0.5 q_{2} wx\\0\\1.0 uz\\- 1.0 uy\end{matrix}\right]\end{split}$
display(sp.simplify(f.jacobian(x)))# A

$\begin{split}\left[\begin{array}{cccccccccccccc}0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\frac{ux \left(2 q_{2}^{2} + 2 q_{3}^{2} - 1\right) + 2 uy \left(q_{0} q_{3} - q_{1} q_{2}\right) - 2 uz \left(q_{0} q_{2} + q_{1} q_{3}\right)}{m^{2}} & 0 & 0 & 0 & 0 & 0 & 0 & \frac{2 \left(q_{2} uz - q_{3} uy\right)}{m} & \frac{2 \left(q_{2} uy + q_{3} uz\right)}{m} & \frac{2 \left(q_{0} uz + q_{1} uy - 2 q_{2} ux\right)}{m} & \frac{2 \left(- q_{0} uy + q_{1} uz - 2 q_{3} ux\right)}{m} & 0 & 0 & 0\\\frac{- 2 ux \left(q_{0} q_{3} + q_{1} q_{2}\right) + uy \left(2 q_{1}^{2} + 2 q_{3}^{2} - 1\right) + 2 uz \left(q_{0} q_{1} - q_{2} q_{3}\right)}{m^{2}} & 0 & 0 & 0 & 0 & 0 & 0 & \frac{2 \left(- q_{1} uz + q_{3} ux\right)}{m} & \frac{2 \left(- q_{0} uz - 2 q_{1} uy + q_{2} ux\right)}{m} & \frac{2 \left(q_{1} ux + q_{3} uz\right)}{m} & \frac{2 \left(q_{0} ux + q_{2} uz - 2 q_{3} uy\right)}{m} & 0 & 0 & 0\\\frac{2 ux \left(q_{0} q_{2} - q_{1} q_{3}\right) - 2 uy \left(q_{0} q_{1} + q_{2} q_{3}\right) + uz \left(2 q_{1}^{2} + 2 q_{2}^{2} - 1\right)}{m^{2}} & 0 & 0 & 0 & 0 & 0 & 0 & \frac{2 \left(q_{1} uy - q_{2} ux\right)}{m} & \frac{2 \left(q_{0} uy - 2 q_{1} uz + q_{3} ux\right)}{m} & \frac{2 \left(- q_{0} ux - 2 q_{2} uz + q_{3} uy\right)}{m} & \frac{2 \left(q_{1} ux + q_{2} uy\right)}{m} & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & - 0.5 wx & - 0.5 wy & - 0.5 wz & - 0.5 q_{1} & - 0.5 q_{2} & - 0.5 q_{3}\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0.5 wx & 0 & 0.5 wz & - 0.5 wy & 0.5 q_{0} & - 0.5 q_{3} & 0.5 q_{2}\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0.5 wy & - 0.5 wz & 0 & 0.5 wx & 0.5 q_{3} & 0.5 q_{0} & - 0.5 q_{1}\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0.5 wz & 0.5 wy & - 0.5 wx & 0 & - 0.5 q_{2} & 0.5 q_{1} & 0.5 q_{0}\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\end{array}\right]\end{split}$
sp.simplify(f.jacobian(u)) # B

$\begin{split}\left[\begin{matrix}- \frac{0.01 ux}{\sqrt{ux^{2} + uy^{2} + uz^{2}}} & - \frac{0.01 uy}{\sqrt{ux^{2} + uy^{2} + uz^{2}}} & - \frac{0.01 uz}{\sqrt{ux^{2} + uy^{2} + uz^{2}}}\\0 & 0 & 0\\0 & 0 & 0\\0 & 0 & 0\\\frac{- 2 q_{2}^{2} - 2 q_{3}^{2} + 1}{m} & \frac{2 \left(- q_{0} q_{3} + q_{1} q_{2}\right)}{m} & \frac{2 \left(q_{0} q_{2} + q_{1} q_{3}\right)}{m}\\\frac{2 \left(q_{0} q_{3} + q_{1} q_{2}\right)}{m} & \frac{- 2 q_{1}^{2} - 2 q_{3}^{2} + 1}{m} & \frac{2 \left(- q_{0} q_{1} + q_{2} q_{3}\right)}{m}\\\frac{2 \left(- q_{0} q_{2} + q_{1} q_{3}\right)}{m} & \frac{2 \left(q_{0} q_{1} + q_{2} q_{3}\right)}{m} & \frac{- 2 q_{1}^{2} - 2 q_{2}^{2} + 1}{m}\\0 & 0 & 0\\0 & 0 & 0\\0 & 0 & 0\\0 & 0 & 0\\0 & 0 & 0\\0 & 0 & 1.0\\0 & -1.0 & 0\end{matrix}\right]\end{split}$

#### Ref¶

• Python implementation of ‘Successive Convexification for 6-DoF Mars Rocket Powered Landing with Free-Final-Time’ paper by Michael Szmuk and Behçet Açıkmeşe.
• inspired by EmbersArc/SuccessiveConvexificationFreeFinalTime: Implementation of “Successive Convexification for 6-DoF Mars Rocket Powered Landing with Free-Final-Time” https://github.com/EmbersArc/SuccessiveConvexificationFreeFinalTime

## Appendix¶

### KF Basics - Part I¶

#### Introduction¶

##### What is the need to describe belief in terms of PDF’s?¶

This is because robot environments are stochastic. A robot environment may have cows with Tesla by side. That is a robot and it’s environment cannot be deterministically modelled(e.g as a function of something like time t). In the real world sensors are also error prone, and hence there’ll be a set of values with a mean and variance that it can take. Hence, we always have to model around some mean and variances associated.

##### What is Expectation of a Random Variables?¶

Expectation is nothing but an average of the probabilites

$\mathbb E[X] = \sum_{i=1}^n p_ix_i$

In the continous form,

$\mathbb E[X] = \int_{-\infty}^\infty x\, f(x) \,dx$
import numpy as np
import random
x=[3,1,2]
p=[0.1,0.3,0.4]
E_x=np.sum(np.multiply(x,p))
print(E_x)

1.4000000000000001

##### What is the advantage of representing the belief as a unimodal as opposed to multimodal?¶

Obviously, it makes sense because we can’t multiple probabilities to a car moving for two locations. This would be too confusing and the information will not be useful.

#### Variance, Covariance and Correlation¶

##### Variance¶

Variance is the spread of the data. The mean does’nt tell much about the data. Therefore the variance tells us about the story about the data meaning the spread of the data.

$\mathit{VAR}(X) = \frac{1}{n}\sum_{i=1}^n (x_i - \mu)^2$
x=np.random.randn(10)
np.var(x)

1.0224618077401504

##### Covariance¶

This is for a multivariate distribution. For example, a robot in 2-D space can take values in both x and y. To describe them, a normal distribution with mean in both x and y is needed.

For a multivariate distribution, mean $$\mu$$ can be represented as a matrix,

$\begin{split}\mu = \begin{bmatrix}\mu_1\\\mu_2\\ \vdots \\\mu_n\end{bmatrix}\end{split}$

Similarly, variance can also be represented.

But an important concept is that in the same way as every variable or dimension has a variation in its values, it is also possible that there will be values on how they together vary. This is also a measure of how two datasets are related to each other or correlation.

For example, as height increases weight also generally increases. These variables are correlated. They are positively correlated because as one variable gets larger so does the other.

We use a covariance matrix to denote covariances of a multivariate normal distribution:

$\begin{split}\Sigma = \begin{bmatrix} \sigma_1^2 & \sigma_{12} & \cdots & \sigma_{1n} \\ \sigma_{21} &\sigma_2^2 & \cdots & \sigma_{2n} \\ \vdots & \vdots & \ddots & \vdots \\ \sigma_{n1} & \sigma_{n2} & \cdots & \sigma_n^2 \end{bmatrix}\end{split}$

Diagonal - Variance of each variable associated.

Off-Diagonal - covariance between ith and jth variables.

\begin{split}\begin{aligned}VAR(X) = \sigma_x^2 &= \frac{1}{n}\sum_{i=1}^n(X - \mu)^2\\ COV(X, Y) = \sigma_{xy} &= \frac{1}{n}\sum_{i=1}^n[(X-\mu_x)(Y-\mu_y)\big]\end{aligned}\end{split}
x=np.random.random((3,3))
np.cov(x)

array([[0.08868895, 0.05064471, 0.08855629],
[0.05064471, 0.06219243, 0.11555291],
[0.08855629, 0.11555291, 0.21534324]])


Covariance taking the data as sample with $$\frac{1}{N-1}$$

x_cor=np.random.rand(1,10)
y_cor=np.random.rand(1,10)
np.cov(x_cor,y_cor)

array([[ 0.1571437 , -0.00766623],
[-0.00766623,  0.13957621]])


Covariance taking the data as population with $$\frac{1}{N}$$

np.cov(x_cor,y_cor,bias=1)

array([[ 0.14142933, -0.0068996 ],
[-0.0068996 ,  0.12561859]])


#### Gaussians¶

##### Central Limit Theorem¶

According to this theorem, the average of n samples of random and independent variables tends to follow a normal distribution as we increase the sample size.(Generally, for n>=30)

import matplotlib.pyplot as plt
import random
a=np.zeros((100,))
for i in range(100):
x=[random.uniform(1,10) for _ in range(1000)]
a[i]=np.sum(x,axis=0)/1000
plt.hist(a)

(array([ 1.,  4.,  9., 12., 12., 20., 16., 16.,  4.,  6.]),
array([5.30943011, 5.34638597, 5.38334183, 5.42029769, 5.45725355,
5.49420941, 5.53116527, 5.56812114, 5.605077  , 5.64203286,
5.67898872]),
<a list of 10 Patch objects>) ##### Gaussian Distribution¶

A Gaussian is a continuous probability distribution that is completely described with two parameters, the mean ($$\mu$$) and the variance ($$\sigma^2$$). It is defined as:

\begin{align}\begin{aligned} f(x, \mu, \sigma) = \frac{1}{\sigma\sqrt{2\pi}} \exp\big [{-\frac{(x-\mu)^2}{2\sigma^2} }\big ]\\Range is\end{aligned}\end{align}
$[-\inf,\inf]$

This is just a function of mean($$\mu$$) and standard deviation ($$\sigma$$) and what gives the normal distribution the charecteristic bell curve.

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() ##### Why do we need Gaussian distributions?¶

Since it becomes really difficult in the real world to deal with multimodal distribution as we cannot put the belief in two seperate location of the robots. This becomes really confusing and in practice impossible to comprehend. Gaussian probability distribution allows us to drive the robots using only one mode with peak at the mean with some variance.

#### Gaussian Properties¶

Multiplication

For the measurement update in a Bayes Filter, the algorithm tells us to multiply the Prior P(X_t) and measurement P(Z_t|X_t) to calculate the posterior:

$P(X \mid Z) = \frac{P(Z \mid X)P(X)}{P(Z)}$

Here for the numerator, $$P(Z \mid X),P(X)$$ both are gaussian.

$$N(\bar\mu, \bar\sigma^1)$$ and $$N(\bar\mu, \bar\sigma^2)$$ are their mean and variances.

New mean is

\begin{align}\begin{aligned}\mu_\mathtt{new} = \frac{\sigma_z^2\bar\mu + \bar\sigma^2z}{\bar\sigma^2+\sigma_z^2}\\New variance is\end{aligned}\end{align}
$\sigma_\mathtt{new} = \frac{\sigma_z^2\bar\sigma^2}{\bar\sigma^2+\sigma_z^2}$
import matplotlib.mlab as mlab
import math
mu1 = 0
variance1 = 2
sigma = math.sqrt(variance1)
x1 = np.linspace(mu1 - 3*sigma, mu1 + 3*sigma, 100)
plt.plot(x1,scipy.stats.norm.pdf(x1, mu1, sigma),label='prior')

mu2 = 10
variance2 = 2
sigma = math.sqrt(variance2)
x2 = np.linspace(mu2 - 3*sigma, mu2 + 3*sigma, 100)
plt.plot(x2,scipy.stats.norm.pdf(x2, mu2, sigma),"g-",label='measurement')

mu_new=(mu1*variance2+mu2*variance1)/(variance1+variance2)
print("New mean is at: ",mu_new)
var_new=(variance1*variance2)/(variance1+variance2)
print("New variance is: ",var_new)
sigma = math.sqrt(var_new)
x3 = np.linspace(mu_new - 3*sigma, mu_new + 3*sigma, 100)
plt.plot(x3,scipy.stats.norm.pdf(x3, mu_new, var_new),label="posterior")
plt.legend(loc='upper left')
plt.xlim(-10,20)
plt.show()

New mean is at:  5.0
New variance is:  1.0 The motion step involves a case of adding up probability (Since it has to abide the Law of Total Probability). This means their beliefs are to be added and hence two gaussians. They are simply arithmetic additions of the two.

$\begin{split}\begin{gathered}\mu_x = \mu_p + \mu_z \\ \sigma_x^2 = \sigma_z^2+\sigma_p^2\, \square\end{gathered}\end{split}$
import matplotlib.mlab as mlab
import math
mu1 = 5
variance1 = 1
sigma = math.sqrt(variance1)
x1 = np.linspace(mu1 - 3*sigma, mu1 + 3*sigma, 100)
plt.plot(x1,scipy.stats.norm.pdf(x1, mu1, sigma),label='prior')

mu2 = 10
variance2 = 1
sigma = math.sqrt(variance2)
x2 = np.linspace(mu2 - 3*sigma, mu2 + 3*sigma, 100)
plt.plot(x2,scipy.stats.norm.pdf(x2, mu2, sigma),"g-",label='measurement')

mu_new=mu1+mu2
print("New mean is at: ",mu_new)
var_new=(variance1+variance2)
print("New variance is: ",var_new)
sigma = math.sqrt(var_new)
x3 = np.linspace(mu_new - 3*sigma, mu_new + 3*sigma, 100)
plt.plot(x3,scipy.stats.norm.pdf(x3, mu_new, var_new),label="posterior")
plt.legend(loc='upper left')
plt.xlim(-10,20)
plt.show()

New mean is at:  15
New variance is:  2 #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]])

# Pack X and Y into a single 3-dimensional array
pos = np.empty(X.shape + (2,))
pos[:, :, 0] = X
pos[:, :, 1] = Y

def multivariate_gaussian(pos, mu, Sigma):
"""Return the multivariate Gaussian distribution on array pos.

pos is an array constructed by packing the meshed arrays of variables
x_1, x_2, x_3, ..., x_k into its _last_ dimension.

"""

n = mu.shape
Sigma_det = np.linalg.det(Sigma)
Sigma_inv = np.linalg.inv(Sigma)
N = np.sqrt((2*np.pi)**n * Sigma_det)
# This einsum call calculates (x-mu)T.Sigma-1.(x-mu) in a vectorized
# way across all the input variables.
fac = np.einsum('...k,kl,...l->...', pos-mu, Sigma_inv, pos-mu)

return np.exp(-fac / 2) / N

# The distribution on the variables X, Y packed into pos.
Z = multivariate_gaussian(pos, mu, Sigma)

# Create a surface plot and projected filled contour plot under it.
fig = plt.figure()
ax = fig.gca(projection='3d')
ax.plot_surface(X, Y, Z, rstride=3, cstride=3, linewidth=1, antialiased=True,
cmap=cm.viridis)

cset = ax.contourf(X, Y, Z, zdir='z', offset=-0.15, cmap=cm.viridis)

# Adjust the limits, ticks and view angle
ax.set_zlim(-0.15,0.2)
ax.set_zticks(np.linspace(0,0.2,5))
ax.view_init(27, -21)

plt.show() This is a 3D projection of the gaussians involved with the lower surface showing the 2D projection of the 3D projection above. The innermost ellipse represents the highest peak, that is the maximum probability for a given (X,Y) value.

** numpy einsum examples **

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

[[ 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]]

#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)

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

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)

array([ 0, 22, 76])

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)

array([ 0, 22, 76])

from scipy.stats import multivariate_normal
x, y = np.mgrid[-5:5:.1, -5:5:.1]
pos = np.empty(x.shape + (2,))
pos[:, :, 0] = x; pos[:, :, 1] = y
rv = multivariate_normal([0.5, -0.2], [[2.0, 0.9], [0.9, 0.5]])
plt.contourf(x, y, rv.pdf(pos))

<matplotlib.contour.QuadContourSet at 0x139196438> #### References:¶

1. Roger Labbe’s repo on Kalman Filters. (Majority of the examples in the notes are from this)
2. Probabilistic Robotics by Sebastian Thrun, Wolfram Burgard and Dieter Fox, MIT Press.
3. Scipy Documentation

### KF Basics - Part 2¶

#### Probabilistic Generative Laws¶

##### 1st Law:¶

The belief representing the state $$x_{t}$$, is conditioned on all past states, measurements and controls. This can be shown mathematically by the conditional probability shown below:

$p(x_{t} | x_{0:t-1},z_{1:t-1},u_{1:t})$
1. $$z_{t}$$ represents the measurement
2. $$u_{t}$$ the motion command
3. $$x_{t}$$ the state (can be the position, velocity, etc) of the robot or its environment at time t.

‘If we know the state $$x_{t-1}$$ and $$u_{t}$$, then knowing the states $$x_{0:t-2}$$, $$z_{1:t-1}$$ becomes immaterial through the property of conditional independence’. The state $$x_{t-1}$$ introduces a conditional independence between $$x_{t}$$ and $$z_{1:t-1}$$, $$u_{1:t-1}$$

Therefore the law now holds as:

$p(x_{t} | x_{0:t-1},z_{1:t-1},u_{1:t})=p(x_{t} | x_{t-1},u_{t})$
##### 2nd Law:¶

If $$x_{t}$$ is complete, then:

$p(z_{t} | x-_{0:t},z_{1:t-1},u_{1:t})=p(z_{t} | x_{t})$

$$x_{t}$$ is complete means that the past states, controls or measurements carry no additional information to predict future.

$$x_{0:t-1}$$, $$z_{1:t-1}$$ and $$u_{1:t}$$ are conditionally independent of $$z_{t}$$ given $$x_{t}$$ of complete.

The filter works in two parts:

$$p(x_{t} | x_{t-1},u_{t})$$ -> State Transition Probability

$$p(z_{t} | x_{t})$$ -> Measurement Probability

#### Conditional dependence and independence example:¶

$$\bullet$$Independent but conditionally dependent

Let’s say you flip two fair coins

C - Your first two flips were the same

A and B here are independent. However, A and B are conditionally dependent given C, since if you know C then your first coin flip will inform the other one.

$$\bullet$$Dependent but conditionally independent

A box contains two coins: a regular coin and one fake two-headed coin ((P(H)=1). I choose a coin at random and toss it twice. Define the following events.

A= First coin toss results in an H.

B= Second coin toss results in an H.

C= Coin 1 (regular) has been selected.

If we know A has occurred (i.e., the first coin toss has resulted in heads), we would guess that it is more likely that we have chosen Coin 2 than Coin 1. This in turn increases the conditional probability that B occurs. This suggests that A and B are not independent. On the other hand, given C (Coin 1 is selected), A and B are independent.

#### Bayes Rule:¶

Posterior =

$\frac{Likelihood*Prior}{Marginal}$

Here,

Posterior = Probability of an event occurring based on certain evidence.

Likelihood = How probable is the evidence given the event.

Prior = Probability of the just the event occurring without having any evidence.

Marginal = Probability of the evidence given all the instances of events possible.

Example:

1% of women have breast cancer (and therefore 99% do not). 80% of mammograms detect breast cancer when it is there (and therefore 20% miss it). 9.6% of mammograms detect breast cancer when its not there (and therefore 90.4% correctly return a negative result).

We can turn the process above into an equation, which is Bayes Theorem. Here is the equation:

$$\displaystyle{\Pr(\mathrm{A}|\mathrm{X}) = \frac{\Pr(\mathrm{X}|\mathrm{A})\Pr(\mathrm{A})}{\Pr(\mathrm{X|A})\Pr(\mathrm{A})+ \Pr(\mathrm{X | not \ A})\Pr(\mathrm{not \ A})}}$$

$$\bullet$$Pr(A|X) = Chance of having cancer (A) given a positive test (X). This is what we want to know: How likely is it to have cancer with a positive result? In our case it was 7.8%.

$$\bullet$$Pr(X|A) = Chance of a positive test (X) given that you had cancer (A). This is the chance of a true positive, 80% in our case.

$$\bullet$$Pr(A) = Chance of having cancer (1%).

$$\bullet$$Pr(not A) = Chance of not having cancer (99%).

$$\bullet$$Pr(X|not A) = Chance of a positive test (X) given that you didn’t have cancer (~A). This is a false positive, 9.6% in our case.

#### Bayes Filter Algorithm¶

The basic filter algorithm is:

for all $$x_{t}$$:

1. $$\overline{bel}(x_t) = \int p(x_t | u_t, x_{t-1}) bel(x_{t-1})dx$$
2. $$bel(x_t) = \eta p(z_t | x_t) \overline{bel}(x_t)$$

end.

$$\rightarrow$$The first step in filter is to calculate the prior for the next step that uses the bayes theorem. This is the Prediction step. The belief, $$\overline{bel}(x_t)$$, is before incorporating measurement($$z_{t}$$) at time t=t. This is the step where the motion occurs and information is lost because the means and covariances of the gaussians are added. The RHS of the equation incorporates the law of total probability for prior calculation.

$$\rightarrow$$ This is the Correction or update step that calculates the belief of the robot after taking into account the measurement($$z_{t}$$) at time t=t. This is where we incorporate the sensor information for the whereabouts of the robot. We gain information here as the gaussians get multiplied here. (Multiplication of gaussian values allows the resultant to lie in between these numbers and the resultant covariance is smaller.

#### Bayes filter localization example:¶

from IPython.display import Image
Image(filename="bayes_filter.png",width=400) Given - A robot with a sensor to detect doorways along a hallway. Also, the robot knows how the hallway looks like but doesn’t know where it is in the map.

1. Initially(first scenario), it doesn’t know where it is with respect to the map and hence the belief assigns equal probability to each location in the map.
2. The first sensor reading is incorporated and it shows the presence of a door. Now the robot knows how the map looks like but cannot localize yet as map has 3 doors present. Therefore it assigns equal probability to each door present.
3. The robot now moves forward. This is the prediction step and the motion causes the robot to lose some of the information and hence the variance of the gaussians increase (diagram 4.). The final belief is convolution of posterior from previous step and the current state after motion. Also, the means shift on the right due to the motion.
4. Again, incorporating the measurement, the sensor senses a door and this time too the possibility of door is equal for the three door. This is where the filter’s magic kicks in. For the final belief (diagram 5.), the posterior calculated after sensing is mixed or convolution of previous posterior and measurement. It improves the robot’s belief at location near to the second door. The variance decreases and peaks.
5. Finally after series of iterations of motion and correction, the robot is able to localize itself with respect to the environment.(diagram 6.)

Do note that the robot knows the map but doesn’t know where exactly it is on the map.

#### Bayes and Kalman filter structure¶

The basic structure and the concept remains the same as bayes filter for Kalman. The only key difference is the mathematical representation of Kalman filter. The Kalman filter is nothing but a bayesian filter that uses Gaussians.

For a bayes filter to be a Kalman filter, each term of belief is now a gaussian, unlike histograms. The basic formulation for the bayes filter algorithm is:

\begin{split}\begin{aligned} \bar {\mathbf x} &= \mathbf x \ast f_{\mathbf x}(\bullet)\, \, &\text{Prediction} \\ \mathbf x &= \mathcal L \cdot \bar{\mathbf x}\, \, &\text{Correction} \end{aligned}\end{split}

$$\bar{\mathbf x}$$ is the prior

$$\mathcal L$$ is the likelihood of a measurement given the prior $$\bar{\mathbf x}$$

$$f_{\mathbf x}(\bullet)$$ is the process model or the gaussian term that helps predict the next state like velocity to track position or acceleration.

$$\ast$$ denotes convolution.

#### Kalman Gain¶

$x = (\mathcal L \bar x)$

Where x is posterior and $$\mathcal L$$ and $$\bar x$$ are gaussians.

Therefore the mean of the posterior is given by:

$\mu=\frac{\bar\sigma^2\, \mu_z + \sigma_z^2 \, \bar\mu} {\bar\sigma^2 + \sigma_z^2}$
$\mu = \left( \frac{\bar\sigma^2}{\bar\sigma^2 + \sigma_z^2}\right) \mu_z + \left(\frac{\sigma_z^2}{\bar\sigma^2 + \sigma_z^2}\right)\bar\mu$

In this form it is easy to see that we are scaling the measurement and the prior by weights:

$\mu = W_1 \mu_z + W_2 \bar\mu$

The weights sum to one because the denominator is a normalization term. We introduce a new term, $$K=W_1$$, giving us:

\begin{split}\begin{aligned} \mu &= K \mu_z + (1-K) \bar\mu\\ &= \bar\mu + K(\mu_z - \bar\mu) \end{aligned}\end{split}

where

$K = \frac {\bar\sigma^2}{\bar\sigma^2 + \sigma_z^2}$

The variance in terms of the Kalman gain:

\begin{split}\begin{aligned} \sigma^2 &= \frac{\bar\sigma^2 \sigma_z^2 } {\bar\sigma^2 + \sigma_z^2} \\ &= K\sigma_z^2 \\ &= (1-K)\bar\sigma^2 \end{aligned}\end{split}

$$K$$ is the Kalman gain. It’s the crux of the Kalman filter. It is a scaling term that chooses a value partway between $$\mu_z$$ and $$\bar\mu$$.

#### Kalman Filter - Univariate and Multivariate¶

Prediction

$$\begin{array}{|l|l|l|} \hline \text{Univariate} & \text{Univariate} & \text{Multivariate}\\ & \text{(Kalman form)} & \\ \hline \bar \mu = \mu + \mu_{f_x} & \bar x = x + dx & \bar{\mathbf x} = \mathbf{Fx} + \mathbf{Bu}\\ \bar\sigma^2 = \sigma_x^2 + \sigma_{f_x}^2 & \bar P = P + Q & \bar{\mathbf P} = \mathbf{FPF}^\mathsf T + \mathbf Q \\ \hline \end{array}$$

$$\mathbf x,\, \mathbf P$$ are the state mean and covariance. They correspond to $$x$$ and $$\sigma^2$$.

$$\mathbf F$$ is the state transition function. When multiplied by $$\bf x$$ it computes the prior.

$$\mathbf Q$$ is the process covariance. It corresponds to $$\sigma^2_{f_x}$$.

$$\mathbf B$$ and $$\mathbf u$$ are model control inputs to the system.

Correction

$$\begin{array}{|l|l|l|} \hline \text{Univariate} & \text{Univariate} & \text{Multivariate}\\ & \text{(Kalman form)} & \\ \hline & y = z - \bar x & \mathbf y = \mathbf z - \mathbf{H\bar x} \\ & K = \frac{\bar P}{\bar P+R}& \mathbf K = \mathbf{\bar{P}H}^\mathsf T (\mathbf{H\bar{P}H}^\mathsf T + \mathbf R)^{-1} \\ \mu=\frac{\bar\sigma^2\, \mu_z + \sigma_z^2 \, \bar\mu} {\bar\sigma^2 + \sigma_z^2} & x = \bar x + Ky & \mathbf x = \bar{\mathbf x} + \mathbf{Ky} \\ \sigma^2 = \frac{\sigma_1^2\sigma_2^2}{\sigma_1^2+\sigma_2^2} & P = (1-K)\bar P & \mathbf P = (\mathbf I - \mathbf{KH})\mathbf{\bar{P}} \\ \hline \end{array}$$

$$\mathbf H$$ is the measurement function.

$$\mathbf z,\, \mathbf R$$ are the measurement mean and noise covariance. They correspond to $$z$$ and $$\sigma_z^2$$ in the univariate filter. $$\mathbf y$$ and $$\mathbf K$$ are the residual and Kalman gain.

The details will be different than the univariate filter because these are vectors and matrices, but the concepts are exactly the same:

• Use a Gaussian to represent our estimate of the state and error
• Use a Gaussian to represent the measurement and its error
• Use a Gaussian to represent the process model
• Use the process model to predict the next state (the prior)
• Form an estimate part way between the measurement and the prior

#### References:¶

1. Roger Labbe’s repo on Kalman Filters. (Majority of text in the notes are from this)
2. Probabilistic Robotics by Sebastian Thrun, Wolfram Burgard and Dieter Fox, MIT Press.