机器人世界

与人类一样，机器人通过它的“感官”来感知世界。例如，无人驾驶汽车使用视频、雷达与激光雷达来观察周围的世界。随着汽车不断地收集数据，它们会建立起一个3D观察世界，在这个世界，汽车可以知道自己在哪里，其他物体（如树木、行人和其他车辆）在哪里，以及它应该去哪里！

机器人世界： 1-D

首先，假设你有一个生活在1-D世界中机器人。你可以将这个1-D世界看做是一个单车道道路

我们可以将这条道路视为一个阵列，并将其分解为很多网格单元，便于机器人理解。在这种情况下，这条道路是一个一维网格，有5个不同的空间。机器人只能向前或向后移动。如果机器人从网格上掉下来，它将环绕回到另一侧（这被称为循环世界）。

均匀分布

机器人有一张地图，因此它知道这个1D世界中只有5个空间，但是，它没有感觉到任何东西或没有发生移动。那么，对于这个5个单元格长度(5个值的列表)，机器人在这些位置中的任何一个位置的概率分布p是多少？
由于机器人最初不知道它在哪里，所以在任何空间中的概率是相同的！这就是概率分布，因此所有这些概率的总和应该等于1，所以1/5 spaces = 0.2。所有概率相同（并且具有最大不确定性）的分布称为均匀分布。

# importing resources
import matplotlib.pyplot as plt
import numpy as np
# uniform distribution for 5 grid cells
# we use "p" to represent probability
p = [0.2, 0.2, 0.2, 0.2, 0.2]
print(p)

下面的函数display_map将会输出一个条形图，用于显示机器人在每个网格空间中的概率。对于概率范围，y轴的范围为0到1。在均匀分布中，这看起来像一条扁平线。如果你想将它们分开，可以选择每个条的宽度<= 1。

def display_map(grid, bar_width=1):
    if(len(grid) > 0):
        x_labels = range(len(grid))
        plt.bar(x_labels, height=grid, width=bar_width, color='b')
        plt.xlabel('Grid Cell')
        plt.ylabel('Probability')
        plt.ylim(0, 1) # range of 0-1 for probability values 
        plt.title('Probability of the robot being at each cell in the grid')
        plt.xticks(np.arange(min(x_labels), max(x_labels)+1, 1))
        plt.show()
    else:
        print('Grid is empty')

# call function on grid, p, from before
display_map(p)

机器人传感器

机器人通过摄像头和其他传感器来感知世界，但这些传感器并不是完全正确。
感知后的概率：
机器人会感知它处于一个红色单元格中，并更新其概率。根据我们的例子：

机器人对颜色感知正确的概率是pHit = 0.6。
机器人对颜色感知不正确的概率是pMiss = 0.2，在这个示例中：它看到了红色，但实际上它在绿色单元格中

# given initial variables
p = initialize_robot(5)
pHit  = 0.6
pMiss = 0.2

# Creates a new grid, with modified probabilities, after sensing
# All values are calculated by a product of 1. the sensing probability for a color (pHit for red)
# and 2. the current probability of a robot being in that location p[i]; all equal to 0.2 at first.
p[0] = p[0]*pMiss
p[1] = p[1]*pHit
p[2] = p[2]*pHit
p[3] = p[3]*pMiss
p[4] = p[4]*pMiss

print(p)
display_map(p)

# given initial variables
p=[0.2, 0.2, 0.2, 0.2, 0.2]
# the color of each grid cell in the 1D world
world=['green', 'red', 'red', 'green', 'green']
# Z, the sensor reading ('red' or 'green')
Z = 'red'
pHit = 0.6
pMiss = 0.2

## Complete this function
def sense(p, Z):
    ''' Takes in a current probability distribution, p, and a sensor reading, Z.
        Returns an unnormalized distribution after the sensor measurement has been made, q.
        This should be accurate whether Z is 'red' or 'green'. '''
    
    q=[]
    for i in range(len(p)):
        hit = (Z == world[i])
        q.append(p[i] * (hit * pHit + (1-hit) * pMiss))
    return q

