深入理解粒子滤波(Particle Filter)#附MATLAB代码

目录1. 粒子滤波算法简介2. 贝叶斯滤波2.1 概率论基础2.2 贝叶斯融合多种观测2.2 贝叶斯融合动作2.3 贝叶斯滤波算法2.3.1 系统输入2.3.2 期望输出2.3.3 模型假设2.3.3 Bayes滤波算法3. 蒙特卡洛(Monte Carlo)积分法4. 重要性采样5. 重采样方法6. SIR滤波(Sampling Importance Resampling Filter)MATLAB代码1. 书包追踪2. 三维位置跟踪参考资料

1. 粒子滤波算法简介

粒子滤波通过非参数化的蒙特卡洛(Monte Carlo)模拟方法和重要性采样来实现递推贝叶斯滤波,适用于任何能用状态空间模型描述的非线性系统,精度可以逼近最优估计。该方法的思想是用一组粒子来近似表示系统的后验概率分布,然后来估计非线性系统的状态。

2. 贝叶斯滤波

贝叶斯滤波方法是很多方法的基础,例如Kalman滤波,扩展Kalman滤波以及粒子滤波。它的工作就是根据不断接收到的新信息和已经提供的一些已经知道的信息,来不断更新概率。更新概率值的方法是根据概率论中的条件概率计算公式来做的。首先我们来回顾一下概率论的一些基础知识吧~

2.1 概率论基础

    X: 表示一个随机变量,如果它有有限个可能的取值{x1,x2,⋯,xn}.

    p(X = xi): 表示变量X的值为 xi的概率。

    p(⋅):称为概率质量函数(probability mass function).
    例:一个家里有3个房间,机器人在各个房间的概率为 p(room)={0.1,0.3,0.6}.

    如果X在连续空间取值,p(x)称为概率密度函数(probability density function)
    深入理解粒子滤波(Particle Filter)#附MATLAB代码

    联合概率:p(X=x and Y=y)=p(x,y),称为联合概率密度分布。如果X和Y是相互独立的随机变量,p(x,y)=p(x)p(y)。

    条件概率:p(X=x|Y=y) 是在已知Y=y的条件下,计算X=x的概率。
    深入理解粒子滤波(Particle Filter)#附MATLAB代码
    如果x和y相互独立,则:
    深入理解粒子滤波(Particle Filter)#附MATLAB代码

    全概率公式:
    离散情况下:
    深入理解粒子滤波(Particle Filter)#附MATLAB代码
    连续情况下:
    深入理解粒子滤波(Particle Filter)#附MATLAB代码

    贝叶斯公式:
    基于条件概率公式和全概率公式,我们可以导出贝叶斯公式:
    深入理解粒子滤波(Particle Filter)#附MATLAB代码
    这里面x一般是某种状态;y一般是代表某种观测。P(x)叫做先验概率,P(y|x)也叫做似然。

2.2 贝叶斯融合多种观测

在很多应用问题中,我们会用多种观测信息对一个状态进行猜测和推理

深入理解粒子滤波(Particle Filter)#附MATLAB代码
所以有
深入理解粒子滤波(Particle Filter)#附MATLAB代码

2.2 贝叶斯融合动作

我们用u来描述动作,在x′状态下,执行了动作u之后,对象状态改变为x的概率表述为:

深入理解粒子滤波(Particle Filter)#附MATLAB代码
执行某一动作后,计算动作后的状态概率,需要考虑动作之前的各种状态情况,把所有情况用全概率公式计算:
连续情况下:深入理解粒子滤波(Particle Filter)#附MATLAB代码
离散情况下:
深入理解粒子滤波(Particle Filter)#附MATLAB代码

2.3 贝叶斯滤波算法
2.3.1 系统输入

    1到t时刻的状态观测和动作:dt={u1,z1…,ut,zt}
    观测模型1:P(z|x)
    动作的状态转移模型:P(x(t)|u,x(t-1))
    系统状态的先验概率分布P(x)

2.3.2 期望输出

计算状态的后验概率,称为状态的置信概率:Bel(xt)=P(xt|u1,z1…,ut,zt)

2.3.3 模型假设

    Markov性假设: t时刻的状态由t−1时刻的状态和t时刻的动作决定。t时刻的观测仅同t时刻的状态相关。
    深入理解粒子滤波(Particle Filter)#附MATLAB代码
    深入理解粒子滤波(Particle Filter)#附MATLAB代码
    静态环境,即对象周边的环境假设是不变的
    观测噪声、模型噪声等是相互独立的

2.3.3 Bayes滤波算法

基于上述设定和假设,我们给出贝叶斯滤波算法的推导过程:
深入理解粒子滤波(Particle Filter)#附MATLAB代码
深入理解粒子滤波(Particle Filter)#附MATLAB代码
请注意上面的推导过程中需要用到积分,这对于一般的非线性,非高斯系统,很难得到后验概率的解析解。为了解决这个问题,就得引进蒙特卡洛采样。

3. 蒙特卡洛(Monte Carlo)积分法

蒙特卡洛法是一类随机算法的统称,它是一种统计模拟方法,通常是利用随机数来解决一些数值计算问题。

    投点法
    深入理解粒子滤波(Particle Filter)#附MATLAB代码

蒙特卡洛的基本做法是通过大量重复试验,通过统计频率,来估计概率,从而得到问题的求解。举个例子,如上图所示,一个矩形内有一个不规则图案,要求解不规则图形的面积。显然,矩形的面积可以简单计算为A=ab*ac,点位于不规则形状内的概率为A

shape_{shape}

shape​/A。现在重复往矩形范围内随机的投射点,样本点有一定概率会落在不规则图形内,若复杂n次试验,落到不规则图形内的次数为k,频率为k/n,若样本数量较大,则有
深入理解粒子滤波(Particle Filter)#附MATLAB代码
根据伯努利大数定理得到的结论就是:随着样本数增加,频率k/n会收敛于概率p,使得该等式成立。

    期望法
    设x是相互独立的样本且服从同一分布,概率密度函数表示为p(x),则函数的积分可以表示为:
    深入理解粒子滤波(Particle Filter)#附MATLAB代码
    证明:
    深入理解粒子滤波(Particle Filter)#附MATLAB代码
    也就是说,当N取的足够大时,结果将无限逼近解析积分,为无偏估计。
    综上所述,蒙特卡洛方法是一种可以近似计算任意函数积分的数值方法。它的计算分为以下步骤:
    1.对一个满足某种概率分布的随机数进行抽样
    2.使用该抽样计算g(x)/p(x)的值,作为样本
    3.最后对所有的样本累加求平均

由贝叶斯滤波可知,它的后验概率计算里要用到积分,为了解决积分难的问题,可以用蒙特卡洛采样来代替计算后验概率。

假设可以从后验概率中采样到N个样本,那么后验概率的计算可表示为:

深入理解粒子滤波(Particle Filter)#附MATLAB代码

δ(x)\delta(x)

δ(x)为狄拉克函数。
则当前状态的期望值为:
深入理解粒子滤波(Particle Filter)#附MATLAB代码
也就是用这些采样的粒子的状态值直接平均就得到了期望值,也就是滤波后的值,这里的 f(x) 就是每个粒子的状态函数。这就是粒子滤波了,只要从后验概率中采样很多粒子,用它们的状态求平均就得到了滤波结果。思路看似简单,但是要命的是,后验概率不知道啊,怎么从后验概率分布中采样?所以这样直接去应用是行不通的,这时候得引入重要性采样这个方法来解决这个问题。

4. 重要性采样

无法从目标分布中采样,就从一个已知的可以采样的分布里去采样如 q(x|y),这样上面的求期望问题就变成了:

深入理解粒子滤波(Particle Filter)#附MATLAB代码
其中,
深入理解粒子滤波(Particle Filter)#附MATLAB代码
由于
深入理解粒子滤波(Particle Filter)#附MATLAB代码
所以可进一步写为
深入理解粒子滤波(Particle Filter)#附MATLAB代码
上面的期望计算都可以通过蒙特卡洛方法来解决它,也就是说,通过采样N个样本(q分布),用样本的平均来求它们的期望,所以上面的(3)式可以近似为:
深入理解粒子滤波(Particle Filter)#附MATLAB代码
注意上面的(4)式,它不再是(1)式中所有的粒子状态直接相加求平均了,而是一种加权和的形式。不同的粒子都有它们相应的权重,如果粒子权重大,说明信任该粒子比较多。

到这里已经解决了不能从后验概率直接采样的问题,但是上面这种每个粒子的权重都直接计算的方法,效率低,因为每增加一个采样,p( x(k) |y(1:k))都得重新计算,并且还不好计算这个式子。所以求权重时能否避开计算p(x(k)|y(1:k))?最佳的形式是能够以递推的方式去计算权重,这就是所谓的序贯重要性采样(SIS),粒子滤波的原型。

权重的递归形式可表示为:
深入理解粒子滤波(Particle Filter)#附MATLAB代码
在实际应用中我们可以假设重要性分布q()满足:
深入理解粒子滤波(Particle Filter)#附MATLAB代码
因此有
深入理解粒子滤波(Particle Filter)#附MATLAB代码
序贯重要性采样 Sequential Importance Sampling (SIS) Filter
深入理解粒子滤波(Particle Filter)#附MATLAB代码
在应用SIS 滤波的过程中,存在一个退化的问题。就是经过几次迭代以后,很多粒子的权重都变得很小,可以忽略了,只有少数粒子的权重比较大。并且粒子权值的方差随着时间增大(权重大的和权重小的之间差距大,表明权值退化越严重),状态空间中的有效粒子数较少。随着无效采样粒子数目的增加,使得大量的计算浪费在对估计后验滤波概率分布几乎不起作用的粒子上,使得估计性能下降。因此,一般采用以下两种途径:(1)选择合适的重要性概率密度函数;(2)在序贯重要性采样之后,采用重采样方法。

5. 重采样方法

深入理解粒子滤波(Particle Filter)#附MATLAB代码
重采样以后,所有的粒子权重一样,都是1/N,只是有的粒子多出现了n(i)次。
类似于遗传算法中那种轮盘赌的思想。

在这里用个简单的例子来说明:
假设有3个粒子,在第k时刻的时候,他们的权重分别是0.1, 0.1 ,0.8, 然后计算他们的概率累计和(matlab 中为cumsum() )得到: [0.1, 0.2, 1]。接着,我们用服从[0,1]之间的均匀分布随机采样3个值,假设为0.15 , 0.38 和 0.54。也就是说,第二个粒子复制一次,第三个粒子复制两次。

在MATLAB中一句命令就可以方便的实现这个过程:

