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

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])