基于含误差近似距离的经纬度三边定位最优解求解

问题说明 #

通过3个已知经纬度固定点的距离测量值近似计算信标位置，距离读数存在最大1km误差，测量值范围1-100km。基于ECEF坐标系实现的三边定位代码因测量误差，计算z值时常出现负数平方根，返回NaN，无法得到有效结果。原代码如下：

public static class Trilateration
{
    public static GeoLocation Compute(DistanceReading point1, DistanceReading point2, DistanceReading point3)
    {
        // not my code :P
        // assuming elevation = 0
        const double earthR = 6371d;

        //using authalic sphere
        //if using an ellipsoid this step is slightly different
        //Convert geodetic Lat/Long to ECEF xyz
        //   1. Convert Lat/Long to radians
        //   2d. Convert Lat/Long(radians) to ECEF
        double xA = earthR * (Math.Cos(Radians(point1.GeoLocation.Latitude)) * Math.Cos(Radians(point1.GeoLocation.Longitude)));
        double yA = earthR * (Math.Cos(Radians(point1.GeoLocation.Latitude)) * Math.Sin(Radians(point1.GeoLocation.Longitude)));
        double zA = earthR * Math.Sin(Radians(point1.GeoLocation.Latitude));

        double xB = earthR * (Math.Cos(Radians(point2.GeoLocation.Latitude)) * Math.Cos(Radians(point2.GeoLocation.Longitude)));
        double yB = earthR * (Math.Cos(Radians(point2.GeoLocation.Latitude)) * Math.Sin(Radians(point2.GeoLocation.Longitude)));
        double zB = earthR * Math.Sin(Radians(point2.GeoLocation.Latitude));

        double xC = earthR * (Math.Cos(Radians(point3.GeoLocation.Latitude)) * Math.Cos(Radians(point3.GeoLocation.Longitude)));
        double yC = earthR * (Math.Cos(Radians(point3.GeoLocation.Latitude)) * Math.Sin(Radians(point3.GeoLocation.Longitude)));
        double zC = earthR * Math.Sin(Radians(point3.GeoLocation.Latitude));

        // a 64 bit Vector3 implementation :)
        Vector3_64 P1 = new(xA, yA, zA);
        Vector3_64 P2 = new(xB, yB, zB);
        Vector3_64 P3 = new(xC, yC, zC);

        //from wikipedia
        //transform to get circle 1 at origin
        //ransform to get circle 2d on x axis
        Vector3_64 ex = (P2 - P1).Normalize();
        double i = Vector3_64.Dot(ex, P3 - P1);
        Vector3_64 ey = (P3 - P1 - i * ex).Normalize();
        Vector3_64 ez = Vector3_64.Cross(ex, ey);
        double d = (P2 - P1).Length;
        double j = Vector3_64.Dot(ey, P3 - P1);

        //from wikipedia
        //plug and chug using above values
        double x = (Math.Pow(point1.DistanceKm, 2d) - Math.Pow(point2.DistanceKm, 2d) + Math.Pow(d, 2d)) / (2d * d);
        double y = ((Math.Pow(point1.DistanceKm, 2d) - Math.Pow(point3.DistanceKm, 2d) + Math.Pow(i, 2d) + Math.Pow(j, 2d)) / (2d * j)) - ((i / j) * x);

        // only one case shown here
        double z = Math.Sqrt(Math.Pow(point1.DistanceKm, 2d) - Math.Pow(x, 2d) - Math.Pow(y, 2d));

        //triPt is a vector with ECEF x,y,z of trilateration point
        Vector3_64 triPt = P1 + x * ex + y * ey + z * ez;

        //convert back to lat/long from ECEF
        //convert to degrees
        double lat = Degrees(Math.Asin(triPt.Z / earthR));
        double lon = Degrees(Math.Atan2(triPt.Y, triPt.X));

        return new GeoLocation(lat, lon);
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    private static double Radians(double degrees) =>
        degrees * Math.Tau / 360d;

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    private static double Degrees(double radians) =>
        radians * 360d / Math.Tau;
}

问题根源 #

标准三边定位假设距离完全准确，三个球面会交于一点或两点。但存在测量误差时，三个球面可能无共同交点，导致x² + y² > r₁²，平方根内出现负数，产生NaN。

解决方案 #

1. 最小二乘法（最优近似解） #

最小二乘法是处理带误差三边定位的经典方案，核心是找到使所有测量距离与计算距离的误差平方和最小的点。

C#实现（线性最小二乘）

通过将方程线性化，构建线性方程组并求解伪逆得到最优解，最后投影到地球球面：

public static class LeastSquaresTrilateration
{
    public static GeoLocation Compute(DistanceReading point1, DistanceReading point2, DistanceReading point3)
    {
        const double earthR = 6371d;

        // 转换已知点到ECEF坐标系
        Vector3_64 P1 = ToECEF(point1.GeoLocation, earthR);
        Vector3_64 P2 = ToECEF(point2.GeoLocation, earthR);
        Vector3_64 P3 = ToECEF(point3.GeoLocation, earthR);

        double r1 = point1.DistanceKm;
        double r2 = point2.DistanceKm;
        double r3 = point3.DistanceKm;

        // 构建线性方程组：Ax = b
        double A1 = 2 * (P2.X - P1.X);
        double B1 = 2 * (P2.Y - P1.Y);
        double C1 = 2 * (P2.Z - P1.Z);
        double D1 = (r1 * r1 - r2 * r2) - (P1.X*P1.X + P1.Y*P1.Y + P1.Z*P1.Z - P2.X*P2.X - P2.Y*P2.Y - P2.Z*P2.Z);

        double A2 = 2 * (P3.X - P1.X);
        double B2 = 2 * (P3.Y - P1.Y);
        double C2 = 2 * (P3.Z - P1.Z);
        double D2 = (r1 * r1 - r3 * r3) - (P1.X*P1.X + P1.Y*P1.Y + P1.Z*P1.Z - P3.X*P3.X - P3.Y*P3.Y - P3.Z*P3.Z);

        // 求解最小二乘解
        Vector3_64 solution = SolveLeastSquares(new double[,] { { A1, B1, C1 }, { A2, B2, C2 } }, new double[] { D1, D2 });

        // 将解投影到地球球面（信标在地表附近）
        double norm = Math.Sqrt(solution.X * solution.X + solution.Y * solution.Y + solution.Z * solution.Z);
        Vector3_64 onSphere = new Vector3_64(
            solution.X * earthR / norm,
            solution.Y * earthR / norm,
            solution.Z * earthR / norm
        );

        // 转换回经纬度
        return ToGeoLocation(onSphere, earthR);
    }

