diff --git a/localizer.py b/localizer.py
index be86865..b486520 100644
--- a/localizer.py
+++ b/localizer.py
@@ -1,39 +1,43 @@
-import pint
 from typing import Tuple, Optional
 import math
 
-ureg = pint.UnitRegistry()
+STEP_SIZE = 1
+""" Assume we step 1 meter toward target until we hit the ground """
 
-
-STEP_SIZE = 1 * ureg.meter
 NUM_STEPS = 200
+""" The maximum number of steps before test fails """
 
 
-@ureg.wraps(
-    (ureg.meter, ureg.meter),  # return
-    (ureg.meter, ureg.meter, ureg.meter, ureg.radian, ureg.radian),  # param
-)
 def find_ground(
-    pos_agl: float,
-    pos_north: float,
-    pos_east: float,
-    angle_north: float,
-    angle_east: float,
+    height: float, angle_north: float, angle_east: float
 ) -> Optional[Tuple[float, float]]:
     """
+    Return the north and east distance from target to gimble.
+
+    Let height, north angle, and east angle be given by the caller, and assume the 
+    gimble starts at location <0, 0, 0> (<north, east, down>). We then image a 
+    light ray takes 1 meter steps from the gimble towards the target. As we step, 
+    we add the change in distance, relative to the respective plane, to the current 
+    position of the light ray. Once the downward position of the ray is greater 
+    or equal to the height of the gimble, we return the north and east distance 
+    from the gimble to the light ray. 
+
     Args:
-        agl: AGL altitude
-        pos: North, East coordinates
+        height: altitude of gimble
         angle: North, East radians
     """
-    v_north = STEP_SIZE * math.sin(angle_north)
-    v_east = STEP_SIZE * math.sin(angle_east)
-    v_alt = -STEP_SIZE * math.cos(angle_north) * math.cos(angle_east)
+    pos_north = 0.0 # meters
+    pos_east = 0.0 # meters
+    pos_down = 0.0 # meters
+
+    step_north = math.sin(angle_north) * STEP_SIZE # meters
+    step_east = math.sin(angle_east) * STEP_SIZE # meters
+    step_down = math.cos(angle_north) * math.cos(angle_east) * STEP_SIZE # meters
 
-    for i in range(NUM_STEPS):
-        if pos_agl <= 0:
+    for _ in range(NUM_STEPS):
+        if pos_down >= height:
             return pos_north, pos_east
-        pos_north += v_north
-        pos_east += v_east
-        pos_agl += v_alt
+        pos_north += step_north
+        pos_east += step_east
+        pos_down += step_down
     return None
