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

时间：2024-04-03 21:50:54

通过测向交叉定位的方法，按理只需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;                       % 统计簇类个数，初始化为0
    
    n=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,且距离大于epsilon
                C=C+1;        % 该点又是新的簇类中心点,簇类个数+1
                ExpandCluster(i,Neighbors,C);    % 如果是新的簇类中心，执行下面的函数
            end
            
        end
    
    end                    % 循环完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  % 将该点包含着同一个簇类当中
                end
            end                              % 退出循环
            if IDX(j)==0                     % 如果还没形成任何簇类
                IDX(j)=C;                    % 将第j个簇类记录到IDX(j)中
            end                              % 退出循坏
            
            k = k + 1;                       % k+1,继续遍历下一个直接密度可达的点
            if k > numel(Neighbors)          % 如果已经遍历完所有直接密度可达的点，则退出循环
                break;
            end
        end
    end                                      % 退出循环
    
    function Neighbors=RegionQuery(i)        % 该函数用来查询周围点中距离小于等于epsilon的个数
        Neighbors=find(D(i,:)<=epsilon);
    end
 
end

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);
end

LocFinalPoint=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);

3.1 效果展示

从下图看出，拟合后的时间-角度曲线更加靠近真实定位点对应的时间-角度曲线，使用拟合后的时间-角度曲线，利用最小二乘法估计的定位点坐标更加接近真实定位点。

相关文章