# 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:
nav_init = False
estPOS[i, :] = est.estPOS[:]
estVEL[i, :] = est.estVEL[:]
estATT[i, :] = est.estATT[:]
# for the purposes 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.
tokens = re.split('[,\s]+', line.rstrip())
time = tokens[0]
lat = tokens[1]
lon = tokens[2]
alt = tokens[3]
vn = tokens[4]
ve = tokens[5]
vd = tokens[6]
unixsec = tokens[7]
sats = tokens[8]
if int(sats) >= 4:
gps = EKF.GPS( float(time), int(status), float(unixsec),
float(lat), float(lon), float(alt),
float(vn), float(ve), float(vd))
gps_data.append(gps)
if len(gps_data) == 0:
print "No gps records loaded, cannot continue..."
quit()
# load filter records if they exist (for comparison purposes)
filter_data = []
ffilter = fileinput.input(filter_file)
for line in ffilter:
time, lat, lon, alt, vn, ve, vd, phi, the, psi, status = re.split('[,\s]+', line.rstrip())
filter_data.append( np.array([time, lat, lon, alt, vn, ve, vd,
phi, the, psi]) )
filter_array = np.nan*np.ones((len(filter_data),len(filter_data[0])))
status = int(token[1])
tow = float(token[2]) / 1000.0
sats = int(token[4])
lat = float(token[6])
lon = float(token[7])
alt = float(token[9])
speed = float(token[10]) # m/s
if speed > max_speed:
max_speed = speed
course = float(token[11])
vz = float(token[12])
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])
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),
float(lat), float(lon), float(alt),
float(vn), float(ve), float(vd))
gps_data.append(gps)
if len(gps_data) == 0:
print "No gps records loaded, cannot continue..."
sys.exit()
# =========================== Results ===============================
drl = len(imu_data)
estPOS = np.nan * np.ones((drl, 3))
estVEL = np.nan * np.ones((drl, 3))
estATT = np.nan * np.ones((drl, 4))
estAB = np.nan * np.ones((drl, 4))
estGB = np.nan * np.ones((drl, 4))