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.')

    inhom_mat = np.mat(np.zeros([sensors - 1, 1]))
    coeff_mat = np.mat(np.zeros([sensors - 1, 3]))
    for i in range(1, sensors):
        coeff_mat[i - 1, :] = diff(sensor_pos[i], sensor_pos[0])
        inhom_mat[i - 1] = time_diff[i] * sound_speed

    angle_vec = lin.pinv(coeff_mat) * inhom_mat

    return normalize(angle_vec)