陈颖频的科学网博客分享 http://blog.sciencenet.cn/u/s110500617

博文

关于粒子滤波的几则比喻

已有 5743 次阅读 2015-12-24 15:01 |系统分类:科研笔记|关键词:学者

    近期迷上基于贝叶斯理论的粒子滤波,很多概念不好理解,看了不少博文和书籍,有两个难点不好理解,一个是采样和由此衍生的重要性采样,另一个是重新采样,针对这两个概念,这里给出我的理解和比喻,在粒子滤波中的采样跟信号与系统中的采样完全是两码事,用一个比喻来理解,现在有个黑箱子,里面放了2个白球和3个红球,你伸手进去拿了一个球,看看什么颜色,这个过程就是一次采样,而球的颜色就是采样得到的状态,你记录了球的颜色以后把球放回去,再从新摇一摇,再取一个球看颜色,这就是第二次采样,以此类推,可以想象,当你取的次数远远超过球的总数后,采样记录中,采到白球和红球的概率显然大致会是2:3,好,采样的概念建立起来了,相信有点概率基础就能理解,现在稍微进阶点,如果我给的不是离散随机变量,而是连续随机变量,那么采样又该如何理解,其实从刚才的距离中就很容易发现,无论是离散还是连续,只要给定了概率分布,那么高概率的状态,最后采样得到的也将是高概率的,OK,这样我们再来讨论重要性采样就好理解很多,重要性采样根据重要性概率密度来做采样,什么是重要性概率密度呢?其实就是已知观测向量和上一次状态前提下的后验概率,有些书上写预测概率密度,一个意思,那么为什么重要呢?很显然,对于一个马尔科夫过程,有无后效性,那么当前状态显然只依赖于前一个状态,你说重要不重要,另外,当前观测向量是由当前状态导致的,也就是已经知道果,要求因的概率,你说重要不重要,好,重要性函数的概念也建立了。
   下面给出一个比喻,假设广场上有一只猫(目标),你通过你的眼睛含噪声地观察(观测值)到不太准确的位置,你并不确信你的观察是正确的,于是你放出一大群狗(粒子),随机地分布在广场上,而离这只猫最近的那些狗,它们的重要性就高于离猫远一些的狗,以离目标的距离为衡量,产生一组重要性排名,排名靠前则权重最大,离猫最近,排名靠后,则狗离猫越远(重要性采样)。你猜如果猫此时不动,其他狗会怎么样?很简单,离猫最近的狗已经咬上猫了,这些狗(粒子)将在这一帧画面中不动,而那些离猫很远的狗,必然恶狠狠地朝着猫飞奔而来,这时候,粒子就发生了重采样,重采样后,作为主人的你,又用含噪声的眼睛读出这群狗的位置,然后把这群狗的位置中心算一遍,用于校正眼睛带来的噪声,于是就完成了粒子滤波的过程。

附录:粒子滤波精简版代码

%修改人:陈颖频
%日期:2015.12.22
% 参数设置

N = 100; %粒子总数
Q = 5;      %过程噪声
R = 5;      %测量噪声
T = 20;     %测量帧数
WorldSize = 100;    %世界大小
X = zeros(2, T);    %存储系统状态
Z = zeros(2, T);    %存储系统的观测状态
P = zeros(2, N);    %建立粒子群
PCenter = zeros(2, T);  %所有粒子的中心位置
w = zeros(N, 1);         %每个粒子的权重
err = zeros(1,T);     %误差
X(:, 1) = [50; 20];     %初始系统状态
Z(:, 1) = [50; 20] + wgn(2, 1, 10*log10(R));    %初始系统的观测状态

%初始化粒子群
for i = 1 : N
   P(:, i) = [WorldSize*rand; WorldSize*rand];
   dist = norm(P(:, i)-Z(:, 1));     %与测量位置相差的距离
   w(i) = (1 / sqrt(R) / sqrt(2 * pi)) * exp(-(dist)^2 / 2 / R);   %求权重
end
PCenter(:, 1) = sum(P, 2) / N;      %所有粒子的几何中心位置

