> 文档中心 > 粒子滤波PF—从贝叶斯滤波到粒子滤波PF—Part-V(粒子滤波PF)

粒子滤波PF—从贝叶斯滤波到粒子滤波PF—Part-V(粒子滤波PF)


粒子滤波PF—从贝叶斯滤波到粒子滤波PF—Part-V(粒子滤波PF)

原创不易,路过的各位大佬请点个赞

机动目标跟踪/非线性滤波/传感器融合/导航等探讨代码联系WX: ZB823618313

粒子滤波PF—从贝叶斯滤波到粒子滤波PF—Part-V(粒子滤波PF)

  • 粒子滤波PF—从贝叶斯滤波到粒子滤波PF—Part-V(粒子滤波PF)
    • 1、贝叶斯滤波
    • 2、 蒙特卡洛方法MC
    • 3、 序贯重要性采样SIS
    • 4、 粒子重采样
    • 5、 标准的粒子滤波PF
    • 6、粒子滤波PF的在目标跟踪应用:
      • 6.1、 仿真参数
      • 6.2、 跟踪轨迹和误差
    • 7、粒子滤波PF的标准验证模型
      • 7.1、 模型参数
      • 7.2、 基于==随机重采样==粒子滤波PF
      • 7.3、 基于==多项式重采样==粒子滤波PF
      • 7.4、 基于==残差重采样==粒子滤波PF
      • 7.5、 基于==系统重采样==粒子滤波PF
      • 7.6、 基于==系统重采样==粒子滤波PF==代码==

本博客主要给出基于系统重采样的SIR粒子滤波PF算法流程及代码

针对基于随机重采样、残差重采样以及多项式重采样的SIR粒子滤波PF只给出运行结果

在非线性条件下,贝叶斯滤波面临一个重要问题是状态分布的表达和积分式的求解,由前面章节中的分析可知,对于一般的非线性/非高斯系统,解析求解的途径是行不通的。在数值近似方法中,蒙特卡罗仿真是一种最为通用、有效的手段,粒子滤波就是建立在蒙特卡罗仿真基础之上的,它通过利用一组带权值的系统状态采样来近似状态的统计分布。由于蒙特卡罗仿真方法具有广泛的适用性,由此得到的粒子滤波算法也能适用于一般的非线性/非高斯系统。但是,这种滤波方法也面临几个重要问题,如有效采样(粒子)如何产生、粒子如何传递以及系统状态的序贯估计如何得到等。

简单的理解,粒子滤波就是使用了大量的随机样本,采用蒙特卡洛(MonteCarlo,MC)仿真技术完成贝叶斯递推滤波(Recursive Bayesian Filter)过程。因此本博客从贝叶斯滤波出发,简单介绍粒子滤波PF的出生、即应用

1、贝叶斯滤波

**贝叶斯滤波细节 见Part-I**

考虑离散时间非线性系统动态模型,
x k = f ( x k − 1 , w k − 1 ) z k = h ( x k , v k ) (1)x_k=f(x_{k-1},w_{k-1}) \\ z_k=h(x_k,v_k ) \tag{1} xk=f(xk1,wk1)zk=h(xk,vk)(1)
其中 x k x_k xk k k k时刻的目标状态向量, z k z_k zk k k k时刻量测向量(传感器数据)。这里不考虑控制器 u k u_k uk w k {w_k} wk v k {v_k} vk分别是过程噪声序列和量测噪声序列。 w k w_k wk v k v_k vk为零均值高斯白噪声。

2、 蒙特卡洛方法MC

**蒙特卡洛近似方法细节 见Part-II**

蒙特卡洛方法是实现的贝叶斯滤波,得到粒子滤波的桥梁。

3、 序贯重要性采样SIS

**序贯重要性采样SIS 见Part-III**

4、 粒子重采样

**序贯重要性采样SIS 见Part-IV**

重采样的思路是:既然那些权重小的不起作用了,那就不要了。要保持粒子数目不变,得用一些新的粒子来取代它们。找新粒子最简单的方法就是将权重大的粒子多复制几个出来,至于复制几个?那就在权重大的粒子里面让它们根据自己权重所占的比例去分配,也就是老大分身分得最多,老二分得次多,以此类推。下面以数学的形式来进行说明。

常用的重采样方法:
系统重采样
多项式重采样
残差重采样
随机重采样

5、 标准的粒子滤波PF

