Advertisement
Guest User

Untitled

a guest
Apr 20th, 2019
115
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 0.83 KB | None | 0 0
  1. def trilateration(anchors: dict):
  2. """
  3. More on: https://stackoverflow.com/questions/2507148/determining-the-coordinates-of-a-point-based-on-its-known-difference-from-three?fbclid=IwAR2HaqTalJf1GPvrp9FaraBdzo2Y1qlmmr3fI18mPvA6a8pwF96dZ74bCs0
  4. :param anchors: dict of anchors with radius for each one
  5. :return: predicted x, y coordinates
  6. """
  7. p1, p2, p3 = anchors.keys()
  8. r1, r2, r3 = anchors.values()
  9. A = p1[0] - p2[0]
  10. B = p1[1] - p2[1]
  11. D = p1[0] - p3[0]
  12. E = p1[1] - p3[1]
  13.  
  14. T = (r1 ** 2 - p1[0] ** 2 - p1[1] ** 2)
  15. C = (r2 ** 2 - p2[0] ** 2 - p2[1] ** 2) - T
  16. F = (r3 ** 2 - p3[0] ** 2 - p3[1] ** 2) - T
  17.  
  18. # Ax + By = C / 2 // this is equation 'l'
  19. # Dx + Ey = F / 2 // this is equation 'n'
  20.  
  21. # Cramer's Rule
  22. Mx = (C * E - B * F) / 2
  23. My = (A * F - D * C) / 2
  24. M = A * E - D * B
  25.  
  26. x = Mx / M
  27. y = My / M
  28.  
  29. return x, y
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement