基于4个经纬度点的TDOA定位代码误差排查请求

我正在基于经纬度坐标开展TDOA（到达时间差）定位研究，参考论文实现了相关代码。测试范围为纬度(0, -20)、经度(-70, -50)的网格点，给时间函数加入20微秒随机误差后，得到的定位误差与预期不符，请求排查代码错误。

距离计算函数（dis） #

原代码存在缩进错误，修正缩进后如下：

import numpy as np
import matplotlib.pyplot as plt

# 传感器位置
X = np.array([-67.02, -55.09, -58.28, -65.51])
Y = np.array([-5.39, -12.79, -3.79, -13.56])


def dis(lat1, lon1, lat2, lon2):
    pi2 = np.pi/2
    rad = np.pi/180
    grd = 180/np.pi
    degtokm = 111.195

    latnp = pi2        # 北极纬度

    lon0 = lon1*rad
    loo1 = lon2*rad

    lat0 = lat1*rad
    laa1 = lat2*rad

    c1 = loo1 - lon0

    if c1 != 0:
        c1 = 1./np.tan(c1/2.)

        lla1 = laa1 - lat0
        llb1 = laa1 + lat0
        
        if lla1 != 0:
            yxa1 = c1 * np.sin(lla1/2.) / np.cos(llb1/2.)
            yxb1 = c1 * np.cos(lla1/2.) / np.sin(llb1/2.)

            yxa1 = np.arctan(yxa1)
            yxb1 = np.arctan(yxb1)

            r1 = np.tan(lla1/2.) * np.sin(yxb1) / np.sin(yxa1)
            r1 = np.arctan(r1) * 2.
            
        if lla1 == 0:
            lla1 = latnp - laa1
            c1 = loo1 - lon0
            r1 = np.sin(c1) * np.sin(lla1)
            r1 = np.arcsin(r1)
    if c1 == 0: 
        r1 = abs(laa1 - lat0)

    r1 = r1 * grd  # 转换为度数
    r1 = abs(r1 * degtokm)   # 转换为公里

    return r1

带随机误差的时间计算函数（tiempo） #

def tiempo(yr, xr, Ye, Xe, j):
    mean, std = 0, 20e-6                        # 均值和标准差
    delta_t = np.random.normal(mean, std, 400)  # 生成随机值
    delta_t = delta_t.reshape(100,4)            

    c = 300000                                  # 光速 3x10^5 km/s

    d1 = dis(yr, xr, Ye[0], Xe[0])
    d2 = dis(yr, xr, Ye[1], Xe[1])
    d3 = dis(yr, xr, Ye[2], Xe[2])
    d4 = dis(yr, xr, Ye[3], Xe[3])

    time = np.asarray([d1,d2,d3,d4])/c + delta_t[j,:]

    return time

线性化求解定位函数（linearize） #

def linearize(x1,x2,x3,x4,y1,y2,y3,y4,d1,d2,d3,d4):
    # 灵敏度矩阵
    A = np.array([[x2 - x1, y2 - y1],   
                  [x3 - x1, y3 - y1],
                  [x4 - x1, y4 - y1]])

    # 数据矩阵
    b = (np.array([[d1**2 - d2**2 + (x2-x1)**2 + (y2-y1)**2],   
                   [d1**2 - d3**2 + (x3-x1)**2 + (y3-y1)**2],
                   [d1**2 - d4**2 + (x4-x1)**2 + (y4-y1)**2]]))/2

    # 求解
    B = np.dot(A.T,A)
    C = np.linalg.inv(B)  # 求矩阵A的逆（实际建议用伪逆提升稳定性）

    D = np.dot(C,A.T)

    p0 = np.asarray([[x1],[y1]])

    x, y = np.dot(D,b) + p0

    return x, y

主函数（main） #

def main(yr, xr, ry, rx, j):
    time = tiempo(yr, xr, ry, rx, j) 

    c = 300000                    # 光速 3x10^5 km/s

    # 转换系数：公里转度数
    N = 1 / 111.195       

    # 计算距离并转换为度数
    d1 = time[0] * c * N
    d2 = time[1] * c * N
    d3 = time[2] * c * N 
    d4 = time[3] * c * N

    # 传感器坐标
    x1, x2, x3, x4 = rx[0], rx[1], rx[2], rx[3]
    y1, y2, y3, y4 = ry[0], ry[1], ry[2], ry[3]

    # 求解定位坐标
    x, y = linearize(x1,x2,x3,x4,y1,y2,y3,y4,d1,d2,d3,d4)

    # 计算定位误差
    e = dis(yr,xr,y,x)

    return e

误差矩阵计算代码 #

v_er = np.zeros([21,21])

xr, yr = -70, 0
for i in range(0,21):
    yr = 0
    yr = yr - i

    for j in range(0,21):
        xr = -70
        xr = xr + j
    
        er = np.zeros(100)
    
        for k in range(0,100):
            er[k] = main(yr, xr, Y, X, k)
        
    v_er[i,j] = np.mean(er)

核心错误排查关键点 #

• 函数缩进错误：dis函数内部代码完全没有缩进，属于语法错误，会直接导致代码无法运行，必须修正缩进。
• TDOA与三边测量逻辑混淆：当前linearize函数实现的是三边测量（基于绝对距离）的求解逻辑，但TDOA的核心是利用距离差（由时间差推导），这是根本性逻辑错误。TDOA的线性化方程组需基于d_i - d_1 = c*(t_i - t_1)推导，而非绝对距离的平方差。
• 时间误差生成逻辑错误：tiempo函数每次调用都会重新生成400个随机数，导致不同模拟的误差不独立，且当j≥100时会出现索引越界。建议改为在函数外一次性生成所有模拟的误差矩阵，或每次仅生成当前模拟的4个误差值。
• 单位混淆错误：经纬度是角度单位，不能直接当作平面坐标代入linearize函数的平面几何公式计算。需先将经纬度投影到平面坐标系（如UTM），或在球面模型下推导TDOA的求解公式。
• 循环缩进错误：误差矩阵赋值v_er[i,j] = np.mean(er)的缩进位置错误，被放在j循环外部，导致每个i对应的所有j位置都会被最后一个j的误差均值覆盖，完全错误地生成了误差矩阵。
• 球面距离计算错误：dis函数的球面距离实现逻辑复杂且存在边界条件错误，建议改用成熟的Haversine公式计算球面距离，避免自定义实现带来的误差。

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