粒子滤波算法原理及Matlab程序(专题)

2、蒙特卡洛原理
• • • 粒子滤波技术是以蒙特卡洛为基础的 蒙特卡洛：用实验模拟的方法解决复杂的积分计算问题 硬币投掷实验（1） 掷一枚均匀硬币，正面朝上的次数X服从参数为１，p的二项分 布,X~B(1,p) 在Matlab中编辑.m文件输入以下命令：
1
function cion_throw_test1 p=0.3; % 正面朝上的概率 m=1000; % 实验次数 fun(p,m); % 修改不同的p,m值 function fun(p,mm) pro=zeros(1,mm); randnum = binornd(1,p,1,mm); % 服从二项分布 a=0;
0.9 0.8 0.7 0.6 0.5 0.4 0.3 0.2 0.1 0 200 400 600 800 1000
2、蒙特卡洛原理
• 蒙特卡洛的应用
从而针线相交的概率为：
应用说明：利用蒙特卡洛模拟 计算圆周率 方法解决： 假设平面上有无数条距离为1的等距平行线， 现向该平面随机投掷一根长度为l的针(l1)， 则我们可计算该针与任一平行线相交的概率。 这里，随机投针指的是：针的中心点与最近 的平行线间的距离X均匀地分布在区间[0,1/2] 上，针与平行线的夹角（不管相交与否） 均匀的分布在区间[0,]上。此时，针与线相 交的充要条件是
80
60
Y/m
40
20
观测站位置 目标真实轨迹 PF算 法 轨 迹 0 20 40 60 X/m 80 100 120
RMSE,q=0.0001 2.5 2
error/m
RMS跟 踪 误 差
1.5 1 0.5 0 0 5 10 15 time/s
• 多目标跟踪系统
3、粒子滤波原理
• 随机重采样
执行仿真程序，得到以下仿真 结果，图中上部就是程序中给 定的W随机样本，而下部分是 由随机采样得到的V样本集合。 从连线关系可以看出，W样本 集中的第2个样本被复制一次， 第3个样本被复制2次，第7个 样本被复制2次，第8个样本被 复制1次，第9个样本被复制2 次，……，W中除了被复制的 样本外，其他样本被舍弃，这 就是“优胜劣汰”的思想。
4、粒子滤波在单目标跟踪中的应用
• 状态方程：
• 观测方程：
• 噪声模型：Q、R
4、纯方位角单目标跟踪
•
• • • • • • • • • • • • • • • • • • • •
纯方位跟踪系统仿真程序
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%% % 程序说明： 单站单目标基于角度的跟踪系统，采用粒子滤波 算法 % 状态方程 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/180*0.1; % 观测角度均方差，可将0.1设置的更 小 F=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1]; %%%%%%%%%%%%%%% 系统初始化 %%%%%%%%%%%%%%%%%% Length=100; % 目标运动的场地空间 Width=100; % 观测站的位置随即部署 Node.x=Width*rand; Node.y=Length*rand;
• 状态方程
250 200
• 观测方程
150
100
50
0
-50 -50
0
50
100
150
200
250
5、粒子滤波在多目标跟踪中的应用
• 近邻法分类
5、粒子滤波在多目标跟踪中的应用
•
• • • • • • • • • • • • • • • • • • • • • •
•
近邻法分类程序
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%% % 函数功能：近邻法分类程序 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%% function Neighbour_Classify_exam2 % 初始化数据 Type=3; % 假设已知三类目标 % 随机产生M个样本点 M=30; % 场地空间的长和宽 Width=4; Length=4; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%% % 样本初始化、并模拟一组样本空间 for i=1:Type x0=10*cos(pi*2*i/3); y0=10*sin(pi*2*i/3); XX{i}=[x0,y0]'; % 已知类别的样本集合 for j=1:M % 模拟真实样本的空间，在类空间的位置 X{i}(:,j)=[x0+Width*randn;y0+Length*randn]; end End
3、粒子滤波原理
• 粒子滤波目前有四大基本 的重采样方法，分别是残 差重采样（Residual resampling），多项式重 采样（Multinomial resampling），系统重采 样（Systematic resampling），随机重采 样（random resampling），关于他们 的原理，读者可以到网上 检索相关的论文。
……
5、粒子滤波在多目标跟踪的应用
•
• • • • • • • • • • • • • • •
基于近邻法的多目标跟踪粒子滤波程序
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%% % 单站多目标跟踪的建模程序，并用近邻法分类 % 主要模拟多目标的运动和观测过程，涉及融合算法---近邻法 function MTT_Model_With_NNClass_PF_SingleStation %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%% % 初始化参数 % 观测站位置，随机的 T=10; % 仿真时间长度 TargetNum=3; % 目标个数 dt=1; % 采样时间间隔 S.x=100*rand; % 观测站水平位置 S.y=100*rand; % 观测站纵向位置 F=[1,dt,0,0;0,1,0,0;0,0,1,dt;0,0,0,1]; % 采用CV模型的状态转移矩 阵 G=[0.5*dt^2,0;dt,0;0,0.5*dt^2;0,dt]; % 过程噪声驱动矩阵 H=[1,0,0,0;0,0,1,0]; % 观测矩阵
粒子滤波算法原理及Matlab程序
主讲： 方牛娃 QQ: 345194112
1、粒子滤波的发展历史
• 90年代初，Gordon、Salmond、和Smith所提出的重采样 （Resampling）技术 。
• 90年中期，计算机的计算能力的提高 • 近年来的新技术，EPF、UPF、RBPF等 • 新的应用领域：目标定位和跟踪、图像处理、语音处理、 故障检测、经济数据处理
7
10 10 10 11 13 13 14 19 19 19 20 20 22 23 24 25 26 27 28 28
参考资料
• • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • 第二部分 应用篇 第四章 粒子滤波在单目标跟踪中的应用 4.1 目标跟踪过程描述 4.2 单站单目标跟踪系统建模 4.3 单站单目标观测距离的系统及仿真程序 4.3.1 基于距离的系统模型 4.3.2 基于距离的跟踪系统仿真程序 4.4 单站单目标纯方位角度观测系统及仿真程序 4.4.1 纯方位目标跟踪系统模型 4.4.2 纯方位跟踪系统仿真程序 4.5 多站单目标纯方位角度观测系统及仿真程序 4.5.1 多站纯方位目标跟踪系统模型 4.5.2 多站纯方位跟踪系统仿真程序 第五章 粒子滤波在多目标跟踪中的应用 5.1 多目标跟踪系统建模 5.1.1 单站多目标跟踪系统建模 5.1.2 多站多目标跟踪系统建模 5.1.3 单站多目标线性跟踪系统的建模仿真程序 5.1.4 多站多目标非线性跟踪系统的建模仿真程序 5.2 多目标跟踪分类算法 5.2.1 多目标数据融合概述 5.2.2 近邻法分类算法及程序 5.2.3 近邻法用于目标跟踪中的航迹关联及算法程序 5.2.4 K-近邻法分类算法 5.3 粒子滤波用于多目标跟算法中的状态估计 5.3.1 原理介绍 5.3.2 基于近邻法的多目标跟踪粒子滤波程序 第六章 粒子滤波在电池寿命预测中的应用 6.1 概述 6.2 电池寿命预测的模型 6.3 基于粒子滤波的电池寿命预测仿真程序 33 33
X l sin 2
l l 2l sin 2 p P X sin 0 02 dxdw ˆ 2
2、蒙特卡洛
• • • • • • • • • • • • • • • • • • • • • %%%%%%%%%%%%%%%%%%%%%%%%% % 说明：利用蒙特卡洛模拟计算圆周率 %%%%%%%%%%%%%%%%%%%%%%%%% function buffon_test l=0.6; m=10000; % 实验次数 buffon(l,m); %%%%%%%%%%%%%%%%%%%%%%%%% function piguji=buffon(llength,mm) %llength 是针的长度 %mm 是随机实验次数 frq=0; xrandnum = unifrnd(0,0.5,1,mm); phi= unifrnd(0,pi,1,mm); for ii=1:mm if (xrandnum(1,ii)<=(llength*sin(phi(1,ii))/2)) frq=frq+1; end end piguji=2*llength/(frq/mm) 实验结果如下：