0

2D でのロボットのローカリゼーションに関するコードを書いていますが、移動ロジックに行き詰まっています。ケースが [1,0,0,1....] の場合の動きは理解できますが、動きが [[1,0],[0,1].... の場合の実装方法がわかりません。 ...]

画像はこちら

#SENSE AND MOVE (a measurment and a movement)
from math import log

p = [0.2, 0.2, 0.2, 0.2, 0.2]           # Initial cell probability
w = ['R', 'G', 'G', 'R','R']# World
w = ['R','R', 'G','R','R']# World
w = ['R','R', 'G', 'G', 'R']# World
w = ['R','R','R','R','R']# World
meas = ['G','G','G','G','G']   # measurements
mov  = [[0,0],[0,1],[1,0],[1,0],[0,1]]  # motion
phit = .6                      # Probability to measure: R->0.6
pmiss = .2                     # Probability to measure: R->0.2
pExact = .8                    # Prob. exact motion
pOver = .1                     # Prob. overshoot
pUnder = .1                    # Prob. undershoot

def entropy (p):
    s = [p[i]*log(p[i]) for i in range(len(p))]
    return round(-sum(s), 2)

def sense(p, z):
    q = []
    for i in range(len(p)):
        hit = w[i]==z
        q.append( p[i]*(phit*hit + pmiss*(1-hit)) )
    s = sum(q)
    q = [i/s for i in q]
    return q


#Moving u cells
def move(p, u):
    q = []
    for i in range(len(p)):
        motion = pExact * p[(i-u)%len(p)]
        motion += pOver * p[(i-u-1)%len(p)]
        motion += pUnder * p[(i-u+1)%len(p)]
        q.append(motion)
    return q

for i in range(len(meas)):
    p = sense(p, meas[i])
    r = [format(j,'.3f') for j in p]
    print "Sense %i:"%(i),
    print r, entropy(p)
    p = move(p, mov[i])
    r = [format(j,'.3f') for j in p]
    print "Move  %i:"%(i),
    print r, entropy(p)
    print
4

0 に答える 0