【目标定位跟踪-原理及应用】Matlab仿真01 质心定位算法及其Matlab实现

目标定位算法是获取目标位置的手段之一，利用观测信息、采用最合理的定位算法、结合实际应用领域的特点进行目标定位能达到较好的效果。定位算法不能完全得到与目标真实情况一致的位置信息，但它能最大限度地逼近真实位置，使定位误差降到合理的阈以下。
非测距定位，广泛地指不依赖任何信息，无须测量目标与观测站之间的距离和角度等信息，或者当测量的目标与观测站之间的距离和角度误差非常大的时候，只能用它们作为权值来处理。这类定位算法，只能依靠观测站的物理部署和简单的探测到目标的“有/无”的二进制探测信息对目标位置进行估计。非测距定位算法主要有质心定位算法、加权质心定位算法和网格定位算法等，它们虽然能够估计出具体位置，但是这个位置与目标真实位置的偏差依赖于观测站的部署密度，一般偏差是比较大的。

质心定位算法主要依赖对目标质心的估计，质量中心简称为质心，指物质系统上被认为质量集中于此的一个假想点。三角形中三条中线的交点、平行四边形的对角线、正六边形的对角线交点，这些点都可以看做这些几何图形的质心。
那么对于多质点的系统，假定每个质点的重量为$m_i$，位置为$p(x_i,y_i,z_i)$，那么该系统的质心计算方法如下：
$$\{\begin{array}{r}{x}_c=\frac{\sum m_i{x}_i}{m_i}\\ y_c=\frac{\sum m_i{y}_i}{m_i}\\ z_c=\frac{\sum m_i{z}_i}{m_i}\end{array}$$
如果考虑各质点的质量一样，那么质心的计算方法则简化为：
$$\{\begin{array}{r}{x}_c=\frac{{\sum }_{i=1}^N{x}_i}{N}\\ y_c=\frac{{\sum }_{i=1}^N{y}_i}{N}\\ z_c=\frac{{\sum }_{i=1}^N{z}_i}{N}\end{array}$$
将质心定理用在定位算法上，那么假定每一个观测站的位置为$p(x_i,y_i,z_i)$，将探测到目标的所有观测站看成一个多质点系统，按照上式可以计算得到一个位置，这个位置便是目标位置的估计值。显然，当观测站的分布很稀疏时，这个定位误差会很大。

实例1：假定有6个观测站，随机分布在长100米，宽100米的场地上，每个观测站完全相同，它们的探测距离是50m。一个目标随机出现在该监测场地，被其中的几个观测站探测到，并计算估计位置，用MATLAB实现这个过程。
主程序代码如下：

%% %%%%%%%%%%%%% function CentroidLocalization 质心点位算法
% 定位初始化
Length = 100;      %场地空间 长
Width =100;         %场地空间 宽
d = 50;      %单个观测站对目标的探测距离
N = 6;        %观测站的个数
% 观测站位置初始化
for i = 1:N
    Node(i).x = Length*rand;
    Node(i).y = Width*rand;
end
% 目标的位置也进行随机化处理
Target.x = Length*rand;
Target.y = Width*rand;
O =[ ];        %空列表，保存能够探测到目标的观测站的位置集合
for i = 1:N
    if getDist(Node(i),Target)<= d      %调用距离计算函数
        O = [O;Node(i).x,Node(i).y];    %记录探测到目标的观测站的位置
    end
end
M = size(O,1);   %求数组O的第一个维度的数量，即探测到目标的观测站的个数
if M>0
    Est_Target.x = sum(O( :,1))/M;     %   调用公式用质心算法估计目标的位置x
    Est_Target.y = sum(O( :,1))/M;
    Error_Dist = getDist(Est_Target,Target);     %计算目标真实位置与估计位置的偏差距离
end

%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure
hold on
box on;
axis([0 100 0 100]);  %输出图形的框架
for i =1:N
    h1 = plot(Node(i).x,Node(i).y,'ko','MarkerFace','g','MarkerSize',12);
    text(Node(i).x+2,Node(i).y,['Node',num2str(i)]);       %标记观察站的号码
end
% 画目标的真实位置和估计位置并标注
h2 = plot(Target.x,Target.y,'k^','MarkerFace','b','MarkerSize',12);
text(Target.x+2,Target.y,'Target Position');
h3 = plot(Est_Target.x,Est_Target.y,'ks','MarkerFace','r','MarkerSize',12);
text(Est_Target.x+2,Est_Target.y,'Estimate Position');
% 将估计位置和真实位置用线连起来
line([Target.x,Est_Target.x],[Target.y,Est_Target.y],'Color','k');
% 画出目标半径d的范围
circle(Target.x,Target.y,d);
% 标明h1,h2,h3
legend([h1,h2,h3],'Observation Station','Target Position','Estimate Position');
xlabel(['error=',num2str(Error_Dist),'m']);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

距离计算子程序代码

function  dist= getDist(A,B)
%UNTITLED Summary of this function goes here
%   Detailed explanation goes here
dist = sqrt((A.x-B.x)^2+(A.y-B.y)^2);

画半径子程序代码

function   circle(x0,y0,r)
%UNTITLED 此处显示有关此函数的摘要
%   此处显示详细说明
theta = 0:pi/20:2*pi;
plot(x0+r*cos(theta),y0+r*sin(theta));

下面是两次运行的结果，可以看到特别是第一次定位误差是非常大的，定位误差和观测站的位置及部署的疏密程度关系密切。