粒子滤波算法原理及Matlab程序
合集下载
相关主题
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
动性越大，轨迹越随机（乱）
❖ 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];
❖ %%%%%%%%%%%%%%% 系统初始化 %%%%%%%%%%%%%%%%%%
这里，随机投针指的是：针的中心点与最近 的平行线间的距离X均匀地分布在区间[0,1/2] 上，针与平行线的夹角（不管相交与否） 均匀的分布在区间[0,]上。此时，针与线相 交的充要条件是
X l sin 2
2、蒙特卡洛
❖ %%%%%%%%%%%%%%%%%%%%%%%%%
❖ % 说明：利用蒙特卡洛模拟计算圆周率
0.8 0.7 0.6 0.5 0.4 0.3 0.2 0.1
0
200
400
600
800
Biblioteka Baidu1000
a=0;
2、蒙特卡洛原理
v 蒙特卡洛的应用
应用说明：利用蒙特卡洛模拟 计算圆周率 方法解决：
从而针线相交的概率为：
p
ˆ
P
X
l 2
sin
0
l sin
2
0
2 dxdw
2l
假设平面上有无数条距离为1的等距平行线， 现向该平面随机投掷一根长度为l的针(l1)， 则我们可计算该针与任一平行线相交的概率。
❖ 粒子滤波技术是以蒙特卡洛为基础的
❖ 蒙特卡洛：用实验模拟的方法解决复杂的积分计算问题
❖ 硬币投掷实验（1）
✓ 掷一枚均匀硬币，正面朝上的次数X服从参数为１，p的二项分 布,X~B(1,p)
✓ 在Matlab中编辑.m文件输入以下命令：
1
0.9
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); % 服从二项分布
3、粒子滤波原理
❖ 随机重采样
执行仿真程序，得到以下仿真 结果，图中上部就是程序中给 定的W随机样本，而下部分是 由随机采样得到的V样本集合。 从连线关系可以看出，W样本 集中的第2个样本被复制一次， 第3个样本被复制2次，第7个 样本被复制2次，第8个样本被 复制1次，第9个样本被复制2 次，……，W中除了被复制的 样本外，其他样本被舍弃，这 就是“优胜劣汰”的思想。
❖
frq=frq+1;
❖
end
❖ end
❖ piguji=2*llength/(frq/mm)
❖ 实验结果如下：
3、粒子滤波原理
❖ 粒子滤波目前有四大基本 的重采样方法，分别是残 差重采样（Residual resampling），多项式重 采样（Multinomial resampling），系统重采 样（Systematic resampling），随机重采 样（random resampling），关于他们 的原理，读者可以到网上 检索相关的论文。
5、粒子滤波在多目标跟踪中的应用
❖ 近邻法分类程序
❖ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%
❖ % 函数功能：近邻法分类程序
❖ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%
❖ function Neighbour_Classify_exam2
4、粒子滤波在单目标跟踪中的应用
❖ 状态方程：
❖ 观测方程： ❖ 噪声模型：Q、R
4、纯方位角单目标跟踪
❖ 纯方位跟踪系统仿真程序
❖ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%
❖ % 程序说明： 单站单目标基于角度的跟踪系统，采用粒子滤波 算法
1、粒子滤波的发展历史
❖ 90年代初，Gordon、Salmond、和Smith所提出的重采样 （Resampling）技术 。
❖ 90年中期，计算机的计算能力的提高
❖ 近年来的新技术，EPF、UPF、RBPF等
❖ 新的应用领域：目标定位和跟踪、图像处理、语音处理、 故障检测、经济数据处理
2、蒙特卡洛原理
RMSE,q=0.0001 2.5
RMS跟 踪 误 差
2
1.5
1
0.5
0
0
5
10
15
20
25
30
time/s
5、粒子滤波在多目标跟踪中的应用
❖ 多目标跟踪系统
❖ 状态方程 ❖ 观测方程
250
200
150
100
50
0
-50
-50
0
50
100
150
200
250
5、粒子滤波在多目标跟踪中的应用
❖ 近邻法分类
❖ %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))
❖ Length=100; % 目标运动的场地空间
❖ Width=100; ❖ % 观测站的位置随即部署
❖ Node.x=Width*rand;
❖ Node.y=Length*rand;
Y/ m
error/m
80
60
40
观测站位置
目标真实轨迹
PF算 法 轨 迹
20
0
20
40
60
80
100 120
X/m
❖ %%%%%%%%%%%%%%%%%%%%%%%%%
❖ function buffon_test
❖ l=0.6;
❖ m=10000; % 实验次数
❖ buffon(l,m);
❖ %%%%%%%%%%%%%%%%%%%%%%%%%
❖ function piguji=buffon(llength,mm)
❖ % 状态方程 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];
❖ %%%%%%%%%%%%%%% 系统初始化 %%%%%%%%%%%%%%%%%%
这里，随机投针指的是：针的中心点与最近 的平行线间的距离X均匀地分布在区间[0,1/2] 上，针与平行线的夹角（不管相交与否） 均匀的分布在区间[0,]上。此时，针与线相 交的充要条件是
X l sin 2
2、蒙特卡洛
❖ %%%%%%%%%%%%%%%%%%%%%%%%%
❖ % 说明：利用蒙特卡洛模拟计算圆周率
0.8 0.7 0.6 0.5 0.4 0.3 0.2 0.1
0
200
400
600
800
Biblioteka Baidu1000
a=0;
2、蒙特卡洛原理
v 蒙特卡洛的应用
应用说明：利用蒙特卡洛模拟 计算圆周率 方法解决：
从而针线相交的概率为：
p
ˆ
P
X
l 2
sin
0
l sin
2
0
2 dxdw
2l
假设平面上有无数条距离为1的等距平行线， 现向该平面随机投掷一根长度为l的针(l1)， 则我们可计算该针与任一平行线相交的概率。
❖ 粒子滤波技术是以蒙特卡洛为基础的
❖ 蒙特卡洛：用实验模拟的方法解决复杂的积分计算问题
❖ 硬币投掷实验（1）
✓ 掷一枚均匀硬币，正面朝上的次数X服从参数为１，p的二项分 布,X~B(1,p)
✓ 在Matlab中编辑.m文件输入以下命令：
1
0.9
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); % 服从二项分布
3、粒子滤波原理
❖ 随机重采样
执行仿真程序，得到以下仿真 结果，图中上部就是程序中给 定的W随机样本，而下部分是 由随机采样得到的V样本集合。 从连线关系可以看出，W样本 集中的第2个样本被复制一次， 第3个样本被复制2次，第7个 样本被复制2次，第8个样本被 复制1次，第9个样本被复制2 次，……，W中除了被复制的 样本外，其他样本被舍弃，这 就是“优胜劣汰”的思想。
❖
frq=frq+1;
❖
end
❖ end
❖ piguji=2*llength/(frq/mm)
❖ 实验结果如下：
3、粒子滤波原理
❖ 粒子滤波目前有四大基本 的重采样方法，分别是残 差重采样（Residual resampling），多项式重 采样（Multinomial resampling），系统重采 样（Systematic resampling），随机重采 样（random resampling），关于他们 的原理，读者可以到网上 检索相关的论文。
5、粒子滤波在多目标跟踪中的应用
❖ 近邻法分类程序
❖ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%
❖ % 函数功能：近邻法分类程序
❖ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%
❖ function Neighbour_Classify_exam2
4、粒子滤波在单目标跟踪中的应用
❖ 状态方程：
❖ 观测方程： ❖ 噪声模型：Q、R
4、纯方位角单目标跟踪
❖ 纯方位跟踪系统仿真程序
❖ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%
❖ % 程序说明： 单站单目标基于角度的跟踪系统，采用粒子滤波 算法
1、粒子滤波的发展历史
❖ 90年代初，Gordon、Salmond、和Smith所提出的重采样 （Resampling）技术 。
❖ 90年中期，计算机的计算能力的提高
❖ 近年来的新技术，EPF、UPF、RBPF等
❖ 新的应用领域：目标定位和跟踪、图像处理、语音处理、 故障检测、经济数据处理
2、蒙特卡洛原理
RMSE,q=0.0001 2.5
RMS跟 踪 误 差
2
1.5
1
0.5
0
0
5
10
15
20
25
30
time/s
5、粒子滤波在多目标跟踪中的应用
❖ 多目标跟踪系统
❖ 状态方程 ❖ 观测方程
250
200
150
100
50
0
-50
-50
0
50
100
150
200
250
5、粒子滤波在多目标跟踪中的应用
❖ 近邻法分类
❖ %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))
❖ Length=100; % 目标运动的场地空间
❖ Width=100; ❖ % 观测站的位置随即部署
❖ Node.x=Width*rand;
❖ Node.y=Length*rand;
Y/ m
error/m
80
60
40
观测站位置
目标真实轨迹
PF算 法 轨 迹
20
0
20
40
60
80
100 120
X/m
❖ %%%%%%%%%%%%%%%%%%%%%%%%%
❖ function buffon_test
❖ l=0.6;
❖ m=10000; % 实验次数
❖ buffon(l,m);
❖ %%%%%%%%%%%%%%%%%%%%%%%%%
❖ function piguji=buffon(llength,mm)
❖ % 状态方程 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; % 过程噪声调整参数，设得越大，目标运行的机