粒子滤波运动轨迹优化案例（Python）

前言

上一篇【机器学习】粒子滤波原理推导（二），我们已经对粒子滤波的原理和实现流程进行了原理推导求解。接下来，我们就案例而言，来看看粒子滤波的实际应用。

案例

假设，有一辆汽车，其初始位置是$x,y=(0,0)$，经过了50个时刻后，按照我们运动方程式，假设它能够去到点$x,y=(0,65)$的位置。也就是做个半圆运动。

但是现实生活中，往往存在各种各样的噪声，也许是地面凹凸不平，亦或者是风阻较大，所以我们引入误差。结果得到的图是这样。

可以看到，每一个时刻，它并不是按照运动方程规定的那样。是存在一定的误差的，也许前一个时刻快了一点，也许下一个时刻又慢了一点。

因为存在误差，所以我们给这辆汽车搭载一个GPS定位，每一个时刻都返回汽车的位置信息。但是GPS的定位也不精准，也存在一定的误差。结果得到的图是这样的

那么如何利用粒子滤波，让其预测相对来说更加合理。

我们定义运动方程，对于x坐标我们用$z^1$表示，假设一个时刻为1秒。小车速度为$2m/s$，角速度$3.6°/s$，所以时刻t的横坐标位置为
$$z_t^1=z_{t-1}^1+v\ast \cos (t\ast angle)+u_1$$
$u_1$为随机误差，v为速度，angle为角速度，$z_{t-1}^1$为上一时刻的横坐标位置

对于y坐标y，我们用$z^2$表示
$$z_t^2=z_{t-1}^2+v\ast \sin (t\ast angle)+u_2$$
$u_2$为随机误差，其他都一样

而同理对于GPS的x坐标我们用$x^1$表示
$$x_t^1=z_t^1+{\omega }_1$$
${\omega }_1$为随机误差，$z_t^1$为运动方程中t时刻的横坐标，同理得y坐标，我们用$x^2$表示
$$x_t^2=z_t^2+{\omega }_2$$
将上述表示成矩阵形式

运动方程
$$z_t=A{z}_{t-1}+B+u$$
GPS观测方程
$$x_t=z_t+\omega$$

$$z_t=\left(\begin{array}{c}{z}_t^1\\ z_t^2\end{array}\right);x_t=\left(\begin{array}{c}{x}_t^1\\ x_t^2\end{array}\right);u=\left(\begin{array}{c}{u}_1\\ u_2\end{array}\right);\omega =\left(\begin{array}{c}{\omega }_1\\ {\omega }_2\end{array}\right)$$
对于系数
$$A=\left(\begin{array}{cc}1& 0\\ 0& 1\end{array}\right);B=\left(\begin{array}{c}v\ast cos(angle\ast t)\\ v\ast sin(angle\ast t)\end{array}\right)$$
为了简单起见，我们将随机误差项$u,\omega$假设服从期望为$0$，协方差矩阵分别为$Q,R$。

粒子滤波流程

当$t=1$时

①在均匀分布中随机初始化粒子群，并且令当前时刻的权重w=$\frac{1}{N}$（N为采样次数）。

当$t\ge 2$

②从$P(z_t|z_{t-1})$中抽样出粒子群记作$p_i,i\in 1,2,\cdots ,N$

③根据公式$w_t=P(x_t|z_t)$当前时刻的权值。并归一化。

④重采样得到新的粒子群，求出粒子群的均值，为当前时刻的预测值。

⑤迭代②~④步骤，得到所有的均值。

系统重采样流程

​ ①归一化权重，令所有的权重的和为1.

​ ②计算权重的累计函数值，记作$c$

​ ③将（0，1）区间分为N份（N为重采样的次数）。得到n个区间，将所有临界点记作数组$d$

​ ④从（0，$\frac{1}{N}$）均匀分布中随机抽样出一个值。记作$a$

​ ⑤计算$s=a+d[i]$($i$为数组d的一个区间的索引)，我们迭代数组d的所有区间，得到数组s。并初始化j=0

​ ⑥for i in N:

​ 如果s[ i ] > c [ j ]：j++

​ 否则将索引 j 保存起来

​ ⑦迭代完N次之后，根据所有的索引 j 取出对应位置的粒子的值。就是我们重采样之后的样本。

代码实现

先看一下效果，左边为轨迹图，右边为误差图，代码中，运动方程的方差我设置的比较小，而对于GPS定位我设定的方差较大，所以对于粒子滤波的轨迹图，它更加的靠近运动方程，误差是相对于运动方程而言的，所以粒子滤波的误差比写GPS定位的要小。

