def load(flight_dir, recalibrate=None):
result = {}
# load imu/gps data files
imu_file = flight_dir + "/imu-0.txt"
imucal_json = flight_dir + "/imucal.json"
imucal_xml = flight_dir + "/imucal.xml"
gps_file = flight_dir + "/gps-0.txt"
air_file = flight_dir + "/air-0.txt"
filter_file = flight_dir + "/filter-0.txt"
pilot_file = flight_dir + "/pilot-0.txt"
act_file = flight_dir + "/act-0.txt"
ap_file = flight_dir + "/ap-0.txt"
imu_bias_file = flight_dir + "/imubias.txt"
# HEY: in the latest aura code, calibrated magnetometer is logged,
# not raw magnetometer, so we don't need to correct here. We
# could 'back correct' if we wanted original values for some
# reason (using the calibration matrix saved with each flight
# log.)
# # vireo_01
# mag_affine = np.array(
# [[ 1.6207467043, 0.0228992488, 0.0398638786, 0.1274248748],
# [-0.0169905025, 1.7164397411, -0.0001290047, -0.1140304977],
# [ 0.0424979948, -0.0038515935, 1.7193766423, -0.1449816095],
# [ 0. , 0. , 0. , 1. ]]
# )
# Tyr
# mag_affine = np.array(
# [[ 1.810, 0.109, 0.285, -0.237],
# [-0.078, 1.931, -0.171, -0.060],
# [ 0.008, 0.109, 1.929, 0.085],
# [ 0. , 0. , 0. , 1. ]]
# )
# telemaster apm2_101
mag_affine = np.array(
[[ 0.0026424919, 0.0001334248, 0.0000984977, -0.2235908546],
[-0.000081925 , 0.0026419229, 0.0000751835, -0.0010757621],
[ 0.0000219407, 0.0000560341, 0.002541171 , 0.040221458 ],
[ 0. , 0. , 0. , 1. ]]
)
# skywalker apm2_105
# mag_affine = np.array(
# [[ 0.0025834778, 0.0001434776, 0.0001434961, -0.7900775707 ],
# [-0.0001903118, 0.0024796553, 0.0001365238, 0.1881142449 ],
# [ 0.0000556437, 0.0001329724, 0.0023791184, 0.1824851582, ],
# [ 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000 ]]
# )
#np.set_printoptions(precision=10,suppress=True)
#print mag_affine
result['imu'] = []
fimu = fileinput.input(imu_file)
for line in fimu:
tokens = re.split('[,\s]+', line.rstrip())
#s = [float(tokens[7]), float(tokens[8]), float(tokens[9]), 1.0]
#hf = np.dot(mag_affine, s)
#print hf
imu = IMUdata()
imu.time = float(tokens[0])
imu.p = float(tokens[1])
imu.q = float(tokens[2])
imu.r = float(tokens[3])
imu.ax = float(tokens[4])
imu.ay = float(tokens[5])
imu.az = float(tokens[6])
imu.hx = float(tokens[7])
imu.hy = float(tokens[8])
imu.hz = float(tokens[9])
imu.temp = float(tokens[10])
result['imu'].append( imu )
result['gps'] = []
fgps = fileinput.input(gps_file)
last_time = -1.0
for line in fgps:
# Note: the aura logs unix time of the gps record, not tow,
# but for the purposes of the insgns algorithm, it's only
# important to have a properly incrementing clock, it doesn't
# really matter what the zero reference point of time is.
tokens = re.split('[,\s]+', line.rstrip())
time = float(tokens[0])
sats = int(tokens[8])
if sats >= 5 and time > last_time:
gps = GPSdata()
gps.time = time
gps.unix_sec = float(tokens[7])
gps.lat = float(tokens[1])
gps.lon = float(tokens[2])
gps.alt = float(tokens[3])
gps.vn = float(tokens[4])
gps.ve = float(tokens[5])
gps.vd = float(tokens[6])
gps.sats = sats
result['gps'].append(gps)
last_time = time
if os.path.exists(air_file):
result['air'] = []
fair = fileinput.input(air_file)
for line in fair:
tokens = re.split('[,\s]+', line.rstrip())
air = Airdata()
air.time = float(tokens[0])
air.static_press = float(tokens[1])
air.diff_press = 0.0 # not directly available in flight log
air.temp = float(tokens[2])
air.airspeed = float(tokens[3])
air.alt_press = float(tokens[4])
air.alt_true = float(tokens[5])
result['air'].append( air )
# load filter records if they exist (for comparison purposes)
result['filter'] = []
ffilter = fileinput.input(filter_file)
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:
nav = NAVdata()
nav.time = float(tokens[0])
nav.lat = lat*d2r
nav.lon = lon*d2r
nav.alt = float(tokens[3])
nav.vn = float(tokens[4])
nav.ve = float(tokens[5])
nav.vd = float(tokens[6])
nav.phi = float(tokens[7])*d2r
nav.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
nav.psi = psi*d2r
result['filter'].append(nav)
if os.path.exists(pilot_file):
result['pilot'] = []
fpilot = fileinput.input(pilot_file)
for line in fpilot:
tokens = re.split('[,\s]+', line.rstrip())
pilot = Controldata()
pilot.time = float(tokens[0])
pilot.aileron = float(tokens[1])
pilot.elevator = float(tokens[2])
pilot.throttle = float(tokens[3])
pilot.rudder = float(tokens[4])
pilot.gear = float(tokens[5])
pilot.flaps = float(tokens[6])
pilot.aux1 = float(tokens[7])
pilot.auto_manual = float(tokens[8])
result['pilot'].append(pilot)
if os.path.exists(act_file):
result['act'] = []
fact = fileinput.input(act_file)
for line in fact:
tokens = re.split('[,\s]+', line.rstrip())
act = Controldata()
act.time = float(tokens[0])
act.aileron = float(tokens[1])
act.elevator = float(tokens[2])
act.throttle = float(tokens[3])
act.rudder = float(tokens[4])
act.gear = float(tokens[5])
act.flaps = float(tokens[6])
act.aux1 = float(tokens[7])
act.auto_manual = float(tokens[8])
result['act'].append(act)
if os.path.exists(ap_file):
result['ap'] = []
fap = fileinput.input(ap_file)
for line in fap:
tokens = re.split('[,\s]+', line.rstrip())
ap = APdata()
ap.time = float(tokens[0])
ap.hdg = float(tokens[1])
ap.roll = float(tokens[2])
ap.alt = float(tokens[3])
ap.pitch = float(tokens[5])
ap.speed = float(tokens[7])
#ap.flight_time = float(tokens[8])
#ap.target_wp = int(tokens[9])
#ap.wp_lon = float(tokens[10])
#ap.wp_lat = float(tokens[11])
#ap.wp_index = int(tokens[12])
#ap.route_size = int(tokens[13])
result['ap'].append(ap)
cal = imucal.Calibration()
if os.path.exists(imucal_json):
imucal_file = imucal_json
elif os.path.exists(imucal_xml):
imucal_file = imucal_xml
else:
imucal_file = None
if imucal_file:
cal.load(imucal_file)
print 'back correcting imu data (to get original raw values)'
result['imu'] = cal.back_correct(result['imu'])
if recalibrate:
print 'recalibrating imu data using alternate calibration file:', recalibrate
rcal = imucal.Calibration()
rcal.load(recalibrate)
result['imu'] = rcal.correct(result['imu'])
return result
def load(csv_file):
result = {}
result['imu'] = []
result['gps'] = []
result['air'] = []
result['filter'] = []
#result['pilot'] = []
result['act'] = []
result['ap'] = []
last_gps_time = -1.0
with open(csv_file, 'rb') as f:
reader = csv.DictReader(f)
for row in reader:
if row['TIME_StartTime'] == '':
# empty time
continue
imu = IMUdata()
imu.time = float(row['TIME_StartTime']) / 1000000.0
imu.p = float(row['IMU_GyroX'])
imu.q = float(row['IMU_GyroY'])
imu.r = float(row['IMU_GyroZ'])
imu.ax = float(row['IMU_AccX'])
imu.ay = float(row['IMU_AccY'])
imu.az = float(row['IMU_AccZ'])
hx = float(row['IMU_MagX'])
hy = float(row['IMU_MagY'])
hz = float(row['IMU_MagZ'])
hf = np.array([hx, hy, hz])
norm = np.linalg.norm(hf)
hf /= norm
imu.hx = hf[0]
imu.hy = hf[1]
imu.hz = hf[2]
#print imu.hx, imu.hy, imu.hz
imu.temp = 15.0
result['imu'].append(imu)
if row['GPS_GPSTime'] != '':
gps = GPSdata()
gps.time = imu.time
gps.unix_sec = float(row['GPS_GPSTime']) / 1000000.0
gps.lat = float(row['GPS_Lat'])
gps.lon = float(row['GPS_Lon'])
gps.alt = float(row['GPS_Alt'])
gps.vn = float(row['GPS_VelN'])
gps.ve = float(row['GPS_VelE'])
gps.vd = float(row['GPS_VelD'])
gps.sats = int(row['GPS_nSat'])
if gps.sats >= 5 and gps.unix_sec > last_gps_time:
result['gps'].append(gps)
last_gps_time = gps.unix_sec
if row['SENS_BaroPres'] != '':
air = Airdata()
air.time = imu.time
air.static_press = float(row['SENS_BaroPres'])
if 'AIR1_DiffPres' in row and row['AIR1_DiffPres'] != '':
air.diff_press = float(row['AIR1_DiffPres'])
else:
air.diff_press = 0.0
if 'AIRS_Temp' in row and row['AIRS_Temp'] != '':
air.temp = float(row['AIRS_Temp'])
else:
air.temp = 0.0
if 'AIRS_IAS' in row and row['AIRS_IAS'] != '':
air.airspeed = float(row['AIRS_IAS']) * mps2kt
else:
air.airspeed = 0.0
if 'AIR1_BaroAlt' in row and row['AIR1_BaroAlt'] != '':
air.alt_press = float(row['AIR1_BaroAlt'])
else:
air.alt_press = 0.0
if 'GPS_Alt' in row and row['GPS_Alt'] != '':
air.alt_true = float(row['GPS_Alt'])
else:
air.alt_true = 0.0
result['air'].append(air)
if row['GPOS_Lat'] != '':
nav = NAVdata()
nav.time = imu.time
nav.lat = float(row['GPOS_Lat']) * d2r
nav.lon = float(row['GPOS_Lon']) * d2r
nav.alt = float(row['GPOS_Alt'])
nav.vn = float(row['GPOS_VelN'])
nav.ve = float(row['GPOS_VelE'])
nav.vd = float(row['GPOS_VelD'])
nav.phi = float(row['ATT_Roll'])
nav.the = float(row['ATT_Pitch'])
psi = float(row['ATT_Yaw'])
if psi > 180.0:
psi = psi - 360.0
if psi < -180.0:
psi = psi + 360.0
nav.psi = psi
result['filter'].append(nav)
if row['GPSP_Alt'] != '':
ap = APdata()
ap.time = imu.time
ap.hdg = my_float(row['ATSP_YawSP']) * r2d
ap.roll = my_float(row['ATSP_RollSP']) * r2d
ap.alt = my_float(row['GPSP_Alt']) * m2ft
ap.pitch = my_float(row['ATSP_PitchSP']) * r2d
ap.speed = my_float(row['TECS_AsSP']) * mps2kt
result['ap'].append(ap)
if row['OUT0_Out0'] != '':
act = Controldata()
ch0 = (float(row['OUT0_Out0']) - 1500) / 500
ch1 = (float(row['OUT0_Out1']) - 1500) / 500
ch2 = (float(row['OUT0_Out2']) - 1000) / 1000
ch3 = (float(row['OUT0_Out3']) - 1500) / 500
#print ch0, ch1, ch2, ch3
act.time = imu.time
act.aileron = (ch0 - ch1)
act.elevator = -(ch0 + ch1)
act.throttle = ch2
act.rudder = ch3
act.gear = 0.0
act.flaps = 0.0
act.aux1 = 0.0
act.auto_manual = 0.0
result['act'].append(act)
return result
def load(mat_filename):
# Name of .mat file that exists in the directory defined above and
# has the flight_data and flight_info structures
filepath = mat_filename
# Load Flight Data: ## IMPORTANT to have the .mat file in the
# flight_data and flight_info structures for this function ##
data = sio.loadmat(filepath, struct_as_record=False, squeeze_me=True)
print 'Loaded Data Summary'
print '* File: %s' % filepath.split(os.path.sep)[-1]
try:
flight_data, flight_info = data['flight_data'], data['flight_info']
print('* Date: %s' % flight_info.date)
print('* Aircraft: %s' % flight_info.aircraft)
except KeyError:
print 'KeyError'
# Convert from Python dictionary to struct-like before
flight_data = dict2struct()
for k in data:
exec("flight_data.%s = data['%s']" % (k, k))
del(data)
# Add both names for pitch: the and theta
try:
flight_data.theta = flight_data.the
except AttributeError:
pass
# Fill in time data
t = flight_data.time
print 't:', t
# Magnetometer data - not used hence don't trust
hm = np.vstack((flight_data.hx, -flight_data.hy, -flight_data.hz)).T
# Geri mag calibration
mag_affine = np.array(
[[ 4.3374191736, 0.9527668819, 0.2106929615, 0.0649324241],
[-3.7617930866, 6.2839014058, -3.5707398622, 2.1616165305],
[-0.8688354599, 0.2205650877, 4.7466115481, -2.7041600008],
[ 0. , 0. , 0. , 1. ]]
)
# Populate IMU Data
imu = np.vstack((t, flight_data.p, flight_data.q, flight_data.r,
flight_data.ax, flight_data.ay, flight_data.az,
hm[:,0], hm[:,1], hm[:,2])).T
# Note that accelerometer and gyro measurements logged by UAV
# after 11/17/2011 flight (seemingly, see
# http://trac.umnaem.webfactional.com/wiki/FlightReports/2011_11_17)
# have the nav-estimated bias removed before datalogging. So to work with raw
# imu-data, we add back the on-board estimated biases.
if not FLAG_UNBIASED_IMU:
try:
imu[:, 1:4] += np.vstack((flight_data.p_bias,
flight_data.q_bias,
flight_data.r_bias)).T
imu[:, 4:7] += np.vstack((flight_data.ax_bias,
flight_data.ay_bias,
flight_data.az_bias)).T
except AttributeError:
print('Note: On board estimated bias not found.')
# Air Data
ias = flight_data.ias # indicated airspeed (m/s)
h = flight_data.h
# Populate GPS sensor data
try:
vn = flight_data.gps_vn
except:
vn = flight_data.vn
try:
ve = flight_data.gps_ve
except:
ve = flight_data.ve
try:
vd = flight_data.gps_vd
except:
vd = flight_data.vd
lat = flight_data.lat
lon = flight_data.lon
alt = flight_data.alt
# kstart set to when the navigation filter used onboard the aircraft
# was initialized and this is accomplished by detecting when navlat is
# no longer 0.0. This choice of kstart will ensure the filter being
# tested is using the same initialization time step as the onboard
# filter allowing for apples to apples comparisons.
kstart = (abs(flight_data.navlat) > 0.0).tolist().index(True)
k = kstart
print('Initialized at Time: %.2f s (k=%i)' % (t[k], k))
# Set previous value of GPS altitude to 0.0. This will be used to
# trigger GPS newData flag which is commonly used in our
# navigation filters for deciding if the GPS data has been
# updated. However, in python we have no log of newData
# (typically). So a comparison of current GPS altitude to the
# previous epoch's GPS altitude is used to determine if GPS has
# been updated.
last_gps_alt = -9999.9
# create data structures for ekf processing
result = {}
result['imu'] = []
result['gps'] = []
result['air'] = []
result['filter'] = []
k = kstart
while k < len(t):
p, q, r = imu[k, 1:4]
ax, ay, az = imu[k, 4:7]
hx, hy, hz = imu[k, 7:10]
s = [hx, hy, hz, 1.0]
#hf = np.dot(mag_affine, s)
hf = s
imu_pt = IMUdata()
imu_pt.time = float(t[k])
imu_pt.p = float(p)
imu_pt.q = float(q)
imu_pt.r = float(r)
imu_pt.ax = float(ax)
imu_pt.ay = float(ay)
imu_pt.az = float(az)
#imu_pt.hx = hx
#imu_pt.hy = hy
#imu_pt.hz = hz
imu_pt.hx = float(hf[0])
imu_pt.hy = float(hf[1])
imu_pt.hz = float(hf[2])
imu_pt.temp = 15.0
result['imu'].append(imu_pt)
if abs(alt[k] - last_gps_alt) > 0.0001:
last_gps_alt = alt[k]
gps_pt = GPSdata()
gps_pt.time = float(t[k])
#gps_pt.status = int(status)
gps_pt.unix_sec = float(t[k])
gps_pt.lat = float(lat[k])
gps_pt.lon = float(lon[k])
gps_pt.alt = float(alt[k])
gps_pt.vn = float(vn[k])
gps_pt.ve = float(ve[k])
gps_pt.vd = float(vd[k])
gps_pt.sats = 8 # force a reasonable value (not logged?)
result['gps'].append(gps_pt)
air_pt = Airdata()
air_pt.time = float(t[k])
air_pt.airspeed = float(flight_data.ias[k]*mps2kt)
air_pt.altitude = float(flight_data.h[k])
result['air'].append(air_pt)
nav = NAVdata()
nav.time = float(t[k])
nav.lat = float(flight_data.navlat[k])
nav.lon = float(flight_data.navlon[k])
nav.alt = float(flight_data.navalt[k])
nav.vn = float(flight_data.navvn[k])
nav.ve = float(flight_data.navve[k])
nav.vd = float(flight_data.navvd[k])
nav.phi = float(flight_data.phi[k])
nav.the = float(flight_data.theta[k])
nav.psi = float(flight_data.psi[k])
result['filter'].append(nav)
k += 1
dir = os.path.dirname(mat_filename)
print 'dir:', dir
filename = os.path.join(dir, 'imu-0.txt')
f = open(filename, 'w')
for imupt in result['imu']:
line = [ '%.5f' % imupt.time, '%.4f' % imupt.p, '%.4f' % imupt.q, '%.4f' % imupt.r, '%.4f' % imupt.ax, '%.4f' % imupt.ay, '%.4f' % imupt.az, '%.4f' % imupt.hx, '%.4f' % imupt.hy, '%.4f' % imupt.hz, '%.4f' % imupt.temp, '0' ]
f.write(','.join(line) + '\n')
filename = os.path.join(dir, 'gps-0.txt')
f = open(filename, 'w')
for gpspt in result['gps']:
line = [ '%.5f' % gpspt.time, '%.10f' % gpspt.lat, '%.10f' % gpspt.lon, '%.4f' % gpspt.alt, '%.4f' % gpspt.vn, '%.4f' % gpspt.ve, '%.4f' % gpspt.vd, '%.4f' % gpspt.time, '8', '0' ]
f.write(','.join(line) + '\n')
filename = os.path.join(dir, 'filter-0.txt')
f = open(filename, 'w')
r2d = 180.0 / math.pi
for filtpt in result['filter']:
line = [ '%.5f' % filtpt.time, '%.10f' % filtpt.lat, '%.10f' % filtpt.lon, '%.4f' % filtpt.alt, '%.4f' % filtpt.vn, '%.4f' % filtpt.ve, '%.4f' % filtpt.vd, '%.4f' % (filtpt.phi*r2d), '%.4f' % (filtpt.the*r2d), '%.4f' % (filtpt.psi*r2d), '0' ]
f.write(','.join(line) + '\n')
return result
def load(csv_base):
result = {}
comb_path = csv_base + '_sensor_combined_0.csv'
gps_path = csv_base + '_vehicle_gps_position_0.csv'
att_path = csv_base + '_vehicle_attitude_0.csv'
pos_path = csv_base + '_vehicle_global_position_0.csv'
result['imu'] = []
with open(comb_path, 'rb') as f:
reader = csv.DictReader(f)
for row in reader:
imu = IMUdata()
imu.time = float(row['timestamp']) / 1000000.0
imu.p = float(row['gyro_rad[0]'])
imu.q = float(row['gyro_rad[1]'])
imu.r = float(row['gyro_rad[2]'])
imu.ax = float(row['accelerometer_m_s2[0]'])
imu.ay = float(row['accelerometer_m_s2[1]'])
imu.az = float(row['accelerometer_m_s2[2]'])
hx = float(row['magnetometer_ga[0]'])
hy = float(row['magnetometer_ga[0]'])
hz = float(row['magnetometer_ga[0]'])
hf = np.array([hx, hy, hz])
norm = np.linalg.norm(hf)
hf /= norm
imu.hx = hf[0]
imu.hy = hf[1]
imu.hz = hf[2]
#print imu.hx, imu.hy, imu.hz
imu.temp = 15.0
result['imu'].append(imu)
result['gps'] = []
with open(gps_path, 'rb') as f:
reader = csv.DictReader(f)
for row in reader:
gps = GPSdata()
gps.time = float(row['timestamp']) / 1000000.0
gps.unix_sec = float(row['time_utc_usec']) / 1000000.0
gps.lat = float(row['lat']) / 1e7
gps.lon = float(row['lon']) / 1e7
gps.alt = float(row['alt']) / 1e3
# print gps.lat, gps.lon, gps.alt
gps.vn = float(row['vel_n_m_s'])
gps.ve = float(row['vel_e_m_s'])
gps.vd = float(row['vel_d_m_s'])
gps.sats = int(row['satellites_used'])
if gps.sats >= 5:
result['gps'].append(gps)
# result['air'] = []
# with open(gps_path, 'rb') as f:
# reader = csv.DictReader(f)
# for row in reader:
# air = Airdata()
# air.time = imu.time
# air.static_press = float(row['SENS_BaroPres'])
# air.diff_press = float(row['SENS_DiffPres'])
# air.temp = float(row['AIRS_AirTemp'])
# air.airspeed = float(row['AIRS_IndSpeed'])
# air.alt_press = float(row['SENS_BaroAlt'])
# air.alt_true = gps.alt
# result['air'].append( air )
att = []
with open(att_path, 'rb') as f:
reader = csv.DictReader(f)
for row in reader:
time = float(row['timestamp']) / 1000000.0
q = [
float(row['q[0]']),
float(row['q[1]']),
float(row['q[2]']),
float(row['q[3]'])
]
# either of these will work ...
(roll, pitch, yaw) = px4_quat2euler(q)
att.append([time, yaw, pitch, roll])
pos = []
with open(pos_path, 'rb') as f:
reader = csv.DictReader(f)
for row in reader:
time = float(row['timestamp']) / 1000000.0
lat = float(row['lat']) * d2r
lon = float(row['lon']) * d2r
alt = float(row['alt'])
vn = float(row['vel_n'])
ve = float(row['vel_e'])
vd = float(row['vel_d'])
pos.append([time, lat, lon, alt, vn, ve, vd])
pos_array = np.array(pos)
lat_interp = interpolate.interp1d(pos_array[:, 0],
pos_array[:, 1],
bounds_error=False,
fill_value='extrapolate')
lon_interp = interpolate.interp1d(pos_array[:, 0],
pos_array[:, 2],
bounds_error=False,
fill_value='extrapolate')
alt_interp = interpolate.interp1d(pos_array[:, 0],
pos_array[:, 3],
bounds_error=False,
fill_value='extrapolate')
vn_interp = interpolate.interp1d(pos_array[:, 0],
pos_array[:, 4],
bounds_error=False,
fill_value=0.0)
ve_interp = interpolate.interp1d(pos_array[:, 0],
pos_array[:, 5],
bounds_error=False,
fill_value=0.0)
vd_interp = interpolate.interp1d(pos_array[:, 0],
pos_array[:, 6],
bounds_error=False,
fill_value=0.0)
result['filter'] = []
for a in att:
nav = NAVdata()
nav.time = a[0]
nav.lat = float(lat_interp(nav.time))
nav.lon = float(lon_interp(nav.time))
nav.alt = float(alt_interp(nav.time))
nav.vn = float(vn_interp(nav.time))
nav.ve = float(ve_interp(nav.time))
nav.vd = float(vd_interp(nav.time))
nav.phi = a[3]
nav.the = a[2]
nav.psi = a[1]
result['filter'].append(nav)
#result['pilot'] = []
#result['act'] = []
#result['ap'] = []
return result
def load(mat_filename):
# Name of .mat file that exists in the directory defined above and
# has the flight_data and flight_info structures
filepath = mat_filename
# Load Flight Data: ## IMPORTANT to have the .mat file in the
# flight_data and flight_info structures for this function ##
data = sio.loadmat(filepath, struct_as_record=False, squeeze_me=True)
print 'Loaded Data Summary'
print '* File: %s' % filepath.split(os.path.sep)[-1]
try:
flight_data, flight_info = data['flight_data'], data['flight_info']
print('* Date: %s' % flight_info.date)
print('* Aircraft: %s' % flight_info.aircraft)
except KeyError:
print 'KeyError'
# Convert from Python dictionary to struct-like before
flight_data = dict2struct()
for k in data:
exec("flight_data.%s = data['%s']" % (k, k))
del (data)
# Add both names for pitch: the and theta
try:
flight_data.theta = flight_data.the
except AttributeError:
pass
# Fill in time data
t = flight_data.time
print 't:', t
# Magnetometer data - not used hence don't trust
hm = np.vstack((flight_data.hx, -flight_data.hy, -flight_data.hz)).T
# Geri mag calibration
mag_affine = np.array(
[[4.3374191736, 0.9527668819, 0.2106929615, 0.0649324241],
[-3.7617930866, 6.2839014058, -3.5707398622, 2.1616165305],
[-0.8688354599, 0.2205650877, 4.7466115481, -2.7041600008],
[0., 0., 0., 1.]])
# Populate IMU Data
imu = np.vstack(
(t, flight_data.p, flight_data.q, flight_data.r, flight_data.ax,
flight_data.ay, flight_data.az, hm[:, 0], hm[:, 1], hm[:, 2])).T
# Note that accelerometer and gyro measurements logged by UAV
# after 11/17/2011 flight (seemingly, see
# http://trac.umnaem.webfactional.com/wiki/FlightReports/2011_11_17)
# have the nav-estimated bias removed before datalogging. So to work with raw
# imu-data, we add back the on-board estimated biases.
if not FLAG_UNBIASED_IMU:
try:
imu[:, 1:4] += np.vstack(
(flight_data.p_bias, flight_data.q_bias, flight_data.r_bias)).T
imu[:, 4:7] += np.vstack((flight_data.ax_bias, flight_data.ay_bias,
flight_data.az_bias)).T
except AttributeError:
print('Note: On board estimated bias not found.')
# Air Data
ias = flight_data.ias # indicated airspeed (m/s)
h = flight_data.h
# Populate GPS sensor data
try:
vn = flight_data.gps_vn
except:
vn = flight_data.vn
try:
ve = flight_data.gps_ve
except:
ve = flight_data.ve
try:
vd = flight_data.gps_vd
except:
vd = flight_data.vd
lat = flight_data.lat
lon = flight_data.lon
alt = flight_data.alt
# kstart set to when the navigation filter used onboard the aircraft
# was initialized and this is accomplished by detecting when navlat is
# no longer 0.0. This choice of kstart will ensure the filter being
# tested is using the same initialization time step as the onboard
# filter allowing for apples to apples comparisons.
kstart = (abs(flight_data.navlat) > 0.0).tolist().index(True)
k = kstart
print('Initialized at Time: %.2f s (k=%i)' % (t[k], k))
# Set previous value of GPS altitude to 0.0. This will be used to
# trigger GPS newData flag which is commonly used in our
# navigation filters for deciding if the GPS data has been
# updated. However, in python we have no log of newData
# (typically). So a comparison of current GPS altitude to the
# previous epoch's GPS altitude is used to determine if GPS has
# been updated.
last_gps_alt = -9999.9
# create data structures for ekf processing
result = {}
result['imu'] = []
result['gps'] = []
result['air'] = []
result['filter'] = []
k = kstart
while k < len(t):
p, q, r = imu[k, 1:4]
ax, ay, az = imu[k, 4:7]
hx, hy, hz = imu[k, 7:10]
s = [hx, hy, hz, 1.0]
#hf = np.dot(mag_affine, s)
hf = s
imu_pt = IMUdata()
imu_pt.time = float(t[k])
imu_pt.p = float(p)
imu_pt.q = float(q)
imu_pt.r = float(r)
imu_pt.ax = float(ax)
imu_pt.ay = float(ay)
imu_pt.az = float(az)
#imu_pt.hx = hx
#imu_pt.hy = hy
#imu_pt.hz = hz
imu_pt.hx = float(hf[0])
imu_pt.hy = float(hf[1])
imu_pt.hz = float(hf[2])
imu_pt.temp = 15.0
result['imu'].append(imu_pt)
if abs(alt[k] - last_gps_alt) > 0.0001:
last_gps_alt = alt[k]
gps_pt = GPSdata()
gps_pt.time = float(t[k])
#gps_pt.status = int(status)
gps_pt.unix_sec = float(t[k])
gps_pt.lat = float(lat[k])
gps_pt.lon = float(lon[k])
gps_pt.alt = float(alt[k])
gps_pt.vn = float(vn[k])
gps_pt.ve = float(ve[k])
gps_pt.vd = float(vd[k])
gps_pt.sats = 8 # force a reasonable value (not logged?)
result['gps'].append(gps_pt)
air_pt = Airdata()
air_pt.time = float(t[k])
air_pt.airspeed = float(flight_data.ias[k] * mps2kt)
air_pt.altitude = float(flight_data.h[k])
result['air'].append(air_pt)
nav = NAVdata()
nav.time = float(t[k])
nav.lat = float(flight_data.navlat[k])
nav.lon = float(flight_data.navlon[k])
nav.alt = float(flight_data.navalt[k])
nav.vn = float(flight_data.navvn[k])
nav.ve = float(flight_data.navve[k])
nav.vd = float(flight_data.navvd[k])
nav.phi = float(flight_data.phi[k])
nav.the = float(flight_data.theta[k])
nav.psi = float(flight_data.psi[k])
result['filter'].append(nav)
k += 1
dir = os.path.dirname(mat_filename)
print 'dir:', dir
filename = os.path.join(dir, 'imu-0.txt')
f = open(filename, 'w')
for imupt in result['imu']:
line = [
'%.5f' % imupt.time,
'%.4f' % imupt.p,
'%.4f' % imupt.q,
'%.4f' % imupt.r,
'%.4f' % imupt.ax,
'%.4f' % imupt.ay,
'%.4f' % imupt.az,
'%.4f' % imupt.hx,
'%.4f' % imupt.hy,
'%.4f' % imupt.hz,
'%.4f' % imupt.temp, '0'
]
f.write(','.join(line) + '\n')
filename = os.path.join(dir, 'gps-0.txt')
f = open(filename, 'w')
for gpspt in result['gps']:
line = [
'%.5f' % gpspt.time,
'%.10f' % gpspt.lat,
'%.10f' % gpspt.lon,
'%.4f' % gpspt.alt,
'%.4f' % gpspt.vn,
'%.4f' % gpspt.ve,
'%.4f' % gpspt.vd,
'%.4f' % gpspt.time, '8', '0'
]
f.write(','.join(line) + '\n')
filename = os.path.join(dir, 'filter-0.txt')
f = open(filename, 'w')
r2d = 180.0 / math.pi
for filtpt in result['filter']:
line = [
'%.5f' % filtpt.time,
'%.10f' % filtpt.lat,
'%.10f' % filtpt.lon,
'%.4f' % filtpt.alt,
'%.4f' % filtpt.vn,
'%.4f' % filtpt.ve,
'%.4f' % filtpt.vd,
'%.4f' % (filtpt.phi * r2d),
'%.4f' % (filtpt.the * r2d),
'%.4f' % (filtpt.psi * r2d), '0'
]
f.write(','.join(line) + '\n')
return result
def load(flight_dir, recalibrate=None):
result = {}
# load imu/gps data files
imu_file = flight_dir + "/imu-0.txt"
imucal_json = flight_dir + "/imucal.json"
imucal_xml = flight_dir + "/imucal.xml"
gps_file = flight_dir + "/gps-0.txt"
air_file = flight_dir + "/air-0.txt"
filter_file = flight_dir + "/filter-0.txt"
filter_post = flight_dir + "/filter-post.txt"
pilot_file = flight_dir + "/pilot-0.txt"
act_file = flight_dir + "/act-0.txt"
ap_file = flight_dir + "/ap-0.txt"
imu_bias_file = flight_dir + "/imubias.txt"
# HEY: in the latest aura code, calibrated magnetometer is logged,
# not raw magnetometer, so we don't need to correct here. We
# could 'back correct' if we wanted original values for some
# reason (using the calibration matrix saved with each flight
# log.)
# # vireo_01
# mag_affine = np.array(
# [[ 1.6207467043, 0.0228992488, 0.0398638786, 0.1274248748],
# [-0.0169905025, 1.7164397411, -0.0001290047, -0.1140304977],
# [ 0.0424979948, -0.0038515935, 1.7193766423, -0.1449816095],
# [ 0. , 0. , 0. , 1. ]]
# )
# Tyr
# mag_affine = np.array(
# [[ 1.810, 0.109, 0.285, -0.237],
# [-0.078, 1.931, -0.171, -0.060],
# [ 0.008, 0.109, 1.929, 0.085],
# [ 0. , 0. , 0. , 1. ]]
# )
# telemaster apm2_101
mag_affine = np.array(
[[0.0026424919, 0.0001334248, 0.0000984977, -0.2235908546],
[-0.000081925, 0.0026419229, 0.0000751835, -0.0010757621],
[0.0000219407, 0.0000560341, 0.002541171, 0.040221458],
[0., 0., 0., 1.]])
# skywalker apm2_105
# mag_affine = np.array(
# [[ 0.0025834778, 0.0001434776, 0.0001434961, -0.7900775707 ],
# [-0.0001903118, 0.0024796553, 0.0001365238, 0.1881142449 ],
# [ 0.0000556437, 0.0001329724, 0.0023791184, 0.1824851582, ],
# [ 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000 ]]
# )
#np.set_printoptions(precision=10,suppress=True)
#print mag_affine
result['imu'] = []
fimu = fileinput.input(imu_file)
for line in fimu:
tokens = re.split('[,\s]+', line.rstrip())
#s = [float(tokens[7]), float(tokens[8]), float(tokens[9]), 1.0]
#hf = np.dot(mag_affine, s)
#print hf
imu = IMUdata()
imu.time = float(tokens[0])
imu.p = float(tokens[1])
imu.q = float(tokens[2])
imu.r = float(tokens[3])
imu.ax = float(tokens[4])
imu.ay = float(tokens[5])
imu.az = float(tokens[6])
imu.hx = float(tokens[7])
imu.hy = float(tokens[8])
imu.hz = float(tokens[9])
imu.temp = float(tokens[10])
result['imu'].append(imu)
result['gps'] = []
fgps = fileinput.input(gps_file)
last_time = -1.0
for line in fgps:
# Note: the aura logs unix time of the gps record, not tow,
# but for the purposes of the insgns algorithm, it's only
# important to have a properly incrementing clock, it doesn't
# really matter what the zero reference point of time is.
tokens = re.split('[,\s]+', line.rstrip())
time = float(tokens[0])
sats = int(tokens[8])
if sats >= 5 and time > last_time:
gps = GPSdata()
gps.time = time
gps.unix_sec = float(tokens[7])
gps.lat = float(tokens[1])
gps.lon = float(tokens[2])
gps.alt = float(tokens[3])
gps.vn = float(tokens[4])
gps.ve = float(tokens[5])
gps.vd = float(tokens[6])
gps.sats = sats
result['gps'].append(gps)
last_time = time
if os.path.exists(air_file):
result['air'] = []
fair = fileinput.input(air_file)
for line in fair:
tokens = re.split('[,\s]+', line.rstrip())
air = Airdata()
air.time = float(tokens[0])
air.static_press = float(tokens[1])
air.diff_press = 0.0 # not directly available in flight log
air.temp = float(tokens[2])
air.airspeed = float(tokens[3])
air.alt_press = float(tokens[4])
air.alt_true = float(tokens[5])
result['air'].append(air)
# load filter records if they exist (for comparison purposes)
result['filter'] = []
ffilter = fileinput.input(filter_file)
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:
nav = NAVdata()
nav.time = float(tokens[0])
nav.lat = lat * d2r
nav.lon = lon * d2r
nav.alt = float(tokens[3])
nav.vn = float(tokens[4])
nav.ve = float(tokens[5])
nav.vd = float(tokens[6])
nav.phi = float(tokens[7]) * d2r
nav.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
nav.psi = psi * d2r
result['filter'].append(nav)
# load filter (post process) records if they exist (for comparison
# purposes)
if os.path.exists(filter_post):
result['filter_post'] = []
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:
nav = NAVdata()
nav.time = float(tokens[0])
nav.lat = lat * d2r
nav.lon = lon * d2r
nav.alt = float(tokens[3])
nav.vn = float(tokens[4])
nav.ve = float(tokens[5])
nav.vd = float(tokens[6])
nav.phi = float(tokens[7]) * d2r
nav.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
nav.psi = psi * d2r
result['filter_post'].append(nav)
if os.path.exists(pilot_file):
result['pilot'] = []
fpilot = fileinput.input(pilot_file)
for line in fpilot:
tokens = re.split('[,\s]+', line.rstrip())
pilot = Controldata()
pilot.time = float(tokens[0])
pilot.aileron = float(tokens[1])
pilot.elevator = float(tokens[2])
pilot.throttle = float(tokens[3])
pilot.rudder = float(tokens[4])
pilot.gear = float(tokens[5])
pilot.flaps = float(tokens[6])
pilot.aux1 = float(tokens[7])
pilot.auto_manual = float(tokens[8])
result['pilot'].append(pilot)
if os.path.exists(act_file):
result['act'] = []
fact = fileinput.input(act_file)
for line in fact:
tokens = re.split('[,\s]+', line.rstrip())
act = Controldata()
act.time = float(tokens[0])
act.aileron = float(tokens[1])
act.elevator = float(tokens[2])
act.throttle = float(tokens[3])
act.rudder = float(tokens[4])
act.gear = float(tokens[5])
act.flaps = float(tokens[6])
act.aux1 = float(tokens[7])
act.auto_manual = float(tokens[8])
result['act'].append(act)
if os.path.exists(ap_file):
result['ap'] = []
fap = fileinput.input(ap_file)
for line in fap:
tokens = re.split('[,\s]+', line.rstrip())
ap = APdata()
ap.time = float(tokens[0])
ap.hdg = float(tokens[1])
ap.roll = float(tokens[2])
ap.alt = float(tokens[3])
ap.pitch = float(tokens[5])
ap.speed = float(tokens[7])
#ap.flight_time = float(tokens[8])
#ap.target_wp = int(tokens[9])
#ap.wp_lon = float(tokens[10])
#ap.wp_lat = float(tokens[11])
#ap.wp_index = int(tokens[12])
#ap.route_size = int(tokens[13])
result['ap'].append(ap)
cal = imucal.Calibration()
if os.path.exists(imucal_json):
imucal_file = imucal_json
elif os.path.exists(imucal_xml):
imucal_file = imucal_xml
else:
imucal_file = None
if imucal_file:
cal.load(imucal_file)
print 'back correcting imu data (to get original raw values)'
result['imu'] = cal.back_correct(result['imu'])
if recalibrate:
print 'recalibrating imu data using alternate calibration file:', recalibrate
rcal = imucal.Calibration()
rcal.load(recalibrate)
result['imu'] = rcal.correct(result['imu'])
return result
def load(csv_base):
result = {}
comb_path = csv_base + '_sensor_combined_0.csv'
gps_path = csv_base + '_vehicle_gps_position_0.csv'
att_path = csv_base + '_vehicle_attitude_0.csv'
pos_path = csv_base + '_vehicle_global_position_0.csv'
ap_path = csv_base + '_vehicle_attitude_setpoint_0.csv'
air_path = csv_base + '_airspeed_0.csv'
act_path = csv_base + '_actuator_outputs_0.csv'
filter_post = csv_base + '_filter_post.txt'
result['imu'] = []
with open(comb_path, 'rb') as f:
reader = csv.DictReader(f)
for row in reader:
imu = IMUdata()
imu.time = float(row['timestamp']) / 1000000.0
imu.p = float(row['gyro_rad[0]'])
imu.q = float(row['gyro_rad[1]'])
imu.r = float(row['gyro_rad[2]'])
imu.ax = float(row['accelerometer_m_s2[0]'])
imu.ay = float(row['accelerometer_m_s2[1]'])
imu.az = float(row['accelerometer_m_s2[2]'])
hx = float(row['magnetometer_ga[0]'])
hy = float(row['magnetometer_ga[0]'])
hz = float(row['magnetometer_ga[0]'])
hf = np.array([hx, hy, hz])
norm = np.linalg.norm(hf)
hf /= norm
imu.hx = hf[0]
imu.hy = hf[1]
imu.hz = hf[2]
#print imu.hx, imu.hy, imu.hz
imu.temp = 15.0
result['imu'].append(imu)
result['gps'] = []
with open(gps_path, 'rb') as f:
reader = csv.DictReader(f)
for row in reader:
gps = GPSdata()
gps.time = float(row['timestamp']) / 1000000.0
gps.unix_sec = float(row['time_utc_usec']) / 1000000.0
gps.lat = float(row['lat']) / 1e7
gps.lon = float(row['lon']) / 1e7
gps.alt = float(row['alt']) / 1e3
# print gps.lat, gps.lon, gps.alt
gps.vn = float(row['vel_n_m_s'])
gps.ve = float(row['vel_e_m_s'])
gps.vd = float(row['vel_d_m_s'])
gps.sats = int(row['satellites_used'])
if gps.sats >= 5:
result['gps'].append(gps)
result['air'] = []
with open(air_path, 'rb') as f:
reader = csv.DictReader(f)
for row in reader:
air = Airdata()
air.time = float(row['timestamp']) / 1000000.0
#air.static_press = float(row['SENS_BaroPres'])
air.diff_press = float(row['differential_pressure_filtered_pa'])
air.temp = float(row['air_temperature_celsius'])
air.airspeed = float(row['indicated_airspeed_m_s']) * mps2kt
#air.alt_press = float(row['SENS_BaroAlt'])
air.alt_true = gps.alt
result['air'].append(air)
att = []
with open(att_path, 'rb') as f:
reader = csv.DictReader(f)
for row in reader:
time = float(row['timestamp']) / 1000000.0
q = [
float(row['q[0]']),
float(row['q[1]']),
float(row['q[2]']),
float(row['q[3]'])
]
# either of these will work ...
(roll, pitch, yaw) = px4_quat2euler(q)
att.append([time, yaw, pitch, roll])
pos = []
with open(pos_path, 'rb') as f:
reader = csv.DictReader(f)
for row in reader:
time = float(row['timestamp']) / 1000000.0
lat = float(row['lat']) * d2r
lon = float(row['lon']) * d2r
alt = float(row['alt'])
vn = float(row['vel_n'])
ve = float(row['vel_e'])
vd = float(row['vel_d'])
pos.append([time, lat, lon, alt, vn, ve, vd])
pos_array = np.array(pos)
lat_interp = interpolate.interp1d(pos_array[:, 0],
pos_array[:, 1],
bounds_error=False,
fill_value='extrapolate')
lon_interp = interpolate.interp1d(pos_array[:, 0],
pos_array[:, 2],
bounds_error=False,
fill_value='extrapolate')
alt_interp = interpolate.interp1d(pos_array[:, 0],
pos_array[:, 3],
bounds_error=False,
fill_value='extrapolate')
vn_interp = interpolate.interp1d(pos_array[:, 0],
pos_array[:, 4],
bounds_error=False,
fill_value=0.0)
ve_interp = interpolate.interp1d(pos_array[:, 0],
pos_array[:, 5],
bounds_error=False,
fill_value=0.0)
vd_interp = interpolate.interp1d(pos_array[:, 0],
pos_array[:, 6],
bounds_error=False,
fill_value=0.0)
if os.path.exists(ap_path):
result['ap'] = []
with open(ap_path, 'rb') as f:
reader = csv.DictReader(f)
for row in reader:
ap = APdata()
ap.time = float(row['timestamp']) / 1000000.0
ap.hdg = float(row['yaw_body']) * r2d
ap.roll = float(row['roll_body']) * r2d
ap.pitch = float(row['pitch_body']) * r2d
ap.alt = 0
ap.speed = 0
#ap.alt = float(row['GPSP_Alt']) * m2ft
#ap.speed = float(row['TECS_AsSP']) * mps2kt
result['ap'].append(ap)
result['filter'] = []
for a in att:
nav = NAVdata()
nav.time = a[0]
nav.lat = float(lat_interp(nav.time))
nav.lon = float(lon_interp(nav.time))
nav.alt = float(alt_interp(nav.time))
nav.vn = float(vn_interp(nav.time))
nav.ve = float(ve_interp(nav.time))
nav.vd = float(vd_interp(nav.time))
nav.phi = a[3]
nav.the = a[2]
nav.psi = a[1]
result['filter'].append(nav)
#result['pilot'] = []
#result['act'] = []
#result['ap'] = []
# load filter (post process) records if they exist (for comparison
# purposes)
if os.path.exists(filter_post):
print "found filter_post.txt file"
result['filter_post'] = []
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:
nav = NAVdata()
nav.time = float(tokens[0])
nav.lat = lat * d2r
nav.lon = lon * d2r
nav.alt = float(tokens[3])
nav.vn = float(tokens[4])
nav.ve = float(tokens[5])
nav.vd = float(tokens[6])
nav.phi = float(tokens[7]) * d2r
nav.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
nav.psi = psi * d2r
result['filter_post'].append(nav)
if os.path.exists(act_path):
result['act'] = []
with open(act_path, 'rb') as f:
reader = csv.DictReader(f)
for row in reader:
act = Controldata()
act.time = float(row['timestamp']) / 1000000.0
ch = [0] * 8
for i in range(len(ch)):
pwm = float(row['output[%d]' % i])
ch[i] = (pwm - 1500) / 500
act.aileron = ch[0]
act.elevator = ch[1]
act.throttle = ch[2]
act.rudder = ch[3]
act.gear = 0
act.flaps = 0
act.aux1 = 0
act.auto_manual = 0
result['act'].append(act)
return result