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(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(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)
for keys in data:
print keys
# print 'Loaded Data Summary'
# print '* File: %s' % filepath.split(os.path.sep)[-1]
# # 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'] = []
last_gps_lon = -9999.0
last_gps_lat = -9999.0
for i in range(len(data['Time_s'])):
# p, q, r = imu[k, 1:4]
# ax, ay, az = imu[k, 4:7]
# hx, hy, hz = imu[k, 7:10]
s = [
data['IMU_Hx_uT'][i], data['IMU_Hy_uT'][i], data['IMU_Hz_uT'][i],
1.0
]
# hf = np.dot(mag_affine, s) # apply mag calibration
hf = s
imu_pt = IMUdata()
imu_pt.time = data['Time_s'][i]
imu_pt.p = data['IMU_Gx_rads'][i]
imu_pt.q = data['IMU_Gy_rads'][i]
imu_pt.r = data['IMU_Gz_rads'][i]
imu_pt.ax = data['IMU_Ax_mss'][i]
imu_pt.ay = data['IMU_Ay_mss'][i]
imu_pt.az = data['IMU_Az_mss'][i]
imu_pt.hx = float(hf[0])
imu_pt.hy = float(hf[1])
imu_pt.hz = float(hf[2])
imu_pt.temp = data['IMU_Temp_C'][i]
result['imu'].append(imu_pt)
lat = data['GPS_Latitude'][i] * r2d
lon = data['GPS_Longitude'][i] * r2d
#print lon,lat
if abs(lat - last_gps_lat) > 0.0000000001 or abs(
lon - last_gps_lon) > 0.0000000000001:
last_gps_lat = lat
last_gps_lon = lon
gps_pt = GPSdata()
gps_pt.time = data['Time_s'][i]
#gps_pt.status = something
gps_pt.unix_sec = data['Time_s'][
i] # hack an incrementing time here
gps_pt.lat = lat
gps_pt.lon = lon
gps_pt.alt = data['GPS_Altitude'][i]
gps_pt.vn = data['GPS_North_Velocity'][i]
gps_pt.ve = data['GPS_East_Velocity'][i]
gps_pt.vd = data['GPS_Down_Velocity'][i]
gps_pt.sats = int(data['GPS_NumberSatellites'][i])
result['gps'].append(gps_pt)
pitot_calibrate = 1.0
diff_pa = data['Pitot_DiffPressure_Pressure_Pa'][i]
if diff_pa < 0:
diff_pa = 0.0
static_pa = data['Pitot_StaticPressure_Pressure_Pa'][i]
airspeed_mps = math.sqrt(2 * diff_pa / 1.225) * pitot_calibrate
P0 = 1013.25
T = 15.0
P = static_pa / 100.0 # convert to mbar
tmp1 = math.pow((P0 / P), 1.0 / 5.257) - 1.0
alt_m = (tmp1 * (T + 273.15)) / 0.0065
air_pt = Airdata()
air_pt.time = data['Time_s'][i]
air_pt.airspeed = airspeed_mps * mps2kt
air_pt.altitude = alt_m
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)
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')
if 'filter' in result:
filename = os.path.join(dir, 'filter-0.txt')
f = open(filename, 'w')
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
