背景：最近有人要我帮忙写个多维运动模型的粒子滤波的目标跟踪的demo，于是开始了接下来的资料分析过程。

参考资料

https://www.cnblogs.com/sanmenyi/p/7091978.html(比较理论的分析了相关知识点)

贝叶斯滤波以递推的形式给出后验(或滤波)概率密度函数的最优解。目标状态的最优估计值可由后验(或滤波)概率密度函数进行计算。通常根据极大后验(MAP)准则或最小均方误差(MMSE)准则，将具有极大后验概率密度的状态或条件均值作为系统状态的估计值。

贝叶斯滤波需要进行积分运算，除了一些特殊的系统模型（如线性高斯系统，有限状态的离散系统）之外，对于一般的非线性、非高斯系统，贝叶斯滤波很难得到后验概率的封闭解析式。因此，现有的非线性滤波器多采用近似的计算方法解决积分问题，以此来获取估计的次优解。在系统的非线性模型可由在当前状态展开的线性模型有限近似的前提下，基于一阶或二阶Taylor级数展开的扩展Kalman滤波得到广泛应用。在一般情况下，逼近概率密度函数比逼近非线性函数容易实现。据此，Julier与Uhlmann提出一种Unscented Kalman滤波器，通过选定的sigma点来精确估计随机变量经非线性变换后的均值和方差，从而更好的近似状态的概率密度函数，其理论估计精度优于扩展Kalman滤波。获取次优解的另外一中方案便是基于蒙特卡洛模拟的粒子滤波器。

标准化粒子滤波算法流程：

1.2.1. 贝叶斯重要性采样
1.2.2 序贯重要性采样算法
1.2.3. 重要密度函数的选择
1.2.4. 重采样方法
- 常用的重采样方法包括多项式(Multinomial resampling)重采样、残差重采样(Residual resampling)、分层重采样(Stratified resampling)与系统重采样(Systematic resampling)等。

残差重采样：

https://blog.csdn.net/zhangquan2015/article/details/79166537 （一维状态PF）
代码挺详细，但有个小bug
https://blog.csdn.net/djfjkj52/article/details/104562430 （自己以前写的PF的理解）
https://blog.csdn.net/djfjkj52/article/details/104881332
https://starsmydestination.github.io/2017/05/09/UKF/ （UKF的理解）

较好的示范代码：

https://github.com/EdwardCooler/3DTrackPrediction

改进粒子滤波的无人机三维航迹预测方法
分别对x、y、z三个方向的位置、速度、加速度进行预测（9维）
通过距离、俯仰角、横向角进行观测（3维）
改进了传统的粒子算法，并与传统的非线性滤波EKF、UKF、PF算法进行对比

还有一份雷达跟踪的，我还没看代码

https://github.com/wangshengzhe/PF-TBD

我思考了很久的权重计算

 %第二步：对粒子集合中的每个粒子，计算其重要性权值for i=1:numSamplesZpre_pf(i,:,k)=Xparticles(i,:,k) + sigma2*randn(1,6);%对每个粒子计算其权重，这里假设量测噪声是高斯分布。所以 w = p(y|x)对应下面的计算公式    %  w(i) = inv(sqrtm(R)) * exp(-0.5*inv(R)*((Z-Zpre(:,i))^(2))) + 1e-99; z1 = (Z(1,:,k) - Zpre_pf(i,:,k))';    weight(i,k) = inv(sqrt(2*pi*det(R_PF)))*exp(-.5*(z1)'*inv(R_PF)*(z1))+ 1e-99;%权值计算，这里其实可以把inv(sqrt(2*pi*det(R)))去掉end

多维运动模型的粒子滤波代码

position_x=0;
position_y=0;
vx = 4/3.6;%x运动速度
vy = 0.1;%y运动速度
ax = 0.15;
ay = 0.05;
x0= [position_x position_y vx vy ax ay];

可选择不同的采样方式：
=1为随机采样，=2为系统采样,=3 多项式重采样子函数,=4 残差重采样函数

以前很多代码就是一维运动模型的，也是因为有人找我做PF滤波简单demo，我遂做了多维运动模型的工作。

代码如下：

