基于距离的粒子滤波跟踪系统及其matlab仿真

Github个人博客：https://joeyos.github.io

系统模型

假设目标做匀速直线运动，目标的状态为X（k）= [ xp(k) xv(k) yp(k) yv(k) ]T，很显然k时刻目标的位置为（xp(k) yp(k)），目标的速度（xv(k) yv(k)）由水平方向和垂直方向的分速度构成，用向量表示为：v(k) = xv(k) + yv(k)

目标匀速运动过程中必然受到风力，摩擦力等因素的干扰。为了便于理解，将目标的水平和垂直方向分解为：

水平位置：xp(k+1) = xp(k) + xv(k)*1 + 0.5wxp(k)*1^2

水平速度：xv(k+1) = xv(k) + wxv(k)*1

垂直位置：yp(k+1) = yp(k) + yv(k)*1 + 0.5wyp(k)*1^2

垂直速度：yv(k+1) = yv(k) + wyv(k)*1

状态方程：X(k+1)=AX(k) + Tw(k)

观测站的位置与目标之间存在关系：

d(k)=((xp(k)-xs)^2+(yp(k)-ys)2)^0.5 + v(k)

式中，d是观测站通过某种测距方式测得的与目标之间的距离，当然这个距离不是百分之百准确的，它是受测量噪声v(k)污染的。通常将上述观测方程表示为：
Z(k) = h(X(k)) + v(k)

函数h表示的是观测站与目标状态之间的线性或非线性函数关系，这里的h便是非线性关系：
h(X(k)) = ((xp(k)-xs)^2+(yp(k)-ys)2)^0.5

仿真程序