CN104022756B - 一种基于gpu架构的改进的粒子滤波方法 - Google Patents

Abstract

本发明属于粒子滤波技术领域，特别涉及一种基于GPU架构的改进的粒子滤波方法。该基于GPU架构的改进的粒子滤波方法包括以下步骤：S1：在CPU端设定粒子个数和观测时刻k；在GPU端初始化粒子；S2：将观测向量传输至GPU显存；当k＝1时，执行步骤S3；S3：在GPU端进行重要性采样；S4：在GPU端进行二次采样，得到k时刻的最大似然采样粒子；S5：利用GPU得出k时刻每个最大似然采样粒子的接受概率；S6：在GPU端计算k时刻估计值。S7：在CPU端计算k时刻每个最大似然采样粒子的重采样索引；在GPU端根据重采样索引得出k时刻的重采样粒子，作为下一时刻的初始粒子；S8：将步骤S3至步骤S7重复执行M次，得出M个时刻的估计值。

Description

一种基于GPU架构的改进的粒子滤波方法

技术领域

本发明属于粒子滤波技术领域，特别涉及一种基于GPU架构的改进的粒子滤波方法。

背景技术

非线性滤波问题广泛存在于信号处理、数据通信、雷达探测、目标跟踪、卫星导航等诸多领域，这类问题可归纳为存在观测噪声时，非线性系统的状态估计问题。粒子滤波方法是一种较为通用的滤波方法，其采用一系列带有权值的样本点对状态的后验概率分布进行近似，这种方法实质上基于状态搜索进行的。由于在状态逼近的过程中使用了大量的粒子，因此该方法的计算复杂度很高。而且在粒子滤波中存在着两个主要的问题：第一，当粒子采样不准确时，如采样得到的粒子位于实际后验分布的拖尾区域，经过状态搜索后，绝大部分的粒子权值都会趋于0。这会为后验分布的近似带来很大的误差，甚至有可能引起滤波发散。第二，在对状态进行迭代估计的过程中，会出现粒子退化以及粒子贫化的现象，降低了状态估计可用粒子的数量和种类，增大了状态估计误差。

发明内容

本发明的目的在于提出一种基于GPU(图形处理器)架构的改进的粒子滤波方法。本发明，对粒子的重采样进行了改进，提出一种基于二次采样的粒子滤波方法，改善了状态估计的精度。同时，针对粒子滤波方法计算复杂度高的问题，提出了一种基于GPU架构的实现方法，使得运算时间大大降低，提高了算法处理的实时性，可以满足实时处理的需要。

本发明的技术思路是：在标准粒子滤波基础上，通过最大化似然函数引入当前时刻的观测信息，进行二次采样，并使用似然函数计算新粒子的权值，最后通过粒子加权对当前的状态进行估计。并且将计算复杂度高的部分在GPU架构上进行实现，提高了算法的实时性。

为实现上述技术目的，本发明采用如下技术方案予以实现。

一种基于GPU架构的改进的粒子滤波方法包括以下步骤：

S1：利用CPU将粒子个数设定为N，利用CPU设定M个观测时刻，N和M均为大于1的自然数，所述M个观测时刻依次表示为1时刻至M时刻；利用GPU生成N个1时刻初始粒子；设置观测时刻参数k，k＝1,2,3,...M；

S2：利用CPU加载每个观测时刻非线性系统的观测向量，CPU将每个观测时刻非线性系统的观测向量传输至GPU显存；当k＝1时，执行步骤S3；

S3：在GPU中，根据重要性密度函数对k时刻的每个初始粒子进行重要性采样，得出k时刻的多个重要性采样粒子；

S4：在GPU中，根据非线性系统的观测模型，建立似然函数；然后，通过最大化似然函数，对k时刻的每个重要性采样粒子进行二次采样，产生k时刻的多个最大似然采样粒子；

S5：利用GPU得出k时刻每个最大似然采样粒子的接受概率；

S6：在GPU中，根据k时刻每个最大似然采样粒子，得出k时刻非线性系统的状态向量的估计值；

S7：GPU将k时刻每个最大似然采样粒子的接受概率传输至CPU；在CPU中，根据所述k时刻每个最大似然采样粒子的接受概率，得出k时刻每个最大似然采样粒子的重采样索引；然后，CPU将k时刻每个最大似然采样粒子的重采样索引传输至GPU；在GPU中，根据k时刻每个最大似然采样粒子的重采样索引，对k时刻每个最大似然采样粒子进行重采样，得出k+1时刻的多个初始粒子；令k值自增1，然后返回至步骤S3；

S8：将步骤S3至步骤S7重复执行M次，得出M个观测时刻的非线性系统的状态向量的估计值。

本发明的特点和进一步改进在于：

