粒子滤波算法原理及python实现

粒子滤波

一、粒子滤波步骤：

1、初始状态：用大量粒子模拟X（t），粒子在空间内均匀分布；

2、预测阶段：根据状态转移方程，每一个粒子得到一个预测粒子；

3、校正阶段：对预测粒子进行评价，越接近于真实状态的粒子，其权重越大；

4、重采样：根据粒子权重对粒子进行筛选，筛选过程中，既要大量保留权重大的粒子，又要有一小部分权重小的粒子；

5、滤波：将重采样后的粒子带入状态转移方程得到新的预测粒子，即步骤2。

（1）粒子初始化：由于对系统状态未知，所以我们认为粒子在系统的状态空间中均匀分布，初始化生成粒子。然后将所有采样输入状态转移方程，得到预测粒子。

（2）粒子预测阶段：粒子滤波根据上一时刻确定的后验概率分布，通过随机采样的方法对每一个粒子的值进行更新，再将更新过的粒子输入状态转移方程，得到一组预测值，通过这组预测值的加权组合得到该时刻的预测结果。

（3）权值更新阶段：在获取一组粒子产生的预测值后，对比该时刻真实的观测值，通过观测方程对预测值进行评价。即第i个粒子输入观测方程后能得到真实观测值的概率，令这个概率为该粒子的权重，越可能获得真实观测值的粒子对应的权重就越高，也就代表该粒子越符合真实的概率分布。

（4）粒子重采样：为了避免粒子退化的现象，在算法迭代步骤中，需要去除权值较低的粒子，对权值较高的粒子进行复制，使得粒子的分布位置更逼近真实的解。在下一次的滤波过程中，使用重采样之后的粒子，使得每一轮滤波都让粒子更符合真实状态的概率分布，再将粒子带入状态转移方程中进行预测，从而使得预测的结果更准确。
如此反复迭代上述几个阶段，将得到状态转移方程的最优估计，即我们需要的预测值。

二、上述过程，每一步的具体做法

首先，看看如下任意状态下的状态方程;

  x(t)=f(x(t-1),u(t),w(t))

  y(t)=h(x(t),e(t))

  其中，x（t）为t时刻状态，u（t）为控制量，w(t)和e(t)分别为状态噪音和观测噪音。前一个方程描述是状态转移，后一个是观测方程。

状态噪音和观测噪音是在系统建模中引入的随机性。它们表示了系统模型和观测模型中的不确定性或误差。

状态噪音（Process Noise）：

状态噪音是描述系统模型中的不确定性或外部干扰的随机变量。它表示了系统在状态转移过程中可能存在的未建模的因素或随机扰动。状态噪音通常用符号w(t)表示，其中t表示时间步。

观测噪音（Measurement Noise）：

观测噪音是描述观测模型中的不确定性或测量误差的随机变量。它表示了观测值与真实值之间的差异或测量过程中的误差。观测噪音通常用符号e(t)表示，其中t表示时间步。
在粒子滤波器中，状态噪音和观测噪音是用来模拟系统和观测的不确定性。通过引入这些噪音，可以更准确地建模实际系统和观测过程中的随机性，从而提高状态估计的准确性。

状态模型：

高斯分布的概率密度函数

系统性重采样（Systematic Resampling）：

系统性重采样是一种高效的重采样方法。它通过将累积权重均匀分布在[0,1]区间上，然后使用一个均匀分布的随机数来选择重采样的起始点，从而避免了传统重采样方法中的随机性。

重采样的思路是：既然那些权重小的不起作用了，那就不要了。要保持粒子数目不变，得用一些新的粒子来取代它们。找新粒子最简单的方法就是将权重大的粒子多复制几个出来，至于复制几个？那就在权重大的粒子里面让它们根据自己权重所占的比例去分配，也就是老大分身分得最多，老二分得次多，以此类推。

下面是系统性重采样的详细步骤：

1. 计算粒子的权重，并将权重进行归一化，使得所有权重之和等于1。
2. 计算累积权重，即将归一化后的权重进行累加，得到一个累积和数组。
3. 生成一个均匀分布的随机数，范围在[0,1]之间。
4. 根据随机数和累积和数组，确定重采样的起始点。起始点的计算方式为：起始点 = (随机数 + 粒子索引) / 粒子数量。
5. 进行系统性重采样，选择新的粒子集合。从起始点开始，每隔一个固定的步长（步长为1/粒子数量）选择一个粒子，直到选择足够数量的粒子。

