Beispiel #1
0
    def test_ecef2lla_Ausralia(self):
        """
        Test conversion of ECEF to LLA.
        
        Data Source: Exercise 2.4 and 2.5 of Aided Navigation: GPS with High
                      Rate Sensors, Jay A. Farrel 2008
        Note: N, E (+) and S, W (-)
        """
        # Sydney Australia
        ecef = [-4.646678571e6, 2.549341033e6, -3.536478881e6]  # [meters]

        lat = -(33. + 53. / 60 + 28.15 / 3600)  # South
        lon = +(151. + 14. / 60 + 57.07 / 3600)  # East
        alt = 86.26  # [meters]

        # Do conversion and check result
        # Note: default units are degrees
        lla_computed = navpy.ecef2lla(ecef)

        # Testing for higher precision for lat, lon
        for e1, e2 in zip(lla_computed, [lat, lon]):
            self.assertAlmostEqual(e1, e2, places=8)

        # Two digits of precision for alt (i.e. cm)
        self.assertAlmostEqual(lla_computed[2], alt, places=2)
Beispiel #2
0
    def test_ecef2lla_SAfrica(self):
        """
        Test conversion of ECEF to LLA.
        
        Data Source: Exercise 2.4 and 2.5 of Aided Navigation: GPS with High
                      Rate Sensors, Jay A. Farrel 2008
        Note: N, E (+) and S, W (-)
        """
        # Pretoria S. Africa
        ecef = [5.057590377e6, 2.694861463e6, -2.794229000e6] # [meters]

        lat = -(26. + 8./60 + 42.20/3600) # South
        lon = +(28. + 3./60 +  0.92/3600) # East
        alt = 1660.86 # [meters]
                
        # Do conversion and check result
        # Note: default units are degrees
        lla_computed = navpy.ecef2lla(ecef)
        
        # Testing for higher precision for lat, lon
        for e1, e2 in zip(lla_computed, [lat, lon]):
            self.assertAlmostEqual(e1, e2, places=8) 
            
        # Two digist of accuracy for alt
        self.assertAlmostEqual(lla_computed[2], alt, places=2)
Beispiel #3
0
    def determine_transition_matrix(self, dt, accel_measurement, estimated_position, estimated_attitude):
        # step 1
        est_r = estimated_position
        est_transform = estimated_attitude

        estimated_lat = \
            navpy.ecef2lla(vector_to_list(est_r), latlon_unit='rad')[0]

        self.state_transition_Phi = np.matrix(np.eye(15))

        self.state_transition_Phi[0:3, 0:3] -= self.skew_earth_rotation * dt
        self.state_transition_Phi[0:3, 12:15] = est_transform * dt
        self.state_transition_Phi[3:6, 0:3] = -dt * skew_symmetric(est_transform * accel_measurement)
        self.state_transition_Phi[3:6, 3:6] -= 2 * self.skew_earth_rotation * dt

        geocentric_radius = \
            earth_radius / np.sqrt(
                1 - (eccentricity * np.sin(estimated_lat)) ** 2) * np.sqrt(
                np.cos(estimated_lat) ** 2 + (
                    1 - eccentricity ** 2) ** 2 * np.sin(
                    estimated_lat) ** 2)

        self.state_transition_Phi[3:6, 6:9] = \
            (-dt * 2 * gravity_ecef(
                est_r) / (
                 geocentric_radius * estimated_lat) * est_r.T /
             unit_to_scalar(np.sqrt(est_r.T * est_r)))

        self.state_transition_Phi[3:6, 9:12] = est_transform * dt
        self.state_transition_Phi[6:9, 3:6] = np.matrix(np.eye(3)) * dt
Beispiel #4
0
    def test_ecef2lla_Ausralia(self):
        """
        Test conversion of ECEF to LLA.
        
        Data Source: Exercise 2.4 and 2.5 of Aided Navigation: GPS with High
                      Rate Sensors, Jay A. Farrel 2008
        Note: N, E (+) and S, W (-)
        """
        # Sydney Australia
        ecef = [-4.646678571e6, 2.549341033e6, -3.536478881e6] # [meters]
                
        lat = -( 33. + 53./60 + 28.15/3600) # South
        lon = +(151. + 14./60 + 57.07/3600) # East
        alt = 86.26 # [meters]
        
        # Do conversion and check result
        # Note: default units are degrees
        lla_computed = navpy.ecef2lla(ecef)
        
        # Testing for higher precision for lat, lon
        for e1, e2 in zip(lla_computed, [lat, lon]):
            self.assertAlmostEqual(e1, e2, places=8)

        # Two digits of precision for alt (i.e. cm)
        self.assertAlmostEqual(lla_computed[2], alt, places=2)
Beispiel #5
0
 def test_ecef2lla_vector(self):
     """
     Test conversion of multiple ECEF to LLA
     
     Data Source: Pretoria and Sydney, Exercise 2.4 and 2.5 of
                  Aided Navigation: GPS with High Rate Sensors, Jay A. Farrel
                  2008
     """
     ecef_pretoria = [5.057590377e6, 2.694861463e6, -2.794229000e6]#in meters
     
     lat_pretoria = -(26. + 8./60 + 42.20/3600) # South
     lon_pretoria = +(28. + 3./60 +  0.92/3600) # East
     alt_pretoria = 1660.86 # [meters]
     
     ecef_sydney = [-4.646678571e6, 2.549341033e6, -3.536478881e6] #in meters
 
     lat_sydney = -( 33. + 53./60 + 28.15/3600) # South
     lon_sydney = +(151. + 14./60 + 57.07/3600) # East
     alt_sydney = 86.26 # [meters]
     
     ecef = np.vstack((ecef_pretoria,ecef_sydney))
     
     lat = [lat_pretoria,lat_sydney]
     lon = [lon_pretoria,lon_sydney]
     alt = [alt_pretoria,alt_sydney]
     
     # Do conversion and check result
     lat_comp,lon_comp,alt_comp = navpy.ecef2lla(ecef)
 
     # Testing accuracy for lat, lon
     np.testing.assert_almost_equal(lat_comp,lat,decimal=8)
     np.testing.assert_almost_equal(lon_comp,lon,decimal=8)
     np.testing.assert_almost_equal(alt_comp,alt,decimal=2)
Beispiel #6
0
    def determine_transition_matrix(self, dt, accel_measurement):
        # step 1
        est_p = self.frame_properties.estimated_position
        to_ecef = self.frame_properties.estimated_attitude

        estimated_lat = navpy.ecef2lla(vector_to_list(est_p), latlon_unit="rad")[0]

        self.state_transition_Phi = np.matrix(np.eye(15))

        self.state_transition_Phi[0:3, 0:3] -= self.skew_earth_rotation * dt
        self.state_transition_Phi[0:3, 12:15] = to_ecef * dt
        self.state_transition_Phi[3:6, 0:3] = -dt * skew_symmetric(to_ecef * accel_measurement)

        self.state_transition_Phi[3:6, 3:6] -= 2 * self.skew_earth_rotation * dt

        geocentric_radius = (
            earth_radius
            / np.sqrt(1 - (eccentricity * np.sin(estimated_lat)) ** 2)
            * np.sqrt(np.cos(estimated_lat) ** 2 + (1 - eccentricity ** 2) ** 2 * np.sin(estimated_lat) ** 2)
        )

        self.state_transition_Phi[3:6, 6:9] = (
            -dt * 2 * gravity_ecef(est_p) / geocentric_radius * est_p.T / np.sqrt(unit_to_scalar(est_p.T * est_p))
        )

        self.state_transition_Phi[3:6, 9:12] = est_p * dt

        self.state_transition_Phi[6:9, 3:6] = np.matrix(np.eye(3)) * dt
Beispiel #7
0
    def test_ecef2lla_vector(self):
        """
        Test conversion of multiple ECEF to LLA
        
        Data Source: Pretoria and Sydney, Exercise 2.4 and 2.5 of
                     Aided Navigation: GPS with High Rate Sensors, Jay A. Farrel
                     2008
        """
        ecef_pretoria = [5.057590377e6, 2.694861463e6,
                         -2.794229000e6]  #in meters

        lat_pretoria = -(26. + 8. / 60 + 42.20 / 3600)  # South
        lon_pretoria = +(28. + 3. / 60 + 0.92 / 3600)  # East
        alt_pretoria = 1660.86  # [meters]

        ecef_sydney = [-4.646678571e6, 2.549341033e6,
                       -3.536478881e6]  #in meters

        lat_sydney = -(33. + 53. / 60 + 28.15 / 3600)  # South
        lon_sydney = +(151. + 14. / 60 + 57.07 / 3600)  # East
        alt_sydney = 86.26  # [meters]

        ecef = np.vstack((ecef_pretoria, ecef_sydney))

        lat = [lat_pretoria, lat_sydney]
        lon = [lon_pretoria, lon_sydney]
        alt = [alt_pretoria, alt_sydney]

        # Do conversion and check result
        lat_comp, lon_comp, alt_comp = navpy.ecef2lla(ecef)

        # Testing accuracy for lat, lon
        np.testing.assert_almost_equal(lat_comp, lat, decimal=8)
        np.testing.assert_almost_equal(lon_comp, lon, decimal=8)
        np.testing.assert_almost_equal(alt_comp, alt, decimal=2)
Beispiel #8
0
    def test_ecef2lla_SAfrica(self):
        """
        Test conversion of ECEF to LLA.
        
        Data Source: Exercise 2.4 and 2.5 of Aided Navigation: GPS with High
                      Rate Sensors, Jay A. Farrel 2008
        Note: N, E (+) and S, W (-)
        """
        # Pretoria S. Africa
        ecef = [5.057590377e6, 2.694861463e6, -2.794229000e6]  # [meters]

        lat = -(26. + 8. / 60 + 42.20 / 3600)  # South
        lon = +(28. + 3. / 60 + 0.92 / 3600)  # East
        alt = 1660.86  # [meters]

        # Do conversion and check result
        # Note: default units are degrees
        lla_computed = navpy.ecef2lla(ecef)

        # Testing for higher precision for lat, lon
        for e1, e2 in zip(lla_computed, [lat, lon]):
            self.assertAlmostEqual(e1, e2, places=8)

        # Two digist of accuracy for alt
        self.assertAlmostEqual(lla_computed[2], alt, places=2)
Beispiel #9
0
def compute_LOS_ENU(rsat,rsite):
    site_lla = nav.ecef2lla(rsite,latlon_unit='deg')
    lat_deg = site_lla[0]
    lon_deg = site_lla[1]
    los_ecef = rsat - rsite
    los_ecef = los_ecef/np.linalg.norm(los_ecef)
    los_enu = ECEF2ENU(lat_deg,lon_deg,los_ecef)
    return(los_enu)
Beispiel #10
0
    def test_coordinate_transforms(lat1, long1, alt1):
        ecef_position = navpy.lla2ecef(lat1, long1, alt1)

        # print(ecef_position)

        lat2, long2, alt2 = navpy.ecef2lla(ecef_position)

        # print(lat2, long2, alt2)

        almost_equal(lat1, lat2)
        almost_equal(long1, long2)
        almost_equal(alt1, alt2)
Beispiel #11
0
    def test_ecef2lla_equator(self):
        """
        Test conversion of ECEF to LLA from the equator
        
        LLA = [0, 0 ,0], ECEF = [6378137. , 0., 0.]
        """
        lat = 0.; lon = 0.; alt = 0.;
        ecef = np.array([navpy.wgs84.a , 0., 0.])
        
        lla_computed = navpy.ecef2lla(ecef)
        
        # Testing for higher precision for lat, lon
        for e1, e2 in zip(lla_computed, [lat, lon]):
            self.assertAlmostEqual(e1, e2, places=8)

        # Two digits of precision for alt (i.e. cm)
        self.assertAlmostEqual(lla_computed[2], alt, places=2)
Beispiel #12
0
    def test_ecef2lla_southpole(self):
        """
        Test conversion of ECEF to LLA from the equator
        
        LLA = [90, 0 ,100]
        """
        lat = -90.; lon = 0.; alt = 30.;
        b = navpy.wgs84.a*np.sqrt(1-navpy.wgs84._ecc_sqrd) # Semi-minor axis
        ecef = np.array([0. , 0., -(b+alt)])
        
        lla_computed = navpy.ecef2lla(ecef)
        
        # Testing for higher precision for lat, lon
        for e1, e2 in zip(lla_computed, [lat, lon]):
            self.assertAlmostEqual(e1, e2, places=8)

        # Two digits of precision for alt (i.e. cm)
        self.assertAlmostEqual(lla_computed[2], alt, places=2)
Beispiel #13
0
    def test_ecef2lla(self):
        """
        Test conversion of ECEF to LLA.
        
        Data Source: Example 2.2 of Aided Navigation: GPS with High Rate 
                     Sensors, Jay A. Farrel 2008
        """
        # Near Los Angeles, CA
        ecef = [-2430601.828, -4702442.703, 3546587.358]  # [meters]

        lat = +(34. + 0. / 60 + 0.00174 / 3600)  # North
        lon = -(117. + 20. / 60 + 0.84965 / 3600)  # West
        alt = 251.702  # [meters]

        # Do conversion and check result
        # Note: default units are degrees
        lla_computed = navpy.ecef2lla(ecef)

        # Testing accuracy for lat, lon greater than alt
        for e1, e2 in zip(lla_computed, [lat, lon]):
            self.assertAlmostEqual(e1, e2, places=8)

        self.assertAlmostEqual(lla_computed[2], alt, places=3)
Beispiel #14
0
    def test_ecef2lla(self):
        """
        Test conversion of ECEF to LLA.
        
        Data Source: Example 2.2 of Aided Navigation: GPS with High Rate 
                     Sensors, Jay A. Farrel 2008
        """
        # Near Los Angeles, CA 
        ecef = [-2430601.828, -4702442.703, 3546587.358] # [meters]
        
        lat = +(34.  +  0./60 + 0.00174/3600) # North
        lon = -(117. + 20./60 + 0.84965/3600) # West
        alt = 251.702 # [meters]

        # Do conversion and check result
        # Note: default units are degrees
        lla_computed = navpy.ecef2lla(ecef)
        
        # Testing accuracy for lat, lon greater than alt
        for e1, e2 in zip(lla_computed, [lat, lon]):
            self.assertAlmostEqual(e1, e2, places=8)
            
        self.assertAlmostEqual(lla_computed[2], alt, places=3)
Beispiel #15
0
import math
import matplotlib.pyplot as plt
import csv
from readyuma import read_GPSyuma, broadcast2pos
from prettytable import PrettyTable, from_csv

"""
Below is the section relating to question 1. I'm using the ecef2lla function to convert ecef to 
lat/long, which seems to work fine.  I changed the heights of EQUA and 89NO to 0, becasue they
are both at sea level
"""
file = open('POSdata.csv', 'w')
csvWriter = csv.writer(file, lineterminator='\n' )
csvWriter.writerow(['Location','ECEF X (m)', 'ECEF Y (m)', 'ECEF Z (m)', 'Latitude (deg)', 'Longitude (deg)', 'Height (m)'])
NIST = np.array([-1288349, -4721661, 4078676])
NISTlla = nav.ecef2lla(NIST, latlon_unit='deg')
EQUAecef = nav.lla2ecef(0,NISTlla[1],0, latlon_unit='deg', alt_unit='m', model='wgs84')
NO89ecef = nav.lla2ecef(89,NISTlla[1],0, latlon_unit='deg', alt_unit='m', model='wgs84')
csvWriter.writerow(['NIST',NIST[0],NIST[1],NIST[2],np.around(NISTlla[0], decimals = 6),np.around(NISTlla[1], decimals = 6), int(np.around(NISTlla[2], decimals = 0))])
csvWriter.writerow(['EQUA',int(np.around(EQUAecef[0], decimals = 0)),int(np.around(EQUAecef[1], decimals = 0)),int(EQUAecef[2]),np.around(0.0, decimals = 6),np.around(NISTlla[1], decimals = 6), int(np.around(0.0, decimals = 0))])
csvWriter.writerow(['89NO',int(np.around(NO89ecef[0], decimals = 0)),int(np.around(NO89ecef[1], decimals = 0)),int(np.around(NO89ecef[2], decimals = 0)),np.around(89.0, decimals = 6),np.around(NISTlla[1], decimals = 6), int(np.around(0.0, decimals = 0))])
file.close()
with open("POSdata.csv", "r") as fp:
    x = from_csv(fp)
print(x)
file.close()
"""
Question 2.  The commented out portions were to check to make sure I was doing 
things correctly by comparing to an SEZ coordinate transformation function we 
wrote in ASEN5050
"""
Beispiel #16
0
 def get_position(self):
     return navpy.ecef2lla(np.array(self.frame_properties.estimated_position.T)[0])
Beispiel #17
0
def load(flight_dir):
    imu_data = []
    gps_data = []
    filter_data = []

    # load imu/gps data files
    imu_file = flight_dir + "/imu.csv"
    gps_file = flight_dir + "/gps.csv"

    # calibration by plotting and eye-balling (just finding center point, no
    # normalization cooked into calibration.)
    #hx_coeffs = np.array([ 1.0,  -1.5], dtype=np.float64)
    #hy_coeffs = np.array([ 1.0, -78.5], dtype=np.float64)
    #hz_coeffs = np.array([ 1.0, -156.5], dtype=np.float64)

    #~/Projects/PILLS/Phantom\ 3\ Flight\ Data/2016-03-22\ --\ imagery_0012\ -\ 400\ ft\ survey
    #hx_coeffs = np.array([ 0.01857771, -0.18006661], dtype=np.float64)
    #hy_coeffs = np.array([ 0.01856938, -1.20854406], dtype=np.float64)
    #hz_coeffs = np.array([ 0.01559645,  2.81011976], dtype=np.float64)

    # ~/Projects/PILLS/Phantom\ 3\ Flight\ Data/imagery_0009 - 0012
    #hx_coeffs = np.array([ 0.01789447,  3.70605872], dtype=np.float64)
    #hy_coeffs = np.array([ 0.017071,    0.7125617], dtype=np.float64)
    #hz_coeffs = np.array([ 0.01447557, -6.54621951], dtype=np.float64)

    # ~/Projects/PILLS/2016-04-04\ --\ imagery_0002
    # ~/Projects/PILLS/2016-04-14\ --\ imagery_0003
    # ~/Projects/PILLS/2016-04-14\ --\ imagery_0004
    #hx_coeffs = np.array([ 0.01658555, -0.07790598], dtype=np.float64)
    #hy_coeffs = np.array([ 0.01880532, -1.26548151], dtype=np.float64)
    #hz_coeffs = np.array([ 0.01339084,  2.61905809], dtype=np.float64)

    # ~/Projects/PILLS/2016-05-12\ --\ imagery_0004
    #hx_coeffs = np.array([ 0.01925678,  0.01527908], dtype=np.float64)
    #hy_coeffs = np.array([ 0.01890112, -1.18040666], dtype=np.float64)
    #hz_coeffs = np.array([ 0.01645011,  2.87769626], dtype=np.float64)

    #hx_func = np.poly1d(hx_coeffs)
    #hy_func = np.poly1d(hy_coeffs)
    #hz_func = np.poly1d(hz_coeffs)

    # ~/Projects/PILLS/2016-06-29\ --\ calibration_0002/
    # mag_affine = np.array(
    #     [[ 0.0223062041, -0.0002700799, -0.0001325525,  1.2016235718],
    #      [-0.0002700799,  0.0229484854,  0.0000356172,  0.1177744077],
    #      [-0.0001325525,  0.0000356172,  0.0206129279, -3.2713740483],
    #      [ 0.          ,  0.          ,  0.          ,  1.          ]]
    # )

    # Phantom 3 - Aug 2016 (ellipse cal)
    mag_affine = np.array(
        [[0.0189725067, 0.0000203615, 0.0002139272, -0.0134053645],
         [0.0000760692, 0.0180178765, 0.0000389461, -1.044762755],
         [0.0002417847, 0.0000458039, 0.0171450614, 2.647911793],
         [0., 0., 0., 1.]])
    # Phantom 3 - Aug 2016 (ekf cal)
    # mag_affine = np.array(
    #     [[ 0.0181297161,  0.000774339,  -0.002037224 , -0.2576406372],
    #      [ 0.0002434548,  0.018469032,   0.0016475328, -0.8452362072],
    #      [ 0.0000145964,  0.000267444,   0.0159433791,  2.5630653789],
    #      [ 0.          ,  0.         ,   0.          ,  1.          ]]
    # )
    print mag_affine

    fimu = fileinput.input(imu_file)
    for line in fimu:
        #print line
        if not re.search('Time', line):
            tokens = line.split(',')
            #print len(tokens)
            if len(tokens) == 11 and isFloat(tokens[10]):
                #print '"' + tokens[10] + '"'
                (time, p, q, r, ax, ay, az, hx, hy, hz, temp) = tokens
                # remap axis before applying mag calibration
                p = -float(p)
                q = float(q)
                r = -float(r)
                ax = -float(ax) * g
                ay = float(ay) * g
                az = -float(az) * g
                hx = -float(hx)
                hy = float(hy)
                hz = -float(hz)
                mag_orientation = 'newer'
                if mag_orientation == 'older':
                    #hx_new = hx_func(float(hx))
                    #hy_new = hy_func(float(hy))
                    #hz_new = hz_func(float(hz))
                    s = [hx, hy, hz]
                elif mag_orientation == 'newer':
                    # remap for 2016-05-12 (0004) data set
                    #hx_new = hx_func(float(-hy))
                    #hy_new = hy_func(float(-hx))
                    #hz_new = hz_func(float(-hz))
                    s = [-hy, -hx, -hz]
                # mag calibration mapping via mag_affine matrix
                hs = np.hstack([s, 1.0])
                hf = np.dot(mag_affine, hs)
                #print hf[:3]
                #norm = np.linalg.norm([hx_new, hy_new, hz_new])
                #hx_new /= norm
                #hy_new /= norm
                #hz_new /= norm

                imu = nav.structs.IMUdata()
                imu.time = float(time) / 1000000.0
                imu.p = p
                imu.q = q
                imu.r = r
                imu.ax = ax
                imu.ay = ay
                imu.az = az
                imu.hx = hf[0]
                imu.hy = hf[1]
                imu.hz = hf[2]
                #float(hf[0]), float(hf[1]), float(hf[2]),
                imu.temp = float(temp)
                imu_data.append(imu)

    fgps = fileinput.input(gps_file)
    for line in fgps:
        if not re.search('Timestamp', line):
            #print line
            tokens = line.split(',')
            #print len(tokens)
            if len(tokens) == 14:
                (time, itow, ecefx, ecefy, ecefz, ecefvx, ecefvy, ecefvz,
                 fixtype, posacc, spdacc, posdop, numsvs, flags) = tokens
            elif len(tokens) == 19:
                (time, itow, lat, lon, alt, ecefx, ecefy, ecefz, ecefvx,
                 ecefvy, ecefvz, fixtype, posacc, spdacc, posdop, numsvs,
                 flags, diff_status, carrier_status) = tokens

            # wgs84 position
            pos_source = 'llh'  # 'llh' or 'ecef'
            llh = navpy.ecef2lla([
                float(ecefx) / 100.0,
                float(ecefy) / 100.0,
                float(ecefz) / 100.0
            ], "deg")
            # velocity
            ned = navpy.ecef2ned([
                float(ecefvx) / 100.0,
                float(ecefvy) / 100.0,
                float(ecefvz) / 100.0
            ], llh[0], llh[1], llh[2])
            if int(numsvs) >= 4:
                gps = nav.structs.GPSdata()
                gps.time = float(time) / 1000000.0
                #gps.status = int(status)
                gps.unix_sec = float(time) / 1000000.0  # make filter happy
                gps.lat = float(lat)
                gps.lon = float(lon)
                gps.alt = float(alt)
                gps.vn = ned[0]
                gps.ve = ned[1]
                gps.vd = ned[2]
                gps_data.append(gps)
    return imu_data, gps_data, filter_data
Beispiel #18
0
import numpy as np
import navpy as nav
import matplotlib.pyplot as plt
import csv
import math
from rinex2_nav import parse_RINEX2_nav_file

####USER INPUTS#####
NIST = np.array([-1288398.360, -4721697.040,
                 4078625.500])  #NIST_loction.txt file
#NIST = np.array([ 1112162.0013, -4842854.3652, 3985497.5097]) # USN8
####################
#NIST = np.array([-1288349.1490, -4721661.0480,  4078676.0170]) #NIST as reported from observation file
NISTlla = nav.ecef2lla(NIST, latlon_unit='deg')
brdc253 = parse_RINEX2_nav_file(
    'brdc2530_19n.txt'
)  #Utilize parse_RINEX function to build a dictionary of GPS information for a specific GPS week
header = brdc253[0]  #This contains the header information
GPSdict = brdc253[1]  #This contains the GPS dcitionary
###############
#Read the csv file produced from matlab script read_rinex_obs8 to produce
#needed plots for C1 and P2 for PRN
tow = []
C1 = []
P2 = []
L1 = []
L2 = []
P3 = []
prns = []

