粒子滤波的公式和matlab

姓名：朱林富 学号：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） 系统观测模型为：1

tan (/)k k k k z y x v -=+ （1.2）

其中[,,,]k k T

k k x k y x x V y V =为系统的k 时刻状态值（目标在坐标系x,y 方向上的位置和速度），11k-1u [,]k k T

x 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 ==

11

111

1

11

()()()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 k

k y y i i i y y y x x V V V y

y

V

V V --------()-()()-()=++μ=+μ=++μ

=+μ

（1.3）

2）权值计算：选取先验概率密度函数()

1()i k k p x x -|为重要性函数，意味着权值更新为()

(

)

1()i i i

k k k k p z x -ω∝ω|，且由观测方程式（ 1.2）得

1

k 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

/N

i i i k k

k i =ω=ω

ω∑ 4）重采样：采用RSR 重采样算法，获得新粒子集{}()

()1

,N

i i k k i x =ω

----------------重采样程序------------------------------------------------------------

indr=1;indd=rN; %设置指针的初始值 K=rN/W; %计算中间变量K

U=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; else

i_buffer(indd,1)=i; % 存储被抛弃粒子的地址 indd=indd-1; end; iR=indr-1;

-------------------------------------------------------------------------------------------------- 5）状态值输出：

()()

1()()1

()()1()()1

ˆˆˆˆˆk k

k k

N

i i k k k i N

i i x k x i N

i i k k k i N

i i y k y i x

x V V y

y 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;