粒子滤波原理及应用仿真.DOC

1、Particle filter for help !-QQ:345194112粒子滤波原理及应用仿真原理介绍 +应用实例+MATLAB 仿真程序本手册主要介绍粒子滤波的基本原理和其在非线性系统的应用。同时本手册最大的优点是介绍原理和应用的同时，给出实现例子的 matlab 代码程序，方便读者对照公式，理解代码。因此，它是相关方面的研究者快速上手和进入研究领域的快捷工具。同时，对于有一定基础的研究者，可以在本手册提供代码的基础上，做算法进一步改进和深入研究。目 录第一部分 原理篇 1第一章 概述 11.1 粒子滤波的发展历史 11.2 粒子滤波的优缺点 21.3 粒子滤波的应用领域 3第二章 蒙

2、特卡洛方法 42.1 概念和定义 42.2 蒙特卡洛模拟仿真程序 5Particle filter for help !-QQ:3451941122.2.1 硬币投掷实验（1） 52.2.2 硬币投掷实验（2） 52.2.3 古典概率实验 64.2.4 几何概率模拟实验 72.2.5 复杂概率模拟实验 72.3 蒙特卡洛理论基础 102.3.1 大数定律 102.3.2 中心极限定律 102.3.3 蒙特卡洛的要点 112.4 蒙特卡洛方法的应用 132.4.1 Buffon 实验及仿真程序 132.4.2 蒙特卡洛方法计算定积分的仿真程序 14第三章 粒子滤波 193.1 粒子滤波概述 19

3、3.1.1 蒙特卡洛采样原理 193.1.2 贝叶斯重要性采样 203.1.3 序列重要性抽样（SIS）滤波器 203.1.4 Bootstrap/SIR 滤波器 223.2 粒子滤波重采样方法实现程序 233.2.1 随机重采样程序 243.2.2 多项式重采样程序 253.2.3 系统重采样程序 263.2.4 残差重采样程序 273.3 粒子滤波原理 283.3.1 高斯模型下粒子滤波的实例程序 28第二部分 应用篇 33第四章 粒子滤波在单目标跟踪中的应用 334.1 目标跟踪过程描述 334.2 单站单目标跟踪系统建模 344.3 单站单目标观测距离的系统及仿真程序 374.3.1

4、基于距离的系统模型 374.3.2 基于距离的跟踪系统仿真程序 384.4 单站单目标纯方位角度观测系统及仿真程序 434.4.1 纯方位目标跟踪系统模型 434.4.2 纯方位跟踪系统仿真程序 444.5 多站单目标纯方位角度观测系统及仿真程序 474.5.1 多站纯方位目标跟踪系统模型 474.5.2 多站纯方位跟踪系统仿真程序 48第五章 粒子滤波在多目标跟踪中的应用 545.1 多目标跟踪系统建模 545.1.1 单站多目标跟踪系统建模 545.1.2 多站多目标跟踪系统建模 555.1.3 单站多目标线性跟踪系统的建模仿真程序 555.1.4 多站多目标非线性跟踪系统的建模仿真程序

5、57Particle filter for help !-QQ:3451941125.2 多目标跟踪分类算法 615.2.1 多目标数据融合概述 615.2.2 近邻法分类算法及程序 625.2.3 近邻法用于目标跟踪中的航迹关联及算法程序 665.2.4 K-近邻法分类算法 695.3 粒子滤波用于多目标跟算法中的状态估计 705.3.1 原理介绍 705.3.2 基于近邻法的多目标跟踪粒子滤波程序 71第六章 粒子滤波在电池寿命预测中的应用 766.1 概述 766.2 电池寿命预测的模型 786.3 基于粒子滤波的电池寿命预测仿真程序 81本书内容章节介绍：2.4.1 Buffon 实验