    private static Vector3_64 ToECEF(GeoLocation loc, double earthR)
    {
        double latRad = Radians(loc.Latitude);
        double lonRad = Radians(loc.Longitude);
        return new Vector3_64(
            earthR * Math.Cos(latRad) * Math.Cos(lonRad),
            earthR * Math.Cos(latRad) * Math.Sin(lonRad),
            earthR * Math.Sin(latRad)
        );
    }

    private static GeoLocation ToGeoLocation(Vector3_64 ecef, double earthR)
    {
        double lat = Degrees(Math.Asin(ecef.Z / earthR));
        double lon = Degrees(Math.Atan2(ecef.Y, ecef.X));
        return new GeoLocation(lat, lon);
    }

    private static Vector3_64 SolveLeastSquares(double[,] A, double[] b)
    {
        int cols = A.GetLength(1);
        // 计算A^T * A
        double[,] ata = new double[cols, cols];
        for (int i = 0; i < cols; i++)
        {
            for (int j = 0; j < cols; j++)
            {
                double sum = 0;
                for (int k = 0; k < A.GetLength(0); k++)
                    sum += A[k, i] * A[k, j];
                ata[i, j] = sum;
            }
        }

        // 计算A^T * b
        double[] atb = new double[cols];
        for (int i = 0; i < cols; i++)
        {
            double sum = 0;
            for (int k = 0; k < A.GetLength(0); k++)
                sum += A[k, i] * b[k];
            atb[i] = sum;
        }

        // 克莱姆法则求解3x3线性方程组
        double det = ata[0,0]*(ata[1,1]*ata[2,2] - ata[1,2]*ata[2,1]) 
                   - ata[0,1]*(ata[1,0]*ata[2,2] - ata[1,2]*ata[2,0]) 
                   + ata[0,2]*(ata[1,0]*ata[2,1] - ata[1,1]*ata[2,0]);

        double detX = atb[0]*(ata[1,1]*ata[2,2] - ata[1,2]*ata[2,1]) 
                    - ata[0,1]*(atb[1]*ata[2,2] - ata[1,2]*atb[2]) 
                    + ata[0,2]*(atb[1]*ata[2,1] - ata[1,1]*atb[2]);

        double detY = ata[0,0]*(atb[1]*ata[2,2] - ata[1,2]*atb[2]) 
                    - atb[0]*(ata[1,0]*ata[2,2] - ata[1,2]*ata[2,0]) 
                    + ata[0,2]*(ata[1,0]*atb[2] - atb[1]*ata[2,0]);

        double detZ = ata[0,0]*(ata[1,1]*atb[2] - atb[1]*ata[2,1]) 
                    - ata[0,1]*(ata[1,0]*atb[2] - atb[1]*ata[2,0]) 
                    + atb[0]*(ata[1,0]*ata[2,1] - ata[1,1]*ata[2,0]);

        return new Vector3_64(detX / det, detY / det, detZ / det);
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    private static double Radians(double degrees) => degrees * Math.Tau / 360d;

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    private static double Degrees(double radians) => radians * 360d / Math.Tau;
}

2. 原代码临时修复（非最优） #

若想继续使用原算法，可在计算z值前判断平方根内的值，将负数截断为0，避免NaN：

// 替换原代码中的z计算行
double zSquared = Math.Pow(point1.DistanceKm, 2d) - Math.Pow(x, 2d) - Math.Pow(y, 2d);
double z = Math.Sqrt(Math.Max(zSquared, 0));

此方法仅能避免NaN，精度较差，仅适用于误差极小的场景。

3. 多测点扩展（可选） #

若能获取更多已知点的距离数据，可使用加权最小二乘法，给误差更小的测量值分配更高权重，进一步提升精度。

注意事项 #

• 已知点分布应尽量均匀，若三点近似共线，定位精度会大幅下降（几何稀释因子GDOP过高）。
• 可替换地球球面模型为椭球模型，进一步提高定位精度，但计算复杂度会增加。
• 实际工程中建议使用成熟数值计算库（如MathNet.Numerics）处理矩阵运算，避免手动实现的误差。

内容的提问来源于stack exchange，提问作者Frederik Hoeft