Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- def trilateration(anchors: dict):
- """
- More on: https://stackoverflow.com/questions/2507148/determining-the-coordinates-of-a-point-based-on-its-known-difference-from-three?fbclid=IwAR2HaqTalJf1GPvrp9FaraBdzo2Y1qlmmr3fI18mPvA6a8pwF96dZ74bCs0
- :param anchors: dict of anchors with radius for each one
- :return: predicted x, y coordinates
- """
- p1, p2, p3 = anchors.keys()
- r1, r2, r3 = anchors.values()
- A = p1[0] - p2[0]
- B = p1[1] - p2[1]
- D = p1[0] - p3[0]
- E = p1[1] - p3[1]
- T = (r1 ** 2 - p1[0] ** 2 - p1[1] ** 2)
- C = (r2 ** 2 - p2[0] ** 2 - p2[1] ** 2) - T
- F = (r3 ** 2 - p3[0] ** 2 - p3[1] ** 2) - T
- # Ax + By = C / 2 // this is equation 'l'
- # Dx + Ey = F / 2 // this is equation 'n'
- # Cramer's Rule
- Mx = (C * E - B * F) / 2
- My = (A * F - D * C) / 2
- M = A * E - D * B
- x = Mx / M
- y = My / M
- return x, y
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement