机器人中的定位

定位：机器人不断地感知移动，再感知再移动，一直循环。

感知（sense）

#代表世界地形分布密度的array
p=[0.2, 0.2, 0.2, 0.2, 0.2]  
#每块地方的颜色
world=['green', 'red', 'red', 'green', 'green'] 
# 感应器在同一个地方一共进行了两次感应，第一次感应到红色，第二次感应到绿色。
measurements = ['red','green'] 
#当感应器感知到一块地方的颜色，它感知正确的概率是感知错误的概率的三倍。
#这两个p不是随便给的，假设感应Z是红色的，那么在这里，pHit是已知Xi是红色，然后感应到Z的概率（0.6）；pMiss是已知Xi是绿色，感应到Z的概率（0.2）
pHit = 0.6
pMiss = 0.2


#在sense之前，当前认为小车的位置分布是p（这个p包括了所有之前的信息）
#进行sense后，sense到的结果是Z，将此次sense的结果更新，更新后得到新的小车位置分布q。
def sense(p, Z):
    q=[]
    #以下为贝叶斯公式
    for i in range(len(p)):
        hit = (Z == world[i])
        q.append(p[i] * (hit * pHit + (1-hit) * pMiss))
    s = sum(q)
    for i in range(len(p)):
        q[i] = q[i]/s
    return q
#小车是在静止的状态下sense了两次，每一次都是在之前的状态上叠加，因此每次传入和更新的都是p。
for k in range(len(measurements)):
    p = sense(p,measurements)

print (p)

移动（move）

#设机器人往前移动了U格，但机器人不一定移动的这么准确，有10%的概率少移动一格，10%的概率多移动一格。
#取余号在这里是简化了问题，认为世界是循环的，机器人走过了最后一格后 自动回到第一格。

p=[0, 1, 0, 0, 0]
world=['green', 'red', 'red', 'green', 'green']
measurements = ['red', 'green']
pHit = 0.6
pMiss = 0.2

pExact = 0.8     #刚好移动U格的概率
pOvershoot = 0.1 #往前多移动了一格的概率
pUndershoot = 0.1#少往前移动了一格的概率

def sense(p, Z):
    q=[]
    for i in range(len(p)):
        hit = (Z == world[i])
        q.append(p[i] * (hit * pHit + (1-hit) * pMiss))
    s = sum(q)
    for i in range(len(q)):
        q[i] = q[i] / s
    return q

#已知当前机器人位置的概率分布是p，计算它移动U格（它认为自己移动了U格，而不是实际移动了U格）后，新的位置的概率分布q。
#用i-U取余是因为，我们要求新的机器人在每一个格子上的概率，那么就要对每一个新格子问：“你有可能是从哪里来的？”
#已知机器人认为自己移动了U步以后，到了格子i，那么它可能来自格子i-U （准确移动），可能来自格子i-U-1（多移动了一步），可能来自格子i-U+1 （少移动了一步）。
#所以在这些原先的概率上，乘以各自对应的转移概率（准确、多移、少移），就可以得到机器人在新的格子i上的概率。
def move(p, U):
    q = []
    for i in range(len(p)):
        s = pExact * p[(i-U)%len(p)]
        s = s + pOvershoot * p[(i-U-1)%len(p)]
        s = s + pUndershoot * p[(i-U+1)%len(p)]
        q.append(s)
    return q


p = move(p,1)
print (p)

感知+移动

#设机器人往前移动了U格，但机器人不一定移动的这么准确，有10%的概率少移动一格，10%的概率多移动一格。
#取余号在这里是简化了问题，认为世界是循环的，机器人走过了最后一格后 自动回到第一格。

p=[0.2, 0.2, 0.2, 0.2, 0.2]
world=['green', 'red', 'red', 'green', 'green']
measurements = ['red', 'green']
motions = [1,1] #每次移动步数
pHit = 0.6
pMiss = 0.2

pExact = 0.8     #刚好移动U格的概率
pOvershoot = 0.1 #往前多移动了一格的概率
pUndershoot = 0.1#少往前移动了一格的概率

def sense(p, Z):
    q=[]
    #以下为全概率公式
    for i in range(len(p)):
        hit = (Z == world[i])
        q.append(p[i] * (hit * pHit + (1-hit) * pMiss))
    s = sum(q)
    for i in range(len(q)):
        q[i] = q[i] / s
    return q

#已知当前机器人位置的概率分布是p，计算它移动U格（它认为自己移动了U格，而不是实际移动了U格）后，新的位置的概率分布q。
#用i-U取余是因为，我们要求新的机器人在每一个格子上的概率，那么就要对每一个新格子问：“你有可能是从哪里来的？”
#已知机器人认为自己移动了U步以后，到了格子i，那么它可能来自格子i-U （准确移动），可能来自格子i-U-1（多移动了一步），可能来自格子i-U+1 （少移动了一步）。
#所以在这些原先的概率上，乘以各自对应的转移概率（准确、多移、少移），就可以得到机器人在新的格子i上的概率。
def move(p, U):
    q = []
    for i in range(len(p)):
        s = pExact * p[(i-U)%len(p)]
        s = s + pOvershoot * p[(i-U-1)%len(p)]
        s = s + pUndershoot * p[(i-U+1)%len(p)]
        q.append(s)
    return q

#定位：机器人不断地感知移动，再感知再移动，一直循环。
for k in range(len(measurements)):
    p = sense(p,measurements[k])
    p = move(p,motions[k])

print (p)

最终输出概率是当前机器人所在位置的概率。

本例中最后输出概率最高的为world[4]。
机器人感知到了红色，向前走了一步最可能到的是world[3]，又感知到了绿色后又向前走了一步最可能到的是world[4]。

贝叶斯公式

 

P(Xi|Z) ： 机器人不移动的情况下，看到了红色(Z)，这时机器人在第i个位置的概率

P(Z|Xi)：如果机器人在第i个位置，看到红色(Z)的概率是多少 （也就是pHit和pMiss，如果第i个位置实际上是红色，pHit就是P(Z|Xi)，如果第i个位置实际上是绿色PMiss就是P(Z|Xi)）

P(Xi) ： 什么条件都不知道的情况下，机器人在第i个格子的概率，题给说均匀分布

P(Z) ： 什么条件都不知道的情况下，机器人看到红色的概率。

全概率公式

对t时刻的格子Xi，机器人在它上面的概率P，等于所有可能转移过来的t-1时刻的格子所对应的概率与转移概率的乘积之和。