定位算法——多边测量法及MATLAB编程

三边测距定位算法简介

 三边测量法是多边测量法的低级应用，即已知三个点的横纵坐标和与未知点的距离$d$，如下图所示：

 如图所示已知$(x_1,y_1)$，$(x_2,y_2)$，$(x_3,y_3)$和$d_1$，$d_2$，$d_3$就可以求出$(x_i,y_i)$。

多边测量法公式推导

 1.建立已知节点与未知节点的距离方程组
$$\{\begin{array}{l}(x_1-x{)}^2+(y_1-y{)}^2=d_1^2\\ ⋮\\ (x_n-x{)}^2+(y_n-y{)}^2=d_n^2\end{array}$$
 2.上式方程组为非线性方程组，用方程组前$n-1$个方程减去第$n$个方程后，得到线性化的方程：
$$AX=B$$
其中：
$$A=\left[\begin{array}{cc}2(x_1-x_n)& 2(y_1-y_n)\\ ⋮& ⋮\\ 2(x_{n-1}-x_n)& 2(y_{n-1}-y_n)\end{array}\right]$$
$$B=\left[\begin{array}{c}{x}_1^2-x_n^2+y_1^2-y_n^2+d_n^2-d_1^2\\ ⋮\\ x_{n-1}^2-x_n^2+y_{n-1}^2-y_n^2+d_n^2-d_{n-1}^2\end{array}\right]$$
 3.用最小二乘法求解上边方程得：
$$X=(A^{T}A{)}^{-1}{A}^{T}b$$

三边测距定位算法MATLAB程序

%% x输入为横坐标矩阵 yx输入为纵坐标矩阵 distance为距离矩阵，距离矩阵里可能包含0值，即未给出距离的值
%%  xx，yy 分别为求得的未知点横纵坐标
function [xx,yy]=Trilat(x,y,distance)
[sz1,sz2]=size(distance);%获取距离矩阵大小
%%处理数据
for i=1:sz1
    n=1;
    for m=1:sz2
        if distance(i,m)~=0%判断距离矩阵是否为0
            x1(i,n)=x(m);%不等于0时取对应的x,y值
            y1(i,n)=y(m);
            d(i,n)=distance(i,m);
            n=n+1;         
        end
    end
end
[sz3,sz4]=size(x1);%获取处理后距离矩阵大小
%%根据三边测量法求解位置节点
for i=1:sz1
n=1;
for o=1:sz4-2
    for p=o+1:sz4-1
        for q=p+1:sz4
            A=[2*(x1(i,o)-x1(i,q)),2*(y1(i,o)-y1(i,q));2*(x1(i,p)-x1(i,q)),2*(y1(i,p)-y1(i,q))];
            B=[x1(i,o)^2-x1(i,q)^2+y1(i,o)^2-y1(i,q)^2+d(i,q)^2-d(i,o)^2;x1(i,p)^2-x1(i,q)^2+y1(i,p)^2-y1(i,q)^2+d(i,q)^2-d(i,p)^2];
            X=(A'*A)^(-1)*A'*B;            
            xx(i,n)=X(1);
            yy(i,n)=X(2);
            n=n+1;
        end
    end    
end
end

end

 多边测量法代码可私信我。创作不易，请大家多多支持！