q = sense(p,Z)
print(q)
display_map(q)

# given initial variables
p=[0.2, 0.2, 0.2, 0.2, 0.2]
# the color of each grid cell in the 1D world
world=['green', 'red', 'red', 'green', 'green']
# Z, the sensor reading ('red' or 'green')
Z = 'red'
pHit = 0.6
pMiss = 0.2

## Complete this function
def sense(p, Z):
    ''' Takes in a current probability distribution, p, and a sensor reading, Z.
        Returns a *normalized* distribution after the sensor measurement has been made, q.
        This should be accurate whether Z is 'red' or 'green'. '''
    
    q=[]
    
    ##TODO: normalize q
    
    # loop through all grid cells
    for i in range(len(p)):
        # check if the sensor reading is equal to the color of the grid cell
        # if so, hit = 1
        # if not, hit = 0
        hit = (Z == world[i])
        q.append(p[i] * (hit * pHit + (1-hit) * pMiss))
        
    # sum up all the components
    s = sum(q)
    # divide all elements of q by the sum
    for i in range(len(p)):
        q[i] = q[i] / s
    return q

q = sense(p,Z)
print(q)
display_map(q)

Move函数

使其返回一个新的分布q，向右运动（U）个单位。此函数下的分布应随着运动U的变化而变化,如果U = 1，你应该可以看到p中的所有值都向右移动1。

## TODO: Complete this move function so that it shifts a probability distribution, p
## by a given motion, U
def move(p, U):
    q=[]
    # iterate through all values in p
    for i in range(len(p)):
        # use the modulo operator to find the new location for a p value
        # this finds an index that is shifted by the correct amount
        index = (i-U) % len(p)
        # append the correct value of p to q
        q.append(p[index])
    return q

p = move(p,2)
print(p)
display_map(p)

不精确的运动

对于初始分布[0,1,0,0,0]和U = 2的运动，当考虑运动中的不确定性时，得到的分布是：[0,0,0.1,0.8,0.1]

修改move函数，使其能够容纳机器人超越或未到达预期目的地的附加概率
此函数下的分布应随着运动U的变化而变化，但其中有一些未达到目的地或超越目的地的概率。对于给定的初始p，你应该可以看到一个U = 1的结果并且包含不确定性：[0.0, 0.1, 0.8, 0.1, 0.0]。

## TODO: Modify the move function to accommodate the added robabilities of overshooting or undershooting 
pExact = 0.8
pOvershoot = 0.1
pUndershoot = 0.1

# Complete the move function
def move(p, U):
    q=[]
    # iterate through all values in p
    for i in range(len(p)):
        # use the modulo operator to find the new location for a p value
        # this finds an index that is shifted by the correct amount
        index = (i-U) % len(p)
        nextIndex = (index+1) % len(p)
        prevIndex = (index-1) % len(p)
        s = pExact * p[index]
        s = s + pOvershoot  * p[nextIndex]
        s = s + pUndershoot * p[prevIndex]
        # append the correct, modified value of p to q
        q.append(s)
    return q

## TODO: try this for U = 2 and see the result
p = move(p,1)
print(p)
display_map(p)

多次不精确移动

# given initial variables
p=[0, 1, 0, 0, 0]
# the color of each grid cell in the 1D world
world=['green', 'red', 'red', 'green', 'green']
# Z, the sensor reading ('red' or 'green')
Z = 'red'
pHit = 0.6
pMiss = 0.2

pExact = 0.8
pOvershoot = 0.1
pUndershoot = 0.1

# Complete the move function
def move(p, U):
    q=[]
    # iterate through all values in p
    for i in range(len(p)):
        # use the modulo operator to find the new location for a p value
        # this finds an index that is shifted by the correct amount
        index = (i-U) % len(p)
        nextIndex = (index+1) % len(p)
        prevIndex = (index-1) % len(p)
        s = pExact * p[index]
        s = s + pOvershoot  * p[nextIndex]
        s = s + pUndershoot * p[prevIndex]
        # append the correct, modified value of p to q
        q.append(s)
    return q

