三種濾波的比較
粒子濾波的關(guān)鍵函數(shù)
這部分主要敘述士嚎，如何初始化粒子群悔叽，然后對粒子群做measurement爵嗅，并與權(quán)重對應(yīng)，最后通過巧妙設(shè)計睹晒，實現(xiàn)按照權(quán)重占比，實現(xiàn)重組（重新生成）粒子群
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)
自行車模型
在這講中，引入自行車模型的概念锉试，這篇博客講的很細(xì)致
https://blog.csdn.net/adamshan/article/details/78696874
自行車模型：一圖勝千言
課件中的表示方法與上圖一致呆盖，其對應(yīng)公式如下如所示：
運動學(xué)模型對應(yīng)公式
注意：上圖中描述的公式，其中綠色部分的beta应又，我認(rèn)為是tan(beta)，不過tan在該范圍內(nèi)是單調(diào)遞增的尤筐，所以關(guān)系不大洞就，不是很影響。
而且在最后藍(lán)色的x`的推導(dǎo)部分旬蟋，分別對兩種情況做了描述：
- 當(dāng)|beta| < 0.001時， 認(rèn)為tanx = sinx = x, 是近似相同，在物理上認(rèn)為沒有小車基本沒有偏轉(zhuǎn)
- 當(dāng)|beta| >= 0.001時稠腊， 用上面的那一排公式求取鸣哀，用到了cx和cy
基于自行車模型的粒子濾波
這部分主要兩塊：
- 面向自行車模型的move函數(shù)
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函數(shù)
與四個landmark求對應(yīng)的方向夾角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.
