当前位置:   article > 正文

粒子滤波运动轨迹优化案例(Python)_粒子滤波应用实例

粒子滤波应用实例

前言

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

案例

在这里插入图片描述

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

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

在这里插入图片描述

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

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

在这里插入图片描述

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

我们定义运动方程,对于x坐标我们用 z 1 z^1 z1表示,假设一个时刻为1秒。小车速度为 2 m / s 2m/s 2m/s,角速度 3.6 ° / s 3.6°/s 3.6°/s,所以时刻t的横坐标位置为
z t 1 = z t − 1 1 + v ∗ cos ⁡ ( t ∗ a n g l e ) + u 1 z^1_{t}=z^{1}_{t-1}+v*\cos(t*angle)+u_1 zt1=zt11+vcos(tangle)+u1
u 1 u_1 u1为随机误差,v为速度,angle为角速度, z t − 1 1 z^{1}_{t-1} zt11为上一时刻的横坐标位置

对于y坐标y,我们用 z 2 z^2 z2表示
z t 2 = z t − 1 2 + v ∗ sin ⁡ ( t ∗ a n g l e ) + u 2 z_t^{2}=z_{t-1}^{2}+v*\sin(t*angle)+u_2 zt2=zt12+vsin(tangle)+u2
u 2 u_2 u2为随机误差,其他都一样

而同理对于GPS的x坐标我们用 x 1 x^1 x1表示
x t 1 = z t 1 + ω 1 x^1_t=z_t^1+\omega_1 xt1=zt1+ω1
ω 1 \omega_1 ω1为随机误差, z t 1 z_t^1 zt1为运动方程中t时刻的横坐标,同理得y坐标,我们用 x 2 x^2 x2表示
x t 2 = z t 2 + ω 2 x_t^2=z_t^2+\omega_2 xt2=zt2+ω2
将上述表示成矩阵形式

运动方程
z t = A z t − 1 + B + u z_t=Az_{t-1}+B+u zt=Azt1+B+u
GPS观测方程
x t = z t + ω x_t=z_t+\omega xt=zt+ω

z t = ( z t 1 z t 2 ) ; x t = ( x t 1 x t 2 ) ; u = ( u 1 u 2 ) ; ω = ( ω 1 ω 2 ) z_t=(z1tz2t)

(z1tz2t)
; x_t=(x1tx2t)
(x1tx2t)
; u=(u1u2)
(u1u2)
; \omega=(ω1ω2)
(ω1ω2)
zt=(zt1zt2);xt=(xt1xt2);u=(u1u2);ω=(ω1ω2)
对于系数
A = ( 1 0 0 1 ) ; B = ( v ∗ c o s ( a n g l e ∗ t ) v ∗ s i n ( a n g l e ∗ t ) ) A=(1001)
(1001)
;B=(vcos(anglet)vsin(anglet))
A=(1001);B=(vcos(anglet)vsin(anglet))

为了简单起见,我们将随机误差项 u , ω u,\omega u,ω假设服从期望为 0 0 0,协方差矩阵分别为 Q , R Q,R Q,R

粒子滤波流程

t = 1 t=1 t=1

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

t ≥ 2 t\ge2 t2

②从 P ( z t ∣ z t − 1 ) P(z_t|z_{t-1}) P(ztzt1)中抽样出粒子群记作 p i , i ∈ 1 , 2 , ⋯   , N p_{i},i\in1,2,\cdots,N pi,i1,2,,N

③根据公式 w t = P ( x t ∣ z t ) w_t=P(x_t|z_t) wt=P(xtzt)当前时刻的权值。并归一化。

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

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

系统重采样流程

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

②计算权重的累计函数值,记作 c c c

③将(0,1)区间分为N份(N为重采样的次数)。得到n个区间,将所有临界点记作数组 d d d

④从(0, 1 N \frac{1}{N} N1)均匀分布中随机抽样出一个值。记作 a a a

⑤计算 s = a + d [ i ] s=a+d[i] s=a+d[i]( i 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() #训练
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65
  • 66
  • 67
  • 68
  • 69
  • 70
  • 71
  • 72
  • 73
  • 74
  • 75
  • 76
  • 77
  • 78
  • 79
  • 80
  • 81
  • 82
  • 83
  • 84
  • 85
  • 86
  • 87
  • 88
  • 89
  • 90
  • 91
  • 92
  • 93

结束

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

在这里插入图片描述

声明:本文内容由网友自发贡献,不代表【wpsshop博客】立场,版权归原作者所有,本站不承担相应法律责任。如您发现有侵权的内容,请联系我们。转载请注明出处:https://www.wpsshop.cn/w/小丑西瓜9/article/detail/124081
推荐阅读
相关标签
  

闽ICP备14008679号