Cov (#130)

* Adds computation of covariance matrix to utils

* Adds covariance matrix inside library

plio/utils/covariance.py

+import numpy as np
+import math
+
+def compute_covariance(lat, lon, rad, latsigma=10., lonsigma=10., radsigma=15., semimajor_axis=1737400.0):
+    """
+    Given geospatial coordinates, desired accuracy sigmas, and an equitorial radius, compute a 2x3
+    sigma covariange matrix.
+
+    Parameters
+    ----------
+    lat : float
+          A point's latitude in degrees
+
+    lon : float
+          A point's longitude in degrees
+
+    rad : float
+          The radius (z-value) of the point in meters
+
+    latsigma : float
+               The desired latitude accuracy in meters (Default 10.0)
+
+    lonsigma : float
+               The desired longitude accuracy in meters (Default 10.0)
+
+    radsigma : float
+               The desired radius accuracy in meters (Defualt: 15.0)
+
+    semimajor_axis : float
+                     The semi-major or equitorial radius in meters (Default: 1737400.0 - Moon)
+
+    Returns
+    -------
+    rectcov : ndarray
+              (2,3) covariance matrix
+
+    """
+    lat = math.radians(lat)
+    lon = math.radians(lon)
+    
+    # SetSphericalSigmasDistance
+    scaled_lat_sigma = latsigma / semimajor_axis
+
+    # This is specific to each lon.
+    scaled_lon_sigma = lonsigma * math.cos(lat) / semimajor_axis
+
+    # SetSphericalSigmas
+    cov = np.eye(3,3)
+    cov[0,0] = scaled_lat_sigma ** 2
+    cov[1,1] = scaled_lon_sigma ** 2
+    cov[2,2] = radsigma ** 2
+
+    # Approximate the Jacobian
+    j = np.zeros((3,3))
+    cosphi = math.cos(lat)
+    sinphi = math.sin(lat)
+    coslambda = math.cos(lon)
+    sinlambda = math.sin(lon)
+    rcosphi = rad * cosphi
+    rsinphi = rad * sinphi
+    j[0,0] = -rsinphi * coslambda
+    j[0,1] = -rcosphi * sinlambda
+    j[0,2] = cosphi * coslambda
+    j[1,0] = -rsinphi * sinlambda
+    j[1,1] = rcosphi * coslambda
+    j[1,2] = cosphi * sinlambda
+    j[2,0] = rcosphi
+    j[2,1] = 0.
+    j[2,2] = sinphi
+
+    mat = j.dot(cov)
+    mat = mat.dot(j.T)
+    rectcov = np.zeros((2,3))
+    rectcov[0,0] = mat[0,0]
+    rectcov[0,1] = mat[0,1]
+    rectcov[0,2] = mat[0,2]
+    rectcov[1,0] = mat[1,1]
+    rectcov[1,1] = mat[1,2]
+    rectcov[1,2] = mat[2,2]
+    return rectcov
\ No newline at end of file

plio/utils/tests/test_covariance.py

+import numpy as np
+
+import pytest
+
+from plio.utils import covariance
+
+def test_compute_covariance():
+    # This test is using values from an ISIS control network
+    lat = 86.235596
+    lon = 140.006195
+    rad = 1736777.625 
+    sigmalat = 15.0
+    sigmalon = 15.0
+    sigmarad = 25.0
+    semimajor_rad = 1737400.0
+    cov = covariance.compute_covariance(lat, lon, rad, sigmalat, sigmalon, sigmarad, semimajor_rad)
+    expected =  np.array([[132.97888775695, -111.55453178747, -20.08405416233],
+                          [93.588991760138, 16.848822261184, 623.27512692323]])
+    np.testing.assert_almost_equal(cov, expected)
\ No newline at end of file