Matlab之提高交叉定位点的定位精度

通过测向交叉定位的方法，按理只需2根测向线即可得出定位点的位置。但由于误差的存在，求出的定位点位置存在一定的偏差。为了得到更加精确的定位点位置，需要对定位点进行冗余测量，从而得到多个定位点，然后通过定位点估计算法得到更加接近真实定位点的位置。

一、基于DBSCAN密度聚类滤除异常定位点

输入参数：

TargetPoint：测量定位点的集合，单位km；

epsilon：ε邻域的半径，单位km；

minPts：最小点数，用于判为同一类的最少点数量；

输出参数：

idx：TargetPoint数组中，属于同一类的索引标志。值为1说明是同一类。

TargetPoint_Cluster：筛选后属于同一类的定位点集合。

%% 对定位点进行聚类，DBSCAN密度聚类
% 定义参数
epsilon = 15;   % ε邻域的半径，单位与定位点的坐标单位相同
minPts = 20;      % 最小点数，判为同一类的最小点数量% 进行DBSCAN聚类
idx= DBSCAN(TargetPoint,epsilon,minPts);%TargetPoint为需要进行聚类筛选的所有定位点位置坐标，idx为同一类标志
TargetPoint_Cluster=TargetPoint((idx~=0),:);%筛选聚类后的结果% 可视化聚类结果
figure(6)
plot(TargetPoint_Cluster(:,2),TargetPoint_Cluster(:,1),'ro');
title('DBSCAN 聚类结果');
xlabel('横坐标/km');
ylabel('纵坐标/km');

1.1 DBSCAN密度聚类算法Matlab程序

输入参数：

X：测量定位点的集合，单位km；

epsilon：ε邻域的半径，单位km；

minPts：最小点数，用于判为同一类的最少点数量；

输出参数：

idx：X数组中，属于同一类的索引标志。值为1说明是同一类。

isnoise：X数组中，属于噪声点的索引标志。值为1说明是噪声类。


function [IDX, isnoise]=DBSCAN(X,epsilon,MinPts)    % DBSCAN聚类函数C=0;                       % 统计簇类个数，初始化为0n=size(X,1);               % 把矩阵X的行数数赋值给n，即一共有n个点IDX=zeros(n,1);            % 定义一个n行1列的矩阵D=pdist2(X,X);             % 计算(X,X)的行的距离visited=false(n,1);        % 创建一维的标记数组，全部初始化为false，代表还未被访问isnoise=false(n,1);        % 创建一维的异常点数组，全部初始化为false，代表该点不是异常点for i=1:n                  % 遍历1~n个所有的点if ~visited(i)         % 未被访问，则执行下列代码visited(i)=true;   % 标记为true，已经访问Neighbors=RegionQuery(i);     % 查询周围点中距离小于等于epsilon的个数if numel(Neighbors)<MinPts    % 如果小于MinPts% X(i,:) is NOISE        isnoise(i)=true;          % 该点是异常点else              % 如果大于MinPts,且距离大于epsilonC=C+1;        % 该点又是新的簇类中心点,簇类个数+1ExpandCluster(i,Neighbors,C);    % 如果是新的簇类中心，执行下面的函数endendend                    % 循环完n个点，跳出循环function ExpandCluster(i,Neighbors,C)    % 判断该点周围的点是否直接密度可达IDX(i)=C;                            % 将第i个C簇类记录到IDX(i)中k = 1;                             while true                           % 一直循环j = Neighbors(k);                % 找到距离小于epsilon的第一个直接密度可达点if ~visited(j)                   % 如果没有被访问visited(j)=true;             % 标记为已访问Neighbors2=RegionQuery(j);   % 查询周围点中距离小于epsilon的个数if numel(Neighbors2)>=MinPts % 如果周围点的个数大于等于Minpts，代表该点直接密度可达Neighbors=[Neighbors Neighbors2];   %#ok  % 将该点包含着同一个簇类当中endend                              % 退出循环if IDX(j)==0                     % 如果还没形成任何簇类IDX(j)=C;                    % 将第j个簇类记录到IDX(j)中end                              % 退出循坏k = k + 1;                       % k+1,继续遍历下一个直接密度可达的点if k > numel(Neighbors)          % 如果已经遍历完所有直接密度可达的点，则退出循环break;endendend                                      % 退出循环function Neighbors=RegionQuery(i)        % 该函数用来查询周围点中距离小于等于epsilon的个数Neighbors=find(D(i,:)<=epsilon);endend

1.2 效果展示

二、最小二乘法求定位点估计值

2.1 算法原理

第一步：由测向原理出发，化简为矩阵形式

测向线从测量节点指向目标定位点，如图所示，则由直角坐标系下直线的倾斜角定义可得：

将其化简为AX=B的矩阵形式，如下：

其中，A、X和B矩阵如下所示：

第二步：引入误差项并用最小二乘法求解

由于测向存在误差，因此代入各测量节点坐标和测向值时，上式不能成立。由此引入误差矩阵E（矩阵维度：与矩阵B相同），则上面的矩阵表达式更正为：

最小二乘法原理：认为该误差的平方和最小时的定位点，为最佳的定位点估计。

该误差的平方和为误差矩阵E的二范数的平方，则

为求得误差平方和的最小值，对其求导数，导数为0时的X即为最佳的定位点估计。

则导数为0时的X为：

矩阵求导的部分运算法则如下：

矩阵转置运算法则如下：

2.2 最小二乘法Matlab代码

输入参数：

AOA：测向线对应的地理正北方位，单位度；

R：测量节点的坐标，单位自定；

输出参数：

LocFinalPoint：估计的定位点位置坐标，单位与R保持一致。

%% 最小二乘法求最终定位点
for i=1:size(AOA,2)A(i,:)=[-tand(Azimuth2ElevationAngle(AOA(i))),1];%倾斜角B(i)=R(i,2)-tand(Azimuth2ElevationAngle(AOA(i)))*R(i,1);
endLocFinalPoint=inv(A'*A)*(A'*B');%最小二乘计算结果

2.3 效果展示

真实坐标点位置：(100,300)

三、利用一元3次方程拟合时间-角度曲线

输入参数：

AOA：测向线对应的地理正北方位，单位度；

输出参数：

yfit：一元3次方程拟合后，对应的地理正北方位，单位度；

%% 三次方程拟合时间-角度曲线后，利用最小二乘法求最终定位点
% 使用polyfit进行线性拟合，1是线性拟合的参数
p = polyfit(1:60,Azimuth2ElevationAngle(AOA), 3);% 使用polyval计算拟合值
yfit = polyval(p, 1:60);