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

系统模型

假设目标做匀速直线运动，目标的状态为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)=A*X(k) + T*w(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

仿真程序

可以发现基于距离的粒子滤波的跟踪效果并不是很理想，偏差逐渐增大。

（1）main.m

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  程序说明： 单站单目标基于距离的跟踪系统，采用粒子滤波算法
%  状态方程  X（k+1）=F*X(k)+Lw(k)
%  观测方程  Z（k）=h（X）+v（k）
%%%%%%%%%%%%%%%
function main

randn('seed',4);
rand('seed',7);
T=1;
%error('下面的参数M请参考书中的值设置，然后删除本行代码')
M=30;
delta_w=1e-4;
Q=delta_w*diag([0.5,1,0.5,1]) ;
R=0.25;
F=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1];
Length=100;
Width=100;

Station.x=Width*rand;
Station.y=Length*rand;

X=zeros(4,M);
Z=zeros(1,M);
w=randn(4,M);
v=randn(1,M);
X(:,1)=[1,Length/M,20,60/M]';
state0=X(:,1)+w(:,1);
for t=2:M
X(:,t)=F*X(:,t-1)+sqrtm(Q)*w(:,t);
end

for t=1:M
Z(1,t)=feval('hfun',X(:,t),Station)+sqrtm(R)*v(1,t);
end

canshu.T=T;
canshu.M=M;
canshu.Q=Q;
canshu.R=R;
canshu.F=F;
canshu.state0=state0;

[Xpf,Tpf]=PF(Z,Station,canshu);

for t=1:M
PFrms(1,t)=distance(X(:,t),Xpf(:,t));
end

figure;hold on;box on

h1=plot(Station.x,Station.y,'ro','MarkerFaceColor','b');

h2=plot(X(1,:),X(3,:),'--m.','MarkerEdgeColor','m');

h3=plot(Xpf(1,:),Xpf(3,:),'-k*','MarkerEdgeColor','b');
xlabel('X/m');ylabel('Y/m');
legend([h1,h2,h3],'观测站位置','目标真实轨迹','PF算法轨迹');

figure;hold on;box on
plot(PFrms(1,:),'-k.','MarkerEdgeColor','m');
xlabel('time/s');ylabel('error/m');
legend('RMS跟踪误差');

figure;hold on;box on
plot(Tpf(1,:),'-k.','MarkerEdgeColor','m');
xlabel('step');ylabel('time/s');
legend('每个采样周期内PF计算时间');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

（2）distance.m

% 程序说明： 求两点之间的距离
function [d]=distance(X,Y)
if length(Y)==4
d=sqrt( (X(1)-Y(1))^2+(X(3)-Y(3))^2 );
end
if length(Y)==2
d=sqrt( (X(1)-Y(1))^2+(X(3)-Y(2))^2 );
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

（3）ffun.m

% 程序说明： 求目标位置函数
% 输入参数： 观测站一次观测值x,观测站的位置（x0,y0)
% 输出参数： 目标的位置信息
function [y]=ffun(x,x0,y0)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if nargin < 3
error('Not enough input arguments.');
end
[row,col]=size(x);
if row~=2|col~=1
error('Input arguments error!');
end
y=zeros(2,1);
y(1)=x(1)*cos(x(2))+x0;
y(2)=x(1)*sin(x(2))+y0;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

（4）hfun.m

% 程序说明： 观测方程函数
% 输入参数： x目标的状态，（x0,y0)是观测站的位置
% 输出参数： y是距离
function [y]=hfun(x,station)

if nargin < 2
error('Not enough input arguments.');
end
[row,col]=size(x);
if row~=4|col~=1
error('Input arguments error!');
end
y=sqrt((x(1)-station.x)^2+(x(3)-station.y)^2);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

（5）PF.m

%  程序说明： 粒子滤波子程序
function [Xout,Tpf]=PF(Z,Station,canshu)
T=canshu.T;
M=canshu.M;
Q=canshu.Q;
R=canshu.R;
F=canshu.F;
state0=canshu.state0;
N=200;
zPred=zeros(1,N);
Weight=zeros(1,N);
xparticlePred=zeros(4,N);
Xout=zeros(4,M);
Xout(:,1)=state0;
Tpf=zeros(1,M);
xparticle=zeros(4,N);
for j=1:N
xparticle(:,j)=state0;
end
Xpf=zeros(4,N);
Xpf(:,1)=state0;
for t=2:M
tic;
XX=0;

for k=1:N
xparticlePred(:,k)=feval('sfun',xparticle(:,k),T,F)+10*sqrtm(Q)*randn(4,1);
end

for k=1:N
zPred(1,k)=feval('hfun',xparticlePred(:,k),Station);
z1=Z(1,t)-zPred(1,k);
Weight(1,k)=inv(sqrt(2*pi*det(R)))*exp(-.5*(z1)'*inv(R)*(z1))+ 1e-99;%权值计算
end

Weight(1,:)=Weight(1,:)./sum(Weight(1,:));

outIndex = randomR(1:N,Weight(1,:)');
xparticle= xparticlePred(:,outIndex);
target=[mean(xparticle(1,:)),mean(xparticle(2,:)),...
mean(xparticle(3,:)),mean(xparticle(4,:))]';
Xout(:,t)=target;
Tpf(1,t)=toc;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

（6）randomR.m

% 随机采样子函数
function outIndex = randomR(inIndex,q)
if nargin < 2
error('Not enough input arguments.');
end
outIndex=zeros(size(inIndex));
[num,col]=size(q);
u=rand(num,1);
u=sort(u);
l=cumsum(q);
i=1;
for j=1:num
while (i<=num)&(u(i)<=l(j))
outIndex(i)=j;
i=i+1;
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

（7）sfun.m

% 子程序说明： 系统状态转移函数
function [y]=sfun(x,T,F)
if nargin < 2
error('Not enough input arguments.');
end
y=F*x;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%