Skip to content

Instantly share code, notes, and snippets.

@el3ment
Created October 28, 2017 17:40
Show Gist options
  • Save el3ment/3bb0d03b1dfc7e602e8ec440ae268ff5 to your computer and use it in GitHub Desktop.
Save el3ment/3bb0d03b1dfc7e602e8ec440ae268ff5 to your computer and use it in GitHub Desktop.
import scipy.io as sio
import numpy as np
import matplotlib.pyplot as plt
from tqdm import tqdm
np.seterr(all='raise')
matfile = sio.loadmat('state_meas_data.mat')
measurements = np.nan_to_num(matfile['z'].T, 150)
states = matfile['X'].T
theta_sensor = matfile['thk'][0]
size = 100
scale_factor = 1.
M = np.zeros((int(size * scale_factor) + 1, int(size * scale_factor) + 1), dtype=np.float32)
mask = np.mgrid[:M.shape[0], :M.shape[1]] / scale_factor
beta = .04
alpha = 1
l_free = np.log(.4 / (1 - .4))
l_occ = np.log(.6 / (1 - .6))
for t, x in enumerate(tqdm(states)):
dist = mask - x[:2, None, None]
bearing = np.arctan2(dist[0], dist[1]) - np.pi / 2. + x[2]
bearing[bearing > np.pi] -= 2 * np.pi
bearing[bearing < -np.pi] += 2 * np.pi
r = np.hypot(dist[0], dist[1])
for l, z in enumerate(measurements[t]):
smallest_angle_difference = np.abs(bearing + z[1])
m = smallest_angle_difference < beta
M[m & (r < z[0])] += l_free
M[m & (r < (z[0] + alpha)) & (r > z[0])] += l_occ
plt.imshow(1 - 1 / (1 + np.exp(M)), cmap='Greys')
plt.colorbar()
plt.show()
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment