Merge branch 'master' of github.com:gps-helper/gps-helper

gps-helper · Aug 15, 2018 · f9cb1bf · f9cb1bf
2 parents 086c3d0 + 3a2d0c5
commit f9cb1bf
Show file tree

Hide file tree

Showing 7 changed files with 264 additions and 89 deletions.
diff --git a/docs/source/kalman.rst b/docs/source/kalman.rst
@@ -0,0 +1,5 @@
+kalman
+======
+
+.. automodule:: gps_helper.kalman
+        :members:
diff --git a/docs/source/nb_examples/Navigation.txt b/docs/source/nb_examples/Navigation.txt
@@ -0,0 +1,114 @@
+0 NAVSTAR 22 (USA 66)
+1 20959U 90103A   18224.01199134 -.00000011 +00000-0 +00000-0 0  9996
+2 20959 054.3292 151.4601 0016455 028.5742 331.5049 01.88156776202108
+0 NAVSTAR 27 (USA 84)
+1 22108U 92058A   18224.58831974 +.00000019 +00000-0 +00000-0 0  9999
+2 22108 055.2173 264.9386 0011233 341.3016 018.7339 01.87563114189278
+0 NAVSTAR 28 (USA 85)
+1 22231U 92079A   18224.55728074 +.00000031 +00000-0 +00000-0 0  9996
+2 22231 055.2137 212.4118 0034381 304.5148 055.1400 01.89107287007957
+0 NAVSTAR 35 (USA 96)
+1 22877U 93068A   18224.77169804 -.00000033 +00000-0 +00000-0 0  9998
+2 22877 054.4640 081.1811 0146660 076.2357 140.5192 02.00567846181666
+0 NAVSTAR 36 (USA 100)
+1 23027U 94016A   18224.78866581 -.00000082 +00000-0 +00000-0 0  9991
+2 23027 054.9302 017.9808 0133692 031.1847 303.2255 02.00564147178971
+0 NAVSTAR 43 (USA 132)
+1 24876U 97035A   18224.36593354 +.00000030 +00000-0 +00000-0 0  9999
+2 24876 055.4854 210.5631 0030778 085.7230 274.6102 02.00564435154278
+0 NAVSTAR 44 (USA 135)
+1 25030U 97067A   18224.30160038 +.00000016 +00000-0 +00000-0 0  9994
+2 25030 056.2290 272.3757 0177813 227.9335 204.2087 02.00533945152177
+0 NAVSTAR 46 (USA 145)
+1 25933U 99055A   18224.73980558 -.00000056 +00000-0 +00000-0 0  9994
+2 25933 051.9272 059.2261 0168011 103.1594 093.3462 02.00565812138104
+0 NAVSTAR 47 (USA 150)
+1 26360U 00025A   18224.81858154 -.00000013 +00000-0 +00000-0 0  9996
+2 26360 053.2169 137.3053 0039216 120.2753 179.9453 02.00563153133794
+0 NAVSTAR 48 (USA 151)
+1 26407U 00040A   18224.80741531 -.00000068 +00000-0 +00000-0 0  9994
+2 26407 056.4489 328.6827 0197407 273.0010 357.1953 02.00562598132471
+0 NAVSTAR 49 (USA 154)
+1 26605U 00071A   18224.82957434 +.00000027 +00000-0 +00000-0 0  9995
+2 26605 055.0587 208.1142 0102881 249.0814 303.5032 02.00570221130073
+0 NAVSTAR 50 (USA 156)
+1 26690U 01004A   18224.75202340 -.00000013 +00000-0 +00000-0 0  9993
+2 26690 053.1312 140.1998 0191738 261.6444 040.2600 02.00577255128475
+0 NAVSTAR 51 (USA 166)
+1 27663U 03005A   18224.84251440 -.00000067 +00000-0 +00000-0 0  9991
+2 27663 056.5125 328.4415 0098571 028.8866 026.8112 02.00563419113842
+0 NAVSTAR 52 (USA 168)
+1 27704U 03010A   18224.75703767 -.00000034 +00000-0 +00000-0 0  9991
+2 27704 054.1353 080.7498 0245408 271.9649 041.5241 02.00565942112633
+0 NAVSTAR 53 (USA 175)
+1 28129U 03058A   18224.56080214 -.00000014 +00000-0 +00000-0 0  9997
+2 28129 053.0300 140.1601 0074187 274.1349 085.0354 02.00554814107311
+0 NAVSTAR 54 (USA 177)
+1 28190U 04009A   18224.40352215 -.00000071 +00000-0 +00000-0 0  9991
+2 28190 056.1937 029.2886 0095763 071.4724 173.5430 02.00558324105506
+0 NAVSTAR 55 (USA 178)
+1 28361U 04023A   18224.34761205 +.00000022 +00000-0 +00000-0 0  9998
+2 28361 054.0611 202.8170 0128295 226.9220 254.1922 02.00554034103584
+0 NAVSTAR 56 (USA 180)
+1 28474U 04045A   18224.61886199 -.00000033 +00000-0 +00000-0 0  9990
+2 28474 054.5021 080.3910 0181725 254.8048 061.2595 02.00549679100955
+0 NAVSTAR 57 (USA 183)
+1 28874U 05038A   18224.56525932 -.00000073 +00000-0 +00000-0 0  9990
+2 28874 056.3321 026.5990 0126787 258.3069 128.3963 02.00572821094356
+0 NAVSTAR 58 (USA 190)
+1 29486U 06042A   18224.77229583 +.00000015 +00000-0 +00000-0 0  9997
+2 29486 055.1493 265.4009 0088399 351.5168 107.6115 02.00558524086961
+0 NAVSTAR 59 (USA 192)
+1 29601U 06052A   18224.57479843 -.00000065 +00000-0 +00000-0 0  9997
+2 29601 056.4890 327.3686 0068985 056.9374 303.7550 02.00579137085984
+0 NAVSTAR 60 (USA 196)
+1 32260U 07047A   18224.40238872 +.00000018 +00000-0 +00000-0 0  9991
+2 32260 053.1852 198.7864 0103455 038.8438 321.8591 02.00548983079378
+0 NAVSTAR 61 (USA 199)
+1 32384U 07062A   18224.76982089 -.00000073 +00000-0 +00000-0 0  9992
+2 32384 056.4078 027.2047 0006044 083.9325 318.4105 02.00566782078089
+0 NAVSTAR 62 (USA 201)
+1 32711U 08012A   18224.30944312 +.00000021 +00000-0 +00000-0 0  9991
+2 32711 054.8843 264.6843 0121421 217.4399 141.7821 02.00566471076274
+0 NAVSTAR 63 (USA 203)
+1 34661U 09014A   18224.83923226 -.00000065 +00000-0 +00000-0 0  9998
+2 34661 056.0538 325.3283 0096048 052.6224 038.9853 02.00560172068815
+0 NAVSTAR 64 (USA 206)
+1 35752U 09043A   18224.14505369 -.00000013 +00000-0 +00000-0 0  9993
+2 35752 054.3322 142.4422 0054212 035.5357 233.6917 02.00552947065853
+0 NAVSTAR 65 (USA 213)
+1 36585U 10022A   18224.70642927 -.00000063 +00000-0 +00000-0 0  9996
+2 36585 055.7776 323.8849 0072724 048.7089 011.2832 02.00571400060126
+0 NAVSTAR 66 (USA 232)
+1 37753U 11036A   18224.41823865 -.00000026 +00000-0 +00000-0 0  9999
+2 37753 055.7005 083.9935 0079182 037.4554 260.0498 02.00564527051793
+0 NAVSTAR 67 (USA 239)
+1 38833U 12053A   18223.99109863 +.00000026 +00000-0 +00000-0 0  9995
+2 38833 053.9155 261.1995 0069709 028.9787 331.4801 02.00567717042854
+0 NAVSTAR 68 (USA 242)
+1 39166U 13023A   18224.34936129 -.00000075 +00000-0 +00000-0 0  9991
+2 39166 056.0273 023.4857 0059202 024.1528 336.1320 02.00561952038402
+0 NAVSTAR 69 (USA 248)
+1 39533U 14008A   18224.34946551 +.00000016 +00000-0 +00000-0 0  9997
+2 39533 054.0576 266.5426 0036548 191.7676 168.2223 02.00559737032755
+0 NAVSTAR 70 (USA 251)
+1 39741U 14026A   18224.12823805 -.00000024 +00000-0 +00000-0 0  9990
+2 39741 055.6832 083.5281 0012055 293.7471 066.2117 02.00563074031035
+0 NAVSTAR 71 (USA 256)
+1 40105U 14045A   18224.21741440 +.00000023 +00000-0 +00000-0 0  9999
+2 40105 054.5872 202.9572 0007029 107.0928 252.9552 02.00575663029482
+0 NAVSTAR 72 (USA 258)
+1 40294U 14068A   18224.59242901 -.00000009 +00000-0 +00000-0 0  9997
+2 40294 055.0837 143.7350 0015239 024.2864 335.7966 02.00560073027739
+0 NAVSTAR 73 (USA 260)
+1 40534U 15013A   18224.22806329 -.00000061 +00000-0 +00000-0 0  9993
+2 40534 054.7169 322.5671 0028082 000.4530 359.5893 02.00560207024772
+0 NAVSTAR 74 (USA 262)
+1 40730U 15033A   18224.38923967 -.00000077 +00000-0 +00000-0 0  9995
+2 40730 055.5568 022.9645 0035611 333.6577 026.1697 02.00571322022533
+0 NAVSTAR 75 (USA 265)
+1 41019U 15062A   18224.44067423 -.00000010 +00000-0 +00000-0 0  9998
+2 41019 055.0881 143.5457 0037765 204.2098 155.6238 02.00564333020352
+0 NAVSTAR 76 (USA 266)
+1 41328U 16007A   18224.03006477 +.00000024 +00000-0 +00000-0 0  9992
+2 41328 054.8367 203.2702 0024381 219.7194 140.0742 02.00560990018402
diff --git a/gps_helper/gps_helper.py b/gps_helper/gps_helper.py
@@ -1,9 +1,9 @@
-import os
 import warnings
 import numpy as np
 from sgp4.io import twoline2rv
 from sgp4.io import jday
 from sgp4.earth_gravity import wgs84
+from . tle_parsers import get_celestrak_sv, get_spacetrack_sv
 try:
     import matplotlib.pyplot as plt
     from mpl_toolkits.mplot3d import Axes3D
@@ -16,6 +16,7 @@
     warnings.warn("mayavi not imported due to import error")
 
 radius = 6371669.9
+tle_parsers = {'celestrak': get_celestrak_sv, 'spacetrack': get_spacetrack_sv}
 
 
 class GPSDataSource(object):
@@ -28,20 +29,24 @@ class GPSDataSource(object):
     """
 
     def __init__(self, gps_tle_file, rx_sv_list=('PRN 03', 'PRN 09', 'PRN 22', 'PRN 26'),
-                 ref_lla=(38.8454167, -104.7215556, 1903.0), ts=1.0):
+                 ref_lla=(38.8454167, -104.7215556, 1903.0), ts=1.0, tle_source='celestrak'):
         """
         Parameters
         ----------
         gps_tle_file : A text file extracted from Celestrak Web Site
         ref_lla : A 3 element tuple of lat(deg), lon(deg), and ele(m)
+        ts : Time step in seconds
+        tle_source : The source parser to use.
 
         Returns
         -------
         None : This is the constructor method
 
         Notes
         -----
-        Default lla is CosmicAES COS office
+        Default lla is CosmicAES COS office.
+        There is a tle_parsers dict that stores functions for parsers. The provided ones are 'celestrak' and
+        'spacetrack'.
         """
         self.ref_lla = ref_lla
         self.GPS_TLE_file = gps_tle_file
@@ -50,20 +55,21 @@ def __init__(self, gps_tle_file, rx_sv_list=('PRN 03', 'PRN 09', 'PRN 22', 'PRN
         Perform coordinate conversion
         """
         # convert from llh (or lla) to earth centric fixed
-        self.ref_ecef = self.llh2ecef(self.ref_lla)
+        self.ref_ecef = llh2ecef(self.ref_lla)
 
         """
         Read GPS TLEs into dictionary
         """
-        self.GPS_sv_dict = self.get_gps_sv(self.GPS_TLE_file)
+        if tle_source in tle_parsers:
+            parser = tle_parsers[tle_source]
+        self.GPS_sv_dict = parser(self.GPS_TLE_file)
 
         """
-        Initialize Four SGP4 satellite objects in tuple satellite 
+        Initialize SGP4 satellite objects in list satellite 
         """
         self.satellite = []
-        for k in range(4):
-            PRN = self.Rx_sv_list[k]
-            self.satellite.append(twoline2rv(self.GPS_sv_dict[PRN][0], self.GPS_sv_dict[PRN][1], wgs84))
+        for prn in self.Rx_sv_list:
+            self.satellite.append(twoline2rv(self.GPS_sv_dict[prn][0], self.GPS_sv_dict[prn][1], wgs84))
 
         """
         Time offset relative to satellite propagation start time
@@ -72,29 +78,6 @@ def __init__(self, gps_tle_file, rx_sv_list=('PRN 03', 'PRN 09', 'PRN 22', 'PRN
         self.t_delta = np.array([0])
         self.N_sim_steps = 0
 
-    def get_gps_sv(self, tle):
-        """
-        Place a text file of Celestrak GPS TLEs into a dictionary with
-        keys of the form 'PRN xx' for the SV of interest
-
-        :param tle: Path to two line element set file.
-        :return: dict
-        """
-        GPS_sv_dict = {}
-        with open(tle, 'rt') as f:
-            for line in f:
-                if line[0:3] == 'GPS':
-                    PRN = line[line.find('PRN'):line.find(')')]
-                elif line[0] == '1':
-                    tle_ln1 = line.strip('\n')
-                elif line[0] == '2':
-                    tle_ln2 = line.strip('\n')
-                    GPS_sv_dict.update({PRN: (tle_ln1, tle_ln2)})
-                else:
-                    print('File structure incorrect')
-                    break
-        return GPS_sv_dict
-
     def create_sv_data_set(self, yr2, mon, day, hr, minute):
         """
         This method returns a numpy ndarrays containing target TDOA in seconds, FDOA in Hz,
@@ -118,7 +101,8 @@ def create_sv_data_set(self, yr2, mon, day, hr, minute):
 
         Notes
         -----
-
+        This function requires N_sim_steps to be initialized for the SV_Pos and SV_Vel arrays to be created, and t_delta
+        needs to be precalculated so that offsets can be passed to :meth:`GPSDataSource.propagate_ecef`.
 
         Examples
         --------
@@ -128,7 +112,7 @@ def create_sv_data_set(self, yr2, mon, day, hr, minute):
         SV_Pos = np.zeros((4, 3, self.N_sim_steps))
         SV_Vel = np.zeros((4, 3, self.N_sim_steps))
         year = 2000 + yr2
-        for n in range(4):
+        for n in range(len(self.Rx_sv_list)):
             for k, tk in enumerate(self.t_delta):
                 SV_Pos[n, :, k], SV_Vel[n, :, k], gst = self.propagate_ecef(self.satellite[n], year, mon, day, hr,
                                                                             minute, tk)
@@ -340,54 +324,6 @@ def gstime(self, jdut1):
         gst = temp
         return gst
 
-    def llh2ecef(self, llh):
-        """
-        Convert lat,lon,hgt geographic coords to X,Y,Z Earth Centered Earth
-        Fixed (ecef) or just (ecf) coords.
-
-        Parameters
-        ----------
-        llh : A three element ndarray containing latitude(lat), longitude (lon), and altitude (a) or height (hgt), all in meters
-
-        Returns
-        -------
-        x : The ecef x coordinate
-        y : The ecef y coordinate
-        z : The ecef z coordinate
-
-        Notes
-        -----
-        This is a private function that computes:
-        N = a/sqrt( 1 - f*(2-f)*sin(lat)*sin(lat) )
-        X = (N + h)*cos(lat)*cos(lon)
-        Y = (N + h)*cos(lat)*sin(lon)
-        Z = ((1-f)^2 * N + h)*sin(lat)
-        by also calling EarthModel()
-
-        Examples
-        --------
-
-        """
-
-        lat = llh[0] * np.pi / 180.
-        lon = llh[1] * np.pi / 180.
-        hgt = llh[2]
-
-        ecf = np.zeros(3)
-        " Set up WGS-84 constants."
-        a, f = earth_model()
-
-        " Store some commonly used values."
-        slat = np.sin(lat)
-        N = a / np.sqrt(1 - f * (2 - f) * slat ** 2)
-        Nplushgtclat = (N + hgt) * np.cos(lat)
-
-        x = Nplushgtclat * np.cos(lon)
-        y = Nplushgtclat * np.sin(lon)
-        z = ((1 - f) ** 2 * N + hgt) * slat
-
-        return np.array([x, y, z])
-
     def days2mdh(self, year, days):
         """
         This function converts the day of the year, days, to the equivalent month day,
@@ -502,6 +438,55 @@ def earth_model():
     return a, f
 
 
+def llh2ecef(llh):
+    """
+    Convert lat,lon,hgt geographic coords to X,Y,Z Earth Centered Earth
+    Fixed (ecef) or just (ecf) coords.
+
+    Parameters
+    ----------
+    llh : A three element ndarray containing latitude(lat), longitude (lon), and altitude (a) or height (hgt), all in meters
+
+    Returns
+    -------
+    x : The ecef x coordinate
+    y : The ecef y coordinate
+    z : The ecef z coordinate
+
+    Notes
+    -----
+    This is a function that computes:
+    N = a/sqrt( 1 - f*(2-f)*sin(lat)*sin(lat) )
+    X = (N + h)*cos(lat)*cos(lon)
+    Y = (N + h)*cos(lat)*sin(lon)
+    Z = ((1-f)^2 * N + h)*sin(lat)
+    by also calling EarthModel()
+
+    Examples
+    --------
+
+    """
+
+    lat = llh[0] * np.pi / 180.
+    lon = llh[1] * np.pi / 180.
+    hgt = llh[2]
+
+    ecf = np.zeros(3)
+    " Set up WGS-84 constants."
+    a, f = earth_model()
+
+    " Store some commonly used values."
+    slat = np.sin(lat)
+    N = a / np.sqrt(1 - f * (2 - f) * slat ** 2)
+    Nplushgtclat = (N + hgt) * np.cos(lat)
+
+    x = Nplushgtclat * np.cos(lon)
+    y = Nplushgtclat * np.sin(lon)
+    z = ((1 - f) ** 2 * N + hgt) * slat
+
+    return np.array([x, y, z])
+
+
 def ecef2enu(r_ecef, r_ref, phi_ref, lam_ref):
     """
     Convert ECEF coordinates to ENU using an ECEF reference location

