GPS坐标的多边定位

Question

我有N个GPS坐标和N个距离，用于确定一个未知位置。

我的第一种方法是使用三个点和三边测量法，就像这里描述的那样。这种方法已经相当准确（最佳误差约为5公里），但我想改进它并增强其稳健性。 由于给定的距离本来就不太准确，所以我考虑使用多次测量和多边测量。 然而，尽管我提供了超过3个点/距离（测试了最多6个），但结果表明，这种方法远不如三边测量法准确（最佳误差约为100公里），现在我想知道是否有人知道我可能犯了什么错误。

简而言之，我进行多边测量的方法如下：

1. 将所有坐标转换为ECEF
2. 按照wikipedia中的Eq.7描述构建矩阵
3. 使用SVD找到最小化器
4. 由于解决方案仅相对比例，因此我使用根查找方法确定归一化，以便将转换回LLA的坐标结果为高度为0（我的初始假设是所有坐标都在零高度处）
5. 转换回LLA

LLA / ECEF转换已经双重检查并且正确。 我用欧几里得坐标（和精确距离）检查了步骤2和3，并且似乎是正确的。 我自己想出了第4步，我不知道这是否是一个好方法，所以欢迎提出建议。

+++更新

我用Python编写了示例代码，以说明问题 带有一些基本事实。 三角定位最接近400m，而 多边形区域范围在10-130km之间。 由于长度，我将其放在ideone处

- zerm

---

http://ideone.com/umq2M 的代码不见了？您能再分享一下吗？我遇到了一些类似的问题！ - Lee Armstrong

@LeeArmstrong 我需要检查一下我旧电脑上是否还有它，但不确定我是否有“损坏”的示例。请看下面我的答案，其中包含最终有效的 Python 代码。 - zerm

Answer 1

最终，我自己解决了这个问题——或者至少显著提高了准确性。

维基百科上描述的方法（公式7）似乎不太适用于此应用程序，但在这种情况下，它已经容易得多了。

考虑维基百科上的公式6，我们可以大大简化它：R_0可以猜测为地球半径，因为ECEF坐标的原点位于地球中心。因此，没有必要将所有内容都移动到一个点成为原点，我们可以使用所有N方程。

在Python中，使用P表示ECEF坐标数组，dists表示到这些点的距离，所有问题都可以归结为：

R = 6378137 # Earth radius in meters
A = []
for m in range(0,len(P)):
    x = P[m][0]
    y = P[m][1]
    z = P[m][2]
    Am = -2*x
    Bm = -2*y
    Cm = -2*z
    Dm = R*R + (pow(x,2)+pow(y,2)+pow(z,2)) - pow(dists[m],2)
    A += [[Am,Bm,Cm,Dm]]
# Solve using SVD
A = numpy.array(A)
(_,_,v) = numpy.linalg.svd(A)
# Get the minimizer
w = v[3,:]
w /= w[3] # Resulting position in ECEF

采用这种方法，我所描述的第4步不再必要。事实上，它甚至会使解决方案更糟。

现在，精度在2公里到275米之间，大多数情况下比误差为464米的“最佳”三边测量法更好。

- zerm

---

为什么这个被踩了？请问您能否友好地解释一下？这种方法有什么问题吗？我有遗漏什么吗？ - zerm

Answer 2

一些评论：

1) 你已经根据精确答案检查了一些步骤。我建议你创建一些玩具问题，加入已知数量的随机噪声到观测中。由于在这种情况下你知道正确答案，你可以看到误差传播的情况。如果你的方法在这里表现良好但在实际数据上表现不佳，那么你可能需要考虑在现实生活中的可怕行为，例如一个或几个距离严重错误。

2) 我不知道为什么你的解决方案只有比例尺度，因为底层数据已经被适当地缩放 - 如果我带着剪成长度并将它们系在固定点上，就不会有任何歧义。当您使用SVD来解决方程（7）时，您是否像www.cse.unr.edu/~bebis/MathMethods/SVD/lecture.pdf中所做的那样得到最小二乘解？这应该给出x、y和z而没有歧义。

3) 我不太确定观测误差如何通过（7）起作用。我不喜欢所有的分割，首先是这个原因。值得写下一个关于测量距离和计算距离之间差异平方和的方程，给出未知位置的x、y、z，然后对x、y、z进行最小化。维基百科文章由于成本而放弃了这种方法，但它可能会给你一个更准确的答案，计算和比较这个答案即使你不能在实践中使用这种方法也可能告诉你一些东西。

- mcdowella

---

