如有你有帮助，请购买下载，谢谢！
1页
目标定位跟踪算法及仿真程序
质心算法是最简单的定位算法，如图2-1所示，四个小圆为观测站，实线三
角形是目标真实的位置，假设四个圆形观测站都探测到目标的存在，则根据质心

定位算法，目标的位置（x,y）可以表示为：44321xxxxx，

4
4321
yyyyy

，这里观测站得位置为),(iiyx，同理，当观测站数目为N

时，这时候的质心定位算法可以表示为：
图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
如有你有帮助，请购买下载，谢谢！
2页
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
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
执行程序，得到仿真结果：
更多目标定位跟踪算法和程序请参考以下书籍：

目 录
第一章 目标跟踪概述 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
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页
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
4.1 概念和定义 52
4.2 蒙特卡洛模拟仿真程序 53
53
53
54
54
55
4.3 蒙特卡洛理论基础 57
57
58
59
4.4 蒙特卡洛方法的应用 60
4.4.1 Buffon实验及仿真程序 61
4.4.2 蒙特卡洛方法计算定积分的仿真程序 62
第五章 粒子滤波 66
5.1 粒子滤波概述 66
5.1.1 蒙特卡洛采样原理 66
5.1.2 贝叶斯重要性采样 67
5.1.3 序列重要性抽样（SIS）滤波器 67
5.1.4 Bootstrap/SIR滤波器 69
5.2 粒子滤波重采样方法实现程序 71
5.2.1 随机重采样程序 71
5.2.2 多项式重采样程序 73
5.2.3 系统重采样程序 74
5.2.4 残差重采样程序 76
5.3 粒子滤波在目标跟踪中的应用 77
5.3.1 高斯模型下粒子滤波的实例程序 77
5.3.2 高斯噪声下粒子滤波用于目标跟踪的程序 81
5.3.3 闪烁噪声下粒子滤波用于目标跟踪的程序 85

本书说明：

该书的研究内容是目标跟踪的状态估计方法，主要有最小二乘估计，Kalman
滤波，扩展Kalman滤波，无迹Kalman滤波以及粒子滤波等，包括理论介绍和
MATLAB源程序两部分。