多站纯方位目标跟踪系统模型
假设目标做匀速直线运动,目标的状态为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;