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