gprn = "{}{:02d}".format('G', 8)
Beispiel #19
0
 def get_position(self):
     return navpy.ecef2lla(
         np.array(self.properties.estimated_position.T)[0])
Beispiel #20
0
def load(flight_dir):
    result = {}
    
    gps_data = []
    filter_data = []

    # load imu/gps data files
    imu_file = os.path.join(flight_dir, "imu.csv")
    gps_file = os.path.join(flight_dir, "gps.csv")
    filter_post = os.path.join(flight_dir, "filter-post-ins.txt")
    
    # calibration by plotting and eye-balling (just finding center point, no
    # normalization cooked into calibration.)
    #hx_coeffs = np.array([ 1.0,  -1.5], dtype=np.float64)
    #hy_coeffs = np.array([ 1.0, -78.5], dtype=np.float64)
    #hz_coeffs = np.array([ 1.0, -156.5], dtype=np.float64)
    
    #~/Projects/PILLS/Phantom\ 3\ Flight\ Data/2016-03-22\ --\ imagery_0012\ -\ 400\ ft\ survey
    #hx_coeffs = np.array([ 0.01857771, -0.18006661], dtype=np.float64)
    #hy_coeffs = np.array([ 0.01856938, -1.20854406], dtype=np.float64)
    #hz_coeffs = np.array([ 0.01559645,  2.81011976], dtype=np.float64)

    # ~/Projects/PILLS/Phantom\ 3\ Flight\ Data/imagery_0009 - 0012
    #hx_coeffs = np.array([ 0.01789447,  3.70605872], dtype=np.float64)
    #hy_coeffs = np.array([ 0.017071,    0.7125617], dtype=np.float64)
    #hz_coeffs = np.array([ 0.01447557, -6.54621951], dtype=np.float64)
    
    # ~/Projects/PILLS/2016-04-04\ --\ imagery_0002
    # ~/Projects/PILLS/2016-04-14\ --\ imagery_0003
    # ~/Projects/PILLS/2016-04-14\ --\ imagery_0004
    #hx_coeffs = np.array([ 0.01658555, -0.07790598], dtype=np.float64)
    #hy_coeffs = np.array([ 0.01880532, -1.26548151], dtype=np.float64)
    #hz_coeffs = np.array([ 0.01339084,  2.61905809], dtype=np.float64)

    # ~/Projects/PILLS/2016-05-12\ --\ imagery_0004
    #hx_coeffs = np.array([ 0.01925678,  0.01527908], dtype=np.float64)
    #hy_coeffs = np.array([ 0.01890112, -1.18040666], dtype=np.float64)
    #hz_coeffs = np.array([ 0.01645011,  2.87769626], dtype=np.float64)

    #hx_func = np.poly1d(hx_coeffs)
    #hy_func = np.poly1d(hy_coeffs)
    #hz_func = np.poly1d(hz_coeffs)

    # ~/Projects/PILLS/2016-06-29\ --\ calibration_0002/
    # mag_affine = np.array(
    #     [[ 0.0223062041, -0.0002700799, -0.0001325525,  1.2016235718],
    #      [-0.0002700799,  0.0229484854,  0.0000356172,  0.1177744077],
    #      [-0.0001325525,  0.0000356172,  0.0206129279, -3.2713740483],
    #      [ 0.          ,  0.          ,  0.          ,  1.          ]]
    # )

    # Phantom 3 - Aug 2016 (ellipse cal)
    # mag_affine = np.array(
    #     [[ 0.0189725067,  0.0000203615,  0.0002139272, -0.0134053645],
    #      [ 0.0000760692,  0.0180178765,  0.0000389461, -1.044762755 ],
    #      [ 0.0002417847,  0.0000458039,  0.0171450614,  2.647911793 ],
    #      [ 0.          ,  0.          ,  0.          ,  1.          ]]
    # )
    # Phantom 3 - Aug 2016 (ekf cal)
    # mag_affine = np.array(
    #     [[ 0.0181297161,  0.000774339,  -0.002037224 , -0.2576406372],
    #      [ 0.0002434548,  0.018469032,   0.0016475328, -0.8452362072],
    #      [ 0.0000145964,  0.000267444,   0.0159433791,  2.5630653789],
    #      [ 0.          ,  0.         ,   0.          ,  1.          ]]
    # )

    # 2017-06-07_23-43-50
    mag_affine = np.array(
        [[ 0.0182094965,  0.0001891445,  0.0005079058, -1.0275778093],
         [ 0.0001891445,  0.0188836673,  0.0003014306, -0.7472003813],
         [ 0.0005079058,  0.0003014306,  0.0176589615,  0.9988130618],
         [ 0.          ,  0.          ,  0.          ,  1.          ]]
    )
    print(mag_affine)

    result['imu'] = []
    with open(imu_file, 'r') as fimu:
        reader = csv.DictReader(fimu)
        for row in reader:
            # print(row)
            imu = Record()
            try:
                imu.time = float(row['Time Stamp (ns since boot)']) / 1000000000.0
                imu.p = float(row['xGyro (rad/s)'])
                imu.q = float(row['yGyro (rad/s)'])
                imu.r = float(row['zGyro (rad/s)'])
                imu.ax = float(row[' xAccel (g)'])*g
                imu.ay = float(row[' yAccel (g)'])*g
                imu.az = float(row[' zAccel (g)'])*g
                imu.hx = float(row[' xMag (uT)']) # not logged
                imu.hy = float(row[' xMag (uT)']) # not logged
                imu.hz = float(row[' xMag (uT)']) # not logged
                temp = row[' Temp (C)']
                if temp != 'N/A':
                    imu.temp = float(row[' Temp (C)']) # not logged
                else:
                    imu.temp = 0.0
                result['imu'].append( imu )
            except:
                print('[IMU] failed to parse incomplete row:', row) 

    result['gps'] = []
    with open(gps_file, 'r') as fgps:
        reader = csv.DictReader(fgps)
        for row in reader:
            #print(row)
            gps = Record()
            try:
                gps.time = float(row['Timestamp (ns since boot)']) / 1000000000.0
                gps.unix_sec = gps.time # hack
                #gps.lat = float(row['Lat (deg)'])
                #gps.lon = float(row['Lon (deg)'])
                #gps.alt = float(row['Alt Geoid EGM 96 (m)'])
                ecefx = float(row['ecefX (cm)'])
                ecefy = float(row['ecefY (cm)'])
                ecefz = float(row['ecefZ (cm)'])
                ecefvx = float(row['ecefVX (cm/s)'])
                ecefvy = float(row['ecefVY (cm/s)'])
                ecefvz = float(row['ecefVZ (cm/s)'])
                gps.sats = int(row['Num SVs Used'])
                # wgs84 position
                pos_source = 'llh'  # 'llh' or 'ecef'
                llh = navpy.ecef2lla([float(ecefx)/100.0,
                                      float(ecefy)/100.0,
                                      float(ecefz)/100.0], "deg")
                gps.lat = llh[0]
                gps.lon = llh[1]
                gps.alt = llh[2]
                # velocity
                ned = navpy.ecef2ned([float(ecefvx)/100.0,
                                      float(ecefvy)/100.0,
                                      float(ecefvz)/100.0],
                                     llh[0], llh[1], llh[2])
                gps.vn = ned[0]
                gps.ve = ned[1]
                gps.vd = ned[2]
                if int(row['Fix Type']) == 3:
                    result['gps'].append(gps)
            except:
                print('[GPS] failed to parse incomplete row:', row) 

    result['filter'] = []
    # load filter (post process) records if they exist (for comparison
    # purposes)
    if os.path.exists(filter_post):
        print('found filter-post-ins.txt, using that for ekf results')
        result['filter'] = []
        ffilter = fileinput.input(filter_post)
        for line in ffilter:
            tokens = re.split('[,\s]+', line.rstrip())
            lat = float(tokens[1])
            lon = float(tokens[2])
            if abs(lat) > 0.0001 and abs(lon) > 0.0001:
                filterpt = Record()
                filterpt.time = float(tokens[0])
                filterpt.lat = lat*d2r
                filterpt.lon = lon*d2r
                filterpt.alt = float(tokens[3])
                filterpt.vn = float(tokens[4])
                filterpt.ve = float(tokens[5])
                filterpt.vd = float(tokens[6])
                filterpt.phi = float(tokens[7])*d2r
                filterpt.the = float(tokens[8])*d2r
                psi = float(tokens[9])
                if psi > 180.0:
                    psi = psi - 360.0
                if psi < -180.0:
                    psi = psi + 360.0
                filterpt.psi = psi*d2r
                result['filter'].append(filterpt)

    return result
