# moved to init(): IMU_CAL_INIT = False
# moved to init(): TU_COUNT = 0
idx_init = []
# moved to init(): tcpu = -1.0; old_tcpu = -1.0
# moved to init(): tow = -1.0; old_tow = -1.0
# =============================== MAIN LOOP ====================================
filter = insgps_quat_15state.Filter()
nav_init = False
for i in range(0, drl):
# prepare the sensor data
imu = insgps_quat_15state.IMU(flight_data.time[i], flight_data.navValid[i],
flight_data.p[i], flight_data.q[i],
flight_data.r[i], flight_data.ax[i],
flight_data.ay[i], flight_data.az[i])
gps = insgps_quat_15state.GPS(flight_data.time[i], flight_data.navValid[i],
flight_data.GPS_TOW[i], flight_data.lat[i],
flight_data.lon[i], flight_data.alt[i],
flight_data.vn[i], flight_data.ve[i],
flight_data.vd[i])
# update the filter
est = filter.update(imu, gps)
# save the results for plotting
if not nav_init and est.valid:
nav_init = True
idx_init.append(i)
elif not est.valid:
time = float(token[13]) / 1000.0
course_rad = math.pi * 0.5 - course * deg2rad
vx = math.cos(course_rad) * speed
vy = math.sin(course_rad) * speed
gps = EKF.GPS(float(time), int(status), float(tow), float(lat),
float(lon), float(alt), float(vy), float(vx), float(-vz))
gps_data.append(gps)
elif token[0] == "IMU":
time = float(token[1]) / 1000.0
p = float(token[2]) # was neg
q = float(token[3]) # was neg
r = float(token[4])
ax = float(token[5]) # was neg
ay = float(token[6]) # was neg
az = float(token[7])
imu = EKF.IMU(float(time), 1, float(p), float(q), float(r), float(ax),
float(ay), float(az), 0.0, 0.0, 0.0, float(baro_temp))
imu_data.append(imu)
# elif token[0] == "IMU":
# time = float(token[1]) / 1000.0
# p = -float(token[2])
# q = -float(token[3])
# r = float(token[4])
# ax = -float(token[5])
# ay = -float(token[6])
# az = float(token[7])
# imu = EKF.IMU( float(time), 1,
# float(p), float(q), float(r),
# float(ax), float(ay), float(az),
# float(baro_temp) )
# imu_data.append(imu)
elif token[0] == "EKF1":
flight_path = sys.argv[1]
# load imu/gps data files
imu_file = flight_path + "/imu-0.txt"
gps_file = flight_path + "/gps-0.txt"
imu_bias_file = flight_path + "/imubias.txt"
cal_file = flight_path + "/imucal.xml"
cal_file_per_flight = flight_path + "/imucal-scale.xml"
imu_data = []
fimu = fileinput.input(imu_file)
for line in fimu:
time, p, q, r, ax, ay, az, hx, hy, hz, temp, status = line.split()
imu = EKF.IMU(float(time), int(status), float(p), float(q), float(r),
float(ax), float(ay), float(az), float(hx), float(hy),
float(hz), float(temp))
imu_data.append(imu)
if len(imu_data) == 0:
print "No imu records loaded, cannot continue..."
sys.exit()
gps_data = []
fgps = fileinput.input(gps_file)
for line in fgps:
# note the avior logs unix time of the gps record, not tow, but
# for the pruposes of the insgns algorithm, it's only important to
# have a properly incrementing clock, it doens't really matter
# what the zero reference point of time is.
time, lat, lon, alt, vn, ve, vd, unixsec, sats, status = line.split()
if int(sats) >= 4:
plot_time = [0.0] * drl
# ============================ VARIABLE INITIALIZER ============================
idx_init = []
# =============================== MAIN LOOP ====================================
filter = EKF.Filter()
gps_index = 1
nav_init = False
for i, imu_list in enumerate(imu_raw):
imu = EKF.IMU( imu_list[0], imu_list[11],
imu_list[1], imu_list[2], imu_list[3],
imu_list[4], imu_list[5], imu_list[6],
imu_list[7], imu_list[8], imu_list[9],
imu_list[10] )
# walk the gps counter forward as needed
while gps_index < len(gps_data) and imu.time >= gps_data[gps_index].time:
gps_index += 1
if gps_index >= len(gps_data):
# no more gps data, stay on the last record
gps_index = len(gps_data)-1
gps = gps_data[gps_index-1]
#print "t(imu) =", imu.time, "t(gps) =", gps.time
# update the filter
plot_time[i] = imu.time
est = filter.update(imu, gps)
if len(sys.argv) != 2:
usage()
sys.exit()
flight_path = sys.argv[1]
# load imu/gps data files
imu_file = flight_path + "/imu.txt"
gps_file = flight_path + "/gps.txt"
imu_data = []
fimu = fileinput.input(imu_file)
for line in fimu:
time, p, q, r, ax, ay, az, hx, hy, hz, temp, status = line.split()
imu = insgps_quat_15state.IMU(float(time), int(status), float(p), float(q),
float(r), float(ax), float(ay), float(az),
float(hx), float(hy), float(hz), float(temp))
imu_data.append(imu)
if len(imu_data) == 0:
print "No imu records loaded, cannot continue..."
sys.exit()
gps_data = []
fgps = fileinput.input(gps_file)
for line in fgps:
# note the avior logs unix time of the gps record, not tow, but
# for the pruposes of the insgns algorithm, it's only important to
# have a properly incrementing clock, it doens't really matter
# what the zero reference point of time is.
time, lat, lon, alt, vn, ve, vd, unixsec, sats, status = line.split()
gps = insgps_quat_15state.GPS(float(time), int(status), float(unixsec),