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

目标定位跟踪算法及仿真程序质心算法是最简单的定位算法，如图2-1所示，四个小圆为观测站，实线三角形是目标真实的位置，假设四个圆形观测站都探测到目标的存在，则根据质心定位算法，目标的位置（x,y）可以表示为：44321xxxxx，44321yyyyy，这里观测站得位置为),(iiyx，同理，当观测站数目为N时，这时候的质心定位算法可以表示为：NiiNiiyNxNyx1111图1质心定位%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%质心定位算法Matlab程序%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%functionmain%定位初始化Length=100;%场地空间，单位：米Width=100;%场地空间，单位：米d=50;%目标离观测站50米以内都能探测到，反之则不能Node_number=6;%观测站的个数fori=1:Node_number%观测站的位置初始化，这里位置是随机给定的Node(i).x=Width*rand;Node(i).y=Length*rand;end%目标的真实位置，这里也随机给定Target.x=Width*rand;Target.y=Length*rand;%观测站探测目标X=[];fori=1:Node_numberifDIST(Node(i),Target)=dX=[X;Node(i).x,Node(i).y];endendN=size(X,1);%探测到目标的观测站个数Est_Target.x=sum(X(:,1))/N;%目标估计位置xEst_Target.y=sum(X(:,2))/N;%目标估计位置yError_Dist=DIST(Est_Target,Target)%目标真实位置与估计位置的偏差距离%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%画图figureholdon;boxon;axis([01000100]);%输出图形的框架fori=1:Node_numberh1=plot(Node(i).x,Node(i).y,'ko','MarkerFace','g','MarkerSize',10);text(Node(i).x+2,Node(i).y,['Node',num2str(i)]);endh2=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],'ObservationStation','TargetPostion','EstimatePostion');xlabel(['error=',num2str(Error_Dist),'m']);%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%子函数，计算两点间的距离functiondist=DIST(A,B)dist=sqrt((A.x-B.x)^2+(A.y-B.y)^2);%子函数，以目标为中心画圆functioncircle(x0,y0,r)sita=0:pi/20:2*pi;plot(x0+r*cos(sita),y0+r*sin(sita));%中心在(x0,y0），半径为r%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%执行程序，得到仿真结果：01020304050607080901000102030405060708090100Node1Node2Node3Node4Node5Node6error=4.6456mObservationStationTargetPostionEstimatePostion更多目标定位跟踪算法和程序请参考以下书籍：目录第一章目标跟踪概述11.1多传感器探测的目标跟踪架构11.2目标定位算法简介21.3目标跟踪过程描述21.4跟踪模型的建立4第二章常用目标定位算法72.1质心定位算法程序72.2加权质心定位算法程序92.3最小二乘/极大似然定位算法122.3.1测距技术122.3.2定位技术142.3.3最小均方误差的二维定位方法程序162.3.4最小均方误差的三维定位方法程序172.3.5最小二乘/极大似然用于目标跟踪(连续定位)程序192.3.6最小二乘/极大似然用于纯方位目标跟踪(连续定位)程序22第三章卡尔曼滤波243.1Kalman滤波243.1.1Kalman滤波原理243.1.2Kalman滤波在目标跟踪中的应用及仿真程序263.2扩展Kalman滤波（EKF）283.2.1扩展Kalman滤波原理283.2.2基于EKF的单站观测距离的目标跟踪程序293.2.3基于EKF的单站纯方位目标跟踪程序323.3无迹Kalman滤波（UKF）353.3.1无迹Kalman滤波原理353.3.2无迹卡尔曼在目标跟踪中的应用的仿真程序363.4交互多模型Kalman滤波（IMM）403.4.1交互多模原理403.4.1交互多模kalman滤波在目标跟踪应用仿真程序47第四章蒙特卡洛方法524.1概念和定义524.2蒙特卡洛模拟仿真程序534.2.1硬币投掷实验（1）534.2.2硬币投掷实验（2）534.2.3古典概率实验544.2.4几何概率模拟实验544.2.5复杂概率模拟实验554.3蒙特卡洛理论基础574.3.1大数定律574.3.2中心极限定律584.3.3蒙特卡洛的要点594.4蒙特卡洛方法的应用604.4.1Buffon实验及仿真程序614.4.2蒙特卡洛方法计算定积分的仿真程序62第五章粒子滤波665.1粒子滤波概述665.1.1蒙特卡洛采样原理665.1.2贝叶斯重要性采样675.1.3序列重要性抽样（SIS）滤波器675.1.4Bootstrap/SIR滤波器695.2粒子滤波重采样方法实现程序715.2.1随机重采样程序715.2.2多项式重采样程序735.2.3系统重采样程序745.2.4残差重采样程序765.3粒子滤波在目标跟踪中的应用775.3.1高斯模型下粒子滤波的实例程序775.3.2高斯噪声下粒子滤波用于目标跟踪的程序815.3.3闪烁噪声下粒子滤波用于目标跟踪的程序85本书说明：该书的研究内容是目标跟踪的状态估计方法，主要有最小二乘估计，Kalman滤波，扩展Kalman滤波，无迹Kalman滤波以及粒子滤波等，包括理论介绍和MATLAB源程序两部分。全书构成：一，讲述原理（原书截图）二，Matlab程序给出详细中文注释三，仿真结果050100050100020406080100Node2Node1Node5Node3Node4error=2.3491mObservationStationTargetPostionEstimatePostion三维定位仿真结果图例01020304050607080901000102030405060708090100Node1Node2Node3Node4Node5ObservationStationTrueTraceMLETtace纯方位目标跟踪轨迹-100-80-60-40-2002040200400600800100012001400真实轨迹UKF轨迹观测距离目标跟踪轨迹四，结果分析01020304050600510152025滤波前误差滤波后误差50100150200250300350400450-200-1000100200x方向估计误差均值50100150200250300350400450-200-1000100200y方向估计误差均值跟踪误差分析图例如果你在研究中遇到原理或者编程的困难，欢迎交流，加入QQ：345194112。本书网址：=13143057686或者：=13143057686