Beispiel #21
0
def load(flight_dir):
    result = {}

    gps_data = []
    filter_data = []

    # load imu/gps data files
    imu_file = flight_dir + "/imu.csv"
    gps_file = flight_dir + "/gps.csv"
    filter_post = flight_dir + "/filter-post-ins.txt"

    # calibration by plotting and eye-balling (just finding center point, no
    # normalization cooked into calibration.)
    #hx_coeffs = np.array([ 1.0,  -1.5], dtype=np.float64)
    #hy_coeffs = np.array([ 1.0, -78.5], dtype=np.float64)
    #hz_coeffs = np.array([ 1.0, -156.5], dtype=np.float64)

    #~/Projects/PILLS/Phantom\ 3\ Flight\ Data/2016-03-22\ --\ imagery_0012\ -\ 400\ ft\ survey
    #hx_coeffs = np.array([ 0.01857771, -0.18006661], dtype=np.float64)
    #hy_coeffs = np.array([ 0.01856938, -1.20854406], dtype=np.float64)
    #hz_coeffs = np.array([ 0.01559645,  2.81011976], dtype=np.float64)

    # ~/Projects/PILLS/Phantom\ 3\ Flight\ Data/imagery_0009 - 0012
    #hx_coeffs = np.array([ 0.01789447,  3.70605872], dtype=np.float64)
    #hy_coeffs = np.array([ 0.017071,    0.7125617], dtype=np.float64)
    #hz_coeffs = np.array([ 0.01447557, -6.54621951], dtype=np.float64)

    # ~/Projects/PILLS/2016-04-04\ --\ imagery_0002
    # ~/Projects/PILLS/2016-04-14\ --\ imagery_0003
    # ~/Projects/PILLS/2016-04-14\ --\ imagery_0004
    #hx_coeffs = np.array([ 0.01658555, -0.07790598], dtype=np.float64)
    #hy_coeffs = np.array([ 0.01880532, -1.26548151], dtype=np.float64)
    #hz_coeffs = np.array([ 0.01339084,  2.61905809], dtype=np.float64)

    # ~/Projects/PILLS/2016-05-12\ --\ imagery_0004
    #hx_coeffs = np.array([ 0.01925678,  0.01527908], dtype=np.float64)
    #hy_coeffs = np.array([ 0.01890112, -1.18040666], dtype=np.float64)
    #hz_coeffs = np.array([ 0.01645011,  2.87769626], dtype=np.float64)

    #hx_func = np.poly1d(hx_coeffs)
    #hy_func = np.poly1d(hy_coeffs)
    #hz_func = np.poly1d(hz_coeffs)

    # ~/Projects/PILLS/2016-06-29\ --\ calibration_0002/
    # mag_affine = np.array(
    #     [[ 0.0223062041, -0.0002700799, -0.0001325525,  1.2016235718],
    #      [-0.0002700799,  0.0229484854,  0.0000356172,  0.1177744077],
    #      [-0.0001325525,  0.0000356172,  0.0206129279, -3.2713740483],
    #      [ 0.          ,  0.          ,  0.          ,  1.          ]]
    # )

    # Phantom 3 - Aug 2016 (ellipse cal)
    # mag_affine = np.array(
    #     [[ 0.0189725067,  0.0000203615,  0.0002139272, -0.0134053645],
    #      [ 0.0000760692,  0.0180178765,  0.0000389461, -1.044762755 ],
    #      [ 0.0002417847,  0.0000458039,  0.0171450614,  2.647911793 ],
    #      [ 0.          ,  0.          ,  0.          ,  1.          ]]
    # )
    # Phantom 3 - Aug 2016 (ekf cal)
    # mag_affine = np.array(
    #     [[ 0.0181297161,  0.000774339,  -0.002037224 , -0.2576406372],
    #      [ 0.0002434548,  0.018469032,   0.0016475328, -0.8452362072],
    #      [ 0.0000145964,  0.000267444,   0.0159433791,  2.5630653789],
    #      [ 0.          ,  0.         ,   0.          ,  1.          ]]
    # )

    # 2017-06-07_23-43-50
    mag_affine = np.array(
        [[0.0182094965, 0.0001891445, 0.0005079058, -1.0275778093],
         [0.0001891445, 0.0188836673, 0.0003014306, -0.7472003813],
         [0.0005079058, 0.0003014306, 0.0176589615, 0.9988130618],
         [0., 0., 0., 1.]])
    print(mag_affine)

    result['imu'] = []
    fimu = fileinput.input(imu_file)
    for line in fimu:
        #print line
        if not re.search('Time', line):
            tokens = line.split(',')
            #print len(tokens)
            if len(tokens) == 11 and isFloat(tokens[10]):
                #print '"' + tokens[10] + '"'
                (time, p, q, r, ax, ay, az, hx, hy, hz, temp) = tokens
                # remap axis before applying mag calibration
                p = -float(p)
                q = float(q)
                r = -float(r)
                ax = -float(ax) * g
                ay = float(ay) * g
                az = -float(az) * g
                hx = -float(hx)
                hy = float(hy)
                hz = -float(hz)
                mag_orientation = 'random1'
                if mag_orientation == 'older':
                    #hx_new = hx_func(float(hx))
                    #hy_new = hy_func(float(hy))
                    #hz_new = hz_func(float(hz))
                    s = [hx, hy, hz]
                elif mag_orientation == 'newer':
                    # remap for 2016-05-12 (0004) data set
                    #hx_new = hx_func(float(-hy))
                    #hy_new = hy_func(float(-hx))
                    #hz_new = hz_func(float(-hz))
                    s = [-hy, -hx, -hz]
                elif mag_orientation == 'random1':
                    # remap for 2016-05-12 (0004) data set
                    #hx_new = hx_func(float(-hy))
                    #hy_new = hy_func(float(-hx))
                    #hz_new = hz_func(float(-hz))
                    s = [-hy, -hx, hz]
                # mag calibration mapping via mag_affine matrix
                hs = np.hstack([s, 1.0])
                hf = np.dot(mag_affine, hs)
                #print hf[:3]
                #norm = np.linalg.norm([hx_new, hy_new, hz_new])
                #hx_new /= norm
                #hy_new /= norm
                #hz_new /= norm

                imu = Record()
                imu.time = float(time) / 1000000.0
                imu.p = p
                imu.q = q
                imu.r = r
                imu.ax = ax
                imu.ay = ay
                imu.az = az
                do_mag_transform = True
                if do_mag_transform:
                    imu.hx = hf[0]
                    imu.hy = hf[1]
                    imu.hz = hf[2]
                else:
                    imu.hx = s[0]
                    imu.hy = s[1]
                    imu.hz = s[2]
                #float(hf[0]), float(hf[1]), float(hf[2]),
                imu.temp = float(temp)
                result['imu'].append(imu)

    result['gps'] = []
    fgps = fileinput.input(gps_file)
    for line in fgps:
        if not re.search('Timestamp', line):
            # print line
            tokens = line.split(',')
            # print 'gps tokens:', len(tokens)
            if len(tokens) == 14:
                (time, itow, ecefx, ecefy, ecefz, ecefvx, ecefvy, ecefvz,
                 fixtype, posacc, spdacc, posdop, numsvs, flags) = tokens
            elif len(tokens) == 19:
                (time, itow, lat, lon, alt, ecefx, ecefy, ecefz, ecefvx,
                 ecefvy, ecefvz, fixtype, posacc, spdacc, posdop, numsvs,
                 flags, diff_status, carrier_status) = tokens

            # wgs84 position
            pos_source = 'llh'  # 'llh' or 'ecef'
            llh = navpy.ecef2lla([
                float(ecefx) / 100.0,
                float(ecefy) / 100.0,
                float(ecefz) / 100.0
            ], "deg")
            # velocity
            ned = navpy.ecef2ned([
                float(ecefvx) / 100.0,
                float(ecefvy) / 100.0,
                float(ecefvz) / 100.0
            ], llh[0], llh[1], llh[2])
            if int(numsvs) >= 4:
                gps = Record()
                gps.time = float(time) / 1000000.0
                #gps.status = int(status)
                gps.unix_sec = float(time) / 1000000.0  # make filter happy
                gps.lat = float(lat)
                gps.lon = float(lon)
                gps.alt = float(alt)
                gps.vn = ned[0]
                gps.ve = ned[1]
                gps.vd = ned[2]
                result['gps'].append(gps)

    result['filter'] = []
    # load filter (post process) records if they exist (for comparison
    # purposes)
    if os.path.exists(filter_post):
        print('found filter-post-ins.txt, using that for ekf results')
        result['filter'] = []
        ffilter = fileinput.input(filter_post)
        for line in ffilter:
            tokens = re.split('[,\s]+', line.rstrip())
            lat = float(tokens[1])
            lon = float(tokens[2])
            if abs(lat) > 0.0001 and abs(lon) > 0.0001:
                filterpt = Record()
                filterpt.time = float(tokens[0])
                filterpt.lat = lat * d2r
                filterpt.lon = lon * d2r
                filterpt.alt = float(tokens[3])
                filterpt.vn = float(tokens[4])
                filterpt.ve = float(tokens[5])
                filterpt.vd = float(tokens[6])
                filterpt.phi = float(tokens[7]) * d2r
                filterpt.the = float(tokens[8]) * d2r
                psi = float(tokens[9])
                if psi > 180.0:
                    psi = psi - 360.0
                if psi < -180.0:
                    psi = psi + 360.0
                filterpt.psi = psi * d2r
                result['filter'].append(filterpt)

    return result
