location

location

基本概念

• 1） 条件概率（后验概率） p(A|B) 表示在事件B的情况下，A发生的概率
• 2） 联合概率 p(AB)、p(A,B)、p(AUB) 表示AB同时发生的概率
• 3） 先验概率（边缘概率） p（A）

基本公式：

• 全概率公式：
  $p(A)={\sum }_{Bi}^{}p(A|Bi)p(Bi))$
  其中Bi可以理解为A发生的一种途径

• 贝叶斯公式：
  $p(A|B)=\frac{p(A)p(B|A)}{P(B)}$

贝叶斯公式是一个从果推因的过程，在B发生的情况求A发生的概率。比如从盒子1里、盒子2里拿球，盒子里面有白球红球，比例各不相同。全概率用来求拿一个球是红球的概率；而贝叶斯是用来求拿到红球时，盒子为1的概率。

根据公式p(盒子1 | 红球) = p(盒子1)*p（从盒子1中拿红球） / p(红球)
p(红球) = p(盒子1)*p（从盒子1中拿红球）+ p(盒子2)*p（从盒子2中拿红球）

p(B|A) 似然
p(A) 先验

https://blog.csdn.net/sdutacm/article/details/50938957

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

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

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

直方图滤波：

sense

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

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

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

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

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

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

move

运动更新实际上是计算全概率的过程（卷积），损失信息的过程
$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)