粒子滤波器 Python 代码实现

粒子滤波器 Python 代码实现 一个简单的粒子滤波器应用实例

机器人在一个二维平面运动，在这个二维平面中随机采样 1000 个粒子（即创建 1000 个机器人）。机器人每移动一步，粒子做完全相同的移动，接着计算粒子权重，并根据粒子权重进行粒子重采样。

1. 重采样方法一：选取权重高的粒子，复制这些被选中的粒子，使总粒子数为 1000，例如：选取权重排名前 100 的粒子，对这 100 个粒子复制 10 倍，最后粒子总和即为 1000。
2. 重采样方法二：详解视频 车轮法选取粒子

详细代码如下：

from math import *
import random

# 机器人四个参照物
landmarks = [[20.0, 20.0],[80.0, 80.0],[20.0, 80.0],[80.0, 20.0]]
# 地图大小
world_size = 100.0
class robot:
    def __init__(self):
        # 给机器人初始化一个坐标和方向
        self.x = random.random() * world_size
        self.y = random.random() * world_size
        self.oriention = random.random() * 2.0 * pi
        
        # 初始化噪声
        self.forward_noise = 0.0
        self.turn_noise = 0.0
        self.sense_noise = 0.0

    def set(self, new_x, new_y, new_orientation):
        # 设定机器人的坐标、方向
        if new_x < 0 or new_x >= world_size:
            raise ValueError('X coordinate out of bound')
        if new_y < 0 or new_y >= world_size:
            raise ValueError('Y coordinate out of bound')
        if new_orientation < 0 or new_orientation >= 2 * pi:
            raise ValueError('Oriention must be in [0, 2pi]')
        self.x = float(new_x)
        self.y = float(new_y)
        self.oriention = float(new_orientation)

    def set_noise(self, new_f_noise, new_t_noise, new_s_noise):
        # makes it possible to change the noise parameters
        # this is often useful in particle filters
        # 设定机器人的噪声
        self.forward_noise = float(new_f_noise)
        self.turn_noise = float(new_t_noise)
        self.sense_noise = float(new_s_noise)

    def sense(self):
        # 测量机器人到四个参照物的距离，添加高斯噪声
        Z = []
        for i in range(len(landmarks)):
            dist = sqrt((self.x - landmarks[i][0])**2 + (self.y - landmarks[i][1])**2) 
            dist += random.gauss(0.0, self.sense_noise) # random.gauss(mu, sigma)
            Z.append(dist)
        return Z

    def move(self, turn, forward):
        # 机器人转向、前进，并返回更新后的机器人新的坐标和噪声大小
        if forward < 0:
            raise ValueError('Robot can't move backwards')
        # turn, and add randomness to the turning command
        orentation = self.oriention + float(turn) + random.gauss(0.0, self.turn_noise)
        orentation %= 2 * pi

        # move, and addd randomness to the motion command
        dist = float(forward) + random.gauss(0.0, self.forward_noise)
        x = self.x + (cos(orentation) * dist)
        y = self.y + (sin(orentation) * dist)
        x %= world_size
        y %= world_size


    def Gaussian(self, mu, sigma, x):
        return exp(- ((mu - x) ** 2) / (sigma ** 2) / 2.0) / sqrt(2.0 * pi * (sigma ** 2))

    def measurement_prob(self, measurement):
        # calculates how likely a measurement should be
        # 计算出的距离相对于正确距离的概率， 计算值距离测量值越近，则概率越大
        prob = 1.0
        for i in range(len(landmarks)):
            dist = sqrt((self.x - landmarks[i][0])**2 + (self.y - landmarks[i][1])**2)
            # 与参考物的距离为期望，感知噪声为方差
            prob *= self.Gaussian(dist, self.sense_noise, measurement[i])
        return prob


def prn_obj(obj):
    print ('n'.join(['%s:%s' % item for item in obj.__dict__.items()]))

'''
### test the robot class
# 初始化一个机器人
myrobot = robot()
# 打印机器人的所有参数
# prn_obj(myrobot)


# 设定初始位置
myrobot.set(30, 50, 0.5)

# 设定噪声
myrobot.set_noise(5.0, 0.1, 5.0)

# 打印机器人
# prn_obj(myrobot)

# 打印与参考物的距离
Z = myrobot.sense()
# print(Z)

# 机器人移动
myrobot.move(pi/2, 10.0)
# prn_obj(myrobot)
Z = myrobot.sense()
# print(Z)
'''
## 在运动初期给机器人初始化1000个位置粒子，这些粒子随机分布在整个地图中
myrobot = robot()
myrobot.move(0.1, 5.0)  # turn: 0.1; forward: 5.0
Z = myrobot.sense()
N = 1000
# 初始化1000 个粒子
p = []
for i in range(N):
    x = robot()
    x.set_noise(0.05, 0.05, 5.0)
    x.move(0.1, 5.0)
    p.append(x)

# 计算各个粒子的权重
# 粒子权重为粒子位置与机器人位置的误差，并将 [index, w] 对存入字典
w = []
dic = {}
for i in range(N):
    prob = p[i].measurement_prob(Z)
    w.append(prob)
    dic.update({i:prob})
# print(w)
# print(dic)

'''
## 粒子重采样：车轮法选取粒子
p_resemple = []
index = int(random.randint(0, N))
beta = 0.0
w_max = max(w)
for i in range(N):
    beta += random.random() * 2.0 * w_max
    while beta > w[index]:
        beta -= w[index]
        index = (index+1) % N
    p_resemple.append(p[index])
print(len(p_resemple))
prn_obj(p_resemple[0])
'''

## 粒子重采样：挑选权重大的前100个粒子，每个粒子复制10次
# index = []
w.sort()
p_resemple = []
for i in range(100):
    index = list(dic.keys())[list(dic.values()).index(w[i])]
    p_resemple.extend([p[index]] * 10)
    # index.extend([index_chosen] * 10)
prn_obj(p_resemple[0])