Beispiel #22
0
def load(flight_dir):
    imu_data = []
    gps_data = []
    filter_data = []

    # load imu/gps data files
    imu_file = flight_dir + "/imu.csv"
    gps_file = flight_dir + "/gps.csv"

    # calibration by plotting and eye-balling (just finding center point, no
    # normalization cooked into calibration.)
    #hx_coeffs = np.array([ 1.0,  -1.5], dtype=np.float64)
    #hy_coeffs = np.array([ 1.0, -78.5], dtype=np.float64)
    #hz_coeffs = np.array([ 1.0, -156.5], dtype=np.float64)
    
    #~/Projects/PILLS/Phantom\ 3\ Flight\ Data/2016-03-22\ --\ imagery_0012\ -\ 400\ ft\ survey
    #hx_coeffs = np.array([ 0.01857771, -0.18006661], dtype=np.float64)
    #hy_coeffs = np.array([ 0.01856938, -1.20854406], dtype=np.float64)
    #hz_coeffs = np.array([ 0.01559645,  2.81011976], dtype=np.float64)

    # ~/Projects/PILLS/Phantom\ 3\ Flight\ Data/imagery_0009 - 0012
    #hx_coeffs = np.array([ 0.01789447,  3.70605872], dtype=np.float64)
    #hy_coeffs = np.array([ 0.017071,    0.7125617], dtype=np.float64)
    #hz_coeffs = np.array([ 0.01447557, -6.54621951], dtype=np.float64)
    
    # ~/Projects/PILLS/2016-04-04\ --\ imagery_0002
    # ~/Projects/PILLS/2016-04-14\ --\ imagery_0003
    # ~/Projects/PILLS/2016-04-14\ --\ imagery_0004
    #hx_coeffs = np.array([ 0.01658555, -0.07790598], dtype=np.float64)
    #hy_coeffs = np.array([ 0.01880532, -1.26548151], dtype=np.float64)
    #hz_coeffs = np.array([ 0.01339084,  2.61905809], dtype=np.float64)

    # ~/Projects/PILLS/2016-05-12\ --\ imagery_0004
    #hx_coeffs = np.array([ 0.01925678,  0.01527908], dtype=np.float64)
    #hy_coeffs = np.array([ 0.01890112, -1.18040666], dtype=np.float64)
    #hz_coeffs = np.array([ 0.01645011,  2.87769626], dtype=np.float64)

    #hx_func = np.poly1d(hx_coeffs)
    #hy_func = np.poly1d(hy_coeffs)
    #hz_func = np.poly1d(hz_coeffs)

    # ~/Projects/PILLS/2016-06-29\ --\ calibration_0002/
    # mag_affine = np.array(
    #     [[ 0.0223062041, -0.0002700799, -0.0001325525,  1.2016235718],
    #      [-0.0002700799,  0.0229484854,  0.0000356172,  0.1177744077],
    #      [-0.0001325525,  0.0000356172,  0.0206129279, -3.2713740483],
    #      [ 0.          ,  0.          ,  0.          ,  1.          ]]
    # )

    # Phantom 3 - Aug 2016 (ellipse cal)
    mag_affine = np.array(
        [[ 0.0189725067,  0.0000203615,  0.0002139272, -0.0134053645],
         [ 0.0000760692,  0.0180178765,  0.0000389461, -1.044762755 ],
         [ 0.0002417847,  0.0000458039,  0.0171450614,  2.647911793 ],
         [ 0.          ,  0.          ,  0.          ,  1.          ]]
    )
    # Phantom 3 - Aug 2016 (ekf cal)
    # mag_affine = np.array(
    #     [[ 0.0181297161,  0.000774339,  -0.002037224 , -0.2576406372],
    #      [ 0.0002434548,  0.018469032,   0.0016475328, -0.8452362072],
    #      [ 0.0000145964,  0.000267444,   0.0159433791,  2.5630653789],
    #      [ 0.          ,  0.         ,   0.          ,  1.          ]]
    # )
    print mag_affine
    
    fimu = fileinput.input(imu_file)
    for line in fimu:
        #print line
        if not re.search('Time', line):
            tokens = line.split(',')
            #print len(tokens)
            if len(tokens) == 11 and isFloat(tokens[10]):
                #print '"' + tokens[10] + '"'
                (time, p, q, r, ax, ay, az, hx, hy, hz, temp) = tokens
                # remap axis before applying mag calibration
                p = -float(p)
                q =  float(q)
                r = -float(r)
                ax = -float(ax)*g
                ay =  float(ay)*g
                az = -float(az)*g
                hx = -float(hx)
                hy =  float(hy)
                hz = -float(hz)
                mag_orientation = 'newer'
                if mag_orientation == 'older':
                    #hx_new = hx_func(float(hx))
                    #hy_new = hy_func(float(hy))
                    #hz_new = hz_func(float(hz))
                    s = [hx, hy, hz]
                elif mag_orientation == 'newer':
                    # remap for 2016-05-12 (0004) data set
                    #hx_new = hx_func(float(-hy))
                    #hy_new = hy_func(float(-hx))
                    #hz_new = hz_func(float(-hz))
                    s = [-hy, -hx, -hz]
                # mag calibration mapping via mag_affine matrix
                hs = np.hstack( [s, 1.0] )
                hf = np.dot(mag_affine, hs)
                #print hf[:3]
                #norm = np.linalg.norm([hx_new, hy_new, hz_new])
                #hx_new /= norm
                #hy_new /= norm
                #hz_new /= norm
                
                imu = nav.structs.IMUdata()
                imu.time = float(time)/1000000.0
                imu.p = p
                imu.q = q
                imu.r = r
                imu.ax = ax
                imu.ay = ay
                imu.az = az
                imu.hx = hf[0]
                imu.hy = hf[1]
                imu.hz = hf[2]
                #float(hf[0]), float(hf[1]), float(hf[2]),
                imu.temp = float(temp)
                imu_data.append( imu )

    fgps = fileinput.input(gps_file)
    for line in fgps:
        if not re.search('Timestamp', line):
            #print line
            tokens = line.split(',')
            #print len(tokens)
            if len(tokens) == 14:
                (time, itow, ecefx, ecefy, ecefz, ecefvx, ecefvy, ecefvz,
                 fixtype, posacc, spdacc, posdop, numsvs, flags) = tokens
            elif len(tokens) == 19:
                (time, itow, lat, lon, alt, ecefx, ecefy, ecefz,
                 ecefvx, ecefvy, ecefvz,
                 fixtype, posacc, spdacc, posdop, numsvs, flags,
                 diff_status, carrier_status) = tokens
                
            # wgs84 position
            pos_source = 'llh'  # 'llh' or 'ecef'
            llh = navpy.ecef2lla([float(ecefx)/100.0,
                                  float(ecefy)/100.0,
                                  float(ecefz)/100.0], "deg")
            # velocity
            ned = navpy.ecef2ned([float(ecefvx)/100.0,
                                  float(ecefvy)/100.0,
                                  float(ecefvz)/100.0],
                                 llh[0], llh[1], llh[2])
            if int(numsvs) >= 4:
                gps = nav.structs.GPSdata()
                gps.time = float(time)/1000000.0
                #gps.status = int(status)
                gps.unix_sec = float(time)/1000000.0 # make filter happy
                gps.lat = float(lat)
                gps.lon = float(lon)
                gps.alt = float(alt)
                gps.vn = ned[0]
                gps.ve = ned[1]
                gps.vd = ned[2]
                gps_data.append(gps)
    return imu_data, gps_data, filter_data