# Here is code for moving twice
# p = move(p, 1)
# p = move(p, 1)
# print(p)
# display_map(p)
## TODO: Write code for moving 1000 times
for k in range(1000):
    p = move(p,1)

print(p)
display_map(p)

谷歌无人车定位功能基本就是以上代码实现，编写了两个部分：感知和移动

感知与移动循环

# importing resources
import matplotlib.pyplot as plt
import numpy as np

def display_map(grid, bar_width=1):
    if(len(grid) > 0):
        x_labels = range(len(grid))
        plt.bar(x_labels, height=grid, width=bar_width, color='b')
        plt.xlabel('Grid Cell')
        plt.ylabel('Probability')
        plt.ylim(0, 1) # range of 0-1 for probability values 
        plt.title('Probability of the robot being at each cell in the grid')
        plt.xticks(np.arange(min(x_labels), max(x_labels)+1, 1))
        plt.show()
    else:
        print('Grid is empty')

给定列表为motions=[1,1]，如果机器人首先感知到红色，则计算后验分布，然后向右移动一个单位，然后感知绿色，之后再次向右移动，从均匀的先验分布p开始。
motions=[1,1]意味着机器人向右移动一个单元格，然后向右移动。你会获得一个初始变量以及完整的sense 与move函数，如下所示。

# given initial variables
p=[0.2, 0.2, 0.2, 0.2, 0.2]
# the color of each grid cell in the 1D world
world=['green', 'red', 'red', 'green', 'green']
# Z, the sensor reading ('red' or 'green')
measurements = ['red', 'green']
pHit = 0.6
pMiss = 0.2

motions = [1,1]
pExact = 0.8
pOvershoot = 0.1
pUndershoot = 0.1

# You are given the complete sense function
def sense(p, Z):
    ''' Takes in a current probability distribution, p, and a sensor reading, Z.
        Returns a *normalized* distribution after the sensor measurement has been made, q.
        This should be accurate whether Z is 'red' or 'green'. '''
    q=[]
    # loop through all grid cells
    for i in range(len(p)):
        # check if the sensor reading is equal to the color of the grid cell
        # if so, hit = 1
        # if not, hit = 0
        hit = (Z == world[i])
        q.append(p[i] * (hit * pHit + (1-hit) * pMiss))
        
    # sum up all the components
    s = sum(q)
    # divide all elements of q by the sum to normalize
    for i in range(len(p)):
        q[i] = q[i] / s
    return q


# The complete move function
def move(p, U):
    q=[]
    # iterate through all values in p
    for i in range(len(p)):
        # use the modulo operator to find the new location for a p value
        # this finds an index that is shifted by the correct amount
        index = (i-U) % len(p)
        nextIndex = (index+1) % len(p)
        prevIndex = (index-1) % len(p)
        s = pExact * p[index]
        s = s + pOvershoot  * p[nextIndex]
        s = s + pUndershoot * p[prevIndex]
        # append the correct, modified value of p to q
        q.append(s)
    return q


## TODO: Compute the posterior distribution if the robot first senses red, then moves 
## right one, then senses green, then moves right again, starting with a uniform prior distribution.
for k in range(len(measurements)):
    # sense and then move, reading the correct measurements/motions at each step
    p = sense(p, measurements[k])
    p = move(p, motions[k])

## print/display that distribution
print(p)
display_map(p)
## print/display that distribution

关于熵值的说明

在更新步骤之后熵值将下降，并且在测量步骤之后熵值将上升。
通常情况下，熵值可以用来测量不确定性的量。由于更新步骤增加了不确定性，因此熵值应该增加。测量步骤降低了不确定性，因此熵值应该减少。
让我们看一看当前的这个例子，其中机器人可以处于五个不同的位置。当所有位置具有相等的概率 [0.2,0.2,0.2,0.2,0.2] 时，会出现最大的不确定性。

进行测量就会降低不确定性，从而降低熵值。假设在进行测量后，概率变为 [0.05,0.05,0.05,0.8,0.05]。现在熵值减少到0.338。因此，一个测量步骤会使熵值降低，而一个更新步骤则会使熵值增加。