%%
err(1) = norm(X(:, 1) - PCenter(:, 1));     %粒子几何中心与系统真实状态的误差
figure(1);
set(gca,'FontSize',12);
hold on
plot(X(1, 1), X(2, 1), 'r.', 'markersize',30)   %系统状态位置
axis([0 100 0 100]);
plot(P(1, :), P(2, :), 'k.', 'markersize',5);   %各个粒子位置
plot(PCenter(1, 1), PCenter(2, 1), 'b.', 'markersize',25); %所有粒子的中心位置
legend('True State', 'Particles', 'The Center of Particles');
title('Initial State');
hold off


%%
%开始运动
for k = 2 : T
     
   X(:, k) = X(:, k-1) +wgn(2, 1, 10*log10(Q));     %状态方程
   Z(:, k) = X(:, k) + wgn(2, 1, 10*log10(R));     %观测方程
 
   %粒子滤波
   %预测
   for i = 1 : N
       P(:, i) = P(:, i)  + wgn(2, 1, 10*log10(Q));             %只需要粒子的移动噪声参数与状态方程的噪声参数相同即可
       dist = norm(P(:, i)-Z(:, k));                                     %与测量位置相差的距离
       w(i) = (1 / sqrt(R) / sqrt(2 * pi)) * exp(-(dist)^2 / 2 / R);   %求权重,距离越大,权重越小,距离越小,权重越大,重要性越大
   end
%归一化权重
   wsum = sum(w);
   for i = 1 : N
       w(i) = w(i) / wsum;
   end
 
   %重采样(更新)
   for i = 1 : N
      wmax =  max(w) * 0.5;  %重要性阈值
       index = randi(N, 1);
       while(wmax > w(index))    
           wmax = wmax ;
           index = index + 1;      %忽略不重要的粒子,在本粒子滤波算法中,认为低于重要性阈值的粒子是不重要的,去掉
           if index > N
               index = 1;
           end          
       end
       P(:, i) = P(:, index);     %复制重要性粒子,这就是所谓的重采样,重新采样的意思就是重要的粒子复制,不重要的忽略
   end
 
   PCenter(:, k) = sum(P, 2) / N;      %所有粒子的中心位置,以重新采样的粒子中心作为跟踪的中心
 
   %计算误差
   err(k) = norm(X(:, k) - PCenter(:, k));     %粒子几何中心与系统真实状态的误差
 
   figure(2);
   set(gca,'FontSize',12);
   clf;
   hold on
   plot(X(1, k), X(2, k), 'r.', 'markersize',50);  %系统状态位置
   axis([0 100 0 100]);
   plot(P(1, :), P(2, :), 'k.', 'markersize',5);   %各个粒子位置
   plot(PCenter(1, k), PCenter(2, k), 'b.', 'markersize',25); %所有粒子的中心位置
   legend('True State', 'Particle', 'The Center of Particles');
   hold off
   pause(0.1);
end

%%
figure(3);
set(gca,'FontSize',12);
plot(X(1,:), X(2,:), 'r', Z(1,:), Z(2,:), 'g', PCenter(1,:), PCenter(2,:), 'b-');
axis([0 100 0 100]);
legend('True State', 'Measurement', 'Particle Filter');
xlabel('x', 'FontSize', 20); ylabel('y', 'FontSize', 20);

%%
figure(4);
set(gca,'FontSize',12);
plot(err,'.-');
xlabel('t', 'FontSize', 20);
title('The err');                           %绘制T帧的误差曲线





https://m.sciencenet.cn/blog-684084-945598.html

上一篇:DIY神经网络Matlab代码
下一篇:大话极大似然估计

1 单泽彪

该博文允许注册用户评论 请点击登录 评论 (0 个评论)

数据加载中...
扫一扫,分享此博文

Archiver|手机版|科学网 ( 京ICP备07017567号-12 )

GMT+8, 2022-8-12 22:55

Powered by ScienceNet.cn

Copyright © 2007- 中国科学报社

返回顶部