改进的自举裂变粒子滤波方法及其DSP硬件实现方法

摘要

本发明一种改进的自举裂变粒子滤波方法及其仿真方法，将权值归一化步骤合并到结果输出、权值锐化检测、重采样等几个步骤中，从而减少了大量的除法运算，降低了硬件开销。根据粒子滤波算法运算量大的特点，选择了TI公司的TMS320VC5509A数字信号处理器(DSP)来实现粒子滤波算法，通过集成在MATLAB 7.1中的CCSLink工具将C5509及其集成开发环境CCS 2.21连接在一起，在DSP中编写了改进FBPF算法的程序，并将数据通过RTDX通道传给MATLAB进行保存、分析和绘图，通过采用MATLAB和CCS联合仿真的方法，分析粒子滤波算法的性能。

说明书

技术领域

本发明涉及一种应用DSP硬件系统实现粒子滤波及仿真的方法，属于非线性系统滤波和电子技术领域。 

背景技术

粒子滤波是一种基于蒙特卡罗方法和递推贝叶斯估计的统计滤波方法，适用于任何能用状态空间模型以及传统的卡尔曼滤波表示的非高斯背景的非线性随机系统。但是粒子滤波存在粒子退化、粒子多样性丧失、粒子数与计算复杂度同比增长等问题。为此人们提出了许多改进方法，其中裂变自举粒子滤波(FBPF)算法就是在粒子滤波算法的基础上改进重采样过程，引入了权值锐化检测和“权值排序(Sorting)-裂变繁殖(Fission)-权值归一(Normalizing)”过程(简称SFN预处理过程)。裂变过程实质上是一种随机采样方法，它的采样计划不是始终保持不变的，而是根据实验结果不断进行调整，设计出新的采样计划，从而克服了粒子匮乏问题。 

在实际应用中，FBPF算法的复杂度主要由状态空间模型的复杂度和算法本身的复杂度共同决定。如果一个算法的应用状态空间模型已经建立，其复杂度也就随之确定，因此要降低一个算法的复杂度最终还要从简化算法入手。FBPF算法的计算中非线性函数较多，而大多数非线性函数计算又集中于权值计算步骤中。在FBPF算法中，一个输入观测值一般要经过N_s次复杂的函数计算，权值计算时要进行N_s次指数运算，权值归一化时要进行N_s次除法运算，输出结果时要进行N_s次乘法运算，因此，粒子生成、权值计算、权值归一化、结果输出分别需要N_s次循环计算，如果粒子数N_s很大时计算量将大得惊人。因此，FBPF算法计算量比较大，实时性差。本发明以提高FBPF算法的运算速度和滤波精度为目的，对FBPF算法进行了一些改进，并发展与算法匹配的硬件结构来实现速度的提高。 

同时，为了提高粒子滤波算法的运算速度和鲁棒性，研究粒子滤波的硬件实现方法尤为关键。粒子滤波硬件实现的基本思想是：将粒子滤波划分为初始采样、重采样、状态更新等不同过程，利用流水线实现分时并行处理。但实用化粒子滤波算法器尚未研制成功，设计一个运算速度快、性能可靠、占用资源少的粒子滤波算法硬件模块是本发明的主要内容。 

发明内容

本发明针对现有技术存在的缺陷提出了一种改进的裂变自举粒子滤波算法，并给出了一种基于DSP的硬件实现及仿真方法。 

本发明改进的自举裂变粒子滤波方法，包括如下步骤： 

步骤1：k＝0时初始化，采样粒子 i＝1，…，N_s；k＝1，2，…时做以下循环迭代，其中，x₀表示0时刻系统的状态向量， 表示系统的状态先验条件概率， 括号中的 表示某个变量，而p(x₀)表示初始状态向量x₀的先验概率， 表示0时刻状态向量的第i个粒子，其权值为 k，Ns表示自然数； 

步骤2：i＝1，…，N_s，采样粒子 计算非归一化权重 计算权重的同时对这一时刻的权重累加求和，得到非归一化的权值-粒子集 和权重累加和 其中，x_k表示k时刻系统的状态向量，y_k表示k时刻系统的输出， 是k时刻状态向量的第i个粒子 的权值； 