在步骤S1中，建立非线性系统的状态模型和观测模型，非线性系统的状态模型和观测模型表示如下：

$\left\{\begin{array}{c}{x}_k=f\left(x_{k-1}\right)+u_k\\ y_k=h\left(x_k\right)+v_k\end{array}\right.$

其中，x_k表示k时刻非线性系统的m维状态向量，y_k表示k时刻非线性系统的n维观测向量，m和n均为大于0的自然数；f(·)为描述非线性系统状态模型的非线性函数，h(·)描述非线性系统的观测模型的非线性函数；u_k为设定的服从高斯分布的k时刻状态噪声，当m＝1时，u_k服从均值为0方差为Q的高斯分布，当m>1时，u_k的均值为m维零向量，协方差矩阵为Q；v_k为设定的服从高斯分布的k时刻观测噪声，当n＝1时，v_k服从均值为0方差为R的高斯分布，当n>1时，v_k的均值为n维零向量，协方差矩阵为R；

在步骤S1中，根据所述非线性系统的状态模型和观测模型，利用GPU生成1时刻至M时刻的状态噪声；在步骤S1中，1时刻第i个初始粒子值为i取1至N。

在步骤S2中，k时刻非线性系统的观测向量为y_k；

在步骤S3中，根据所述非线性系统的状态模型，得出初始粒子的状态转移密度函数p(x_k|x_k-1)，选取状态转移密度函数为重要性密度函数q(x_k|x_k-1,Y_k)＝p(x_k|x_k-1)，得出k时刻第i个初始粒子的重要性密度函数 $q\left(x_k^i\right|x_{k-1}^i,Y_k)=p\left(x_k^i\right|x_{k-1}^i);$ 根据k时刻第i个初始粒子的重要性密度函数对k时刻的每个初始粒子进行重要性采样，得出k时刻的多个重要性采样粒子，k时刻的第i个重要性采样粒子值为

在步骤S3中，得出的k时刻的第i个重要性采样粒子值为

在步骤S4中，根据以下公式得出k时刻第i个最大似然采样粒子值

${\hat{x}}_k^i={\left(H_k^T{R}^{-1}{H}_k\right)}^{-1}{H}_k^T{R}^{-1}[y_k-h\left(x_{k|k-1}^i\right)+H_k{x}_{k|k-1}^i]$

其中，T表示矩阵或向量的转置，上标-1表示矩阵的逆， 为h(x_k)对x_k的一阶导数。

在步骤S5中，根据所述非线性系统的观测模型，得出似然函数p(y_k|x_k)；确定k时刻第i个最大似然采样粒子的重要性权值当k＝1时，当k>1时，根据k时刻第i个最大似然采样粒子的重要性权值得出k时刻第i个最大似然采样粒子的接受概率

${\stackrel{\‾}{w}}_k^i=\frac{w_k^i}{w_{k\_\mathrm{sum}}}$

$w_{k\_\mathrm{sum}}=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w}_k^i;$

在步骤S6中，根据以下公式得出k时刻非线性系统的状态向量的估计值x_{k_est}：

$x_{k\_\mathrm{est}}=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\stackrel{\‾}{w}}_k^i{\hat{x}}_k^i$

其中，i取1至N，k取1至M。

在步骤S1中，利用GPU生成N×M个在0到1之间均匀分布的随机数，在N×M个在0到1之间均匀分布的随机数中，与k时刻第i个初始粒子对应的随机数表示为其中，k取1至M，i取1至N；

在步骤S7中，在GPU端，计算出与k时刻第i个初始粒子对应的过渡取值

${\mathrm{\λ}}_k^i=\frac{(i-1)+r_k^i}{N};$

然后，将与k时刻每个初始粒子对应的过渡取值、以及k时刻每个最大似然采样粒子的接受概率传输至CPU；

在CPU中，针对至进行累加计算，得出N个累加计算结果，其中，第m'个累加计算结果为：j取1至m'，m'取1至N；

利用CPU找出满足下式的整数

$\underset{g=1}{\overset{m_k^i-1}{\mathrm{\&Sigma;}}}{\stackrel{\&OverBar;}{w}}_k^g<{\mathrm{\&lambda;}}_k^i<\underset{h=1}{\overset{m_k^i}{\mathrm{\&Sigma;}}}{\stackrel{\&OverBar;}{w}}_k^h$

其中，g取1至h取1至将整数作为k时刻第i个最大似然采样粒子的重采样索引；然后，将k时刻每个最大似然采样粒子的重采样索引传输至GPU；

在步骤S7中，根据k时刻每个最大似然采样粒子的重采样索引，对k时刻每个最大似然采样粒子进行重采样，得出k+1时刻的多个初始粒子。

