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

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

质心算法是最简单的定位算法，如图2-1所示，四个小圆为观测站，实线三角形是目标真实的位置，假设四个圆形观测站都探测到目标的存在，则根据质心定位算法，目标的位置（x,y ）可以表示为：4

4321x x x x x +++=，4

4321y y y y y +++=，这里观测站得位置为),(i i y x ，同理，当观测站数目为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

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.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源程序两部分。