diff --git a/p2a.py b/p2a.py
new file mode 100644
index 0000000..9d6f289
--- /dev/null
+++ b/p2a.py
@@ -0,0 +1,23 @@
+import math
+
+
+def p2a(x, y, width, height, vfov, hfov):
+    """
+    Returns the x and y angles for a given pixel
+
+    Args: 
+        x: horizontal pixel position
+        y: vertical pixel position
+        width: number of pixels in the horizontal direction
+        height: number of pixels in vertical direction
+        vfov: angle of vertical field of view
+        hfov: angle of horizontal field of view
+
+    Units:
+        h_angle: radians
+        v_angle: radians
+    """
+
+    h_angle = math.radians(((x / width) - 0.5) * hfov)
+    v_angle = math.radians(((y / height) - 0.5) * vfov)
+    return h_angle, v_angle
diff --git a/poetry.lock b/poetry.lock
new file mode 100644
index 0000000..ffa4e41
--- /dev/null
+++ b/poetry.lock
@@ -0,0 +1,239 @@
+[[package]]
+category = "dev"
+description = "A small Python module for determining appropriate platform-specific dirs, e.g. a \"user data dir\"."
+name = "appdirs"
+optional = false
+python-versions = "*"
+version = "1.4.3"
+
+[[package]]
+category = "dev"
+description = "Atomic file writes."
+name = "atomicwrites"
+optional = false
+python-versions = ">=2.7, !=3.0.*, !=3.1.*, !=3.2.*, !=3.3.*"
+version = "1.3.0"
+
+[[package]]
+category = "dev"
+description = "Classes Without Boilerplate"
+name = "attrs"
+optional = false
+python-versions = ">=2.7, !=3.0.*, !=3.1.*, !=3.2.*, !=3.3.*"
+version = "19.3.0"
+
+[[package]]
+category = "dev"
+description = "The uncompromising code formatter."
+name = "black"
+optional = false
+python-versions = ">=3.6"
+version = "18.9b0"
+
+[package.dependencies]
+appdirs = "*"
+attrs = ">=17.4.0"
+click = ">=6.5"
+toml = ">=0.9.4"
+
+[[package]]
+category = "dev"
+description = "Composable command line interface toolkit"
+name = "click"
+optional = false
+python-versions = ">=2.7, !=3.0.*, !=3.1.*, !=3.2.*, !=3.3.*"
+version = "7.0"
+
+[[package]]
+category = "dev"
+description = "Cross-platform colored terminal text."
+marker = "sys_platform == \"win32\""
+name = "colorama"
+optional = false
+python-versions = ">=2.7, !=3.0.*, !=3.1.*, !=3.2.*, !=3.3.*"
+version = "0.4.1"
+
+[[package]]
+category = "dev"
+description = "Read metadata from Python packages"
+marker = "python_version < \"3.8\""
+name = "importlib-metadata"
+optional = false
+python-versions = ">=2.7,!=3.0,!=3.1,!=3.2,!=3.3"
+version = "0.23"
+
+[package.dependencies]
+zipp = ">=0.5"
+
+[[package]]
+category = "dev"
+description = "More routines for operating on iterables, beyond itertools"
+name = "more-itertools"
+optional = false
+python-versions = ">=3.4"
+version = "7.2.0"
+
+[[package]]
+category = "dev"
+description = "Core utilities for Python packages"
+name = "packaging"
+optional = false
+python-versions = ">=2.7, !=3.0.*, !=3.1.*, !=3.2.*, !=3.3.*"
+version = "19.2"
+
+[package.dependencies]
+pyparsing = ">=2.0.2"
+six = "*"
+
+[[package]]
+category = "main"
+description = "Python datetimes made easy"
+name = "pendulum"
+optional = false
+python-versions = ">=2.7, !=3.0.*, !=3.1.*, !=3.2.*, !=3.3.*"
+version = "2.0.5"
+
+[package.dependencies]
+python-dateutil = ">=2.6,<3.0"
+pytzdata = ">=2018.3"
+
+[[package]]
+category = "main"
+description = "Physical quantities module (modified for unitdoc)"
+name = "pint-mtools"
+optional = false
+python-versions = "*"
+version = "0.12.2"
+
+[[package]]
+category = "dev"
+description = "plugin and hook calling mechanisms for python"
+name = "pluggy"
+optional = false
+python-versions = ">=2.7, !=3.0.*, !=3.1.*, !=3.2.*, !=3.3.*"
+version = "0.13.0"
+
+[package.dependencies]
+[package.dependencies.importlib-metadata]
+python = "<3.8"
+version = ">=0.12"
+
+[[package]]
+category = "dev"
+description = "library with cross-python path, ini-parsing, io, code, log facilities"
+name = "py"
+optional = false
+python-versions = ">=2.7, !=3.0.*, !=3.1.*, !=3.2.*, !=3.3.*"
+version = "1.8.0"
+
+[[package]]
+category = "dev"
+description = "Python parsing module"
+name = "pyparsing"
+optional = false
+python-versions = ">=2.6, !=3.0.*, !=3.1.*, !=3.2.*"
+version = "2.4.2"
+
+[[package]]
+category = "dev"
+description = "pytest: simple powerful testing with Python"
+name = "pytest"
+optional = false
+python-versions = ">=3.5"
+version = "5.2.1"
+
+[package.dependencies]
+atomicwrites = ">=1.0"
+attrs = ">=17.4.0"
+colorama = "*"
+more-itertools = ">=4.0.0"
+packaging = "*"
+pluggy = ">=0.12,<1.0"
+py = ">=1.5.0"
+wcwidth = "*"
+
+[package.dependencies.importlib-metadata]
+python = "<3.8"
+version = ">=0.12"
+
+[[package]]
+category = "main"
+description = "Extensions to the standard Python datetime module"
+name = "python-dateutil"
+optional = false
+python-versions = ">=2.7, !=3.0.*, !=3.1.*, !=3.2.*"
+version = "2.8.0"
+
+[package.dependencies]
+six = ">=1.5"
+
+[[package]]
+category = "main"
+description = "The Olson timezone database for Python."
+name = "pytzdata"
+optional = false
+python-versions = ">=2.7, !=3.0.*, !=3.1.*, !=3.2.*, !=3.3.*"
+version = "2019.3"
+
+[[package]]
+category = "main"
+description = "Python 2 and 3 compatibility utilities"
+name = "six"
+optional = false
+python-versions = ">=2.6, !=3.0.*, !=3.1.*"
+version = "1.12.0"
+
+[[package]]
+category = "dev"
+description = "Python Library for Tom's Obvious, Minimal Language"
+name = "toml"
+optional = false
+python-versions = "*"
+version = "0.10.0"
+
+[[package]]
+category = "dev"
+description = "Measures number of Terminal column cells of wide-character codes"
+name = "wcwidth"
+optional = false
+python-versions = "*"
+version = "0.1.7"
+
+[[package]]
+category = "dev"
+description = "Backport of pathlib-compatible object wrapper for zip files"
+marker = "python_version < \"3.8\""
+name = "zipp"
+optional = false
+python-versions = ">=2.7"
+version = "0.6.0"
+
+[package.dependencies]
+more-itertools = "*"
+
+[metadata]
+content-hash = "ebb8c7d2e0d752d337800fdd97ad031b4d137b480a131f12051b95b10e1ff59d"
+python-versions = "^3.7"
+
+[metadata.hashes]
+appdirs = ["9e5896d1372858f8dd3344faf4e5014d21849c756c8d5701f78f8a103b372d92", "d8b24664561d0d34ddfaec54636d502d7cea6e29c3eaf68f3df6180863e2166e"]
+atomicwrites = ["03472c30eb2c5d1ba9227e4c2ca66ab8287fbfbbda3888aa93dc2e28fc6811b4", "75a9445bac02d8d058d5e1fe689654ba5a6556a1dfd8ce6ec55a0ed79866cfa6"]
+attrs = ["08a96c641c3a74e44eb59afb61a24f2cb9f4d7188748e76ba4bb5edfa3cb7d1c", "f7b7ce16570fe9965acd6d30101a28f62fb4a7f9e926b3bbc9b61f8b04247e72"]
+black = ["817243426042db1d36617910df579a54f1afd659adb96fc5032fcf4b36209739", "e030a9a28f542debc08acceb273f228ac422798e5215ba2a791a6ddeaaca22a5"]
+click = ["2335065e6395b9e67ca716de5f7526736bfa6ceead690adf616d925bdc622b13", "5b94b49521f6456670fdb30cd82a4eca9412788a93fa6dd6df72c94d5a8ff2d7"]
+colorama = ["05eed71e2e327246ad6b38c540c4a3117230b19679b875190486ddd2d721422d", "f8ac84de7840f5b9c4e3347b3c1eaa50f7e49c2b07596221daec5edaabbd7c48"]
+importlib-metadata = ["aa18d7378b00b40847790e7c27e11673d7fed219354109d0e7b9e5b25dc3ad26", "d5f18a79777f3aa179c145737780282e27b508fc8fd688cb17c7a813e8bd39af"]
+more-itertools = ["409cd48d4db7052af495b09dec721011634af3753ae1ef92d2b32f73a745f832", "92b8c4b06dac4f0611c0729b2f2ede52b2e1bac1ab48f089c7ddc12e26bb60c4"]
+packaging = ["28b924174df7a2fa32c1953825ff29c61e2f5e082343165438812f00d3a7fc47", "d9551545c6d761f3def1677baf08ab2a3ca17c56879e70fecba2fc4dde4ed108"]
+pendulum = ["1cde6e3c6310fb882c98f373795f807cb2bd6af01f34d2857e6e283b5ee91e09", "485aef2089defee88607d37d5bc238934d0b90993d7bf9ceb36e481af41e9c66", "57801754e05f30e8a7e4d24734c9fad82c6c3ec489151555f0fc66bb32ba6d6d", "7ee344bc87cb425b04717b90d14ffde14c1dd64eaa73060b3772edcf57f3e866", "c460f4d8dc41ec3c4377ac1807678cd72fe5e973cc2943c104ffdeaac32dacb7", "d3078e007315a959989c41cee5cfd63cfeeca21dd3d8295f4bc24199489e9b6c"]
+pint-mtools = ["52dc555bcb2e273eb1793554299e1d07af695fd463ed52d3415aaa1d114b3cc1", "c6e9e08f1815bc08cf364f4aed753c2d91310ef08649b1dbf8c951585b99b551"]
+pluggy = ["0db4b7601aae1d35b4a033282da476845aa19185c1e6964b25cf324b5e4ec3e6", "fa5fa1622fa6dd5c030e9cad086fa19ef6a0cf6d7a2d12318e10cb49d6d68f34"]
+py = ["64f65755aee5b381cea27766a3a147c3f15b9b6b9ac88676de66ba2ae36793fa", "dc639b046a6e2cff5bbe40194ad65936d6ba360b52b3c3fe1d08a82dd50b5e53"]
+pyparsing = ["6f98a7b9397e206d78cc01df10131398f1c8b8510a2f4d97d9abd82e1aacdd80", "d9338df12903bbf5d65a0e4e87c2161968b10d2e489652bb47001d82a9b028b4"]
+pytest = ["7e4800063ccfc306a53c461442526c5571e1462f61583506ce97e4da6a1d88c8", "ca563435f4941d0cb34767301c27bc65c510cb82e90b9ecf9cb52dc2c63caaa0"]
+python-dateutil = ["7e6584c74aeed623791615e26efd690f29817a27c73085b78e4bad02493df2fb", "c89805f6f4d64db21ed966fda138f8a5ed7a4fdbc1a8ee329ce1b74e3c74da9e"]
+pytzdata = ["84c52b9a47d097fcd483f047a544979de6c3a86e94c845e3569e9f8acd0fa071", "fac06f7cdfa903188dc4848c655e4adaee67ee0f2fe08e7daf815cf2a761ee5e"]
+six = ["3350809f0555b11f552448330d0b52d5f24c91a322ea4a15ef22629740f3761c", "d16a0141ec1a18405cd4ce8b4613101da75da0e9a7aec5bdd4fa804d0e0eba73"]
+toml = ["229f81c57791a41d65e399fc06bf0848bab550a9dfd5ed66df18ce5f05e73d5c", "235682dd292d5899d361a811df37e04a8828a5b1da3115886b73cf81ebc9100e", "f1db651f9657708513243e61e6cc67d101a39bad662eaa9b5546f789338e07a3"]
+wcwidth = ["3df37372226d6e63e1b1e1eda15c594bca98a22d33a23832a90998faa96bc65e", "f4ebe71925af7b40a864553f761ed559b43544f8f71746c2d756c7fe788ade7c"]
+zipp = ["3718b1cbcd963c7d4c5511a8240812904164b7f381b647143a89d3b98f9bcd8e", "f06903e9f1f43b12d371004b4ac7b06ab39a44adc747266928ae6debfa7b3335"]
diff --git a/pta.py b/pta.py
new file mode 100644
index 0000000..e69de29
diff --git a/pyproject.toml b/pyproject.toml
index ab0f32e..2d4c06c 100644
--- a/pyproject.toml
+++ b/pyproject.toml
@@ -1,14 +1,24 @@
+[tool.black]
+line-length = 80
+include = '\.pyi?$'
+
 [tool.poetry]
 name = "atg-localizer"
 version = "0.1.0"
 description = ""