核心思想:是使用一组具有相应权值的随机样本(粒子)来表示状态的后验分布。该方法的基本思路是选取一个重要性概率密度并从中进行随机抽样,得到一些带有相应权值的随机样本后,在状态观测的基础上调节权值的大小。和粒子的位置,再使用这些样本来逼近状态后验分布,最后将这组样本的加权求和作为状态的估计值。粒子滤波不受系统模型的线性和高斯假设约束,采用样本形式而不是函数形式对状态概率密度进行描述,使其不需要对状态变量的概率分布进行过多的约束,因而在非线性非高斯动态系统中广泛应用。尽管如此,粒子滤波目前仍存在计算量过大、粒子退化等关键问题亟待突破。

通常情况下选择先验分布作为重要性密度函数、即
q (xk ∣x k − 1( i ) ,zk ) = p (xk ∣x k − 1( i ) ) q(x_k |x_{k-1}^{(i)}, z_{k})=p(x_k |x_{k-1}^{(i)}) q(xkxk1(i),zk)=p(xkxk1(i))
对该函数取重要性权值为
wk ( i ) =w k − 1( i ) p (zk ∣xk ( i ) ) w_k^{(i)}=w_{k-1}^{(i)}p(z_k |x_{k}^{(i)}) wk(i)=wk1(i)p(zkxk(i))
同样 w k(i) w_k^{(i)} wk(i)需要归一化得到w~ k(i) \tilde{w}_k^{(i)} w~k(i)

标准的粒子滤波算法步骤为:

粒子滤波PF:
Step 1: 根据p (x0 ) p(x_{0}) p(x0)采样得到N N N个粒子 x0 ( i ) ∼ p (x0 ) x_0^{(i)} \sim p(x_{0}) x0(i)p(x0)
For i = 2 : N i=2:N i=2:N
  Step 2: 根据状态转移函数产生新的粒子为:$ x k ( i )∼p( x k ∣ x k − 1 ( i )) x_k^{(i)} \sim p(x_{k} |x_{k-1}^{(i)})xk(i)p(xkxk1(i))
  Step 3: 计算重要性权值: w k ( i )= w k − 1 ( i )p( z k ∣ x k ( i )) w_k^{(i)}=w_{k-1}^{(i)}p(z_k |x_{k}^{(i)})wk(i)=wk1(i)p(zkxk(i))
  Step 4: 归一化重要性权值:w ~k ( i )= w k ( i ) ∑ j = 1 N w k ( j ) \tilde{w}_k^{(i)}=\frac{w_k^{(i)}}{\sum_{j=1}^Nw_k^{(j)}} w~k(i)=j=1Nwk(j)wk(i)
  Step 5: 使用重采样方法对粒子进行重采样(以系统重采样为例)
  Step 6: 得到k k k时刻的后验状态估计:
E[ x ^k ]= ∑ i = 1Nx k ( i ) w ~k ( i )E[\hat{x}_{k}]= \sum_{i=1}^Nx_{k}^{(i)}\tilde{w}_k^{(i)}E[x^k]=i=1Nxk(i)w~k(i)
End For

粒子滤波PF算法结构图
粒子滤波PF—从贝叶斯滤波到粒子滤波PF—Part-V(粒子滤波PF)

6、粒子滤波PF的在目标跟踪应用:

6.1、 仿真参数

**一、目标模型:CT(细节见另一个博客) **

X k + 1 =[ 1 sin ⁡ ( ω T ) ω 0 − 1−cos⁡(ωT) ω 0 cos ⁡ ( ω T ) 0 − sin ⁡ ( ω T ) 0 1 − cos ⁡ ( ω T ) ω 1 sin ⁡ ( ω T ) ω 0 sin ⁡ ( ω T ) 0 cos ⁡ ( ω T ) ] Xk +[ T 2 / 2 0 T 0 0 T 2 / 2 0 T ] Wk X_{k+1}=\begin{bmatrix}1&\frac{\sin(\omega T)}{\omega}&0&-\frac{1-\cos(\omega T)}{\omega}\\0&\cos(\omega T)&0&-\sin(\omega T)\\0&\frac{1-\cos(\omega T)}{\omega}&1&\frac{\sin(\omega T)}{\omega}\\0&\sin(\omega T)&0&\cos(\omega T)\end{bmatrix}X_{k} + \begin{bmatrix}T^2/2&0\\T&0\\0&T^2/2\\0&T\end{bmatrix}W_k Xk+1=1000ωsin(ωT)cos(ωT)ω1cos(ωT)sin(ωT)0010ω1cos(ωT)sin(ωT)ωsin(ωT)cos(ωT)Xk+T2/2T0000T2/2TWk

CV CT 模型的具体方程形式见另一个博客