步骤3：输出结果，$x_k=(\underset{i=1}{\overset{N_s}{\Sigma }}{x}_k^i*w_k^i)/S_k;$

步骤4：权值锐化检测，估计粒子集X_k的有效样本容量 

步骤5：若 则进行SFN预处理过程，得到新的权值-粒子集 并对新裂变粒子的 置为1/N_s，然后返回步骤2，否则直接转入下一步； 

步骤6：重采样：按粒子权值从大到小进行排列，将大权值的粒子进行裂变，然后覆盖后面小权值的十个粒子，然后返回步骤2。 

改进的自举裂变粒子滤波方法的DSP硬件实现方法，包括如下步骤： 

第一步：根据上述的改进的自举裂变粒子滤波方法，在DSP中编写程序，首先设置好系统模型和参数，包括数据样点数、粒子个数、有效样本容量限值、权值裂变的方差； 

第二步：对编写好的程序进行编译，应用CCS设置工程文件Project，将编译好的C文件、汇编文件、头文件、LIB文件以及存储器配置(CMD)文件添加到Project中，并根据目标板的实际存储器和要求来完成该工程的编译链接； 

第三步：应用MATLAB集成的RTDX接口将DSP中的数据通过RTDX通道传给MATLAB进行保存、分析和绘图； 

第四步：将传输到MATLAB中的数据以图形的形式输出，输出的图形包括改进的自举裂变粒子滤波方法中每个样点粒子分布和有效样本容量N_eff的关系图，通过该图分析粒子权值的锐化情况，从而分析粒子的多样性情况；同时还可以输出算法的滤波结果图，计算出算法的平均滤波时间和滤波方差，从而分析算法的滤波精度和实时性。 

本发明具体改进方法包括如下几点： 

1.FBPF算法中输出结果是每个粒子值分别与对应归一化的粒子权重相乘，改进FBPF算法输出结果改为每个粒子值分别与对应的未归一化的粒子权重相乘然后除以权重之和， 即把输出结果由 改为 其中x_k表示k时刻系统的状态向量， 是k时刻状态向量的第i个粒子 的权值，S_k表示粒子权重之和。 

2.FBPF算法中权值锐化检测过程的有效样本容量是N_s个归一化权值平方和的倒数，改进FBPF算法中权值锐化检测过程的有效样本容量是N_s个未归一化权值平方和的倒数与权值和的平方相乘，即把FBPF算法中的权值锐化检测过程的有效样本容量 改为 

3.FBPF算法中重采样过程产生的均匀分布的随机数是在区间[0，1/N_s)，改进FBPF算法中重采样过程产生的均匀分布的随机数是在区间[0，S_K/N_s)。 

本发明通过以上的改进省去了权值归一化中的N_s次除法运算，大大减少了运算量。同时为了提高权值计算的精确性，将原算法中的权值计算公式 改为 y_k表示k时刻系统的输出。同时，经大量实验发现，权值锐化门限 越大权值锐化越严重，权值越小粒子退化越严重，因此每一次滤波都采用重采样步骤来消除粒子退化现象，并且当 时进行SFN预处理过程。 

为了应用DSP硬件实现改进的FBPF算法，选择瑞泰创新公司生产的ICETEK-5509-A开发板，此开发板的DSP芯片是美国TI公司的TMS320VC5509ADSP(简称C5509A)，此芯片是美国TI公司生产的高性能、高性价比的16位定点DSP芯片，其结构是专门针对实时信号处理而设计的。 

TI公司提供了一套强大的专用于进行TMS320系列DSP软件设计的集成软件开发环境，即Code Composer Studio(CCS)。CCS支持实时数据交换(RTDX)技术，它利用DSP的内部仿真逻辑和JTAG接口实现主机与目标机之间的数据交换，而不占用DSP的资源。同时MATLAB集成了RTDX接口，本发明在DSP中编写了改进FBPF算法的程序，并将数据通过RTDX通道传给MATLAB进行保存、分析和绘图，通过采用MATLAB和CCS联合仿真的方法，分析粒子滤波算法的性能。 

