pinv.py 文件源码

def tdoa_to_position(time_diff, sensor_pos):
    sensors = len(time_diff)
    if len(time_diff) != len(sensor_pos):
        raise Exception('Channel number mismatch.')

    dist_diff = []
    for x in time_diff:
        dist_diff.append(x * sound_speed)

    inhom_mat = np.mat(np.zeros([sensors - 2, 1]))
    coeff_mat = np.mat(np.zeros([sensors - 2, 3]))
    for i in range(2, sensors):
        args = dist_diff[1], dist_diff[i], \
               sensor_pos[0], sensor_pos[1], sensor_pos[i]
        coeff_mat[i - 2, :] = coeff(*args)
        inhom_mat[i - 2] = -inhom(*args)

    x_sol = lin.pinv(coeff_mat) * inhom_mat
    return x_sol[0, 0], x_sol[1, 0], x_sol[2, 0]