二、测量模型:2D主动雷达
在二维情况下,雷达量测为距离和角度
rkm =rk + r ~ k bkm =bk + b ~ k {r}_k^m=r_k+\tilde{r}_k\\ b^m_k=b_k+\tilde{b}_k rkm=rk+r~kbkm=bk+b~k
其中
rk = ( x k− x 0 ) +( y k− y 0 ) 2) bk = tan ⁡− 1y k− y 0 x k− x 0 r_k=\sqrt{(x_k-x_0)^+(y_k-y_0)^2)}\\ b_k=\tan^{-1}{\frac{y_k-y_0}{x_k-x_0}}\\ rk=(xkx0)+(yky0)2) bk=tan1xkx0yky0
[ x 0 , y 0 ] [x_0,y_0] [x0,y0]为雷达坐标,一般情况为0。雷达量测为 z k = [ r k , b k ] ′ z_k=[r_k,b_k]' zk=[rk,bk]。雷达量测方差为
Rk = cov (vk ) =[ σ r 2 0 0 σ b 2 ] R_k=\text{cov}(v_k)=\begin{bmatrix}\sigma_r^2 & 0 \\0 & \sigma_b^2 \end{bmatrix} Rk=cov(vk)=[σr200σb2]

6.2、 跟踪轨迹和误差

粒子滤波PF—从贝叶斯滤波到粒子滤波PF—Part-V(粒子滤波PF)
粒子滤波PF—从贝叶斯滤波到粒子滤波PF—Part-V(粒子滤波PF)

7、粒子滤波PF的标准验证模型

7.1、 模型参数