谢谢，这些是一些好的观点，我会进一步验证。到目前为止有一些评论：1）我现在有了一个没有噪音设置的“真实”玩具案例。在最好的情况下，我得到了5-10公里的误差，但是如果我添加到一个远离位置的另一个距离，误差就会增加到150公里。2）我正在使用齐次线性方程组，它始终是按比例缩放的（如果U是解决方案，则任何标量r的rU也是解决方案）。我想我应该发布一些代码让其他人也可以玩耍。 - zerm

关于eqn（7），有些可疑。文章中说它是齐次的，这确实会引入一个尺度的歧义，但它具有非零常数系数Dm。无论如何，你最好找到（x，y，z）来最小化预测距离和测量距离之间平方差的总和，因为这是假设正常误差的最大似然估计 - 请参见http://en.wikipedia.org/wiki/Maximum_likelihood以了解理论论证，这可能具有良好的性质。 - mcdowella

公式7实际上在我的情况下并不太适用。我可以进行修改，因为我知道ECEF坐标系的原点是地球中心（因此它们的长度是常数）。尽管如此，我现在得到了约15公里的误差，但非常稳健。 - zerm

希望能比3点情况做得更好。我看了你的代码，但我需要安装Python和numpy才能使用它（我主要在家里使用Java，在工作中使用C++/Java）。如果你有一点运气并且不介意测试需要多长时间，你可以轻松地尝试最小化平方距离差异-尝试Nelder-Mead单纯形优化算法-它只需要函数值，而不需要导数-参见例如http://docs.scipy.org/doc/scipy/reference/generated/scipy.optimize.fmin.html#scipy.optimize.fmin。Torczon单纯形会更好，但找不到适用于Python的版本。 - mcdowella

是的，检查我的答案，我为自己提供了一种解决方案，该方法在许多情况下应该比3点法更好。关于Nelder-Mead，那也是我最喜欢的方法之一。但是这个问题需要最小二乘/奇异值分解，显然现在这种方法已经很好用了。虽然我想还有改进的空间。 - zerm

Answer 3

我按照@zerm上面展示的代码进行了操作，效果还不错（黄色是从3个塔计算出来的点）。结果在Folium片段中展示。使用linalg.SVD进行多基站定位

然而，当我按照@mcdowella建议的更改（＃2）使用MxN系统解的最小二乘法算法时，结果要好得多。使用最小二乘法MxN进行多基站定位

这是修改后的代码：

A = []
b = []
for m in range(0,len(P)):
    x = P[m][0]
    y = P[m][1]
    z = P[m][2]
    Am = 2*x
    Bm = 2*y
    Cm = 2*z
    Dm = R*R + (pow(x,2)+pow(y,2)+pow(z,2)) - pow(dists[m],2)
    A += [[Am,Bm,Cm]]
    b += [[Dm]]
# Solve using Least Squares of an MxN System
# A*x = b --> x = (ATA)_inv.AT.b = A+.b
A = np.array(A)
b = np.array(b)
AT = A.T
ATA = np.matmul(AT,A)
ATA_inv = np.linalg.inv(ATA)
Aplus = np.matmul(ATA_inv,AT)
x = np.matmul(Aplus,b)
# convert back to lat/long from ECEF
# convert to degrees
lat = math.degrees(math.asin(x[2] / R))
lon = math.degrees(math.atan2(x[1],x[0]))

我仍在探索其他多边定位方法，但这篇文章确实让我理解了N点MLAT的基础知识。谢谢！

- BruskiDuski

Answer 4

为了改进被接受的答案，提高SVD解决方案的方法之一是考虑地球半径随纬度变化的影响；这特别影响高度估计，但也对纬度和经度产生了一些连锁反应。 "简单"的解决方案是使用R的平均值，根据维基百科的数据，该值为6371008.8米，而不是6378137米。

更准确的估计是根据纬度调整R：

def EarthRadiusAtLatitude(lat):
    rlat = np.deg2rad(lat)

    a = np.float64(6378137.0)
    b = np.float64(6356752.3)

    rad = np.sqrt(((a*a*np.cos(rlat))**2 + (b*b*np.sin(rlat))**2) /
                  ((a*np.cos(rlat))**2 + (b*np.sin(rlat))**2))
    return rad

然后根据初始点之一的纬度设置R。或者，如果纬度变化很大，您可以基于R的估计值计算SVD，并使用初步解的纬度来使用更接近的R的估计值进行求解。

在进行此调整后，在我对LTE eNodeB定时提前值为基础的构造示例和“真实世界”数据的实验中，SVD解通常在纬度和经度上与迭代优化（即最小化距离残差）的解相比仅相差一秒，除了某些退化情况外。

- Chris Lawrence