import matplotlib.pyplot as plt
import numpy as np
from scipy import stats
plt.rcParams['font.sans-serif'] = ['SimHei']
plt.rcParams['axes.unicode_minus'] = False
fig,axes=plt.subplots(1,2) #绘图，一行两列的子图
np.random.seed(3) #随机数种子
class Particle_Filter():
    def __init__(self):
        pass
    def train(self):
        #初始化权重和粒子群
        P=np.random.uniform(0,40,size=(2,N)) #从[0,40]均匀分布中抽样
        w=np.ones(shape=N)/N #设置权重都为 1/N
        centers=np.zeros(shape=(2,T)) #粒子群的中心点，用于储存每一个时刻的中心点
        for t in np.arange(1,T): #从第一个时刻开始
            #B
            B = np.array([[np.cos(angle_speed * t) * 2],
                          [np.sin(angle_speed * t) * 2]])

            #采样粒子并更新权重
            for i in np.arange(w.shape[0]):
                #从P(z_t|z_t-1)中采样
                P[:,i] =stats.multivariate_normal.rvs(mean=A@P[:,i]+B.reshape(-1), cov=Q).reshape(-1)

                #利用P(x_t|z_t)计算权重，其中里面的z_t就是粒子，x_t就是GPS定位所得的值
                w[i]=stats.multivariate_normal.pdf(x=X[:,t],mean=P[:,i],cov=R)
            #归一化
            w=w/np.sum(w)

            #重采样(系统重采样)
            cum=np.cumsum(w)  #累计函数

            s=(np.random.uniform(0,1/N)+np.arange(0,1,1/N)) #区间s

            P_index=np.zeros(shape=N,dtype=int) #初始化一个数组，用于储存对应索引
            index=0  #从第一项开始
            for i in np.arange(N):

                while s[i]>cum[index]:
                    index+=1
                P_index[i]=index #储存索引
            P=P[:,P_index]  #取出索引对应的粒子
            center=P.mean(axis=1) #求出均值
            centers[:,t]=center #储存结果

        plot_figure(Z, X,centers)



def plot_figure(x,y,centers): #绘图函数
    axes[0].plot(x[0,:],x[1,:],label="运动方程",linewidth=2,marker="o") #绘制运动方程轨迹图
    axes[0].plot(y[0,:],y[1,:],label="GPS",linewidth=2,marker="o") #绘制GPS轨迹图
    axes[0].plot(centers[0, :], centers[1, :], label="粒子滤波", linewidth=2, marker="o")#粒子滤波轨迹图
    axes[0].set_title("轨迹")
    axes[0].set_xlabel("位置x")
    axes[0].set_ylabel("位置y")
    axes[0].legend()
    err_X = np.linalg.norm(X - Z, axis=0) #观测误差
    err_particle = np.linalg.norm(centers - Z, axis=0) #粒子滤波误差
    axes[1].plot(np.arange(T), err_X, label="观测误差", linewidth=2, marker="o")
    axes[1].plot(np.arange(T), err_particle, label="滤波误差", linewidth=2, marker="o")
    axes[1].set_title("误差")
    axes[1].set_xlabel("时刻t")
    axes[1].set_ylabel("误差")
    axes[1].legend()
    plt.show()

if __name__ == '__main__':
    pi=np.pi
    V=2  #  速度 2m/s
    T=50 #  记录10个时刻
    angle_speed=pi/T  #角速度
    X=np.zeros(shape=(2,T)) # 观测变量（GPS定位） 2行10列  用于储存每个时刻数据
    Z=np.zeros(shape=(2,T)) #  隐变量（运动方程）  2行10列  用于储存每个时刻数据
    Q=np.array([[0.1,0],
                [0,0.1]])  #运动方程的过程噪声，假设服从均值为0，协方差矩阵为Q的正态分布
    R=np.array([[10,0],
               [0,0.10]]) #GPS的过程噪声，假设服从均值为0，协方差矩阵为R的正态分布
    mean = np.array([0, 0]) #随机误差(过程噪声)的均值
    A=np.array([[1,0],
                [0,1]]) #运动方程的系数
    N=1000 #每次抽样的粒子数
    for t in np.arange(1,T):
        B=np.array([[np.cos(angle_speed*t)*2],
                    [np.sin(angle_speed*t)*2]]) #截距项B

        #计算每一个时刻的运动方程，并且加上误差项
        Z[:,t]=A@Z[:,t-1]+B.reshape(-1)+stats.multivariate_normal.rvs(mean=mean,cov=Q)
        #计算每一个时刻的GPS位置观测数据
        X[:,t]=Z[:,t]+stats.multivariate_normal.rvs(mean=mean,cov=R)
    particle_filter=Particle_Filter() #初始化
    particle_filter.train() #训练

结束

归根究底，这个问题仍然是一个线性问题，实际上跟卡尔曼滤波没啥区别，只是用的粒子滤波去求解罢了。如有问题，还望指出。阿里嘎多。