在步骤S4之后，在得出k时刻第i个最大似然采样粒子值之后，将步骤S4重复执行K次，在每次重复执行步骤S4之前，将上次执行步骤S4得出的k时刻第i个最大似然采样粒子值赋值给K为大于0的自然数。

在步骤S1中，利用CPU分配CPU内存空间和GPU显存空间；

在步骤S8之后，利用CPU将已分配的CPU内存空间和GPU显存空间销毁。

本发明的有益效果为：1)本发明借助最大似然估计，将当前时刻的观测信息引入到粒子的二次采样中，充分利用了可信(有用)的观测信息，避免了传统粒子滤波中观测信息仅用来评判粒子好坏的问题。2)本发明使用似然函数计算新粒子的权值，最后通过粒子加权对当前的状态进行估计，估计完后再对粒子进行二次采样，提高了位于真实后验分布附近采样粒子的数量，缓解了迭代过程中出现的粒子退化现象，进而改善了状态估计的精度。3)本发明对通过似然函数计算新粒子的权值这一方法重复多次进行，进一步提高了状态估计的精度。4)本发明借助于并行计算能力强大的GPU平台，将改进后的粒子滤波方法在GPU平台上进行实现，降低了处理时间，提高其处理的实时性。

附图说明

图1为本发明的一种基于GPU架构的改进的粒子滤波方法的流程图；

图2为仿真实验1中本发明和标准粒子滤波方法的非线性系统的状态向量估计精度对比示意图；

图3为图2的局部放大示意图；

图4为仿真实验1中本发明和标准粒子滤波方法的非线性系统的状态向量均方根误差的对比示意图；

图5为仿真实验2中两种方法的非线性系统的状态向量均方根误差的对比示意图；

图6，仿真实验3中N为不同取值时采用两种方法的处理时间对比示意图。

图7，仿真实验4中K为不同取值时本发明的估计精度对比图。

具体实施方式

下面结合附图对本发明作进一步说明：

参照图1，为本发明的一种基于GPU架构的改进的粒子滤波方法的流程图。本发明应用的粒子滤波方法是基于GPU与CPU(中央处理器)异构平台实现的，其中CPU完成逻辑判断较多的新粒子索引产生的计算，其余计算量大且并行性好的部分(如重要性权值计算、归一化、状态向量估计、下一时刻初始粒子生成、最大似然采样粒子生成)在GPU平台上进行。

本发明实施例中，该基于GPU架构的改进的粒子滤波方法包括以下步骤：

S1：利用CPU将粒子个数设定为N，利用CPU设定M个观测时刻，N和M均为大于1的自然数，上述M个观测时刻依次表示为1时刻至M时刻；利用GPU生成N个1时刻(即初始时刻)初始粒子；1时刻第i个初始粒子值为i取1至N。设置观测时刻参数k，k＝1,2,3,...M。具体说明如下：

在步骤S1中，当k大于1时，建立非线性系统的状态模型和观测模型，非线性系统的状态模型和观测模型表示如下：

