location

location

• 如图所示，一开始机器人的位置分布是均匀分布的，

  

• 当观测到第一个门时，概率发生了变化，出现了三个峰值，表明这三处都可能是机器人的位置；随着机器人的移动，峰值相应的平移，但却更加平坦，因为移动增加了不确定性，或者说移动过程中损失了信息；这个过程称为卷积。

• 当观测到第二个门时，上一次的位置为先验概率，与观测相乘就得到了后验概率。此时机器人的位置已经很明确了。

直方图滤波：

sense

• 假设有这样一个地图，初始时机器人在每个格子的概率相同，均为0.2；当机器人观测到红色，则*0.6，若观测到绿色则*0.2，那么概率更新.（增加信息的过程）
  p(xi|z)=p(z|x)∗p(xi) $p(x_i|z)=p(z|x)\ast p(x_i)$

  p(xi|z)表示观测z下状态xi发生的概率，这边即指机器人在xi处的概率$p(x_i|z)表示观测z下状态x_i发生的概率，这边即指机器人在x_i处的概率$

  p(z|xi)表示在xi处观测到z的概率$p(z|x_i)表示在x_i处观测到z的概率$

• 得到的概率是非标准化的概率，需要将其归一化

  p(x|z)=p(z|x)∗p(x)p(z)$p(x|z)=\frac{p(z|x)\ast p(x)}{p(z)}$

  而p(z)=∑p(xi|z)$而p(z)=\sum p(x_i|z)$

move

运动更新实际上是计算全概率的过程（卷积），损失信息的过程
p(xi)=∑p(xi|xj)∗p(xj)$p(x_i)=\sum p(x_i|x_j)\ast p(x_j)$

举个例子：
0 | 0.5 | 0.5 | 0 | 0
机器人移动一步：
移动成功的概率为0.8，留在原地概率0.2
则移动后概率为
0 | 0.1 | 0.5 | 0.4 | 0 | 0

• 代码

map = [ ['G', 'G', 'G'],
        ['G', 'R', 'R'],
        ['G', 'G', 'G']]

# Motion:
#  [0,0] - stay
#  [0,1] - right
#  [0,-1] - left
#  [1,0] - down
#  [-1,0] - up

measurements=['R','G']
motions=[[0,0],[0,1]]
sensor_right = 1
pMove = 1

sensor_wrong = 1 - sensor_right
pStay = 1 - pMove

### 定义定位函数，输入是地图，观测和运动
def location(map,measurements,motions):
    pInit=1.0/float(len(map))/float(len(map[0]))
    p = [[pInit for row in range(len(map[0]))] for col in range(len(map))]

    for k in range(len(measurements)):
        p = move(p,motions[k])
        p = sense(p,map,measurements[k])
    return p

### 定义观测，更新概率
def sense(p,map,measurement):
    aux = [[0.0 for row in range(len(p[0]))] for col in range(len(p))]

    s = 0.0
    for i in range(len(p)):
        for j in range(len(p[i])):
            hit=(measurement == map[i][j])
            aux[i][j]=p[i][j]*(hit * sensor_right +(1-hit)*sensor_wrong)
            s+=aux[i][j]

    for i in range(len(aux)):
        for j in range(len(aux[0])):
            aux[i][j]/=s
    return aux

### 定义移动，更新概率
def move(p,motion):
    aux = [[0.0 for row in range(len(p[0]))] for col in range(len(p))]

    for i in range(len(p)):
        for j in range(len(p[i])):
            aux[i][j]=(pMove*p[(i-motion[0])%len(p)][(j-motion[1])%len(p[i])] + pStay*p[i][j])
    return aux

### 显示概率
def show(p):
    for i in range(len(p)):
        for j in range(len(p[0])):
            print(p[i][j])
        print('\n')

### 调用函数
prob = []
prob = location(map,measurements,motions)
show(prob)