bearing + angle of elevation calculations

bearing.py

from collections import namedtuple
from math import radians, sin, cos, atan2, sqrt, degrees

# Point is a convenience wrapper to hold the co-ordinates belonging to
# a particular "point" - i.e. the base station location, or the remote
# device location.
#
# Units: lat, long = radians. alt = meters.
Point = namedtuple('Point', ['lat', 'lon', 'alt'])

def point(lat: float, lon: float, alt: float) -> Point:
  return Point(lat=radians(lat), lon=radians(lon), alt=alt)

# Direction is a convenience wrapper that holds the information about the
# "direction" between two Points; these should translate pretty well for
# use with a 2-axis gimbal.
#
# Units: bearing = degrees. elevation, distance = meters.
Direction = namedtuple('Direction', ['bearing', 'elevation', 'distance'])

class Location:
  def __init__(self, location: Point, precision: int = 2) -> None:
    self.location = location
    self.precision = precision


  def update(self, **kwargs):
    update = { k : kwargs[k] for k in kwargs if k in ['lat', 'lon', 'alt'] }
    if not (len(update) > 0 and len(update) < 3):
      raise ValueError(f"location update must consist of a subset of ['lat', 'lon', 'alt'] - got {list(kwargs.keys())}")

    self.location = self.location._replace(**update)


  @staticmethod
  def _calculate(base: Point, remote: Point, precision: int = 2) -> tuple:
    R = 6371000.0 # Radius of the Earth

    delta_lat = remote.lat - base.lat
    delta_lon = remote.lon - base.lon

    # Distance: Haversine Formula
    distance = R * (2 * atan2(
      sqrt((a := sin(delta_lat / 2)**2 + cos(base.lat) * cos(remote.lat) * sin(delta_lon / 2)**2)),
      sqrt(1 - a)
    ))

    # Angle of Elevation
    angle_of_elevation = degrees(atan2(remote.alt - base.alt, distance))

    # Bearing
    y = sin(delta_lon) * cos(remote.lat)
    x = cos(base.lat) * sin(remote.lat) - (sin(base.lat) * cos(remote.lat) * cos(delta_lon))
    bearing = (degrees(atan2(y, x)) + 360) % 360 # @note: is the degrees() call in the correct location here?

    return Direction(
      bearing   = round(bearing, precision),
      elevation = round(angle_of_elevation, precision),
      distance  = round(distance, precision)
    )


  def direction(self, remote: Point) -> Direction:
    return self._calculate(self.location, remote, self.precision)


if __name__ == "__main__":
  def geographic_tests():
    TestCase = namedtuple("TestCase", ["RemotePoint", "ExpectedDirection"])

    for idx, t in enumerate([
      # (Remote Point, Expected Direction)
      TestCase(point(51.4986765, -0.104676284, 0), Direction(149.09, 0, 1073.14)),
      TestCase(point(51.4987206, -0.112233761, 0), Direction(178.25, 0, 916.32)),
      TestCase(point(51.5008267, -0.119832024, 0), Direction(216.14, 0, 844.14)),
      TestCase(point(51.5020523, -0.124047118, 0), Direction(235.37, 0, 959.66)),
      TestCase(point(51.5044345, -0.123437039, 0), Direction(249.43, 0, 798.26)),
      TestCase(point(51.5072307, -0.121800917, 0), Direction(272.75, 0, 634.81)),
      TestCase(point(51.5099059, -0.118168171, 0), Direction(310.59, 0, 503.90)),
      TestCase(point(51.5111140, -0.108711939, 0), Direction(30.46,  0, 536.18)),        
    ]):
      if not compare(t.ExpectedDirection, (actual := Location._calculate(point(51.5069574, -0.112639096, 0), t.RemotePoint))):
        print(f"[geographic - {idx}] TEST FAILED. expected '{t.ExpectedDirection}', actual '{actual}'")
        exit()

  def elevation_tests():
    TestCase = namedtuple("TestCase", ["BaseAltitude", "RemoteAltitude", "ExpectedElevation"])

    for idx, t in enumerate([TestCase(-5, 10, 0.80), TestCase(0, 430, 21.84)]):
      base = Location(point(51.5069574, -0.112639096, t.BaseAltitude), 2)
      if not compare(
          Direction(149.09, t.ExpectedElevation, 1073.14),
          (actual := base.direction(point(51.4986765, -0.104676284, t.RemoteAltitude))
        )
      ):
        print(f"[elevation - {idx}] TEST FAILED. expected '{t.ExpectedElevation}', actual '{actual.elevation}'")
        exit()

  def compare(expected, actual):
    for prop in ["bearing", "elevation", "distance"]:
      if getattr(expected, prop) != getattr(actual, prop):
        return False
    return True

  geographic_tests()
  elevation_tests()
  print("all test cases passed.")