$\left\{\begin{array}{c}{x}_k=f\left(x_{k-1}\right)+u_k\\ y_k=h\left(x_k\right)+v_k\end{array}\right.$

其中，x_k表示k时刻非线性系统的m维状态向量(x_k∈R^m)，例如，x_k表示k时刻雷达目标的位置、速度和方位角。y_k表示k时刻非线性系统的n维观测向量(y_k∈Rⁿ)，例如，y_k表示k时刻雷达目标的回波数据。m和n均为大于0的自然数；f(·)为描述非线性系统的状态模型的非线性函数，h(·)描述非线性系统的观测模型的非线性函数；u_k为设定的服从高斯分布的k时刻状态噪声，当m＝1时，u_k服从均值为0方差为Q的高斯分布，当m>1时，u_k的均值为m维零向量(m维零向量指其中的所有m个元素均为0)，协方差矩阵为Q；v_k为设定的服从高斯分布的k时刻观测噪声，当n＝1时，v_k服从均值为0方差为R的高斯分布，当n>1时，v_k的均值为n维零向量，协方差矩阵为R；u_k和v_k相互独立。

在步骤S1中，根据所述非线性系统的状态模型和观测模型，利用GPU生成1时刻至M时刻的状态噪声；具体地说，当m＝1时，在GPU端，调用CUDA的CURAND库函数cudaGenerateNormal()，生成N×M个服从高斯分布(正态分布)的随机数，N×M个服从高斯分布的随机数的均值为0，方差为Q；将N×M个服从高斯分布的随机数划分为M组随机数，每组随机数中随机数的个数均为N，将其中的第k组随机数作为k时刻的状态噪声u_k，并将第k组随机数加到对应的N个k时刻初始粒子值上。当m>1时，在GPU端，调用CUDA的CURAND库函数cudaGenerateNormal()，生成N×M个m维向量，将N×M个m维向量划分为M组向量，每组向量均包括N个m维向量，将其中的第k组向量作为k时刻的状态噪声u_k，并第k组向量加到对应的N个k时刻初始粒子值上。

在步骤S1中，在GPU端，调用CUDA的CURAND库函数cudaGenerateUniform()，生成N×M个在0到1之间均匀分布的随机数，在N×M个在0到1之间均匀分布的随机数中，与k时刻第i个初始粒子对应的随机数表示为其中，k取1至M，i取1至N。此时生成的N×M个在0到1之间均匀分布的随机数，将会在粒子重采样时使用。

在步骤S1中，根据设定粒子个数N、每个粒子所占存储空间等因素，分配CPU内存空间和GPU显存空间。

S2：利用CPU加载每个观测时刻非线性系统的观测向量，在CPU端为GPU端核函数执行分配线程，即划分GPU程序执行网格。然后，CPU调用英伟达CUDA运行时函数cudaMemcpy()，将每个观测时刻非线性系统的观测向量传输至GPU显存；其中，k时刻非线性系统的观测向量为y_k。当k＝1时，执行步骤S3。具体说明如下：

在步骤S2中，非线性系统的观测向量根据不同的情况可以表示不同的物理含义。例如在进行雷达目标跟踪时，首先利用雷达获取雷达目标的回波数据，此时，非线性系统的观测向量指雷达目标的回波数据。非线性系统的状态向量指多个时刻雷达目标的位置、速度和方位角。

S3：利用GPU根据重要性密度函数进行重要性采样(通过编写对应的核函数得出k时刻的重要性采样粒子)；具体说明如下：

首先，根据所述非线性系统的状态模型，得出初始粒子的状态转移密度函数p(x_k|x_k-1)，选取状态转移密度函数为重要性密度函数q(x_k|x_k-1,Y_k)＝p(x_k|x_k-1)，y₁至y_k组成k时刻非线性系统的观测序列Y_k，Y_k为：Y_k＝{y₁,y₂,…,y_k}，Y_k表示所有观测量组成的观测序列；

得出第i个初始粒子的重要性密度函数根据重要性密度函数进行重要性采样，得到重要性采样粒子在GPU中，运行核函数ImportantSample()，根据重要性密度函数，对k时刻每个初始粒子进行重要性采样，得出k时刻的多个重要性采样粒子。具体地，在步骤S3中，得出的k时刻的第i个重要性采样粒子值为

S4：在GPU中，根据非线性系统的观测模型，建立似然函数；然后，通过最大化似然函数，对k时刻的每个重采样粒子进行再次采样，产生k时刻的最大似然采样粒子；GPU将k时刻每个最大似然采样粒子传输至CPU。具体说明如下：

在u_k服从高斯分布(u_k设为高斯噪声)的设定条件下，似然函数p(y_k|x_k)可表示为：

$p\left(y_k\right|x_k)={\left(2\mathrm{\&pi;}\right)}^{-\frac{n}{2}}{\left|R\right|}^{-\frac{1}{2}}\exp \{-\frac{1}{2}{[y_k-h\left(x_k\right)]}^T{R}^{-1}[y_k-h\left(x_k\right)\left]\right\}$

通过最大化似然函数，将k时刻非线性系统的n维观测向量集成到最大似然采样粒子中，得出k时刻非线性系统的m维状态向量x_k的最大似然估计值即求解以下优化问题： ${\hat{x}}_k=\arg \underset{x_k}{\max }p\left(y_k\right|x_k)$

在求解上述优化问题(无约束极大化问题)时，采用一阶泰勒展开式对非线性函数h(x_k)进行线性化逼近。h(x_k)的一阶导数x_k|k-1为k时刻非线性系统的m维状态向量x_k的先验估计，整理变形后，可得x_k的最新估计为：

${\hat{x}}_k={\left(H_k^T{R}^{-1}{H}_k\right)}^{-1}{H}_k^T{R}^{-1}[y_k-h\left(x_{k|k-1}\right)+H_k{x}_{k|k-1}].$

根据上述求解过程，即可得出k时刻第i个最大似然采样粒子值

${\hat{x}}_k^i={\left(H_k^T{R}^{-1}{H}_k\right)}^{-1}{H}_k^T{R}^{-1}[y_k-h\left(x_{k|k-1}^i\right)+H_k{x}_{k|k-1}^i]$

其中，T表示矩阵或向量的转置，上标-1表示矩阵的逆， 为h(x_k)对x_k的一阶导数，表示当x_k取时h(x_k)的值；特别地，当n＝1时，R表示方差，此时上标-1表示-1次方。

优选地，在得出k时刻第i个最大似然采样粒子值之后，将步骤S4重复执行K次，在每次重复执行步骤S4之前，将上次执行步骤S4得出的k时刻第i个最大似然采样粒子值赋值给K为设定的自然数，以K＝1为例进行说明，在GPU端，运行核函数SecondSampleFun()，将首次得出的k时刻第i个最大似然采样粒子值赋值给(实际实现中，该函数直接包含了两次的重复)，然后重复执行步骤S4一次。重复执行步骤S4，可以进一步提高粒子滤波的精度。

S5：利用GPU得出k时刻每个最大似然采样粒子的接受概率(通过编写对应的核函数得出k时刻的每个最大似然采样粒子的接受概率)；具体说明如下：

首先，根据所述非线性系统的观测模型，得出似然函数p(y_k|x_k)；

然后，在GPU端，运行核函数CaculateWeight()，计算出k时刻第i个初始粒子的重要性权值具体地，当k＝1时，当k>1时， $w_k^i=p\left(y_k\right|{\hat{x}}_k^i)w_{k-1}^i.$

在GPU端，运行核函数SumWeight()，对k时刻所有N个初始粒子的重要性权值进行求和运算，得出w_{k_sum}。w_{k_sum}为：

$w_{k\_\mathrm{sum}}=\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{w}_k^i.$

在GPU端，运行核函数NormalWeight()，对k时刻每个初始粒子的重要性权值进行归一化处理，得出k时刻第i个最大似然采样粒子的接受概率

${\stackrel{\&OverBar;}{w}}_k^i=\frac{w_k^i}{w_{k\_\mathrm{sum}}}.$

S6：在GPU中，根据k时刻每个最大似然采样粒子，得出k时刻非线性系统的状态向量的估计值；

具体地，在步骤S6中，根据以下公式得出k时刻非线性系统的状态向量的估计值x_{k_est}：

$x_{k\_\mathrm{esy}}=\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{w}_k^i{\hat{x}}_k^i$

其中，i取1至N，k取1至M。

在进行步骤S6之后，即得出1时刻至k时刻非线性系统的状态向量的估计值。如果非线性系统的状态向量指多个时刻雷达目标的位置、速度和方位角，明显地，此时就可以对雷达目标进行跟踪。

然后，转到步骤S7，进行重采样得到下一时刻的初始粒子；

S7：GPU将k时刻每个最大似然采样粒子的接受概率传输至CPU；在CPU中，根据所述k时刻每个最大似然采样粒子的接受概率，得出k时刻每个最大似然采样粒子的重采样索引；然后，CPU将k时刻每个最大似然采样粒子的重采样索引传输至GPU；根据k时刻每个最大似然采样粒子的重采样索引，对k时刻每个最大似然采样粒子进行重采样，得出k+1时刻(下一时刻)的多个初始粒子，令k值自增1，然后返回至步骤S3。

具体说明如下：

在GPU端，运行核函数UIncrease()，计算出与k时刻第i个最大似然采样粒子对应的过渡取值

${\mathrm{\&lambda;}}_k^i=\frac{(i-1)+r_k^i}{N}.$

然后，调用CUDA运行时函数cudaMemcpy()，将与k时刻每个初始粒子对应的过渡取值、以及k时刻每个最大似然采样粒子的接受概率传输至CPU。

在CPU端，针对至进行累加计算，得出N个累加计算结果，其中，第m'个累加计算结果为：j取1至m'，m'取1至N。累加计算过程可以用如下数学表达式说明：

$[{\stackrel{\&OverBar;}{w}}_k^1,{\stackrel{\&OverBar;}{w}}_k^2,...,{\stackrel{\&OverBar;}{w}}_k^N]\&RightArrow;[{\stackrel{\&OverBar;}{w}}_k^1,{\stackrel{\&OverBar;}{w}}_k^1+{\stackrel{\&OverBar;}{w}}_k^2,...,{\stackrel{\&OverBar;}{w}}_k^1+{\stackrel{\&OverBar;}{w}}_k^2+...+{\stackrel{\&OverBar;}{w}}_k^N]$

其中，箭头指向处为累加计算之后的结果。

利用CPU找出满足下式的整数(的取值范围为2至N)：

$\underset{g=1}{\overset{m_k^i-1}{\mathrm{\&Sigma;}}}{\stackrel{\&OverBar;}{w}}_k^g<{\mathrm{\&lambda;}}_k^i<\underset{h=1}{\overset{m_k^i}{\mathrm{\&Sigma;}}}{\stackrel{\&OverBar;}{w}}_k^h$

其中，g取1至h取1至将整数作为k时刻第i个最大似然采样粒子的重采样索引。然后，调用CUDA运行时函数cudaMemcpy()，将k时刻每个最大似然采样粒子的重采样索引发送至GPU。

S8：将步骤S3至步骤S7重复执行M次，得出M个观测时刻的非线性系统的状态向量的估计值。

在步骤S8之后，则可以利用CPU将已分配的CPU内存空间和GPU显存空间销毁，具体地说，调用CUDA运行时API函数cudaFree()销毁分配的GPU显存空间，通过调用free()函数销毁分配的CPU内存空间。

本发明的效果可通过以下仿真进一步说明：

1)实验条件：

