纯方位目标跟踪系统模型
假设目标做匀速直线运动，目标的状态为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)-y0) / (x(k)-x0)) + v(k)
式中Z是观测站通过某种测距方式测得的与目标之间的角度，它是受得测量噪声v(k)的污染的。通常将上述观测方程表示为：Z(k) = h(X(k)) + v(k)
仿真程序
（1）main.m
% 程序说明： 单站单目标基于角度的跟踪系统，采用粒子滤波算法
% 状态方程 X（k+1）=F*X(k)+Lw(k)
% 观测方程 Z（k）=h（X）+v（k）
function main
clear;
T=1; %采样周期
%error('下面的参数M请参考书中的值设置，然后删除本行代码')
M=30; %采样点数
delta_w=1e-4; %过程噪声调整参数，设得越大，机动性越大，轨迹越随机
Q=delta_w*diag([0.5,1,0.5,1]) ; %过程噪声均方差
R=pi/180*0.1; %观测角度均方差，可将0.1设置得更小
F=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1];
%系统初始化
Length=100; %目标运动的场地空间
Width=100;
%观测站的位置随机部署
Node.x=Width*rand;
Node.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); %估计的初始化
for t=2:M
X(:,t)=F*X(:,t-1)+sqrtm(Q)*w(:,t); %目标真实轨迹
end
%模拟目标运动过程，观测站对目标观测获取角度数据
for t=1:M
x0=Node.x;
y0=Node.y;
Z(1,t)=feval('hfun',X(:,t),x0,y0)+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,Node,canshu);
% RMS比较图
for t=1:M
PFrms(1,t)=distance(X(:,t),Xpf(:,t));
end
%%%%%%%%% 画图
% 轨迹图
figure
hold on
box on
% 观测站位置
h1=plot(Node.x,Node.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算法轨迹');
hold off
%%%%%%%%%%%%%%%%%%%%%%%%%%
% RMS跟踪误差图
figure
hold on
box on
plot(PFrms(1,:),'-k.','MarkerEdgeColor','m');
xlabel('time/s');
ylabel('error/m');
legend('RMS跟踪误差');
title(['RMSE,q=',num2str(delta_w)])
hold off
% 实时性比较图
figure
hold on
box on
plot(Tpf(1,:),'-k.','MarkerEdgeColor','m');
xlabel('step');
ylabel('time/s');
legend('每个采样周期内PF计算时间');
title('Comparison of Realtime')
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,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.x,Station.y);
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;