三种滤波的比较
粒子滤波的关键函数
这部分主要叙述，如何初始化粒子群，然后对粒子群做measurement，并与权重对应，最后通过巧妙设计，实现按照权重占比，实现重组（重新生成）粒子群
def get_position(p):
x = 0.0
y = 0.0
orientation = 0.0
for i in range(len(p)):
x += p[i].x
y += p[i].y
# orientation is tricky because it is cyclic. By normalizing
# around the first particle we are somewhat more robust to
# the 0=2pi problem [-pi,pi]
orientation += (((p[i].orientation - p[0].orientation + pi) % (2.0 * pi))
+ p[0].orientation - pi)
return [x / len(p), y / len(p), orientation / len(p)]
def particle_filter(motions, measurements, N=500): # I know it's tempting, but don't change N!
# --------
#
# Make particles
#
p = []
for i in range(N):
r = robot()
r.set_noise(bearing_noise, steering_noise, distance_noise)
p.append(r)
# --------
#
# Update particles
#
for t in range(len(motions)):
# motion update (prediction)
p2 = []
for i in range(N):
p2.append(p[i].move(motions[t]))
p = p2
# measurement update
w = []
for i in range(N):
w.append(p[i].measurement_prob(measurements[t]))
# resampling
p3 = []
index = int(random.random() * N)
beta = 0.0
mw = max(w)
for i in range(N):
beta += random.random() * 2.0 * mw
while beta > w[index]:
beta -= w[index]
index = (index + 1) % N
p3.append(p[index])
p = p3
return get_position(p)
自行车模型
在这讲中，引入自行车模型的概念，这篇博客讲的很细致
https://blog.csdn.net/adamshan/article/details/78696874
自行车模型：一图胜千言
课件中的表示方法与上图一致，其对应公式如下如所示：
运动学模型对应公式
注意：上图中描述的公式，其中绿色部分的beta，我认为是tan(beta)，不过tan在该范围内是单调递增的，所以关系不大，不是很影响。
而且在最后蓝色的x`的推导部分，分别对两种情况做了描述：
- 当|beta| < 0.001时， 认为tanx = sinx = x, 是近似相同，在物理上认为没有小车基本没有偏转
- 当|beta| >= 0.001时， 用上面的那一排公式求取，用到了cx和cy
基于自行车模型的粒子滤波
这部分主要两块：
- 面向自行车模型的move函数
def move(self, motion): # Do not change the name of this function
alfa = motion[0]
d = motion[1]
if alfa > max_steering_angle:
raise ValueError('the steer angle is too much')
if d < 0:
raise ValueError('the distance is less than zero')
result = robot()
result.distance_noise = self.distance_noise
result.steering_noise = self.steering_noise
result.bearing_noise = self.bearing_noise
result.length = self.length
steer = random.gauss(alfa, steering_noise)
distance = random.gauss(d, distance_noise)
beta = d / self.length * tan(steer)
R = distance / beta
result.orientation = (self.orientation + beta) % (2 * pi)
if abs(beta) < 0.001:
result.x = self.x + distance * cos(self.orientation)
result.y = self.y + distance * sin(self.orientation)
else:
cx = result.x - sin(self.orientation) * R
cy = result.y + cos(self.orientation) * R
result.x = cx + sin(beta+self.orientation) * R
result.y = cy - cos(beta+self.orientation) * R
return result
- 新的感知sense函数
与四个landmark求对应的方向夹角bearings
def sense(self,flag): # do not change the name of this function
Z = []
# ENTER CODE HERE
# HINT: You will probably need to use the function atan2()
for i in range(len(landmarks)):
if flag:
Z.append(((atan2((landmarks[i][0] - self.y), (landmarks[i][1] - self.x)) - self.orientation) % (2 * pi)) + random.gauss(0,self.bearing_noise))
else:
Z.append(((atan2((landmarks[i][0] - self.y), (landmarks[i][1] - self.x)) - self.orientation) % (2 * pi)))
return Z # Leave this line here. Return vector Z of 4 bearings.