在仿真实验中，硬件平台选用HP Z820工作站，GPU显卡型号为NVIDATelsa K20c，Intel Xeon多核处理器，Win7系统，软件平台为Visual Stdio2008+CUDA5.5和MATLAB2009b。

在该仿真实验中，n＝1，m＝1，仿真实验使用的非线性系统的状态模型和观测模型表示为：

$x_k=0.5x_{k-1}-\frac{20}{x_{k-1}^2+1}+20+u_k$

$y_k=\frac{x_k^3}{20}+v_k$

上式中，u_k服从均值为0标准差为1的高斯分布，v_k服从均值为0标准差为0.1的高斯分布，1时刻第i个初始粒子的状态向量表示为为0.1，观测周期(两个相邻观测时刻之间的时间长度)为1，在仿真实验中，重复执行步骤S6两次。

2)实验内容及结果：

仿真实验1，在M＝50、N＝256时，在MATLAB上分别采用标准粒子滤波方法和本发明提出的方法，对1时刻至M时刻非线性系统的状态向量进行估计。在仿真实验1中，标准粒子滤波和本发明提出的方法分别进行100次蒙特卡洛实验，将100次蒙特卡洛实验的统计平均分别作为标准粒子滤波和本发明粒子滤波的结果，并将结果与真值作比较，分别计算两种方法得出的非线性系统的状态向量的估计值与真值的均方根误差，参照图2，为仿真实验1中本发明和标准粒子滤波方法的非线性系统的状态向量估计精度对比示意图。参照图3，为图2的局部放大示意图。在图2和图3中，横坐标表示时刻，单位为1，纵坐标表示状态值，单位根据实际情况而定；参照图4，为仿真实验1中本发明和标准粒子滤波方法的非线性系统的状态向量均方根误差的对比示意图。在图4中，横坐标表示时刻，单位为1，纵坐标表示均方根误差，单位为根据实际情况而定。

