Introduction
In this assignment you will recursively estimate the position of a vehicle along a trajectory using
available measurements and a motion model.
The vehicle is equipped with a very simple type of LIDAR sensor, which returns range and
bearing measurements corresponding to individual landmarks in the environment. The global
positions of the landmarks are assumed to be known beforehand. We will also assume known
data association, that is, which measurment belong to which landmark.
Motion and Measurement Models
Motion Model
The vehicle motion model recieves linear and angular velocity odometry readings as inputs, and
outputs the state (i.e., the 2D pose) of the vehicle:
$$\begin{align} \mathbf{x}_{k} &= \mathbf{x}_{k-1} + T \begin{bmatrix} \cos\theta_{k-1}
&0 \\ \sin\theta_{k-1} &0 \\ 0 &1 \end{bmatrix} \left( \begin{bmatrix} v_k \\ \omega_k
\end{bmatrix} + \mathbf{w}_k \right) \, , \, \, \, \, \, \mathbf{w}_k =
\mathcal{N}\left(\mathbf{0}, \mathbf{Q}\right) \end{align}$$
is the current 2D pose of the vehicle
and are the linear and angular velocity odometry readings, which
we use as inputs to the model
The process noise has a (zero mean) normal distribution with a constant
covariance .
Measurement Model
The measurement model relates the current pose of the vehicle to the LIDAR range and bearing
measurements .
$$\begin{align} \mathbf{y}^l_k = \begin{bmatrix} \sqrt{(x_l - x_k - d\cos\theta_{k})^2 + (y_l -
y_k - d\sin\theta_{k})^2} \\ atan2\left(y_l - y_k - d\sin\theta_{k},x_l - x_k -
d\cos\theta_{k}\right) - \theta_k \end{bmatrix} + \mathbf{n}^l_k \, , \, \, \, \, \, \mathbf{n}^l_k =
\mathcal{N}\left(\mathbf{0}, \mathbf{R}\right) \end{align}$$
and are the ground truth coordinates of the landmark
and and represent the current pose of the vehicle
is the known distance between robot center and laser rangefinder (LIDAR)
The landmark measurement noise has a (zero mean) normal distribution with a constant
covariance .
Getting Started
Since the models above are nonlinear, we recommend using the extended Kalman filter (EKF) as
the state estimator. Specifically, you will need to provide code implementing the following steps:
the prediction step, which uses odometry measurements and the motion model to
produce a state and covariance estimate at a given timestep, and
the correction step, which uses the range and bearing measurements provided by the
LIDAR to correct the pose and pose covariance estimates
Unpack the Data
First, let's unpack the available data:
In [1]:
import pickle
import numpy as np
import matplotlib.pyplot as plt
with open('data/data.pickle', 'rb') as f:
data = pickle.load(f)
t = data['t'] # timestamps [s]
x_init = data['x_init'] # initial x position [m]
y_init = data['y_init'] # initial y position [m]
th_init = data['th_init'] # initial theta position [rad]
# input signal
v = data['v'] # translational velocity input [m/s]
om = data['om'] # rotational velocity input [rad/s]
# bearing and range measurements, LIDAR constants
b = data['b'] # bearing to each landmarks center in the frame attached to
the laser [rad]
r = data['r'] # range measurements [m]
l = data['l'] # x,y positions of landmarks [m]
d = data['d'] # distance between robot center and laser rangefinder [m]
In [2]:
x_init.shape
Out[2]:
()
In [3]:
y_init.shape
Out[3]:
()
In [4]:
th_init.shape
Out[4]:
()
In [5]:
print(v.shape)
print(len(v))
(501,)
501
In [6]:
om.shape
Out[6]:
(501,)
In [7]:
b.shape
Out[7]:
(501, 8)
In [8]:
r.shape
Out[8]:
(501, 8)
In [9]:
l.shape
Out[9]:
(8, 2)
In [10]:
print(d.shape)
print(d[0])
(1,)
0
Note that distance from the LIDAR frame to the robot center is provided and loaded as an array
into the d variable.
Ground Truth
If available, it is useful to plot the ground truth position and orientation before starting the
assignment.
Notice that the orientation values are wrapped to the range in radians.
Initializing Parameters
Now that our data is loaded, we can begin getting things set up for our solver. One of the most
important aspects of designing a filter is determining the input and measurement noise
covariance matrices, as well as the initial state and covariance values. We set the values here:
In [11]:
v_var = 0.01 # translation velocity variance
om_var = 0.01 # rotational velocity variance
# allowed to tune these values
# r_var = 0.1 # range measurements variance
r_var = 0.01
# b_var = 0.1 # bearing measurement variance
b_var = 10
Q_km = np.diag([v_var, om_var]) # input noise covariance
cov_y = np.diag([r_var, b_var]) # measurement noise covariance
x_est = np.zeros([len(v), 3]) # estimated states, x, y, and theta
P_est = np.zeros([len(v), 3, 3]) # state covariance matrices
x_est[0] = np.array([x_init, y_init, th_init]) # initial state
P_est[0] = np.diag([1, 1, 0.1]) # initial state covariance
In [12]:
print(Q_km.shape)
print(Q_km)
(2, 2)
[[0.01 0. ]
[0. 0.01]]
In [13]:
print(cov_y.shape)
print(cov_y)
(2, 2)
[[ 0.01 0. ]
[ 0. 10. ]]
In [14]:
x_est.shape
Out[14]:
(501, 3)
In [15]:
print(P_est.shape)
print(P_est[0])
print(P_est[1])
(501, 3, 3)
[[1. 0. 0. ]
[0. 1. 0. ]
[0. 0. 0.1]]
[[0. 0. 0.]
[0. 0. 0.]
[0. 0. 0.]]
Remember: that it is neccessary to tune the measurement noise variances r_var, b_var in order
for the filter to perform well!
In order for the orientation estimates to coincide with the bearing measurements, it is also
neccessary to wrap all estimated values to the range.
In [16]:
# Wraps angle to (-pi,pi] range
def wraptopi(x):
# x = ((x / np.pi + 1) % 2 - 1) * np.pi
if x > np.pi:
x = x - (np.floor(x / (2 * np.pi)) + 1) * 2 * np.pi
elif x < -np.pi:
x = x + (np.floor(x / (-2 * np.pi)) + 1) * 2 * np.pi
return np.array(x)
Correction Step
First, let's implement the measurement update function, which takes an available landmark
measurement and updates the current state estimate . For each landmark
measurement received at a given timestep , you should implement the following steps:
Compute the measurement model Jacobians at \begin{align}
\mathbf{y}^lk = &\mathbf{h}(\mathbf{x}{k}, \mathbf{n}^lk) \\ \mathbf{H}{k} =
\frac{\partial \mathbf{h}}{\partial \mathbf{x}{k}}\bigg|{\mathbf{\check{x}}
{k},0}& \, , \, \, \, \, \mathbf{M}{k} = \frac{\partial \mathbf{h}}{\partial \mathbf{n}
{k}}\bigg|{\mathbf{\check{x}}_{k},0} \, . \end{align}
Compute the Kalman Gain
Correct the predicted state
Correct the covariance
In [17]:
def measurement_update(lk, rk, bk, P_check, x_check):
x_k = x_check[0]
y_k = x_check[1]
theta_k = wraptopi(x_check[2])
x_l = lk[0]
y_l = lk[1]
d_x = x_l - x_k - d*np.cos(theta_k)
d_y = y_l - y_k - d*np.sin(theta_k)
r = np.sqrt(d_x**2 + d_y**2)
phi = np.arctan2(d_y, d_x) - theta_k
# 1. Compute measurement Jacobian
H_k = np.zeros((2,3))
H_k[0,0] = -d_x/r
H_k[0,1] = -d_y/r
H_k[0,2] = d*(d_x*np.sin(theta_k) - d_y*np.cos(theta_k))/r
H_k[1,0] = d_y/r**2
H_k[1,1] = -d_x/r**2
H_k[1,2] = -1-d*(d_y*np.sin(theta_k) + d_x*np.cos(theta_k))/r**2
M_k = np.identity(2)
y_out = np.vstack([r, wraptopi(phi)])
y_mes = np.vstack([rk, wraptopi(bk)])
# 2. Compute Kalman Gain
K_k = P_check.dot(H_k.T).dot(np.linalg.inv(H_k.dot(P_check).dot(H_k.T) +
M_k.dot(cov_y).dot(M_k.T)))
# 3. Correct predicted state (remember to wrap the angles to [-pi,pi])
x_check = x_check + K_k.dot(y_mes - y_out)
x_check[2] = wraptopi(x_check[2])
# 4. Correct covariance
P_check = (np.identity(3) - K_k.dot(H_k)).dot(P_check)
return x_check, P_check
Prediction Step
Now, implement the main filter loop, defining the prediction step of the EKF using the motion
model provided:
Where
$$\begin{align} \mathbf{F}_{k-1} = \frac{\partial \mathbf{f}}{\partial \mathbf{x}_{k-
1}}\bigg|_{\mathbf{\hat{x}}_{k-1},\mathbf{u}_{k},0} \, , \, \, \, \, \mathbf{L}_{k-1} =
\frac{\partial \mathbf{f}}{\partial \mathbf{w}_{k}}\bigg|_{\mathbf{\hat{x}}_{k-
1},\mathbf{u}_{k},0} \, . \end{align}$$
In [18]:
#### 5. Main Filter Loop
#######################################################################
# set the initial values
P_check = P_est[0]
x_check = x_est[0, :].reshape(3,1)
for k in range(1, len(t)): # start at 1 because we've set the initial
prediciton
delta_t = t[k] - t[k - 1] # time step (difference between timestamps)
theta = wraptopi(x_check[2])
# 1. Update state with odometry readings (remember to wrap the angles to
[-pi,pi])
# x_check = np.zeros(3)
F = np.array([[np.cos(theta), 0],
[np.sin(theta), 0],
[0, 1]], dtype='float')
inp = np.array([[v[k-1]], [om[k-1]]])
x_check = x_check + F.dot(inp).dot(delta_t)
x_check[2] = wraptopi(x_check[2])
# 2. Motion model jacobian with respect to last state
F_km = np.zeros([3, 3])
F_km = np.array([[1, 0, -np.sin(theta)*delta_t*v[k-1]],
[0, 1, np.cos(theta)*delta_t*v[k-1]],
[0, 0, 1]], dtype='float')
# dtype='float'
# 3. Motion model jacobian with respect to noise
L_km = np.zeros([3, 2])
L_km = np.array([[np.cos(theta)*delta_t, 0],
[np.sin(theta)*delta_t, 0],
[0,1]], dtype='float')
# 4. Propagate uncertainty
P_check = F_km.dot(P_check.dot(F_km.T)) + L_km.dot(Q_km.dot(L_km.T))
# 5. Update state estimate using available landmark measurements
for i in range(len(r[k])):
x_check, P_check = measurement_update(l[i], r[k, i], b[k, i], P_check,
x_check)
# Set final state predictions for timestep
x_est[k, 0] = x_check[0]
x_est[k, 1] = x_check[1]
x_est[k, 2] = x_check[2]
P_est[k, :, :] = P_check
Let's plot the resulting state estimates:
In [19]:
e_fig = plt.figure()
ax = e_fig.add_subplot(111)
ax.plot(x_est[:, 0], x_est[:, 1])
ax.set_xlabel('x [m]')
ax.set_ylabel('y [m]')
ax.set_title('Estimated trajectory')
plt.show()
e_fig = plt.figure()
ax = e_fig.add_subplot(111)
ax.plot(t[:], x_est[:, 2])
ax.set_xlabel('Time [s]')
ax.set_ylabel('theta [rad]')
ax.set_title('Estimated trajectory')
plt.show()
Are you satisfied wth your results? The resulting trajectory should closely resemble the ground
truth, with minor "jumps" in the orientation estimate due to angle wrapping. If this is the case,
run the code below to produce your solution file.
In [20]:
with open('submission.pkl', 'wb') as f:
pickle.dump(x_est, f, pickle.HIGHEST_PROTOCOL)
In [ ]:
