import math
def range_to(x1, x2):
dx = x1[0] - x2[0]
dy = x1[1] - x2[1]
print('dx',dx,dy)
return math.sqrt(dx*dx + dy*dy)
s1 = (0., 2.) # satellite 1
s2 = (1., 2.) # satellite 2
p = (-0.5, 1.) # platform postion
s1 = (0., 200.) # satellite 1
s2 = (100., 220.) # satellite 2
p = (10, 1.) # platform postion
r1 = range_to(s1, p)
r2 = range_to(s2, p)
x1, y1 = s1[0], s1[1]
x2, y2 = s2[0], s2[1]
A = -(y2 - y1) / (x2 - x1)
B = (r1**2 - r2**2 - x1**2 - y1**2 + x2**2 + y2**2) / (2*(x2 - x1))
a = 1 + A**2
b = -2*A*x1 + 2*A*B - 2*y1
c = x1**2 - 2*x1*B + y1**2 - r1**2
y = (-b - math.sqrt(b**2 - 4*a*c)) / (2*a)
x = A*y + B
print('p', x,y)
print('err', x-p[0], y-p[1])
dx -10.0 199.0 dx 90.0 219.0 p 10.051726583768032 0.7413670811596572 err 0.05172658376803163 -0.2586329188403428
A
-0.2