粒子滤波的公式和matlab
matlab 粒子滤波重采样
MATLAB粒子滤波重采样1. 简介粒子滤波（Particle Filter）是一种基于蒙特卡洛方法的滤波算法，用于在非线性和非高斯系统中进行状态估计。
粒子滤波通过使用一组粒子来近似系统的后验概率分布，从而实现对系统状态的估计。
重采样是粒子滤波算法中的一个重要步骤，用于根据粒子的权重对粒子进行重新采样，以提高估计的准确性。
在本文中，我们将使用MATLAB编写粒子滤波算法，并实现重采样步骤。
2. 粒子滤波算法步骤粒子滤波算法通常包括以下步骤：1.初始化粒子集合：根据先验分布或已知信息，生成一组随机粒子，表示系统的可能状态。
2.预测步骤：根据系统的动力学模型，对粒子进行状态预测。
3.更新步骤：使用测量模型和观测值对粒子进行权重更新。
4.规范化权重：对粒子的权重进行规范化，使其总和等于1。
5.重采样步骤：根据粒子的权重，对粒子进行重新采样，以提高估计的准确性。
6.重复步骤2-5，直到达到停止条件。
在本文中，我们将重点关注重采样步骤的实现。
3. 粒子滤波重采样算法重采样步骤的目标是根据粒子的权重，从当前粒子集合中生成新的粒子集合，以便更好地表示后验概率分布。
常用的重采样方法包括多项式重采样和系统性重采样。
下面是系统性重采样算法的伪代码：1. 初始化：给定粒子集合P和对应的权重W。
2. 计算累积权重：计算累积权重C，其中C(i) = sum(W(1:i))，i为粒子的索引。
3. 生成随机数：生成一个均匀分布的随机数r，取值范围为[0, 1]。
4. 重采样：对于每个粒子i，找到满足C(j) > r且C(j-1) <= r的最小索引j，将粒子j 添加到新的粒子集合中。
5. 返回新的粒子集合。
下面是MATLAB代码实现粒子滤波重采样的函数：numParticles = size(particles, 2);newParticles = zeros(size(particles));cumulativeWeights = cumsum(weights);r = rand(1) / numParticles;index = 1;for i = 1:numParticlesu = r + (i - 1) / numParticles;while cumulativeWeights(index) < uindex = index + 1;endnewParticles(:, i) = particles(:, index);endend4. 示例应用为了演示粒子滤波重采样算法的应用，我们将使用一个简单的二维机器人定位问题。
粒子滤波（PF）MATLAB实现程序
粒子滤波（PF）MATLAB实现程序x = 0; % 初始状态R = input('请输入过程噪声方差R的值: ');; % 测量噪声协方差Q = input('请输入观测噪声方差Q的值: '); % 过程状态协方差tf = 100; % 模拟长度N = 100; % 粒子滤波器中的粒子个数xhat = x;P = 2;xhatPart = x;%粒子滤波器初期for i = 1 : Nxpart(i) = x + sqrt(P) * randn;endxArr = [x];yArr = [x^2 / 20 + sqrt(R) * randn];xhatArr = [x];PArr = [P];xhatPartArr = [xhatPart];close all;for k = 1 : tf% 模拟系统x = 0.5 * x + 25 * x / (1 + x^2) + 8 * cos(1.2*(k-1)) + sqrt(Q) * randn;y = x^2 / 20 + sqrt(R) * randn;% 粒子滤波器for i = 1 : Nxpartminus(i) = 0.5 * xpart(i) + 25 * xpart(i) / (1 + xpart(i)^2) + 8 * cos(1.2*(k-1)) + sqrt(Q) * randn;ypart = xpartminus(i)^2 / 20;vhat = y - ypart;% 观测和预测的差q(i) = (1 / sqrt(R) / sqrt(2*pi)) * exp(-vhat^2 / 2 / R); end% 平均每一个估计的可能性qsum = sum(q);for i = 1 : Nq(i) = q(i) / qsum;%归一化权值end% 重采样for i = 1 : Nu = rand;qtempsum = 0;for j = 1 : Nqtempsum = qtempsum + q(j);if qtempsum >= uxpart(i) = xpartminus(j);break;endendendxhatPart = mean(xpart);% 在图片中表示出数据xArr = [xArr x];yArr = [yArr y];xhatArr = [xhatArr xhat];PArr = [PArr P];xhatPartArr = [xhatPartArr xhatPart];endt = 0 : tf;figure;plot(t, xArr, 'b.', t, xhatPartArr, 'k-');set(gca,'FontSize',10); set(gcf,'Color','yellow'); xlabel('time step'); ylabel('state');legend('真实值', 'PF估计值');xhatRMS = sqrt((norm(xArr - xhatArr))^2 / tf); xhatPartRMS = sqrt((norm(xArr - xhatPartArr))^2 / tf); disp(['PF估计误差均方值= ', num2str(xhatPartRMS)]);。
matlab 粒子滤波代码
以下是一个简单的 MATLAB 粒子滤波器的代码示例：```matlab% 初始化参数N = 100; % 粒子数量dt = 0.1; % 时间步长x = [0 0]; % 初始位置P = eye(2); % 初始协方差矩阵Q = eye(2); % 过程噪声协方差矩阵R = eye(2); % 观测噪声协方差矩阵G = [0.9 0.1; 0.1 0.9]; % 转换矩阵N_particles = size(Q,1); % 粒子数量particles = zeros(N_particles,2); % 初始化粒子particles(:,1) = x(1); % 设置粒子的 x 分量particles(:,2) = x(2); % 设置粒子的 y 分量weights = ones(N_particles,1) / N_particles; % 初始化权重% 模拟观测数据z = [1.2 0.5]; % 观测位置R_inv = inv(R); % 观测噪声协方差矩阵的逆H = [z(1) -z(2); z(2) z(1)]; % 观测矩阵y = H * x; % 预测的观测值% 粒子滤波步骤for t = 1:100% 重采样步骤weights = weights / sum(weights);index = randsample(1:N_particles, N, true, weights); particles = particles(index,:);% 预测步骤x_pred = particles;P_pred = Q;x_pred = G * x_pred;P_pred = P_pred + dt * G * P_pred;P_pred = P_pred + P_pred * G' + R;% 更新步骤y_pred = H * x_pred;S = H * P_pred * H' + R_inv;K = P_pred * H' * inv(S);x = x_pred + K * (z - y_pred);P = P_pred - P_pred * K * H';end```在这个代码示例中，我们使用了两个步骤：重采样步骤和预测/更新步骤。
粒子滤波算法原理及Matlab程序(专题)
从而针线相交的概率为：
p ˆP X2 lsin 0 0 2 lsin 2dx d2 lw
2、蒙特卡洛
• %%%%%%%%%%%%%%%%%%%%%%%%%
• % 说明：利用蒙特卡洛模拟计算圆周率
• %%%%%%%%%%%%%%%%%%%%%%%%%
• function buffon_test
% 目标个数
• dt=1;
% 采样时间间隔
• S.x=100*rand;
% 观测站水平位置
• S.y=100*rand;
% 观测站纵向位置
• F=[1,dt,0,0;0,1,0,0;0,0,1,dt;0,0,0,1]; % 采用CV模型的状态转移矩 阵
• G=[0.5*dt^2,0;dt,0;0,0.5*dt^2;0,dt]; % 过程噪声驱动矩阵
观测站位置
目标真实轨迹
PF算 法 轨 迹
20
0
20
40
60
80
100 120
X/m
RMSE,q=0.0001 2.5
RMS跟 踪 误 差
2
1.5
1
0.5
0
0
5
10
15
20
25
30
time/s
5、粒子滤波在多目标跟踪中的应用
• 多目标跟踪系统
• 状态方程 • 观测方程
250
200
150
100
50
0
-50
• 粒子滤波目前有四大基本 的重采样方法，分别是残 差重采样（Residual resampling），多项式重 采样（Multinomial resampling），系统重采 样（Systematic resampling），随机重采 样（random resampling），关于他们 的原理，读者可以到网上 检索相关的论文。
粒子滤波算法matlab实例
一、介绍粒子滤波算法粒子滤波算法是一种基于蒙特卡洛方法的非线性、非高斯滤波算法，它通过一组随机产生的粒子来近似表示系统的后验概率分布，从而实现对非线性、非高斯系统的状态估计。
在实际应用中，粒子滤波算法被广泛应用于目标跟踪、导航、机器人定位等领域。
本文将以matlab 实例的形式介绍粒子滤波算法的基本原理和应用。
二、粒子滤波算法的原理及步骤粒子滤波算法的主要原理是基于贝叶斯滤波理论，通过一组随机产生的粒子来近似表示系统的后验概率分布。
其具体步骤如下：1. 初始化：随机生成一组粒子，对于状态变量的初始值和方差的估计，通过随机抽样得到一组粒子。
2. 预测：根据系统模型，对每个粒子进行状态预测，得到预测状态。
3. 更新：根据测量信息，对每个预测状态进行权重更新，得到更新后的状态。
4. 重采样：根据更新后的权重，对粒子进行重采样，以满足后验概率分布的表示。
5. 输出：根据重采样后的粒子，得到对系统状态的估计。
三、粒子滤波算法的matlab实例下面以一个简单的目标跟踪问题为例，介绍粒子滤波算法在matlab中的实现。
假设存在一个目标在二维空间中运动，我们需要通过一系列测量得到目标的状态。
我们初始化一组粒子来近似表示目标的状态分布。
我们根据目标的运动模型，预测每个粒子的状态。
根据测量信息，对每个预测状态进行权重更新。
根据更新后的权重，对粒子进行重采样，并输出对目标状态的估计。
在matlab中，我们可以通过编写一段简单的代码来实现粒子滤波算法。
我们需要定义目标的运动模型和测量模型，然后初始化一组粒子。
我们通过循环来进行预测、更新、重采样的步骤，最终得到目标状态的估计。
四、总结粒子滤波算法是一种非线性、非高斯滤波算法，通过一组随机产生的粒子来近似表示系统的后验概率分布。
在实际应用中，粒子滤波算法被广泛应用于目标跟踪、导航、机器人定位等领域。
本文以matlab实例的形式介绍了粒子滤波算法的基本原理和应用，并通过一个简单的目标跟踪问题，展示了粒子滤波算法在matlab中的实现过程。
pf算法举例及其matlab实现-概述说明以及解释
pf算法举例及其matlab实现-概述说明以及解释1.引言1.1 概述PF算法（Particle Filter Algorithm），又称为粒子滤波算法，是一种基于蒙特卡洛方法的非线性滤波算法。
与传统的滤波算法相比，PF算法具有更大的灵活性和鲁棒性，在估计复杂非线性系统状态的过程中表现出良好的性能。
PF算法基于一种随机采样的思想，通过对系统状态进行一系列粒子的采样，再通过对这些粒子的权重进行重要性重采样，最终获得对状态估计的准确性更高的结果。
在PF算法中，粒子的数量决定了滤波算法的精度，粒子越多，估计结果越准确，但也会增加计算复杂度。
因此，在实际应用中需要根据实际情况灵活选择粒子数量。
作为一种高效的滤波算法，PF算法在众多领域都有广泛的应用。
例如，粒子滤波算法在目标跟踪、传感器网络定位、机器人定位与导航等领域都有着重要的作用。
其在目标跟踪领域的应用尤为突出，由于PF算法可以处理非线性和非高斯分布的情况，使得目标跟踪更加准确和稳定。
在Matlab中，PF算法也得到了广泛的应用和实现。
Matlab提供了丰富的函数和工具箱，可以便捷地实现PF算法。
借助Matlab的强大数据处理和可视化功能，我们可以更加便捷地进行粒子滤波算法的实现和结果分析。
本文将从PF算法的基本概念出发，介绍其应用举例和在Matlab中的具体实现。
通过对PF算法的研究和实践，我们可以更好地理解和应用这一强大的滤波算法，为实际问题的解决提供有效的手段。
通过对Matlab 的使用，我们还可以更加高效地实现和验证粒子滤波算法的性能，为进一步的研究和应用奠定基础。
在接下来的章节中，我们将详细介绍PF算法的原理及其在现实应用中的具体案例。
随后，我们将展示如何使用Matlab实现PF算法，并通过实验结果对其性能进行评估和分析。
最后，我们将总结PF算法和Matlab 实现的主要特点，并对未来的发展进行展望。
文章结构的设定在撰写一篇长文时非常重要，它能够为读者提供一个整体的概览，帮助他们更好地理解文章的内容安排。
在MATLAB中使用粒子滤波进行状态估计
在MATLAB中使用粒子滤波进行状态估计Introduction:在许多实时系统或者控制系统中，状态估计是至关重要的一环。
状态估计涉及通过测量数据来推断或估计系统的当前状态。
而粒子滤波(Particle filter)作为一种无模型非线性滤波器，被广泛应用于状态估计问题中。
在本文中，我们将重点介绍如何在MATLAB中使用粒子滤波进行状态估计。
Particle filter基本原理:粒子滤波基于贝叶斯滤波理论，并通过一系列随机样本表示系统的可能状态。
它的基本原理是通过一个粒子集合来近似表示系统状态的概率密度函数。
粒子滤波的核心思想是通过对每个状态进行加权采样来逼近概率密度函数。
粒子的数量越多，逼近的精度就越高，但同时计算量也会增加。
在粒子滤波算法中，每个粒子表示系统的一个假设状态，粒子的权重表示此假设状态的似然度。
而粒子的更新则通过重采样和预测两个步骤来实现。
重采样过程会根据粒子的权重来决定哪些粒子要留下来，而预测过程则通过系统动力学方程来生成新的粒子。
在状态估计问题中，粒子滤波可以通过将传感器测量数据与系统模型相结合，来估计系统的状态。
在MATLAB中使用粒子滤波:使用MATLAB进行粒子滤波非常方便，因为MATLAB提供了强大的工具箱和函数来支持粒子滤波算法，比如Statistics and Machine Learning Toolbox和Sensor Fusion and Tracking Toolbox。
在这里，我们将使用Statistics and Machine Learning Toolbox来进行演示。
步骤一: 初始化粒子集合首先，我们需要根据系统的先验信息，生成一组初始化的粒子。
我们可以根据先验概率密度函数来对粒子赋初值。
```MATLABnumParticles = 1000; % 粒子的数量particleSet = rand(numParticles, 2); % 初始化粒子集合```步骤二: 测量更新接下来，我们需要使用传感器测量数据来对粒子进行加权更新。
粒子滤波PF算法在无线传感器网络定位跟踪的matlab源码
% 二维直线运动模型:%X=FX+V 状态模型%Z=[z1;z2] 观测模型clc;clear all;%%N1=300; %粒子数time=60;x_state(1)=1;vx(1)=5;y_state(1)=1;vy(1)=7;%%Process Noise Covariance%%%%%%%% 都是标准差xstate_noise=10; %没有用的参数Vx_noise=1;%%Measurement Noise Covariance%%%% 都是标准差theta_noise=0.1; %3/180*pidistance1_noise=3;xobs = [];yobs = [];theta1(1)=0;%%Ture State%%%%%%%%for i=2:time%% State model%%%%%%%%%%%accx = normrnd(0,Vx_noise,1,1);x_state(i)=x_state(i-1)+vx(i-1)+0.5*accx;vx(i)=vx(i-1)+accx;accy = normrnd(0,Vx_noise,1,1);y_state(i)=y_state(i-1)+vy(i-1)+0.5*accy;vy(i)=vy(i-1)+accy;end%%Measurement Value%%%%%for i=1:time%%Measure model%%%%%%%%%distance1(i)=sqrt(x_state(i)^2+y_state(i)^2)+distance1_noise*randn(1);%theta1(i)=atan(y_state(i)/x_state(i))+theta_noise*randn(1);%使用下面增加了象限判断的角度计算方式[-pi,pi]if x_state(i)>0 && y_state(i)>=0theta1(i) = atan(y_state(i)/x_state(i))+theta_noise*randn(1) ; %观测方程endif x_state(i)<0 && y_state(i)>=0theta1(i) = (atan(y_state(i)/x_state(i))+pi) +theta_noise*randn(1); %观测方程endif x_state(i)<0 && y_state(i)<=0theta1(i) = (atan(y_state(i)/x_state(i))-pi) +theta_noise*randn(1); %观测方程endif x_state(i)>0 && y_state(i)<=0theta1(i) = atan(y_state(i)/x_state(i)) +theta_noise*randn(1); %观测方程endxobs = [xobs distance1(i)*cos(theta1(i))];yobs = [yobs distance1(i)*sin(theta1(i))];end%%%Particle Filtering%%%%%%%%%%%%%%x_pf(1)=x_state(1);vx_pf(1)=vx(1);y_pf(1)=y_state(1);vy_pf(1)=vy(1);xp1=zeros(1,N1);xp2=zeros(1,N1);xp3=zeros(1,N1);xp4=zeros(1,N1); %%%%%Initial particles 得到初始化的粒子群%%%%%%%%for n=1:N1;%M1=[delta1*randn(1),delta2*randn(1),delta3*randn(1),delta4*randn(1)];%M1=diag(M1);xp1(n)=x_pf(1)+normrnd(0,Vx_noise,1,1);xp2(n)=vx_pf(1)+normrnd(0,Vx_noise,1,1);xp3(n)=y_pf(1)+normrnd(0,Vx_noise,1,1);xp4(n)=vy_pf(1)+normrnd(0,Vx_noise,1,1);end%**filter process*** angel and distance**************** for t=2:time%%%Prediction Process%%%%for n=1:N1accx = normrnd(0,Vx_noise,1,1);xpre_pf(n)=xp1(n)+xp2(n)+0.5*accx;vxpre_pf(n)=xp2(n)+accx;accy = normrnd(0,Vx_noise,1,1);ypre_pf(n)=xp3(n)+xp4(n)+0.5*accy;vypre_pf(n)=xp4(n)+accy;end%%%Calculate Weight Particles%%%%for n=1:N1vhat1=sqrt(xpre_pf(n)^2+ypre_pf(n)^2)-distance1(t);%vhat2=atan(ypre_pf(n)/xpre_pf(n))-theta1(t);%使用下面增加了象限判断的角度计算方式if xpre_pf(n)>0 && ypre_pf(n)>=0ag = atan(ypre_pf(n)/xpre_pf(n)) ; %观测方程endif xpre_pf(n)<0 && ypre_pf(n)>=0ag = (atan(ypre_pf(n)/xpre_pf(n))+pi); %观测方程endif xpre_pf(n)<0 && ypre_pf(n)<=0ag = (atan(ypre_pf(n)/xpre_pf(n))-pi) ; %观测方程endif xpre_pf(n)>0 && ypre_pf(n)<=0ag = atan(ypre_pf(n)/xpre_pf(n)); %观测方程endvhat2=ag-theta1(t);q1=(1/distance1_noise/sqrt(2*pi))*exp(-vhat1^2/2/distance1_noise^2);q2=(1/theta_noise/sqrt(2*pi))*exp(-vhat2^2/2/theta_noise^2);q(n)=q1*q2+1e-99;endq = q./sum(q);P_pf = cumsum(q);%%Resampling Process 这是一种均匀的重采样方法，随机数的产生不再是从[0,1]上任意产生，而是使这个随机数渐进式的增大，与权重累加和一样，都是交替上升，这样的比较更有规律性，更周到%%%%%%%%%%%%%%ut(1)=rand(1)/N1;k = 1;hp = zeros(1,N1);for j = 1:N1ut(j)=ut(1)+(j-1)/N1;while(P_pf(k)<ut(j));k = k + 1;end;hp(j) = k;q(j)=1/N1;end;xp1 = xpre_pf(hp); xp2 = vxpre_pf(hp); % The new particles xp3 = ypre_pf(hp); xp4 = vypre_pf(hp);%% Compute the estimate%%%%%%%%%%%%%x_pf(t)=mean(xp1);y_pf(t)=mean(xp3);end%%%%Result of Tracking%%%%%%%%%%%%% figure;plot(x_state,y_state,'r-*',x_pf,y_pf,'b-o',xobs,yobs,'g-d') xlabel('x state'); ylabel('y state');legend('实际轨迹','滤波轨迹','观测轨迹');set(gcf,'Color','White');%figure;%plot(1:time,distance_error,'r');%legend('distance error');。
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
姓名：朱林富 学号：08120361 研究方向：粒子滤波基于粒子滤波实现的目标被动跟踪1.1被动定位系统是一个仅有角测量的系统，通常对于目标距离是不可测的。
由于实时处理和计算存储量的需求，通常选用递推滤波算法来实现。
由于系统本身的弱观测性，状态空间模型非线性强，导致滤波算法的收敛精度和收敛时间满足不了要求。
处理这种非线性、非高斯问题，粒子滤波算法有很好的表现。
粒子滤波的基本思想是：首先依据系统状态向量的经验条件分布，在状态空间抽样产生一组随机样本集合，这些样本集合称为粒子；然后根据观测值不断调整粒子的权重大小和样本位置；最后通过调整后的粒子信息修正最初的的经验条件分布，估计出系统状态和参数。
该算法是一种递推滤波算法，可以用来估计任意非线性非高斯随机系统的状态和参数。
粒子滤波主要有三步基本操作：采样（从不含观察值的状态空间产生新的粒子）、权值计算（根据观察值计算各个粒子的权值）、重采样（抛弃权值小的粒子，使用权值大的粒子代替），这三步构成了粒子滤波的基本算法。
SIRF(Sample Importance Resampling Filter)算法是粒子滤波的一种基本算法。
1.2被动定位系统中二维纯方位目标跟踪模型如上图所示。
以观测站为原点，建立二维直角坐标系，图中标出了目标在k 时刻、k-1时刻的位置，目标到观测站的距离R 不可测。
系统状态模型为：11,1,2,...,k k k x x u k n --=Φ+Γ= （1.1） 系统观测模型为：1tan (/)k k k k z y x v -=+ （1.2）其中[,,,]k k Tk k x k y x x V y V =为系统的k 时刻状态值（目标在坐标系x,y 方向上的位置和速度），11k-1u [,]k k Tx y u u --=为k-1时刻x,y 方向上的系统噪声，k v 为k 时刻的观测噪声。
k z 为k 时刻的观测角度。
1100010000110001⎡⎤⎢⎥⎢⎥Φ=⎢⎥⎢⎥⎣⎦ 100.500100.5⎡⎤⎢⎥⎢⎥Γ=⎢⎥⎢⎥⎣⎦为了实现方便，设定系统噪声为一零均值高斯白噪声。
初始状态x 描述了被跟踪目标的初始状态。
传感器测量目标的角度，会产生测量误差和受到测量噪声的污染，测量方程式如1.2所示。
为了简便，其中测量噪声设定为一零均值高斯白噪声v k ～N （0，r ）1.3在被动定位系统的二维纯方位目标跟踪中，SIRF 算法的步骤如下：1）采样:根据系统状态方程式（1.1）采样得到k 时刻粒子集{}()1,1,2,...,N i k i x i N ==11111111()()()1()()()()()1()()0.50.5k k k k k k k k k k i i i i k k x x i i i x x x i i i i kk y y i i i y y y x x V V V yyVV V --------()-()()-()=++μ=+μ=++μ=+μ（1.3）2）权值计算：选取先验概率密度函数()1()i k k p x x -|为重要性函数，意味着权值更新为()()1()i i ik k k k p z x -ω∝ω|，且由观测方程式（ 1.2）得1k v t a n (/)k k k z y x -=- （1.4）由于观测噪声与系统状态相互独立，根据式（1.4）得()()1()()()1()()()()((tan (/)))(tan (/))i i i i i i i x k k v k k v k k k k v k k k p z x p v x p z y x x p z y x --|=|=-|=- 假设观测噪声为一零均值高斯白噪声，则权值计算为：式1.5*()()1()()1()()21()(tan (/))exp (tan (/))2i i i i i i k x k k v k k k k k k p z x p z y x z y x --2⎡⎤ω=|=-=--⎢⎥δ⎣⎦ 3）权值归一化：当各个粒子权值计算完成后进行权值归一化()*()*()1/Ni i i k kk i =ω=ωω∑ 4）重采样：采用RSR 重采样算法，获得新粒子集{}()()1,Ni i k k i x =ω----------------重采样程序------------------------------------------------------------indr=1;indd=rN; %设置指针的初始值 K=rN/W; %计算中间变量KU=rand(1,1); %随机生成一个随机阈值 for i=1:rN; %主循环temp=K.*w_buffer(i,1)-U; %添加一个中间变量temp r_buffer(indr,1)=quzheng(temp); %U=r_buffer(indr,1)-temp; %更新阈值 if r_buffer(indr,1)>0 %i_buffer(indr,1)=i; %存储被复制粒子的地址 indr=indr+1; elsei_buffer(indd,1)=i; % 存储被抛弃粒子的地址 indd=indd-1; end; iR=indr-1;-------------------------------------------------------------------------------------------------- 5）状态值输出：()()1()()1()()1()()1ˆˆˆˆˆk kk kNi i k k k i Ni i x k x i Ni i k k k i Ni i y k y i xx V V yy V V =====ω=ω=ω=ω∑∑∑∑----------------状态输出程序---------------------------------------------------------- X=0;Vx=0;Y=0;Vy=0; for i=1:rN;X=X+w_buffer(i,1).*x_buffer(i,1); % 计算x 方位值Vx=Vx+w_buffer(i,1).*Vx_buffer(i,1); % 计算x 方向速度值 Y=Y+w_buffer(i,1).*y_buffer(i,1); % 计算y 方位值 Vy=Vy+w_buffer(i,1).*Vy_buffer(i,1); % 计算y 方向速度值 end;rX(t,1)=X/W; % 输出值归一化 rVx(t,1)=Vx/W;rY(t,1)=Y/W; rVy(t,1)=Vy/W;-------------------------------------------------------------------------------------------------- SIRF 算法功能框图如下图所示：每输入一个观测值k z 都将经过5步产生一个状态值输出ˆk x，这5步是：采样、权值计算、权值归一化、重采样、状态值输出。
1.4在MATLAB 中仿真目标跟踪过程，取粒子数N=1024，为实现简便设系统噪声和测量噪声均为零均值高斯白噪声。
采样粒子位置和速度的初始分布分别为：00202022()(4.5,0.2)()(4,0.3)()(0.2,0.01)()(0.15,0.02)x y p x N p y N p V N p V N ====设真实的轨迹初始位置和速度为：00004.5; 4.5;0.2;0.2x y x y V V ====--------------------------------系统参数初始化程序--------------------------------------------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;为了便于比较，用一段程序描述目标的真实运动轨迹。
-----------------------------真实运动轨迹程序-------------------------------------------------- x0=4.5;y0=4.5;Vx0=0.2;Vy0=0.2; % 初始化真实状态的初值tX(1,1)=x0;tY(1,1)=y0;tVx(1,1)=Vx0;tVy(1,1)=Vy0;w0=gauss(UxMEAN,UxSIGMA,rSTEP); %噪声w1=gauss(UyMEAN,UySIGMA,rSTEP);w2=gauss(UzMEAN,UzSIGMA,rSTEP);tZ(1,1)=atan(tY(1,1)./tX(1,1))+w2(1,1); %初始测量角for t=2:rSTEPtX(t,1)=tX(t-1,1)+tVx(t-1,1)+w0(t,1); % tX(t,1)存储目标x方向的真实坐标值tY(t,1)=tY(t-1,1)+tVy(t-1,1)+w1(t,1); % tY(t,1)存储目标y方向的真实坐标值tVx(t,1)=tVx(t-1,1)+0.5.*w0(t,1); % tVx(t,1)存储目标x方向的真实速度值tVy(t,1)=tVy(t-1,1)+0.5.*w1(t,1); % tVy(t,1)存储目标y方向的真实速度值tZ(t,1)=atan(tY(t,1)./tX(t,1))+w2(t,1); % tZ(t,1)用来存储每一时刻的测量角end;初始化和真实轨迹描述完成后，用1.2中讲到的SIRF算法实现被动跟踪。
具体步骤分为：1）采样2）权值计算3)权值归一化4）重采样5）状态值输出。
详细程序及注释见附件中particle.m文件仿真结果如下图所示，虚线为目标实际运动轨迹，实线为估计的目标运动轨迹。