已知航向而非距离的2D平面超定多点定位求解方法咨询

核心问题建模 #

先将航向约束转化为线性方程：对于每个基站 ( P_{ti} = (x_i, y_i) )，待测点 ( X=(X_x,X_y) ) 指向该基站的航向为 ( \theta_i )（先将角度转为弧度），对应直线的斜率为 ( k_i = \tan(\theta_i) )，直线方程可整理为：
[ k_i X_x - X_y = k_i x_i - y_i ]
若航向为 ( 90^\circ ) 或 ( 270^\circ )（斜率不存在），则直线方程简化为 ( X_x = x_i )，同样可转化为线性约束 ( 1 \cdot X_x + 0 \cdot X_y = x_i )。

最终N个航向会形成超定线性方程组 ( A\mathbf{x} = \mathbf{b} )，其中 ( \mathbf{x} = [X_x, X_y]^T )，矩阵A的每一行对应单个航向的约束系数，向量b对应方程右侧的常数项。

鲁棒求解方法 #

1. 最小二乘法（OLS/WLS） #

• 普通最小二乘法（OLS）：直接最小化所有方程的残差平方和，解为 ( \mathbf{x} = (A^T A)^{-1} A^T \mathbf{b} )。计算效率高，适合观测误差均匀且无异常值的场景，是超定方程组的基础求解方式。
• 加权最小二乘法（WLS）：若已知各航向观测的误差方差，可给每个方程赋予对应权重（方差的倒数），进一步提升求解精度，公式为：
  [ \min_{\mathbf{x}} (A\mathbf{x} - \mathbf{b})^T W (A\mathbf{x} - \mathbf{b}) ]
  其中W为对角权重矩阵。

2. 鲁棒回归算法（应对异常值） #

当存在少数误差极大的异常航向时，OLS会被严重带偏，此时可采用：

• RANSAC算法：随机选取2个航向生成候选解，统计其他航向直线与该解的距离在阈值内的数量（内点），保留内点最多的候选解，再用所有内点重新做OLS优化。能有效过滤粗差，适合存在异常观测的场景。
• 最小中值平方（LMedS）：最小化所有残差的中位数平方，无需设置阈值，对异常值的鲁棒性强于OLS，计算复杂度略高但结果稳定。

3. 迭代加权最小二乘法（IRLS） #

假设航向误差服从重尾分布（如学生t分布），通过迭代更新每个观测的权重，逐步降低异常值的影响，最终收敛到鲁棒性更强的解，兼顾精度与抗干扰能力。

现有思路的不足对比 #

• 仅用两个航向求解：完全丢弃了其余观测的信息，若所选航向本身存在误差，结果偏差会很大。
• 遍历所有航向对取中位数：效率低下（N较大时组合数为O(N²)），且未利用所有观测的整体约束关系，对异常值的鲁棒性不如RANSAC或LMedS。

实现步骤建议 #

1. 将所有航向角度转换为弧度，单独处理垂直方向的特殊情况；
2. 构建超定线性方程组的矩阵A和向量b；
3. 先用OLS得到初始解，若怀疑存在异常值，再用RANSAC或LMedS进行鲁棒求解；
4. 计算每个观测的残差，剔除残差过大的异常航向，重新求解以优化结果。

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