由图2和图3可以看出，标准粒子滤波方法的非线性系统的状态向量估计过程不够稳定，在部分采样点上出现了轻微的滤波发散现象，而本发明的粒子滤波方法在滤波的稳定度和与真实值的拟合度上，都要高于标准粒子滤波估计算法。并且由图4可以看出，本发明的粒子滤波方法的估计误差明显低于标准粒子滤波算法。由此可以说明本发明具有更高的状态估计精度和更强的稳定性。

仿真实验2，在M＝50、N＝256时，使用两种方法对1时刻至M时刻非线性系统的状态向量进行估计，其中第一种方法为本发明的粒子滤波方法，此时在GPU端，采用单精度浮点数(float)进行计算。第二种方法与本发明类似，区别在于不使用GPU，单独使用CPU进行粒子滤波。对仿真实验2，采用以上两种方法分别进行100次蒙特卡洛实验，将100次蒙特卡洛实验的统计平均分别作为两种方法的不同结果，然后分别将两种方法的不同结果与真值作比较，求出对应的均方根误差，将GPU处理结果的均方根误差和CPU处理结果的均方根误差相减，得到二者处理结果的差别。参照图5，为仿真实验2中两种方法的非线性系统的状态向量均方根误差差别的对比示意图。在图5中，横坐标表示时刻，单位为1，纵坐标表示均方根误差之间的差别。

由图5可见，本发明使用单精度浮点类型进行滤波的结果，相比于单独使用CPU进行处理时，相对误差在10^-3量级下，验证了本发明所提出方法的正确性和高精度。

仿真实验3，在M＝100，N为256、512、和1024时，分别采用两种方法对1时刻至M时刻非线性系统的状态向量进行估计，其中第一种方法为本发明的粒子滤波方法，此时在GPU端，采用单精度浮点数(float)进行计算。第二种方法与本发明类似，区别在于不使用GPU，单独使用CPU进行粒子滤波。在仿真实验3中，不论N的取值情况、以及采用哪种方法进行粒子滤波，均进行100次蒙特卡洛实验，将100次蒙特卡洛实验的运算时间的统计平均分别作为对应的处理时间。参照图6，仿真实验3中N为不同取值时采用两种方法的处理时间对比示意图。在图6中，横坐标表示粒子数，单位为个，纵坐标表示处理时间，单位为ms。

