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

目标定位跟踪算法及仿真程序
质心算法是最简单的定位算法，如图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
4.1 概念和定义52 4.2 蒙特卡洛模拟仿真程序53 4.2.1硬币投掷实验（1）53 4.2.2硬币投掷实验（2）53 4.2.3古典概率实验54 4.2.4几何概率模拟实验54 4.2.5复杂概率模拟实验55 4.3 蒙特卡洛理论基础57 4.3.1大数定律57 4.3.2中心极限定律58 4.3.3蒙特卡洛的要点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源程序两部分。

全书构成：
一，讲述原理（原书截图）
二，Matlab 程序给出详细中文注释
三，仿真结果
50
50
100
020*********
Node 2
Node 1
Node 5
Node 3
Node 4error=2.3491m
Observation Station Target Postion Estimate Postion
三维定位仿真结果图例
纯方位目标跟踪轨迹
观测距离目标跟踪轨迹四，结果分析
x方向估计误差均值
y方向估计误差均值
跟踪误差分析图例
如果你在研究中遇到原理或者编程的困难，欢迎交流，QQ：345194112。

本书淘宝网地址：
/item.htm?id=14716944857。