粒子滤波与卡曼滤波实现教程及MATLAB代码

http://www.ilovematlab.cn/thread-133464-1-1.html




如果有研究粒子滤波，无迹粒子滤波（UPF）等用于目标跟踪或者其他非线性系统中的滤波算法，本人可以提供帮助和指导。如果合适也可以帮助编写matlab程序。欢迎交流！ 我的联系QQ3-4-5-1-9-4-1-1-2.
以下是一个粒子滤波的入门学习程序，应用于纯方位目标角度观测跟踪，希望对大家有用。
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 功能：观测目标角度，达到对目标三位跟踪
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear;
echo off;
%-----------------Parameter--------------------------
xMEAN=4.5;xSIGMA=0.2;              %初始化采样粒子的位置和速度                
yMEAN=4;ySIGMA=0.3;
VxMEAN=0.2;VxSIGMA=0.01;             
VyMEAN=0.15;VySIGMA=0.02;
UxMEAN=0;UxSIGMA=0.015625;          % 初始化x方向和y方向噪声，均为零均值高斯白噪声
UyMEAN=0;UySIGMA=0.015625;          % 标准差 为 0.015625 
UzMEAN=0;UzSIGMA=0.001;             
rSTEP=30;                         % 采样30个时刻
wk=-1/(2*UzSIGMA);                % 粒子权重常数
wj=1/sqrt(2*pi*UzSIGMA);          % 粒子权重常数1/sqr(2*pi)
rN=1024;                          % 粒子数为1024
w_buffer=zeros(rN,1);             % 存储权值的空间
r_buffer=zeros(rN,1);             % 存储复制次数的空间
i_buffer=zeros(rN,1);             % 存储粒子指针的空间
tX=zeros(rSTEP,1);tY=zeros(rSTEP,1);tVx=zeros(rSTEP,1);tVy=zeros(rSTEP,1);tZ=zeros(rSTEP,1);  %每一时刻状态的估计值
rX=zeros(rSTEP,1);rY=zeros(rSTEP,1);rVx=zeros(rSTEP,1);rVy=zeros(rSTEP,1);                    %每一时刻状态的真实值
x0=4.5;y0=4.5;Vx0=0.2;Vy0=0.2;           % 初始化真实状态的初值