粒子滤波的关键函数

这部分主要叙述，如何初始化粒子群，然后对粒子群做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.