状态模型:
x ( k ) = f ( x ( k − 1 ) , k ) + w ( k − 1 ) x(k) = f (x(k-1), k) + w(k-1) x(k)=f(x(k1),k)+w(k1)
其中
f ( x ( ( k − 1 ) , k ) = 0.5 x ( k − 1 ) + 2.5 x ( k − 1 ) / ( 1 + x ( k − 1)2 ) + 8 cos ⁡ ( 1.2 k ) f(x((k-1), k) = 0.5x(k-1) + 2.5x(k-1) / (1+x(k-1)^2) + 8\cos(1.2k) f(x((k1),k)=0.5x(k1)+2.5x(k1)/(1+x(k1)2)+8cos(1.2k)
测量方程为:
z ( k ) = x ( k)2 / 20 + v ( k ) z(k) = x(k)^2 / 20 +v(k) z(k)=x(k)2/20+v(k)

w ( k ) w(k) w(k), v ( k ) v(k) v(k)为均值为 0 0 0、方差分别为 Q ( k ) = 10 Q(k)=10 Q(k)=10, R ( k ) = 1 R(k)=1 R(k)=1的高斯噪声。状态 x ( k ) x(k) x(k) x ( k − 1 ) x(k-1) x(k1)为非线性关系,观测方程中 z ( k ) z(k) z(k)和x ( k ) (k) (k)也是非线性关系。

7.2、 基于随机重采样粒子滤波PF

粒子滤波PF—从贝叶斯滤波到粒子滤波PF—Part-V(粒子滤波PF)

粒子滤波PF—从贝叶斯滤波到粒子滤波PF—Part-V(粒子滤波PF)

7.3、 基于多项式重采样粒子滤波PF

粒子滤波PF—从贝叶斯滤波到粒子滤波PF—Part-V(粒子滤波PF)

7.4、 基于残差重采样粒子滤波PF

粒子滤波PF—从贝叶斯滤波到粒子滤波PF—Part-V(粒子滤波PF)

7.5、 基于系统重采样粒子滤波PF

粒子滤波PF—从贝叶斯滤波到粒子滤波PF—Part-V(粒子滤波PF)

7.6、 基于系统重采样粒子滤波PF代码

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 粒子滤波一维系统仿真%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%function ParticleFilter_standardmodelclear all;close all;clc;randn('seed',1); %为了保证每次运行结果一致,给定随机数的种子点%初始化相关参数T=50;%采样点数dt=1;%采样周期Q=10;%过程噪声方差R=1;%测量噪声方差v=sqrt(R)*randn(T,1);%测量噪声w=sqrt(Q)*randn(T,1);%过程噪声numSamples=100;%粒子数%%%%%%%%%%%%%%%%%%%%%%%%%%%x0=0.1;%初始状态%产生真实状态和观测值X=zeros(T,1);%真实状态Z=zeros(T,1);%量测X(1,1)=x0;%真实状态初始化Z(1,1)=(X(1,1)^2)./20+v(1,1);%观测值初始化for k=2:T%状态方程X(k,1)=0.5*X(k-1,1)+2.5*X(k-1,1)/(1+X(k-1,1)^2)+8*cos(1.2*k)+w(k-1,1);%观测方程Z(k,1)=(X(k,1).^2)./20+v(k,1);end%%%%%%%%%%%%%%%%%%%%%%%%%%%%粒子滤波器初始化,需要设置用于存放滤波估计状态,粒子集合,权重等数组Xpf=zeros(numSamples,T);%粒子滤波估计状态Xparticles=zeros(numSamples,T);%粒子集合Zpre_pf=zeros(numSamples,T);%粒子滤波观测预测值weight=zeros(numSamples,T);%权重初始化%给定状态和观测预测的初始采样:Xpf(:,1)=x0+sqrt(Q)*randn(numSamples,1);Zpre_pf(:,1)=Xpf(:,1).^2/20;%更新与预测过程for k=2:T%第一步:粒子集合采样过程for i=1:numSamplesQQ=Q;%跟卡尔曼滤波不同,这里的Q不要求与过程噪声方差一致net=sqrt(QQ)*randn;%这里的QQ可以看成是网的半径,数值可调Xparticles(i,k)=0.5.*Xpf(i,k-1)+2.5.*Xpf(i,k-1)./(1+Xpf(i,k-1).^2)+8*cos(1.2*k)+net;end%第二步:对粒子集合中的每个粒子,计算其重要性权值for i=1:numSamplesZpre_pf(i,k)=Xparticles(i,k)^2/20;weight(i,k)=exp(-.5*R^(-1)*(Z(k,1)-Zpre_pf(i,k))^2);%省略了常数项endweight(:,k)=weight(:,k)./sum(weight(:,k));%归一化权值%第三步:根据权值大小对粒子集合重采样,权值集合和粒子集合是一一对应的%选择采样策略outIndex = systematicR(weight(:,k)');%第四步:根据重采样得到的索引,去挑选对应的粒子,重构的集合便是滤波后的状态集合%对这个状态集合求均值,就是最终的目标状态、Xpf(:,k)=Xparticles(outIndex,k);end%计算后验均值估计、最大后验估计及估计方差Xmean_pf=mean(Xpf);%后验均值估计,及上面的第四步,也即粒子滤波估计的最终状态bins=20;Xmap_pf=zeros(T,1);for k=1:T[p,pos]=hist(Xpf(:,k,1),bins);map=find(p==max(p));Xmap_pf(k,1)=pos(map(1));%最大后验估计endfor k=1:TXstd_pf(1,k)=std(Xpf(:,k)-X(k,1));%后验误差标准差估计end%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%画图figure();clf;%过程噪声和测量噪声图subplot(221);plot(v);%测量噪声xlabel('时间');ylabel('测量噪声');subplot(222);plot(w);%过程噪声xlabel('时间');ylabel('过程噪声');subplot(223);plot(X);%真实状态xlabel('时间');ylabel('状态X');subplot(224);plot(Z);%观测值xlabel('时间');ylabel('观测Z');%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%figure();k=1:dt:T;plot(k,X,'.-k',k,Xmean_pf,'--ro',k,Xmap_pf,'-.g+','LineWidth',1);%注:Xmean_pf就是粒子滤波结果legend('系统真实状态值','后验均值估计','最大后验概率估计');xlabel('时间');ylabel('状态估计');%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%figure();subplot(121);plot(Xmean_pf,X,'+');%粒子滤波估计值与真实状态值如成1:1关系,则会对称分布xlabel('后验均值估计');ylabel('真值');hold on;c=-25:1:25;plot(c,c,'r');%画红色的对称线y=xhold off;subplot(122);%最大后验估计值与真实状态值如成1:1关系,则会对称分布plot(Xmap_pf,X,'+');xlabel('Map估计');ylabel('真值');hold on;c=-25:25;plot(c,c,'r');%画红色的对称线y=xhold off;%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%画直方图,此图形是为了看粒子集的后验密度domain=zeros(numSamples,1);range=zeros(numSamples,1);bins=10;support=[-20:1:20];figure();hold on;%直方图xlabel('时间');ylabel('样本空间');vect=[0 1];caxis(vect);for k=1:T%直方图反映滤波后的粒子集合的分布情况[range,domain]=hist(Xpf(:,k),support);%调用waterfall函数,将直方图分布的数据画出来waterfall(domain,k,range);endaxis([-20 20 0 T 0 100]);%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%figure();xlabel('样本空间');ylabel('后验密度');k=30;%k=?表示要查看第几个时刻的粒子分布与真实状态值的重叠关系[range,domain]=hist(Xpf(:,k),support);plot(domain,range);%真实状态在样本空间中的位置,画一条红色直线表示XXX=[X(k,1),X(k,1)];YYY=[0,max(range)+10];line(XXX,YYY,'Color','r');axis([min(domain) max(domain) 0 max(range)+10]);figure();k=1:dt:T;plot(k,Xstd_pf,'-ro','LineWidth',1);xlabel('时间');ylabel('状态估计误差标准差');axis([0,T,0,10]);%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%函数功能:实现随机重采样算法%输入参数:weight为原始数据对应的权重大小%输出参数:outIndex是根据weight对inIndex筛选和复制结果function outIndex=randomR(weight)%获得数据的长度L=length(weight);%初始化输出索引向量,长度与输入索引向量相等outIndex=zeros(1,L);%第一步:产生[0,1]上均匀分布的随机数组,并升序排序u=unifrnd(0,1,1,L);u=sort(u);%u=(1:L)/L%这个是完全均匀%第二步:计算粒子权重积累函数cdfcdf=cumsum(weight);%第三步:核心计算i=1;for j=1:L%此处的基本原理是:u是均匀的,必然是权值大的地方%有更多的随机数落入该区间,因此会被多次复制while(i<=L)&(u(i)<=cdf(j))%复制权值大的粒子outIndex(i)=j;%继续考察下一个随机数,看它落在哪个区间i=i+1;endend%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 多项式重采样子函数% 输入参数:weight为原始数据对应的权重大小% 输出参数:outIndex是根据weight筛选和复制结果function outIndex = multinomialR(weight);%获取数据长度Col=length(weight)N_babies= zeros(1,Col);%计算粒子权重累计函数cdf cdf= cumsum(weight); %产生[0,1]均匀分布的随机数u=rand(1,Col)%求u^(j^-1)次方 uu=u.^(1./(Col:-1:1)) %如果A是一个向量,cumprod(A)将返回一个包含A各元素积累连乘的结果的向量 %元素个数与原向量相同ArrayTemp=cumprod(uu) %fliplr(X)使矩阵X沿垂直轴左右翻转u = fliplr(ArrayTemp);j=1;for i=1:Col    %此处跟随机采样相似    while (u(i)>cdf(j)) j=j+1;    end    N_babies(j)=N_babies(j)+1;end;index=1;for i=1:Col    if (N_babies(i)>0) for j=index:index+N_babies(i)-1     outIndex(j) = i; end;    end;    index= index+N_babies(i);end%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 函数功能说明:残差重采样函数% 输入参数:一组权重weight向量% 输出参数:为该权重重采样后的索引outIndexfunction outIndex = residualR(weight)N= length(weight);N_babies= zeros(1,N);q_res = N.*weight;N_babies = fix(q_res);N_res=N-sum(N_babies);if (N_res~=0)    q_res=(q_res-N_babies)/N_res;    cumDist= cumsum(q_res);    u = fliplr(cumprod(rand(1,N_res).^(1./(N_res:-1:1))));    j=1;    for i=1:N_res while (u(1,i)>cumDist(1,j))     j=j+1; end N_babies(1,j)=N_babies(1,j)+1;    end;end;index=1;for i=1:N    if (N_babies(1,i)>0) for j=index:index+N_babies(1,i)-1     outIndex(j) = i; end;    end;    index= index+N_babies(1,i);end%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 系统重采样子函数% 输入参数:weight为原始数据对应的权重大小% 输出参数:outIndex是根据weight筛选和复制结果function outIndex = systematicR(weight);N=length(weight);N_children=zeros(1,N);label=zeros(1,N);label=1:1:N;s=1/N;auxw=0;auxl=0;li=0;T=s*rand(1);j=1;Q=0;i=0;u=rand(1,N);while (T<1)    if (Q>T) T=T+s; N_children(1,li)=N_children(1,li)+1;    else i=fix((N-j+1)*u(1,j))+j; auxw=weight(1,i); li=label(1,i); Q=Q+auxw; weight(1,i)=weight(1,j); label(1,i)=label(1,j); j=j+1;    endendindex=1;for i=1:N    if (N_children(1,i)>0) for j=index:index+N_children(1,i)-1     outIndex(j) = i; end;    end;    index= index+N_children(1,i);end%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

PF的妈妈贝叶斯滤波Part-I、蒙特卡洛方法Part-II、序贯重采样Part-III

原创不易,路过的各位大佬请点个赞

51银饰网