贝叶斯公式 全概率公式 定位问题 机器人寻路

p=[0.2, 0.2, 0.2, 0.2, 0.2]
#the lable of real world
world=['green', 'red', 'red', 'green', 'green']
#sense value before move
measurements = ['red', 'red']
#number of steps after measurements
motions = [1,1]
"""
process:
1. check measurements[0], find init position(probability), assume the probability we'r in each grid
2. move setp of each grid, also the last position is important(product one by one)
loop
"""
#sense may be error or correct
pHit = 0.6
pMiss = 0.
#move can be overshoot or not
pExact