6、及仿真程序假设平面上有无数条距离为 1 的等距平行线，现向该平面随机投掷一根长度为 l 的针( l1)，则我们可计算该针与任一平行线相交的概率。这里，随机投针指的是：针的中心点与最近的平行线间的距离 X 均匀地分布在区间0,1/2 上，针与平行线的夹角 （不管相交与否）均匀的分布在区间0,上。此时，针与线相交的充要条件是注意：从而针线相交的概率为：根据上式，若我们做大量的投针试验并记录针与线相交的次数，则由大数定理可以估计出针线相交的概率 p，从而得到 的估计值。 针与线的位置关系2sinl12)(wfxX ldxwlXPl2sin20sin2Particle filter for help

7、!-QQ:345194112% 说明：古典概率实验%function buffon_testl=0.6; m=10000; % 实验次数buffon(l,m); %function piguji=buffon(llength,mm)%llength 是针的长度%mm 是随机实验次数frq=0;xrandnum = unifrnd(0,0.5,1,mm);3.2.1 随机重采样程序执行仿真程序，得到以下仿真结果，图中上部就是程序中给定的 W 随机样本，而下部分是由随机采样得到的 V 样本集合。从连线关系可以看出， W 样本集中的第 2 个样本被复制一次，第 3 个样本被复制 2 次，第 7 个样

8、本被复制2 次，第 8 个样本被复制 1 次，第 9 个样本被复制 2 次，W 中除了被复制的样本外，其他样本被舍弃，这就是“优胜劣汰”的思想。Particle filter for help !-QQ:345194112% 随机采样测试程序%function testN=20;W=rand(1,N); % 任意给定一组随机数样本W(1,:)=W(1,:)./sum(W(1,:); % 归一化outIndex = randomR(1:N,W(1,:); % 调用随机采样方法% 经过随机采样后得到的样本 V，读者要细细比较它与 W 样本的区别V=W(1,outIndex); % 画图直观显示区别

9、 第二部分 应用篇第四章 粒子滤波在单目标跟踪中的应用4.1 目标跟踪过程描述基于多观测站的目标跟踪其实质是多传感器之间协作地完成对目标探测，定位预测的过程。一般地，我们将其分为探测、分类、定位、状态估计建立轨迹方程等阶段。（1）探测探测是目标跟踪的前提。其目的就是为了发现目标。每个传感器站点周期Particle filter for help !-QQ:345194112性地利用自身的传感模块，通过无线信号强度、红外、超声或者震动传感器探测区域内目标是否存在。一般地，在网络存在两种探测方式，一种是主动探测，例如雷达通过无线方式扫描区域内的目标。另一种是被动探测，例如声纳主要是接受环境中传来的

10、声音信号。在军事领域，被动探测越来越受欢迎，因为被动探测是目标驱动的，具有很好的潜伏性和隐蔽性。4.4 单站单目标纯方位角度观测系统及仿真程序4.4.1 纯方位目标跟踪系统模型假设目标做匀速直线运动，目标的状态为，很显然，k 时刻目标的位置为Tvpvpykxk)()()X(，目标的速度 由水平方向和垂直方向的分速度构成，,yx,x那么目标的状态方程如下： )()1(kwXk其中 ， ， ， 10TT05.022 )()()(kyxXvpv上述状态方程只与目标真实运动信息，那么观测站是根本不知道目标的运Particle filter for help !-QQ:345194112动状态的，观测站

11、的位置为 ，通过角度传感器采集目标与观测站之间的),(syx角度信息，得到观测方程如下：)()(arctn)(0kvxkZ4.4.2 纯方位跟踪系统仿真程序% 程序说明： 单站单目标基于角度的跟踪系统，采用粒子滤波算法% 状态方程 X（k+1）=F*X(k)+Lw(k)% 观测方程 Z（k）=h（X）+v（k）function main% 初始化参数clear;T=1; % 采样周期M=30; % 采样点数delta_w=1e-4; % 过程噪声调整参数，设得越大，目标运行的机动性越大，轨迹越随机（乱）Q=delta_w*diag(0.5,1,0.5,1) ; % 过程噪声均方差R=pi/18

12、0*0.1; % 观测角度均方差，可将 0.1 设置的更小F=1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1;运行程序，得到轨迹跟踪效果图0 20 40 60 80 100 12020406080X/mY/m一一一一一一一一一一一PF一一一一跟踪偏差图（估计轨迹与真实值之间的偏差）Particle filter for help !-QQ:3451941120 5 10 15 20 25 3000.511.522.5time/serror/mRMSE,q=0.0001RMS一一一一4.5.1 多站纯方位目标跟踪系统模型多站多目标跟踪系统框架如下4.5.2 多站纯方位跟踪系统仿真

13、程序运行程序，得到以下仿真结果，从途中看出，粒子滤波较好地对目标真实轨迹进行了跟踪，从计算时间图上可以看出，每个采样周期内各观测站运行的时间大约在 0.2 秒左右，而跟踪误差图显示的是随着时间的推移，粒子滤波跟踪逐渐误差偏大，这与粒子匮乏等因素有关。Particle filter for help !-QQ:3451941120 20 40 60 80 100 1200102030405060708090100Node1Node2Node3Node4Node5Node6X/mY/m一一一一一一一一一一一PF一一一一图 1 跟踪轨迹0 5 10 15 20 25 3000.050.10.150.

14、20.25steptime/s一一一一一一一PF一一一一0 5 10 15 20 25 3001234time/serror/mRMS一一一一图 2 每个周期内计算时间 图 3 跟踪误差程序清单如下：% 程序说明： 多观测站单目标基于角度观测的目标跟踪系统% 状态方程 X（k+1）=F*X(k)+Lw(k)% 观测方程 Z（k）=h（X）+v（k）function main% 初始化参数clear;T=1; % 采样周期M=30; % 采样点数delta_w=1e-3; % 过程噪声调整参数，设得越大，目标运行的机动性越大，轨迹越随机（乱）Q=delta_w*diag(0.5,1,0.5,1)

15、 ; % 过程噪声均方差Particle filter for help !-QQ:3451941125.2.2 近邻法分类算法及程序最近邻数据关联算法的计算量小，便于实现，适用于信噪比高、量测密度小的环境。假定有 c 个目标 ， ， ，每个目标有 个样本。12ciN-15 -10 -5 0 5 10 15 20-20-15-10-5051015一一一一一一一一一近邻法分类程序一% 函数功能：近邻法分类程序%function Neighbour_Classify_exam1% 初始化数据Type=3; % 假设已知三类目标% 随机产生 M 个样本点M=15;% 场地空间的长和宽Width=4;Length=4;% 样本初始化、并模拟一组样本空间for i=1:Type5.1.2 多站多目标跟踪系统建模