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

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 = 0.8
pOvershoot = 0.1
pUndershoot = 0.1

"""self drive located is made of
move and sense circle
"""

def sense(p, Z):
q=[]
for i in range(len(p)):
hit = (Z == world[i])
#actually it is bayes rule
#p[i] is prior and p(i|Z) is measurements probability
q.append(p[i] * (hit * pHit + (1-hit) * pMiss))
s = sum(q)
for i in range(len(q)):
q[i] = q[i] / s
print q
return q

def move(p, U):
"""move many (U) steps
Args:
p : list
probolity list
U : int
object of shot,
Returns:
q:list
after moving
"""
near = 0
success = 0
over = 0
if U == 0:
return p
q = [0 for i in range(len(p))]
# in every shotting process:
for i in range(len(p)):
if p[i] != 0:
#if U is very large the follow should change
#now U is in (-len(p), len(p))
if U > 0:
near = (i + U - 1 - len(p)) if (i + U - 1 - len(p) >= 0) else (i + U - 1)
success = (i + U - len(p)) if (i + U - len(p) >= 0 ) else (i + U)
over = (i + U + 1 - len(p)) if (i + U + 1 - len(p) >= 0 ) else (i + U + 1)
if U < 0:
near = i + U + 1
success = i + U
over = i + U - 1

q[near] += p[i] * pUndershoot
q[success] += p[i] * pExact
q[over] += p[i] * pOvershoot
else:
continue
return q

def move_simple(p, U):
"""better version to move, can single step or many

- can also design a move 1 step func and a util func to
move many steps
- always go roght
- it is a circle list
-i-u means go right and i+u means go left

Args:
p:list
U:int
"""
q = []
#there, i-u means go right and i+u means go left
for i in range(len(p)):
#actually it is total probability
#pExact is prior
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)
print q
return q

# p = [0,1,0,0,0]
#
# print move_simple(p,3)

# print move(p,-2)
for i in range(len(measurements)):
print "sense"
p = sense(p, measurements[i])
print "move"
p = move_simple(p, motions[i])