Robot Localization¶

Chris Tralie¶

Online and offline bayesian robot localization in a simulated 2D environment

In [1]:
#%matplotlib notebook
%load_ext autoreload
%autoreload 2
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.cm as cm
import matplotlib.animation as animation
import IPython.display as ipd
from environment import *

Step 1: Setup Map And Measurements¶

First, setup the map and a trajectory through it. Compute perfect measurements from every location on the map, as well as noisy observations from every point on the trajectory

In [37]:
env = Environment("Maze1.png")
res = 50
# Devise a path through that environment that passes through 3 locations
X = env.simulate_trajectory([[0, 15], [27, 40], [26, 12]])

# Compute perfect scans from each location to use in the observation model
N = len(env.X)
state_scans = [env.get_range_scan(env.X[i], res, alpha=0) for i in range(N)]

# Create observed scans from all locations on the trajectory
alpha = 4
np.random.seed(0)
observed_scans = [env.get_range_scan(X[i, :], res, alpha) for i in range(X.shape[0])]
In [30]:
# Plot the map and the trajectory
# Observed 55, states 172, 459
plt.figure(figsize=(12, 6))
plt.subplot(121)
env.plot()
plt.plot(X[:, 0], X[:, 1])
plt.scatter(X[55, 0], X[55, 1])
plt.scatter(env.X[172, 0], env.X[172, 1])
plt.scatter(env.X[459, 0], env.X[459, 1])
plt.subplot(122)
plt.plot(observed_scans[55])
plt.plot(state_scans[172], linestyle='--')
plt.plot(state_scans[459], linestyle='--')
plt.legend([
    "Observed",
    "{:.3f}".format(get_measurement_prob(state_scans[172], observed_scans[55], alpha, use_log=True)),
    "{:.3f}".format(get_measurement_prob(state_scans[459], observed_scans[55], alpha, use_log=True))
])
Out[30]:
<matplotlib.legend.Legend at 0x7fb2c9ea5bd0>

Step 2: Online Tracking with Bayes Filter¶

In [38]:
frames = [] # for storing the generated images
fig = plt.figure(figsize=(6, 6))
num_states = env.X.shape[0] # Number of states
probs = np.zeros(num_states)
belief = np.ones(num_states)/num_states # Initial belief (every state is equally likely)

for t in range(X.shape[0]): # T Filter every measurement that comes in   T = X.shape[0]
    ## TODO: Bayes filtering for this frame index
    g = np.zeros(num_states)
    for j in range(num_states):
        ## Step 1: Compute the prior (transition*prior belief across all neighboring states)
        ## Restrict transition model to be equal probability to all direct
        ## neighbors (up/down/left/right)
        for k in env.neighbors[j]:
            p_trans = 1/len(env.neighbors[k])
            g[j] += p_trans*belief[k]
        ## Step 2: Multiply by the observation likelihood
        g[j] *= get_measurement_prob(state_scans[j], observed_scans[t], alpha)
    belief = g/np.sum(g)
    plot = env.plot_probabilities(belief, p=1e-2)
    plot.append(plt.scatter([X[t, 0]], [X[t, 1]], c='C0'))
    frames.append(plot)
ani = animation.ArtistAnimation(fig, frames, interval=200, blit=True, repeat_delay=1000)
ani.save("BayesFilter.gif")

Step 3: Offline Tracking with Viterbi¶

Be sure to take log probabilities instead of straight up probabilities to avoid arithmetic underflow! The rule here is

$\log(AB) = \log(A) + \log(B)$¶

In [48]:
T = len(observed_scans)
num_states = env.X.shape[0]

L = np.zeros((num_states, T)) # Maximum accumulated log probabilities at every time at every state
B = np.zeros((num_states, T), dtype=int) # Previous states that led to maximum accumulated probabilities, used for backtracing
B[:, 0] = -1

## Step 1: Initial Conditions
for k in range(num_states):
    L[k, 0] = get_measurement_prob(state_scans[k], observed_scans[0], alpha, use_log=True)
    
## Step 2: Dynamic programming
for t in range(1, T):
    # Loop through every state j
    for j in range(num_states):
        mx = -np.inf
        for k in env.neighbors[j]:
            p_trans = 1/len(env.neighbors[k])
            prob = L[k, t-1] + np.log(p_trans) # L[k, t-1]*p_trans
            if prob > mx:
                mx = prob
                B[j, t] = k # Remember that k was the best I came from
        L[j, t] = mx + get_measurement_prob(state_scans[j], observed_scans[t], alpha, use_log=True)
        
        
## Step 3: Maximum selection and backtracing
# Find the last state that has a maximum accumulated probability
t = T-1
j = np.argmax(L[:, t])
states = [j]
while t > 0:
    # Follow the best states that we recorded before
    j = B[j, t]
    t -= 1
    states.append(j)

states.reverse()


states = np.array(states, dtype=int)
Y = env.X[states, :]


plt.figure()
plt.plot(X[:, 0], X[:, 1], 'k', linewidth=4)
plt.plot(Y[:, 0], Y[:, 1], 'C1', linestyle='--')
plt.legend(["Ground Truth", "Estimated"])
plt.axis("equal")
plt.title("Estimated Trajectory, $\\alpha={:.3f}$".format(alpha))
plt.savefig("Est.svg", facecolor='white')
In [ ]: