目标定位跟踪算法及仿真程序(修改后)

目标定位跟踪算法及仿真程序

质心算法是最简单的定位算法，如图2-1所示，四个小圆为观测站，实线三角形是目标真实的位置，假设四个圆形观测站都探测到目标的存在，则根据质心定位算法，目标的位置（x,y ）可以表示为：4

4

321x x x x x +++=

，

4

4

321y y y y y +++=

，这里观测站得位置为),(i i y x ，同理，当观测站数目为N

时，这时候的质心定位算法可以表示为：

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢⎣⎡=⎥⎦⎤⎢⎣⎡∑∑==N i i N i i y N x N y x 11

11

图1 质心定位

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

% 质心定位算法Matlab 程序

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function main % 定位初始化

Length=100; % 场地空间，单位：米 Width=100; % 场地空间，单位：米

d=50; % 目标离观测站50米以内都能探测到，反之则不能 Node_number=6; % 观测站的个数

for i=1:Node_number % 观测站的位置初始化，这里位置是随机给定的 Node(i).x=Width*rand; Node(i).y=Length*rand; end

% 目标的真实位置，这里也随机给定 Target.x=Width*rand; Target.y=Length*rand; % 观测站探测目标 X=[];

for i=1:Node_number

if DIST(Node(i),Target)<=d

X=[X;Node(i).x,Node(i).y];

end

end

N=size(X,1); % 探测到目标的观测站个数

Est_Target.x=sum(X(:,1))/N; % 目标估计位置x

Est_Target.y=sum(X(:,2))/N; % 目标估计位置y

Error_Dist=DIST(Est_Target,Target) % 目标真实位置与估计位置的偏差距离%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 画图

figure

hold on;box on;axis([0 100 0 100]); % 输出图形的框架

for i=1:Node_number

h1=plot(Node(i).x,Node(i).y,'ko','MarkerFace','g','MarkerSize',10);

text(Node(i).x+2,Node(i).y,['Node ',num2str(i)]);

end

h2=plot(Target.x,Target.y,'k^','MarkerFace','b','MarkerSize',10);

h3=plot(Est_Target.x,Est_Target.y,'ks','MarkerFace','r','MarkerSize',10);

line([Target.x,Est_Target.x],[Target.y,Est_Target.y],'Color','k');

circle(Target.x,Target.y,d);

legend([h1,h2,h3],'Observation Station','Target Postion','Estimate Postion');

xlabel(['error=',num2str(Error_Dist),'m']); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 子函数，计算两点间的距离

function dist=DIST(A,B)

dist=sqrt( (A.x-B.x)^2+(A.y-B.y)^2 );

% 子函数，以目标为中心画圆

function circle(x0,y0,r)

sita=0:pi/20:2*pi;

plot(x0+r*cos(sita),y0+r*sin(sita)); % 中心在(x0,y0），半径为r %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 执行程序，得到仿真结果：

error=4.6456m

更多目标定位跟踪算法和程序请参考以下书籍：

目录

第一章目标跟踪概述 1 1.1 多传感器探测的目标跟踪架构 1 1.2 目标定位算法简介 2 1.3 目标跟踪过程描述 2 1.4 跟踪模型的建立 4 第二章常用目标定位算法7 2.1 质心定位算法程序7 2.2 加权质心定位算法程序9 2.3 最小二乘/极大似然定位算法12 2.3.1 测距技术12 2.3.2 定位技术14 2.3.3最小均方误差的二维定位方法程序16 2.3.4 最小均方误差的三维定位方法程序17 2.3.5 最小二乘/极大似然用于目标跟踪(连续定位)程序19 2.3.6 最小二乘/极大似然用于纯方位目标跟踪(连续定位)程序22 第三章卡尔曼滤波24 3.1 Kalman滤波24 3.1.1 Kalman滤波原理24 3.1.2 Kalman滤波在目标跟踪中的应用及仿真程序26 3.2 扩展Kalman滤波（EKF）28 3.2.1 扩展Kalman滤波原理28 3.2.2 基于EKF的单站观测距离的目标跟踪程序29 3.2.3 基于EKF的单站纯方位目标跟踪程序32 3.3 无迹Kalman滤波（UKF）35 3.3.1 无迹Kalman滤波原理35 3.3.2 无迹卡尔曼在目标跟踪中的应用的仿真程序36 3.4 交互多模型Kalman滤波（IMM）40 3.4.1 交互多模原理40 3.4.1 交互多模kalman滤波在目标跟踪应用仿真程序47 第四章蒙特卡洛方法52