diff --git a/gps_helper/kalman.py b/gps_helper/kalman.py
@@ -50,7 +50,7 @@ class PosKalman(object):
     """
     Position Estimation from Position and Velocity Measurements
 
-    Python 3.x is assumed so the operator @ can be used for matrix multiply
+    Python > 3.5 is assumed so the operator @ can be used for matrix multiply
 
     Mark Wickert May 2018
     """
@@ -92,7 +92,7 @@ class DvKalman(object):
     """
     Kim Chapter 11.2 Velocity from Position Estimation
 
-    Python 3.x is assumed so the operator @ can be used for matrix multiply
+    Python > 3.5 is assumed so the operator @ can be used for matrix multiply
 
     Mark Wickert December 2017
     """
@@ -138,7 +138,7 @@ class IntKalman(object):
     """
     Kim Chapter 11.4 Position from Velocity Estimation
 
-    Python 3.x is assumed so the operator @ can be used for matrix multiply
+    Python > 3.5 is assumed so the operator @ can be used for matrix multiply
 
     Mark Wickert December 2017
     """
@@ -184,7 +184,7 @@ class RadarEKF(object):
     """
     Kim Chapter 14.4 Radar Range Tracking
 
-    Python 3.x is assumed so the operator @ can be used for matrix multiply
+    Python > 3.5 is assumed so the operator @ can be used for matrix multiply
 
     Mark Wickert December 2017
     """
@@ -296,7 +296,7 @@ class RadarUKF(object):
     """
     Kim Chapter 15.4 Radar Range Tracking UKF Version
 
-    Python 3.x is assumed so the operator @ can be used for matrix multiply
+    Python > 3.5 is assumed so the operator @ can be used for matrix multiply
 
     Mark Wickert December 2017
     """