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

目标定位跟踪算法及仿真程序质心算法是最简单的定位算法，如图2-1所示，四个小圆为观测站，实线三角形是目标真实的位置，假设四个圆形观测站都探测到目标的存在，则根据质心定位算法，目标的位置（x,y）可以表示为：，这里观测站得位置为，同理，当观测站数目为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; endendN=size(X,1); % 探测到目标的观测站个数Est_Target.x=sum(X(:,1)/N; % 目标估计位置xEst_Target.y=sum(X(:,2)/N; % 目标估计位置yError_Dist=DIST(Est_Target,Target) % 目标真实位置与估计位置的偏差距离% 画图figurehold 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);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,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%执行程序，得到仿真结果：更多目标定位跟踪算法和程序请参考以下书籍：目 录第一章 目标跟踪概述 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.1 Kalman滤波 243.1.1 Kalman滤波原理 243.1.2 Kalman滤波在目标跟踪中的应用及仿真程序 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.1 Buffon实验及仿真程序 614.4.2 蒙特卡洛方法计算定积分的仿真程序 62第五章 粒子滤波 665.1 粒子滤波概述 665.1.1 蒙特卡洛采样原理 665.1.2 贝叶斯重要性采样 675.1.3 序列重要性抽样（SIS）滤波器 675.1.4 Bootstrap/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程序给出详细中文注释三， 仿真结果三维定位仿真结果图例纯方位目标跟踪轨迹观测距离目标跟踪轨迹四， 结果分析跟踪误差分析图