三维三边定位（Trilateration）实现结果异常，求代码排查

我正在实现一个三维三边定位的功能，这个函数接收三个基站的3D坐标以及目标点到每个基站的距离，需要返回目标点的三维空间位置。我写的Python代码如下（依赖numpy）：

import numpy as np

def trilateration(P1, P2, P3, r1, r2, r3):
    p1 = np.array([0, 0, 0])
    p2 = np.array([P2[0] - P1[0], P2[1] - P1[1], P2[2] - P1[2]])
    p3 = np.array([P3[0] - P1[0], P3[1] - P1[1], P3[2] - P1[2]])
    v1 = p2 - p1
    v2 = p3 - p1
    Xn = (v1)/np.linalg.norm(v1)
    tmp = np.cross(v1, v2)
    Zn = (tmp)/np.linalg.norm(tmp)
    Yn = np.cross(Xn, Zn)
    i = np.dot(Xn, v2)
    d = np.dot(Xn, v1)
    X = ((r1**2)-(r2**2)+(d**2))/(2*d)
    Y = (((r1**2)-(r3**2)+(i**2)+(j**2))/(2*j))-((i/j)*(X))
    Z1 = np.sqrt(r1**2-X**2-Y**2)
    Z2 = np.sqrt(r1**2-X**2-Y**2)*(-1)
    K1 = P1 + X*Xn + Y * Yn + Z1 * Zn
    K2 = p1 + X * Xn + Y * Yn - Z2 * Zn
    return K1

我用以下测试用例验证功能：

• 基站坐标：P1=(2,2,0)、P2=(3,3,0)、P3=(1,4,0)
• 目标点到基站的距离：r1=1、r2=1、r3=1.4142（这个值是√2的近似）
• 预期返回的目标点坐标：(2,3,0)

但实际运行后，函数返回的结果是 [3.33253331 1.66746669 1.33373281]，和预期完全不符。有没有大佬能帮忙找出代码里的问题？

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