function Particle_For_UnlineOneDiv
clear all;clc;close all;
Ptx = -10;%发射功率
gama = 4;%衰减稀疏
dt = 1;%时间间隔
sigma = 8;%阴影衰落因子
N0 = -95;%噪声稀疏
R = 200;%小区半径
TLen = 5;%时长
NN = [5 10 20];%ES数目
Dsnr = 8;%解调门限sigma1 = 0.2;%目标运动扰动
sigma2 = 0.2;%目标观测扰动
Qn = eye(6)*sigma1;%运动方程噪声协方差矩阵
Rn = eye(6)*sigma2;%观测方程噪声协方差矩阵iters = 1e2;%迭代次数
%% PF_config
randn('seed',1); %为了保证每次运行结果一致，给定随机数的种子点
%初始化相关参数
T=50;%采样点数
dt=dt;%采样周期
Q_PF=Qn;%过程噪声方差
R_PF=Rn;%测量噪声方差for i = 1:T
v(:,:,i)=sqrt(Q_PF)*randn(6,1);%过程噪声
w(:,:,i)=sqrt(R_PF)*randn(6,1);%测量噪声
endnumSamples=100;%粒子数
ResampleStrategy=4;%=1为随机采样，=2为系统采样,=3 多项式重采样子函数,=4 残差重采样函数%粒子滤波器初始化，需要设置用于存放滤波估计状态，粒子集合，权重等数组
Xpf=zeros(numSamples,6,T);%粒子滤波估计状态
Xparticles=zeros(numSamples,6,T);%粒子集合
Zpre_pf=zeros(numSamples,6,T);%粒子滤波观测预测值
weight=zeros(numSamples,T);%权重初始化%给定状态和观测预测的初始采样：
position_x=0;
position_y=0;
vx = 4/3.6;%x运动速度
vy = 0.1;%y运动速度
ax = 0.15;
ay = 0.05;
x0= [position_x position_y vx vy ax ay];X=zeros(1,6,T);%真实状态
Z=zeros(1,6,T);%量测
Z(:,:,1)= x0;for k = 2:TA= [1,0,dt,0,0.5*dt^2,0;0,1,0,dt,0,0.5*dt^2;0,0,1,0,dt,0;0,0,0,1,0,dt;0,0,0,0,1,0;0,0,0,0,0,1];%观测方程temp = zeros(1,6);temp(:,:) = Z(1,:,k-1);Z(:,:,k)=((A * temp')+v(:,:,k))';
endfor i = 1:numSamplesXpf(i,:,1)=x0+randn(1,6)*sqrt(Q_PF);Zpre_pf(i,:,1)=x0+randn(1,6)*sqrt(R_PF);
end%更新与预测过程
for k=2:T%第一步：粒子集合采样过程for i=1:numSamplesQQ=Q_PF;%跟卡尔曼滤波不同，这里的Q不要求与过程噪声方差一致net=randn(1,6)*sqrt(QQ);%这里的QQ可以看成是网的半径，数值可调,1x6A= [1,0,dt,0,0.5*dt^2,0;0,1,0,dt,0,0.5*dt^2;0,0,1,0,dt,0;0,0,0,1,0,dt;0,0,0,0,1,0;0,0,0,0,0,1];temp = A * Xpf(i,:,k-1)';Xparticles(i,:,k)=temp'+net;end%第二步：对粒子集合中的每个粒子，计算其重要性权值for i=1:numSamplesZpre_pf(i,:,k)=Xparticles(i,:,k) + sigma2*randn(1,6);%对每个粒子计算其权重，这里假设量测噪声是高斯分布。所以 w = p(y|x)对应下面的计算公式    %  w(i) = inv(sqrtm(R)) * exp(-0.5*inv(R)*((Z-Zpre(:,i))^(2))) + 1e-99; z1 = (Z(1,:,k) - Zpre_pf(i,:,k))';    weight(i,k) = inv(sqrt(2*pi*det(R_PF)))*exp(-.5*(z1)'*inv(R_PF)*(z1))+ 1e-99;%权值计算，这里其实可以把inv(sqrt(2*pi*det(R)))去掉endweight(:,k)=weight(:,k)./sum(weight(:,k));%归一化权值%第三步：根据权值大小对粒子集合重采样，权值集合和粒子集合是一一对应的%选择采样策略if ResampleStrategy==1outIndex = randomR(weight(:,k));elseif ResampleStrategy==2outIndex = systematicR(weight(:,k)');elseif ResampleStrategy==3outIndex = multinomialR(weight(:,k));elseif ResampleStrategy==4outIndex = residualR(weight(:,k)');end%第四步：根据重采样得到的索引，去挑选对应的粒子，重构的集合便是滤波后的状态集合%对这个状态集合求均值，就是最终的目标状态、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));%最大后验估计
end
for k=1:TXstd_pf(1,k)=std(Xpf(:,1,k)-Z(:,1,k));%后验误差标准差估计
end%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 画图，三维轨迹图
NodePostion = [100,800,100;200,800,900;0,0,0];
figure
t=1:T;
hold on;
box on;
grid on;% plot3(squeeze(Z(1,1,:)),squeeze(Z(1,2,:)),squeeze(Z(1,2,:)))p1 = plot3(squeeze(Z(1,1,:)),squeeze(Z(1,2,:)),1:50,'-k.','lineWidth',1);p5 = plot3(squeeze(Xpf(1,1,:)),squeeze(Xpf(1,2,:)),1:50,'-g*','lineWidth',1);xlabel('x轴位置');
ylabel('y轴位置');
zlabel('z轴位置');
view(3);figure
hold on;
box on;
p1=plot(1:T,Xstd_pf,'-k.','lineWidth',2);
xlabel('time step');
ylabel('RMS预测偏差');%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%函数功能：实现随机重采样算法
%输入参数：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%这个是完全均匀
%第二步：计算粒子权重积累函数cdf
cdf=cumsum(weight);
%第三步：核心计算
i=1;
for j=1:L%此处的基本原理是：u是均匀的，必然是权值大的地方%有更多的随机数落入该区间，因此会被多次复制while(i<=L)&(u(i)<=cdf(j))%复制权值大的粒子outIndex(i)=j;%继续考察下一个随机数，看它落在哪个区间i=i+1;end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 多项式重采样子函数
% 输入参数：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;endN_babies(j)=N_babies(j)+1;
end;
index=1;
for i=1:Colif (N_babies(i)>0)for j=index:index+N_babies(i)-1outIndex(j) = i;end;end;index= index+N_babies(i);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 函数功能说明：残差重采样函数
% 输入参数：一组权重weight向量
% 输出参数：为该权重重采样后的索引outIndex
function 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_reswhile (u(1,i)>cumDist(1,j))j=j+1;endN_babies(1,j)=N_babies(1,j)+1;end;
end;
index=1;
for i=1:Nif (N_babies(1,i)>0)for j=index:index+N_babies(1,i)-1outIndex(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;elsei=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;end
end
index=1;
for i=1:Nif (N_children(1,i)>0)for j=index:index+N_children(1,i)-1outIndex(j) = i;end;end;index= index+N_children(1,i);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

【11月29】PF 粒子滤波的多维运动模型代码相关推荐

11月29日云栖精选夜读：阿里传奇工程师多隆的程序世界
摘要: 写代码写到入定,是一种什么样的体验?11月29日(本周三),<阿里技术人纪录片系列>将带大家走进大神多隆的代码世界.在此之前,我们先来重温一篇关于多隆的旧文,来自<淘宝技术这 ...
赛尔号星球大战服务器维修,赛尔号星球大战11月29日更新公告
赛尔号星球大战11月29日新增了什么玩法?服务器的维护时间是多少?来看看9k9k小编rayxx带来的赛尔号星球大战11月29日更新公告. 11月29号下午15点至17点更新公告: 1.精灵新增了两种 ...
质量与效能｜ 11月29日TF84
质量和效能的平衡一直是软件研发过程中永恒的话题,从短期看质量和效能似乎是矛盾的,但是从长期看质量和效能却能做到有机地统一,面对质量和效能的平衡,我们需要的不仅是战略层面的思考,还需要微观层面的工程实践 ...
逆水寒服务器维护公告,《逆水寒》2018年11月29日更新公告
各位自在同门: 为了保证服务器的运行稳定和服务质量,<逆水寒>将于2018年11月29日早8:00停机进行维护工作,预计维护到上午10:00.如在维护期间无法完成维护内容,开机时间将顺延. ...
1849 年 11 月 29 日：真空管的发明者 John Fleming 诞生
约翰·弗莱明爵士(John Ambrose Fleming)出生于 1864 年 11 月 29 日,他是英国电气工程师和物理学家,以发明真空管(二极管).物理电磁学中使用的右手法则而闻名.弗莱明生于 ...
Tuscany SCA V1.0中的扩展机制和启动过程中的扩展点[11月29日更新]
2007年9月24日Tuscany SCA 发布了V1.0版本的实现 .本文讲述的内容使用的就是基于这个版本的,代码下载地址 http://incubator.apache.org/tuscany/s ...
手把手教你申请EVUS美国十年签证！11月29日以后要收费了！
输入旅行文件信息: 输入登记者信息登记美国联系人信息 (如果在美国没有联系人,可以写unknown) 资格问题仔细核实所登记的信息内容.核实无误后点击"确认并继续"到下一步. ...
方舟服务器维护公告11月19日,《方舟指令》11月29日维护公告
致诸位御灵士: 奥利吉奈尔区誓灵协会将于11月29日(周四)9:00~13:00对以下服务器进行4小时的升级维护. 维护期间将无法进入游戏,还请各位御灵士做好下线准备, 感谢各位御灵士的理解和支持. ...
逆水寒服务器维护,逆水寒11月29日更新到几点进游戏逆水寒更新维护公告
逆水寒在11月29日进行了一次新版本的更新,新团本风雪铁牢关要上线了,很多奖励接踵而来,一些小伙伴还不知道更新了什么,下面就来为大家分享一下逆水寒的更新维护公告. 各位自在同门: 为了保证服务器的运行 ...
第20届哥谭独立电影奖提名揭晓 11月29日颁奖
第20届哥谭独立电影奖提名揭晓 11月29日颁奖 2010-10-24 21:22:15 作者:yxlady 来源:伊秀女性网转播到微博北京时间10月24日消息,据国外媒体报道,第20届哥谭 ...

【11月29】PF 粒子滤波的多维运动模型代码