《目标定位跟踪算法及仿真程序(修改后)》由会员分享，可在线阅读，更多相关《目标定位跟踪算法及仿真程序(修改后)（7页珍藏版）》请在金锄头文库上搜索。
1、目标定位跟踪算法及仿真程序质心算法是最简单的定位算法，如图2-1 所示，四个小圆为观测站，实线三角形是目标真实的位置， 假设四个圆形观测站都探测到目标的存在，则根据质心定 位 算 法 ， 目 标 的 位 置 （ x,y ） 可 以 表 示 为 ： 44321xxxxx，44321yyyyy，这里观测站得位置为),(iiyx，同理，当观测站数目为N时，这时候的质心定位算法可以表示为：NiiNiiy Nx Nyx11 11图 1 质心定位% % 质心定位算法Matlab 程序% function main % 定位初始化Length=100; % 场地空间，单位：米Width=100; % 场地空
2、间，单位：米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 e
3、nd 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(Nod
4、e(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 Post
5、ion,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 % 执行程序，得到仿真结果：0102030405060708090100010203040506
6、0708090100Node 1Node 2Node 3Node 4Node 5Node 6error=4.6456mObservation StationTarget PostionEstimate Postion更多目标定位跟踪算法和程序请参考以下书籍：目录第一章目标跟踪概述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 最小
7、均方误差的二维定位方法程序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
8、无迹 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 大数定律
9、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 粒子滤波在目
10、标跟踪中的应用77 5.3.1 高斯模型下粒子滤波的实例程序77 5.3.2 高斯噪声下粒子滤波用于目标跟踪的程序81 5.3.3 闪烁噪声下粒子滤波用于目标跟踪的程序85 本书说明：该书的研究内容是目标跟踪的状态估计方法，主要有最小二乘估计， Kalman滤波，扩展 Kalman 滤波，无迹 Kalman 滤波以及粒子滤波等，包括理论介绍和MATLAB 源程序两部分。全书构成：一，讲述原理（原书截图）二，Matlab 程序给出详细中文注释三，仿真结果050100050100020406080100Node 2Node 1Node 5Node 3Node 4error=2.3491mObser
11、vation StationTarget PostionEstimate Postion三维定位仿真结果图例01020304050607080901000102030405060708090100Node 1Node 2Node 3Node 4Node 5Observation StationTrue TraceMLE Ttace纯方位目标跟踪轨迹-100-80-60-40-2002040200400600800100012001400 真 实 轨 迹UKF 轨 迹观测距离目标跟踪轨迹四，结果分析01020304050600510152025 滤 波 前 误 差滤 波 后 误 差50100150200250300350400450-200-1000100200x方 向 估 计 误 差 均 值50100150200250300350400450-200-1000100200y方 向 估 计 误 差 均 值跟踪误差分析图例如果你在研究中遇到原理或者编程的困难，欢迎交流，QQ：345194112 。本书淘宝网地址：http:/
