近期迷上基于贝叶斯理论的粒子滤波,很多概念不好理解,看了不少博文和书籍,有两个难点不好理解,一个是采样和由此衍生的重要性采样,另一个是重新采样,针对这两个概念,这里给出我的理解和比喻,在粒子滤波中的采样跟信号与系统中的采样完全是两码事,用一个比喻来理解,现在有个黑箱子,里面放了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://wap.sciencenet.cn/blog-684084-945598.html
上一篇:
DIY神经网络Matlab代码 下一篇:
大话极大似然估计