由图6可以看到，在N为256时，第一种方法的处理时间与第二种方法相比有60倍左右的加速比；而当N增加为512时，第二种方法的处理时间(单独使用CPU进行粒子滤波)为1.084s，而第一种方法的处理时间为9.64ms，其加速比提高到112倍；当N增加为1024时，第二种方法的处理时间为2.19s，而第一种方法的处理时间为11.618ms，其加速比提高到188倍。这种处理时间是在意料之中的，因为在CPU端对不同粒子的处理采用的是串行的，当粒子数增加其处理时间必然增加，而GPU端对不同粒子是并行处理的，因此当粒子数增加时，其处理时间并没有显著增加。

仿真实验4，在M＝100，N为256时，采用本发明提出的方法对1时刻至M时刻非线性系统的状态向量进行估计，其中第一种方法为本发明的粒子滤波方法，在GPU端，此时K＝1，采用单精度浮点数(float)进行计算。第二种方法与第一种方法类似，区别在于K＝2。在仿真实验4中，采用以上两种方法分别进行100次蒙特卡洛实验，将100次蒙特卡洛实验的统计平均分别作为两种方法的不同结果，然后分别将两种方法的不同结果与真值作比较，求出对应的均方根误差。参照图7，为仿真实验4中K取不同值时估计值域真值的均方误差对比图。在图7中，横坐标表示时刻，单位为1，纵坐标表示均方根误差。由图7可以看出，当K取2时，其估计的估计误差要小于K取1时估计误差，由此可以说明对最大似然估计的重复进行可以进一步提高估计精度。

显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。

Claims (5)

1.一种基于GPU架构的改进的粒子滤波方法，其特征在于，包括以下步骤：

S1：利用CPU将粒子个数设定为N，利用CPU设定M个观测时刻，N和M均为大于1的自然数，所述M个观测时刻依次表示为1时刻至M时刻；利用GPU生成N个1时刻初始粒子；设置观测时刻参数k，k＝1，2，3，...M；

在步骤S1中，建立非线性系统的状态模型和观测模型，非线性系统的状态模型和观测模型表示如下：

