M = 50
N = 60
true_map = np.zeros((M, N))
true_map[0:10, 0:10] = 1
true_map[30:35, 40:45] = 1
true_map[3:6,40:60] = 1;
true_map[20:30,25:29] = 1;
true_map[40:50,5:25] = 1;

# Initializing the robot's location.
x_0 = [30, 30, 0]# The sequence of robot motions.
u = np.array([[3, 0, -3, 0], [0, 3, 0, -3]])
u_i = 1# Initialize the vector of states for our simulation.
x = np.zeros((3, len(time_steps)))
x[:, 0] = x_0while(Some Conditon...) :# Perform robot motion.move = np.add(x[0:2, t-1], u[:, u_i]) # If we hit the map boundaries, or a collision would occur, remain still.if (move[0] >= M - 1) or (move[1] >= N - 1) or (move[0] <= 0) or (move[1] <= 0) or true_map[int(round(move[0])), int(round(move[1]))] == 1:x[:, t] = x[:, t-1]u_i = (u_i + 1) % 4else:x[0:2, t] = move

x[2, t] = (x[2, t-1] + w[t]) % (2 * math.pi)

# Parameters for the sensor model.
meas_phi = np.arange(-0.4, 0.4, 0.05)
rmax = 30 # Max beam range.
alpha = 1 # Width of an obstacle (distance about measurement to fill in).
beta = 0.05 # Angular width of a beam.

def get_ranges(true_map, X, meas_phi, rmax):(M, N) = np.shape(true_map)x = X[0]y = X[1]theta = X[2]meas_r = rmax * np.ones(meas_phi.shape)# Iterate for each measurement bearing.for i in range(len(meas_phi)):# Iterate over each unit step up to and including rmax.for r in range(1, rmax+1):# Determine the coordinates of the cell.xi = int(round(x + r * math.cos(theta + meas_phi[i])))yi = int(round(y + r * math.sin(theta + meas_phi[i])))# If not in the map, set measurement there and stop going further.if (xi <= 0 or xi >= M-1 or yi <= 0 or yi >= N-1):meas_r[i] = rbreak# If in the map, but hitting an obstacle, set the measurement range# and stop ray tracing.elif true_map[int(round(xi)), int(round(yi))] == 1:meas_r[i] = rbreakreturn meas_r

r = math.sqrt((i - x)**2 + (j - y)**2)
phi = (math.atan2(j - y, i - x) - theta + math.pi) % (2 * math.pi) - math.pi# Find the range measurement associated with the relative bearing.
k = np.argmin(np.abs(np.subtract(phi, meas_phi)))

def inverse_scanner(num_rows, num_cols, x, y, theta, meas_phi, meas_r, rmax, alpha, beta):m = np.zeros((M, N))for i in range(num_rows):for j in range(num_cols):# Find range and bearing relative to the input state (x, y, theta).r = math.sqrt((i - x)**2 + (j - y)**2)phi = (math.atan2(j - y, i - x) - theta + math.pi) % (2 * math.pi) - math.pi# Find the range measurement associated with the relative bearing.k = np.argmin(np.abs(np.subtract(phi, meas_phi)))# If the range is greater than the maximum sensor range, or behind our range# measurement, or is outside of the field of view of the sensor, then no# new information is available.if (r > min(rmax, meas_r[k] + alpha / 2.0)) or (abs(phi - meas_phi[k]) > beta / 2.0):m[i, j] = 0.5# If the range measurement lied within this cell, it is likely to be an object.elif (meas_r[k] < rmax) and (abs(r - meas_r[k]) < alpha / 2.0):m[i, j] = 0.7# If the cell is in front of the range measurement, it is likely to be empty.elif r < meas_r[k]:m[i, j] = 0.3return m

meas_rs = []
meas_r = get_ranges(true_map, x[:, 0], meas_phi, rmax)
meas_rs.append(meas_r)
invmods = []
invmod = inverse_scanner(M, N, x[0, 0], x[1, 0], x[2, 0], meas_phi, meas_r, rmax, alpha, beta)
invmods.append(invmod)
ms = []
ms.append(m)# Main simulation loop.
for t in range(1, len(time_steps)):# Perform robot motion.move = np.add(x[0:2, t-1], u[:, u_i]) # If we hit the map boundaries, or a collision would occur, remain still.if (move[0] >= M - 1) or (move[1] >= N - 1) or (move[0] <= 0) or (move[1] <= 0) or true_map[int(round(move[0])), int(round(move[1]))] == 1:x[:, t] = x[:, t-1]u_i = (u_i + 1) % 4else:x[0:2, t] = movex[2, t] = (x[2, t-1] + w[t]) % (2 * math.pi)# Gather the measurement range data, which we will convert to occupancy probabilitiesmeas_r = get_ranges(true_map, x[:, t], meas_phi, rmax)meas_rs.append(meas_r)# Given our range measurements and our robot location, apply inverse scanner modelinvmod = inverse_scanner(M, N, x[0, t], x[1, t], x[2, t], meas_phi, meas_r, rmax, alpha, beta)invmods.append(invmod)# Calculate and update the log odds of our occupancy grid, given our measured occupancy probabilities from the inverse model.L = np.log(np.divide(invmod, np.subtract(1, invmod))) + L - L0# Calculate a grid of probabilities from the log odds.m = np.divide(np.exp(L), np.add(1, np.exp(L)))ms.append(m)