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)
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)
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
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)
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)
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
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)
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)
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)
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)
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)
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)
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)
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)
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
"""
def get_position(self):
return navpy.ecef2lla(np.array(self.frame_properties.estimated_position.T)[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
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)
def get_position(self):
return navpy.ecef2lla(
np.array(self.properties.estimated_position.T)[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
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
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