$\left\{\begin{array}{c}{x}_k=f\left(x_{k-1}\right)+u_k\\ y_k=h\left(x_k\right)+v_k\end{array}\right.$

其中，x_k表示k时刻非线性系统的m维状态向量，y_k表示k时刻非线性系统的n维观测向量，m和n均为大于0的自然数；f(·)为描述非线性系统状态模型的非线性函数，h(·)描述非线性系统的观测模型的非线性函数；u_k为设定的服从高斯分布的k时刻状态噪声，当m＝1时，u_k服从均值为0方差为Q的高斯分布，当m＞1时，u_k的均值为m维零向量，协方差矩阵为Q；v_k为设定的服从高斯分布的k时刻观测噪声，当n＝1时，v_k服从均值为0方差为R的高斯分布，当n＞1时，v_k的均值为n维零向量，协方差矩阵为R；

在步骤S1中，根据所述非线性系统的状态模型和观测模型，利用GPU生成1时刻至M时刻的状态噪声；在步骤S1中，1时刻第i个初始粒子值为i取1至N；

S2：利用CPU加载每个观测时刻非线性系统的观测向量，CPU将每个观测时刻非线性系统的观测向量传输至GPU显存；当k＝1时，执行步骤S3；

S3：在GPU中，根据重要性密度函数对k时刻的每个初始粒子进行重要性采样，得出k时刻的多个重要性采样粒子；

在步骤S2中，k时刻非线性系统的观测向量为y_k；

在步骤S3中，根据所述非线性系统的状态模型，得出初始粒子的状态转移密度函数p(x_k|x_k-1)，选取状态转移密度函数为重要性密度函数q(x_k|x_k-1，Y_k)＝p(x_k|x_k-1)，得出k时刻第i个初始粒子的重要性密度函数根据k时刻第i个初始粒子的重要性密度函数对k时刻的每个初始粒子进行重要性采样，得出k时刻的多个重要性采样粒子，k时刻的第i个重要性采样粒子值为Y_k为由观测向量y₁至y_k组成的观测序列；

S4：在GPU中，根据非线性系统的观测模型，建立似然函数；然后，通过最大化似然函数，对k时刻的每个重要性采样粒子进行二次采样，产生k时刻的多个最大似然采样粒子；

根据非线性系统的观测模型，建立似然函数p(y_k|x_k)表示为：

$p\left(y_k\right|x_k)={\left(2\mathrm{\&pi;}\right)}^{-\frac{n}{2}}\left|R{|}^{-\frac{1}{2}}\exp \right\{-\frac{1}{2}{\&lsqb;y_k-h\left(x_k\right)\&rsqb;}^T{R}^{-1}\&lsqb;y_k-h\left(x_k\right)\&rsqb;\}$

根据以下公式得出k时刻第i个最大似然采样粒子值

${\hat{x}}_k^i={\left(H_k^T{R}^{-1}{H}_k\right)}^{-1}{H}_k^T{R}^{-1}\&lsqb;y_k-h\left(x_{k|k-1}^i\right)+H_k{x}_{k|k-1}^i\&rsqb;$

其中，T表示矩阵或向量的转置，上标-1表示矩阵的逆， 为h(x_k)对x_k的一阶导数；

S5：利用GPU得出k时刻每个最大似然采样粒子的接受概率；

S6：在GPU中，根据k时刻每个最大似然采样粒子，得出k时刻非线性系统的状态向量的估计值；

S7：GPU将k时刻每个最大似然采样粒子的接受概率传输至CPU；在CPU中，根据所述k时刻每个最大似然采样粒子的接受概率，得出k时刻每个最大似然采样粒子的重采样索引；然后，CPU将k时刻每个最大似然采样粒子的重采样索引传输至GPU；在GPU中，根据k时刻每个最大似然采样粒子的重采样索引，对k时刻每个最大似然采样粒子进行重采样，得出k+1时刻的多个初始粒子；令k值自增1，然后返回至步骤S3；

S8：将步骤S3至步骤S7重复执行M次，得出M个观测时刻的非线性系统的状态向量的估计值。

2.如权利要求1所述的一种基于GPU架构的改进的粒子滤波方法，其特征在于，在步骤S5中，根据所述非线性系统的观测模型，得出似然函数p(y_k|x_k)；确定k时刻第i个最大似然采样粒子的重要性权值当k＝1时，当k＞1时，根据k时刻第i个最大似然采样粒子的重要性权值得出k时刻第i个最大似然采样粒子的接受概率

${\stackrel{\&OverBar;}{w}}_k^i=\frac{w_k^i}{w_{k\_sum}}$

$w_{k\_sum}=\underset{i=1}{\overset{N}{\&Sigma;}}{w}_k^i;$

在步骤S6中，根据以下公式得出k时刻非线性系统的状态向量的估计值x_{k_est}：

$x_{k\_est}=\underset{i=1}{\overset{N}{\&Sigma;}}{\stackrel{\&OverBar;}{w}}_k^i{\hat{x}}_k^i$

其中，i取1至N，k取1至M。

3.如权利要求2所述的一种基于GPU架构的改进的粒子滤波方法，其特征在于，在步骤S1中，利用GPU生成N×M个在0到1之间均匀分布的随机数，在N×M个在0到1之间均匀分布的随机数中，与k时刻第i个初始粒子对应的随机数表示为其中，k取1至M，i取1至N；

在步骤S7中，在GPU端，计算出与k时刻第i个初始粒子对应的过渡取值

${\mathrm{\&lambda;}}_k^i=\frac{(i-1)+r_k^i}{N};$

然后，将与k时刻每个初始粒子对应的过渡取值、以及k时刻每个最大似然采样粒子的接受概率传输至CPU；

在CPU中，针对至进行累加计算，得出N个累加计算结果，其中，第m′个累加计算结果为：j取1至m′，m′取1至N；

利用CPU找出满足下式的整数

$\underset{g=1}{\overset{m_k^i-1}{\&Sigma;}}{\stackrel{\&OverBar;}{w}}_k^g<{\mathrm{\&lambda;}}_k^i<\underset{h=1}{\overset{m_k^i}{\&Sigma;}}{\stackrel{\&OverBar;}{w}}_k^h$

其中，g取1至h取1至将整数作为k时刻第i个最大似然采样粒子的重采样索引；然后，将k时刻每个最大似然采样粒子的重采样索引传输至GPU；

在步骤S7中，根据k时刻每个最大似然采样粒子的重采样索引，对k时刻每个最大似然采样粒子进行重采样，得出k+1时刻的多个初始粒子。

4.如权利要求1所述的一种基于GPU架构的改进的粒子滤波方法，其特征在于，在步骤S4之后，在得出k时刻第i个最大似然采样粒子值之后，将步骤S4重复执行K次，在每次重复执行步骤S4之前，将上次执行步骤S4得出的k时刻第i个最大似然采样粒子值赋值给K为大于0的自然数。

5.如权利要求1所述的一种基于GPU架构的改进的粒子滤波方法，其特征在于，在步骤S1中，利用CPU分配CPU内存空间和GPU显存空间；

在步骤S8之后，利用CPU将已分配的CPU内存空间和GPU显存空间销毁。