求前缀和是为了将权重数组转化为一个区间数组，其中每个区间的长度与对应粒子的权重成比例，区间越大对应随机生成的数所占就越多

我们生成一个随机数，并根据这个随机数的值选择一个粒子。通过将随机数与累积权重数组进行比较，我们可以确定随机数落在哪个区间内，从而选择相应的粒子

最终效果图

code

import numpy as np
import matplotlib.pyplot as plt


def estimate(particles, weights):# 返回加权粒子的均值和方差
    """returns mean and variance of the weighted particles"""
    mean = np.average(particles, weights=weights)
    var = np.average((particles - mean) ** 2, weights=weights)
    return mean, var


def simple_resample(particles, weights):
    N = len(particles)
    cumulative_sum = np.cumsum(weights) # 求权重数组的前缀和
    # 将权重数组转化为一个区间数组，其中每个区间的长度与对应粒子的权重成比例
    cumulative_sum[-1] = 1.  # avoid round-off error 避免舍入误差 确保前缀和的最后一个元素确实等于1
    rn = np.random.rand(N)
    # 找到前缀和数组中第一个大于随机数 rn[i] 的元素的索引值，将其放入indexs数组
    indexes = np.searchsorted(cumulative_sum, rn)
    # resample according to indexes
    particles[:] = particles[indexes]# 把indexs数组对应的索引所代表的粒子赋值给新的x_P
    weights.fill(1.0 / N)
    return particles, weights


x = 0.1  # 初始真实状态
x_N = 1  # 系统过程噪声的协方差（由于是一维的，这里就是方差）
x_R = 1  # 实际测量的协方差
T = 75  # 共进行75次
N = 100  # 粒子数，越大效果越好，计算量也越大

V = 2  # 初始分布的方差,随机分布
x_P = x + np.random.randn(N) * np.sqrt(V) # 随机初始化的粒子
x_P_out = [x_P] #保存粒子每次的轨迹分布

z_out = [x ** 2 / 20 + np.random.randn(1) * np.sqrt(x_R)]  # 保存实际测量值
x_out = [x]  # 实际测量值的输出向量
x_est = x  # 估计值
x_est_out = [x_est]
# print(x_out)

for t in range(1, T):
    x = 0.5 * x + 25 * x / (1 + x ** 2) + 8 * np.cos(1.2 * (t - 1)) + np.random.randn() * np.sqrt(x_N) #状态方程
    z = x ** 2 / 20 + np.random.randn() * np.sqrt(x_R) # 测量方程
    # 更新粒子
    # 从先验p(x(k) | x(k - 1))中采样
    x_P_update = 0.5 * x_P + 25 * x_P / (1 + x_P ** 2) + 8 * np.cos(1.2 * (t - 1)) + np.random.randn(N) * np.sqrt(x_N)
    z_update = x_P_update ** 2 / 20

    # 计算权重
    P_w = (1 / np.sqrt(2 * np.pi * x_R)) * np.exp(-(z - z_update) ** 2 / (2 * x_R))#权重
    # 估计矩阵  高斯分布的概率密度函数
    # 计算每一个粒子预测与实际测量的匹配程度
    P_w /= np.sum(P_w) # 归一化
    x_est, var = estimate(x_P_update, P_w)
    # 重采样
    x_P, P_w = simple_resample(x_P_update, P_w)
    # 保存数据
    x_out.append(x)
    z_out.append(z)
    x_est_out.append(x_est)
    x_P_out.append(x_P)

# 显示粒子轨迹、真实值、估计值
t = np.arange(0, T)
x_P_out = np.asarray(x_P_out)
for i in range(0, N):
    plt.plot(t, x_P_out[:, i], color='gray') #从粒子轨迹列表 x_P_out 中获取第 i 个粒子在所有时间步骤上的值
plt.plot(t, x_out, color='lime', linewidth=2, label='true value')
plt.plot(t, x_est_out, color='red', linewidth=2, label='estimate value')
plt.legend()
plt.show()