Google概率无人车基础模型

p = [0.2,0.2,0.2,0.2,0.2]                #从一个均匀的先验分布开始
world = ['green','red','red','green','green'] #世界状态
measurements = ['red','green']    #测量得到的结果，红色和蓝色
motions = [1,1]        #两次移动，第一次移动1格，第二次移动2格，先感知到红色，然后移动感知到绿色，之后再移动一次
pHit = 0.6      #机器人得到正确颜色的因子
pMiss = 0.2      #机器人得到错误颜色的因子
pExact = 0.8      #机器人准确移动格子数的概率
pOvershoot = 0.1    #机器人移动超一格的概率
pUndershoot = 0.1    #机器人移动少一格的概率  

def sense(p, Z):              #定义感知函数 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              #返回感知后的格子概率

def move(p, U):            #定义移动函数 U是向左向右移动单元格的数目
    q = []
    for i in range(len(p)):          #range 0 到 p-1，不包括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)