Udacity CS373:无人驾驶汽车编程学习笔记一

一:定位

蒙特卡罗定位是感知和运动的循环,每次感知都会获得信息,每次运动都会丢失信息,感知函数利用了贝叶斯规则,运动函数利用了全概率定理。

基于一维的蒙特卡罗定位程序如下:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
p = [0, 1, 0, 0, 0)
world = ['green', 'red', 'red', 'green', 'green']
measurements = ['red', 'green']
motions = [1, 1]
pHit = 0.6
pMiss = 0.2
pExact = 0.8
pOvershoot = 0.1
pUndershoot = 0.1


def sense(p, Z):
q = []
for i in range(len(p)):
hit = (Z == world[i])
q.append(p[i] * (hit * pHit + (1-hit) * pMiss))
s = sum(q)
for i in range(len(p)):
q[i] = q[i]/s
return q


def move(p, U):
q = []
for i in range(len(p)):
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)
return q


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