[~, j] = histc(rand(N,1), [0 cumsum(w')])

深入理解粒子滤波(Particle Filter)#附MATLAB代码
当然,权重大,如果是分子大,即后验概率大,那说明确实应该在后验概率大的地方多放几个粒子。但权重大也有可能是分母小造成的,这时候的分子也可能小,也就是实际的后验概率也可能小,这时候的大权重可能就没那么优秀了。何况,这种简单的重采样会使得粒子的多样性丢失,到最后可能都变成了只剩一种粒子的分身。在遗传算法中好歹还引入了变异来解决多样性的问题。当然,粒子滤波里也有专门的方法:正则粒子滤波。

6. SIR滤波(Sampling Importance Resampling Filter)

SIR滤波器很容易由前面的基本粒子滤波推导出来,只要对粒子的重要性概率密度函数做出特定的选择即可。在SIR中,选取:

深入理解粒子滤波(Particle Filter)#附MATLAB代码
这个式子代入到第二章SIS推导出的权重公式中:
深入理解粒子滤波(Particle Filter)#附MATLAB代码
得到
深入理解粒子滤波(Particle Filter)#附MATLAB代码
由之前的重采样我们知道,实际上每次重采样以后粒子的权重都变成了1/N,所以可以进一步简化成:
深入理解粒子滤波(Particle Filter)#附MATLAB代码
它表示在状态x出现的条件下,测量y出现的概率。要知道y出现的概率,需要知道此时y的分布。测量是在真实值附近添加了一个高斯噪声。因此,y的分布就是以真实测量值为均值,以噪声方差为方差的一个高斯分布。因此,权重的采样过程就是:当粒子处于x状态时,能够得到该粒子的测量y。要知道这个测量y出现的概率,就只要把它放到以真实值为均值,噪声方差为方差的高斯分布里去计算就行了
深入理解粒子滤波(Particle Filter)#附MATLAB代码
到这里,就可以看成SIR只和系统状态方程有关了,不用自己另外去设计概率密度函数,所以在很多程序中都是用的这种方法。
深入理解粒子滤波(Particle Filter)#附MATLAB代码

MATLAB代码
1. 书包追踪

%% Parameters
% reference : http://www.mathworks.com/matlabcentral/fileexchange/33666-simple-particle-filter-demo
F_update = [1 0 1 0; 0 1 0 1; 0 0 1 0; 0 0 0 1];

Npop_particles = 5000;

Xstd_rgb = 30; %观测方差50
Xstd_pos = 15;%25
Xstd_vec = 5;%5

Xrgb_trgt = [0; 0; 0];%rbg,[255,0,0]为红色

%% Loading Movie

vr = VideoReader('me.mp4');

Npix_resolution = [vr.Width vr.Height];%Height - 视频帧的高度,Width - 视频帧的宽度
Nfrm_movie = floor(vr.Duration * vr.FrameRate);%视频多少帧

%% Object Tracking by Particle Filter

X = create_particles(Npix_resolution, Npop_particles); % 粒子初始化,在画面中产生均匀分布的随机粒子

for k = 1:Nfrm_movie

% Getting Image
Y_k = read(vr, k);

% Forecasting
%通过状态模型预测 这里采用的是在上一时刻基础上叠加噪声
X = update_particles(F_update, Xstd_pos, Xstd_vec, X);

% Calculating Log Likelihood
L = calc_log_likelihood(Xstd_rgb, Xrgb_trgt, X(1:2, :), Y_k);

% Resampling
X = resample_particles(X, L);

% Showing Image
show_particles(X, Y_k);
% show_state_estimated(X, Y_k);

end
function X = update_particles(F_update, Xstd_pos, Xstd_vec, X)
% X 所有粒子组成的矩阵
% X(1:2, :) 各粒子在画面中的位置
N = size(X, 2);

X = F_update * X;

X(1:2,:) = X(1:2,:) + Xstd_pos * randn(2, N);
X(3:4,:) = X(3:4,:) + Xstd_vec * randn(2, N);
function L = calc_log_likelihood(Xstd_rgb, Xrgb_trgt, X, Y) %#codegen

Npix_h = size(Y, 1);
Npix_w = size(Y, 2);

N = size(X,2);

L = zeros(1,N);
Y = permute(Y, [3 1 2]); %重新安排矩阵

A = -log(sqrt(2 * pi) * Xstd_rgb);
B = - 0.5 / (Xstd_rgb.^2);

X = round(X);

for k = 1:N
% m,n 粒子k在画面中的位置
m = X(1,k);
n = X(2,k);

I = (m >= 1 & m <= Npix_h);
J = (n >= 1 & n <= Npix_w);

if I && J

C = double(Y(:, m, n));

D = C - Xrgb_trgt;

D2 = D' * D; % 欧氏距离

L(k) = A + B * D2; %高斯似然 D2 越小 L越大 注意B为负数
else

L(k) = -Inf;
end
end
function X = resample_particles(X, L_log)

% Calculating Cumulative Distribution

L = exp(L_log - max(L_log));
Q = L / sum(L, 2); % a = sum(L,2) a中元素为各行向量累加值
R = cumsum(Q, 2);

% Generating Random Numbers

N = size(X, 2);
T = rand(1, N);

% Resampling

[W, I] = histc(T, R);
X = X(:, I + 1);
function show_particles(X, Y_k)

figure(1)
image(Y_k)
title('Particle filter')

hold on
plot(X(2,:), X(1,:), '.')
hold off

drawnow%可以理解为立即执行的plot,变化的plot。当代码执行时间长,需要反复执行plot时,Matlab程序不会马上把图像画到figure上,这时,要想实时看到图像的每一步变化情况,需要使用这个语句。

效果图深入理解粒子滤波(Particle Filter)#附MATLAB代码

2. 三维位置跟踪

% 运动轨迹
t=0:pi/10:20*pi;
z=t;
x=t.*sin(t);
y=t.*cos(t);
N = 100; %粒子数
Pa_Po = 20*pi*rand(3,N);%初始值
ParticleEstimate = zeros(3,201);
SystemVariance = [3;4;5];
ObeserveVariance = 5;
Observe = [x;y;z]+sqrt(ObeserveVariance)*rand(3,201);
for k = 1:201

Pa_Po = stateTransfer(Pa_Po,SystemVariance,k);%计算粒子转移后的状态/重要性概率密度函数采样

L = calc_log_likelihood2(Observe(:,k),Pa_Po,ObeserveVariance);%计算测量方程的概率

Pa_Po = resample_particles2(Pa_Po,L);

ParticleEstimate(:,k) = sum(Pa_Po,2)/N;%取均值

end
plot3(ParticleEstimate(1,:),ParticleEstimate(2,:),ParticleEstimate(3,:))
hold on
plot3(x,y,z)
legend('ParticleFilterEstimate','Real')
function x = stateTransfer(x,y,k)
[m,n] = size(x);
for p = 1:n
x(1,p) = k*pi/10*sin(k*pi/10)+sqrt(y(1))*randn;
x(2,p) = k*pi/10*cos(k*pi/10)+sqrt(y(2))*randn;
x(3,p) = k*pi/10+sqrt(y(3))*randn;
end
end

function L = calc_log_likelihood2(object,x,y)
n = size(x,2);

L = zeros(1,n);
for k = 1:n
dist = norm(object - x(:,k));
L(k) = 1/(sqrt(2*pi*y))*exp(-dist.^2/2/y);
end
L = L/sum(L);
end

function Pa_Po= resample_particles2(Pa_Po,L)
[m,n] = size(Pa_Po);
R = cumsum(L, 2);
T = rand(1, n);
[~, I] = histc(T, R);
Pa_Po =Pa_Po(:, I + 1);
end

效果图深入理解粒子滤波(Particle Filter)#附MATLAB代码

参考资料

十分钟理解与推导贝叶斯滤波(Bayes Filter)算法
细说贝叶斯滤波
蒙特卡洛积分
Particle Filter Tutorial 粒子滤波:从推导到应用(一)
Particle Filter Tutorial 粒子滤波:从推导到应用(二)
Particle Filter Tutorial 粒子滤波:从推导到应用(三)
Particle Filter Tutorial 粒子滤波:从推导到应用(四)
粒子滤波原理(百度文库)


  1. 其实无论是观测模型还是状态转移模型,本质上就是噪声的分布叠加上一个常数,可以看作是偏移过的噪声。此外,噪声通常可看作高斯分布,因此,信号本身就是一个高斯分布。 ↩︎

原创:https://www.panoramacn.com
源码网提供WordPress源码,帝国CMS源码discuz源码,微信小程序,小说源码,杰奇源码,thinkphp源码,ecshop模板源码,微擎模板源码,dede源码,织梦源码等。

专业搭建小说网站,小说程序,杰奇系列,微信小说系列,app系列小说

深入理解粒子滤波(Particle Filter)#附MATLAB代码

免责声明,若由于商用引起版权纠纷,一切责任均由使用者承担。

您必须遵守我们的协议,如果您下载了该资源行为将被视为对《免责声明》全部内容的认可-> 联系客服 投诉资源
www.panoramacn.com资源全部来自互联网收集,仅供用于学习和交流,请勿用于商业用途。如有侵权、不妥之处,请联系站长并出示版权证明以便删除。 敬请谅解! 侵权删帖/违法举报/投稿等事物联系邮箱:2640602276@qq.com
未经允许不得转载:书荒源码源码网每日更新网站源码模板! » 深入理解粒子滤波(Particle Filter)#附MATLAB代码
关注我们小说电影免费看
关注我们,获取更多的全网素材资源,有趣有料!
120000+人已关注
分享到:
赞(0) 打赏

评论抢沙发

  • 昵称 (必填)
  • 邮箱 (必填)
  • 网址

您的打赏就是我分享的动力!

支付宝扫一扫打赏

微信扫一扫打赏