基于多站纯方位粒子滤波跟踪系统及其matlab仿真

多站纯方位目标跟踪系统模型

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

状态方程：X(k+1)= A*X(k) + T*w(k)

观测方程：Z(k) = arctan((y(k)-ys_i) / (x(k)-xs_i)) + v_i(k)

式中Z是观测站通过某种测距方式测得的与目标之间的角度，它是受得测量噪声v(k)的污染的。通常将上述观测方程表示为：Z(k) = h(X(k)) + v(k)

仿真程序

粒子滤波较好地对目标真实轨迹进行了跟踪，每个采样周期内各观测站运行的时间大约在0.2s左右。随着时间的推移，粒子滤波跟踪误差组件增大，这与粒子匮乏等因素有关。



（1）main.m

function main
clear;
T=1;  %采样周期 
%error('下面的参数M请参考书中的值设置，然后删除本行代码')
M=30; %采样点数  
delta_w=1e-3; %过程噪声调整参数，设得越大，目标运行的机动性越大，轨迹越随机 
Q=delta_w*diag([0.5,1,0.5,1]) ;  %过程噪声均方差
%R是观测噪声，设都相等，即所有观测站功能完全一样，传感器性能完全一样
%如果要考虑更真实的情况，需要将其设为不同的值，以便做更复杂数据融合算法
R=2;   %观测角度方差
F=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1];
Node_number=6; %观测站个数
Length=100;   %目标运动的场地空间
Width=100;     %设长为100m,宽为100m
for i=1:Node_number
    Node(i).x=Width*rand;   %随机部署观测站的位置
    Node(i).y=Length*rand;
end
for i=1:Node_number   %保存观测站位置到一个矩阵上
    NodePostion(:,i)=[Node(i).x,Node(i).y]';
end
X=zeros(4,M);  %目标状态          
Z=zeros(Node_number,M);%观测数据   
w=randn(4,M);
v=randn(Node_number,M);
X(:,1)=[1,Length/M,20,60/M]';  %初始化目标状态
state0=X(:,1);                 %估计的初始化
%模拟目标运动
for t=2:M  
    %状态方程
    X(:,t)=F*X(:,t-1)+sqrtm(Q)*w(:,t); 
end
%模拟目标运动过程中，各个观测站采集角度信息
for t=1:M
    for i=1:Node_number
        x0=NodePostion(1,i);
        y0=NodePostion(2,i);
        %观测方程
        Z(i,t)=feval('hfun',X(:,t),x0,y0)+sqrtm(R)*v(i,t);
    end
end
%便于函数调用，将参数打包
canshu.T=T;
canshu.M=M;
canshu.Q=Q;
canshu.R=R;
canshu.F=F;
canshu.state0=state0;
canshu.Node_number=Node_number;
%滤波 
[Xpf,Tpf]=PF(Z,NodePostion,canshu);
%RMS比较图
for t=1:M
    PFrms(1,t)=distance(X(:,t),Xpf(:,t));
end
%画图
%轨迹图 
figure
hold on
box on
for i=1:Node_number
    %观测站位置
    h1=plot(NodePostion(1,i),NodePostion(2,i),'ro','MarkerFaceColor','b');
    text(NodePostion(1,i)+0.5,NodePostion(2,i),['Node',num2str(i)])
end
%目标真实轨迹
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算法轨迹');
hold off
%RMS图，跟踪误差图 
figure
hold on
box on
plot(PFrms(1,:),'-k.','MarkerEdgeColor','m');
xlabel('time/s');
ylabel('error/m');
legend('RMS跟踪误差');
hold off
%实时性比较图 
figure
hold on
box on
plot(Tpf(1,:),'-k.','MarkerEdgeColor','m');  
xlabel('step');
ylabel('time/s');
legend('每个采样周期内PF计算时间');
hold off
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

（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,x0,y0)

if nargin < 3
    error('Not enough input arguments.'); 
end
[row,col]=size(x);
if row~=4|col~=1
    error('Input arguments error!');
end
xx=x(1)-x0;
yy=x(3)-y0;
y=atan2(yy,xx);

（5）PF.m

%  程序说明： 粒子滤波子程序
function [Xout,Tpf]=PF(Z,NodePostion,canshu)

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

N=100;       
zPred=zeros(1,N);
Weight=zeros(1,N);
xparticlePred=zeros(4,N);
Xout=zeros(4,M);
Xout(:,1)=state0;
Tpf=zeros(1,M);
for i=1:Node_number
    xparticle{i}=zeros(4,N);
    for j=1:N    
        xparticle{i}(:,j)=state0;
    end
    Xpf{i}=zeros(4,N);
    Xpf{i}(:,1)=state0;
end

for t=2:M
    tic;
    XX=0;
    for i=1:Node_number
        x0=NodePostion(1,i);
        y0=NodePostion(2,i);

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

        for k=1:N
            zPred(1,k)=feval('hfun',xparticlePred(:,k),x0,y0);
            z1=Z(i,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{i}= xparticlePred(:,outIndex);  
        target=[mean(xparticle{i}(1,:)),mean(xparticle{i}(2,:)),...
            mean(xparticle{i}(3,:)),mean(xparticle{i}(4,:))]';
        Xpf{i}(:,t)=target;

        XX=XX+Xpf{i}(:,t);
    end
    Xout(:,t)=XX/Node_number;
    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;