gistfile1.txt

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