# 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),