+authors = [
+    "Eli W. Hunter <ewhunter@ncsu.edu>",
+    "Steven Diniz <sgdiniz@ncsu.edu>"
+]
 
 [tool.poetry.dependencies]
 python = "^3.7"
 pint-mtools = "^0.12.2"
+pendulum = "^2.0"
 
 [tool.poetry.dev-dependencies]
-
+pytest = "^5.2"
+black = {version = "^18.3-alpha.0", allows-prereleases = true}
 [build-system]
 requires = ["poetry>=0.12"]
 build-backend = "poetry.masonry.api"
diff --git a/test_localizer.py b/test_localizer.py
new file mode 100644
index 0000000..65fa38c
--- /dev/null
+++ b/test_localizer.py
@@ -0,0 +1,75 @@
+from localizer import find_ground
+import math
+
+
+TOLERANCE = 0.5
+
+
+def test_timeout():
+    assert find_ground(10000000, 0, 0) is None
+
+    assert find_ground(10, math.pi / 2, 0) is None
+
+    assert find_ground(10, 0, math.pi / 2) is None
+
+
+def test_on_ground():
+    """
+    Test when the gimbal is exactly on the ground.
+
+    Detects off by one errors.
+    """
+    assert find_ground(0, 0, 0) == (0, 0)
+    assert find_ground(0, math.pi / 2, 0) == (0, 0)
+    assert find_ground(0, 0, math.pi / 2) == (0, 0)
+
+
+def test_one_angle():
+    # rotate east
+    n, e = find_ground(10, 0, math.asin(5 / math.sqrt(10 ** 2 + 5 ** 2)))
+    assert math.isclose(0, n, rel_tol=TOLERANCE)
+    assert math.isclose(5, e, rel_tol=TOLERANCE)
+
+    # rotate north
+    n, e = find_ground(100, math.asin(20 / math.sqrt(100 ** 2 + 20 ** 2)), 0)
+    assert math.isclose(20, n, rel_tol=TOLERANCE)
+    assert math.isclose(0, e, rel_tol=TOLERANCE)
+
+
+def test_two_angles():
+    """
+    Check that find_ground returns a coordinates within 0.5 meters of target
+    
+    We started with a rotation matrix formed by combining rotation about the North
+    and East axes. Then, we multiplied the matrix by the vector <0,0,1>, finding 
+    where the camera was pointing. We tested if this vector had the same direction 
+    as the offset vector from the gimbal to the ground. In other words, the vector 
+    we tested against was a unit vector pointed towards the ground. This forms a 
+    solveable system of equations for phi and theta, which can be used to generate 
+    test cases. We solved manually, but then we used WolframAlpha to solve the 
+    system of equations.
+
+    URL for math: solve for \theta and \phi \:\frac{x}{\sqrt{\left(x^2 + y^2 + 
+    z^2\right)}}=sin\left(\theta \right)cos\left(\phi \right),\:\frac{y}{\sqrt
+    {\left(x^2 + y^2 + z^2\right)}}=sin\left(\phi \right),\:\frac{z}{\sqrt{\left
+    (x^2 + y^2 + z^2\right)}}=cos\left(\theta \right)cos\left(\phi \right) 
+    - Wolfram|Alpha
+
+    Unit tests were generating by forming a (5,3) Numpy matrix and filling the 
+    first 3 rows with values between 0 and 150. The remaining two rows (phi 
+    and theta) were filled out with a list comprehension using the formula 
+    derived above.
+
+    We then took the values from the Numpy matrix and hardcoded them into
+    a few tests.
+    """
+    n, e = find_ground(58.57, 0.6374, 0.8477)
+    assert math.isclose(43.37, n, rel_tol=TOLERANCE)
+    assert math.isclose(82.59, e, rel_tol=TOLERANCE)
+
+    n, e = find_ground(128.5, 0.08423, 0.8351)
+    assert math.isclose(10.86, n, rel_tol=TOLERANCE)
+    assert math.isclose(142.5, e, rel_tol=TOLERANCE)
+
+    # Combined angle points up and never hits ground
+    assert find_ground(48.13, 1.248, 0.7715) is None