附图说明

图1改进的FBPF算法功能框图。 

具体实施方式

下面结合附图对本发明的实施例做详细的说明。 

FBPF算法计算量比较大，不利于应用于实时性较高的系统中，本发明以提高FBPF算法的运算速度和滤波精度为目的，采取减少除法运算和改进权值计算的方法。通过对FBPF 算法的复杂度分析可知，FBPF算法的复杂度主要由状态空间模型的复杂度和算法本身的复杂度共同决定。要降低一个算法的复杂度最终还要从简化算法入手；FBPF算法中的大多数非线性函数计算集中于权值计算步骤中，权值归一化使得算法复杂度提高。另一方面，因为除法运算对硬件的开销很大，所以尽可能的减少除法运算是提高FBPF算法运算速度的关键。经过对FBPF算法反复研究发现，可以将权值归一化步骤合并到结果输出、权值锐化检测、重采样等几个步骤中，从而减少大量的除法运算，降低算法的硬件开销。 

改进的FBPF算法如图1所示，算法具体步骤如下： 

步骤1：k＝0时初始化，采样粒子 i＝1，…，N_s；k＝1，2，…时做以下循环迭代。其中，x₀表示0时刻系统的状态向量， 表示系统的状态先验条件概率， 表示0时刻状态向量的第i个样本(粒子)，其权值为 

步骤2：i＝1，…，N_s，采样粒子 计算非归一化权重 计算权重的同时对这一时刻的权重累加求和，得到非归一化的权值-粒子集 和权重累加和 其中，x_k表示k时刻系统的状态向量，y_k表示k时刻系统的输出， 是k时刻状态向量的第i个粒子 的权值。 

步骤3：输出结果，$x_k=(\underset{i=1}{\overset{N_s}{\Sigma }}{x}_k^i*w_k^i)/S_k.$

步骤4：权值锐化检测，估计粒子集X_k的有效样本容量 

步骤5：若 则进行SFN预处理过程，得到新的权值-粒子集 并对新裂变粒子的 置为1/N_s，然后返回第2步，否则直接转入下一步。 

步骤6：重新采样过程是复制权重大的粒子并用其覆盖权重小的粒子，然后返回第2步。 

本发明采用MATLAB与CCS联合仿真和实时数据交换(RTDX)程序设计方法来实现改进的FBPF算法，RTDX利用DSP的内部仿真逻辑和JTAG接口实现主机与目标机之间的数据交换，不占用DSP的系统总线和串口等I/O资源，数据传送完全可以在应用程序的后台运行。通过集成在MATLAB 7.1中的CCSLink工具将C5509及其集成开发环境CCS 2.21连接在一起，进行MATLAB与CCS联合仿真。在MATLAB环境下完成对CCS和DSP目标板的操作，包括与目标板之间的数据交换、检测处理器的状态、控制DSP程序的运行等。 

具体的联合仿真步骤如下： 

第一步：根据上述的改进FBPF算法，应用C语言，在DSP中编写程序，以做进一步的硬件仿真实验。首先设置好系统模型和参数，包括数据样点数、粒子个数、有效样本容量 限值等。 

第二步：对编写好的算法程序进行编译，应用CCS设置工程文件Project，将编译好的C文件、汇编文件、头文件、LIB文件以及存储器配置(CMD)文件添加到Project中，并根据目标板的实际存储器和要求来完成该工程的编译链接。 

第三步：应用MATLAB集成的RTDX的接口将DSP中的数据通过RTDX通道传给MATLAB进行保存、分析和绘图。 

第四步：将传输到MATLAB中的数据以图形的形式输出，因此能够很准确的知道每次滤波所用的时间和粒子的退化程度。输出的图形包括改进FBPF算法中每个样点粒子分布和有效样本容量N_eff的关系图，通过该图分析粒子权值的锐化情况(即粒子退化情况)，从而可以分析粒子的多样性情况；同时还可以输出算的法滤波结果图，计算出算法的平均滤波时间和滤波方差，从而可以分析算法的滤波精度和实时性。