Pp = np.nan*np.ones((drl,3))
Pvel = np.nan*np.ones((drl,3))
Patt = np.nan*np.ones((drl,3))
Pab = np.nan*np.ones((drl,3))
Pgb = np.nan*np.ones((drl,3))
stateInnov = np.nan*np.ones((drl,6))
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
stateInnov = np.nan * np.ones((drl, 6))
# ============================ VARIABLE INITIALIZER ============================
# moved to init(): H = np.hstack( (np.eye(6), np.zeros((6,9))) )
# moved to init(): NAV_INIT = False
# 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)
def run_filter(imu_data, gps_data):
drl = len(imu_data)
# result structures
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))
Pp = np.nan * np.ones((drl, 3))
Pvel = np.nan * np.ones((drl, 3))
Patt = np.nan * np.ones((drl, 3))
Pab = np.nan * np.ones((drl, 3))
Pgb = np.nan * np.ones((drl, 3))
stateInnov = np.nan * np.ones((drl, 6))
plot_time = [0.0] * drl
# Variable Initializer
idx_init = []
# Main Loop
filter = EKF.Filter()
gps_index = 1
nav_init = False
for i, imu in enumerate(imu_data):
# walk the gps counter forward as needed
if imu.time >= gps_data[gps_index].time:
gps_index += 1
if gps_index >= len(gps_data):
# no more gps data, stick on the last record
gps_index = len(gps_data) - 1
gps = gps_data[gps_index - 1]
# print "t(imu) = " + str(imu.time) + " t(gps) = " + str(gps.time)
# update the filter
plot_time[i] = imu.time
est = filter.update(imu, gps, verbose=False)
# 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[:]
estAB[i, 0:3] = est.estAB[:]
estAB[i, 3] = imu.temp
estGB[i, 0:3] = est.estGB[:]
estGB[i, 3] = imu.temp
Pp[i, :] = np.diag(est.P[0:3, 0:3])
Pvel[i, :] = np.diag(est.P[3:6, 3:6])
Patt[i, :] = np.diag(est.P[6:9, 6:9])
Pab[i, :] = np.diag(est.P[9:12, 9:12])
Pgb[i, :] = np.diag(est.P[12:15, 12:15])
stateInnov[i, :] = est.stateInnov[:]
psi, theta, phi = navpy.quat2angle(estATT[:, 0],
estATT[:, 1:4],
output_unit='deg')
# Calculate Attitude Error
delta_att = np.nan * np.zeros((drl, 3))
for i in range(0, drl):
# when processing real data, we have no truth reference
#C1 = navpy.angle2dcm(flight_data.psi[i],flight_data.theta[i],flight_data.phi[i]).T
C1 = navpy.angle2dcm(psi[i], theta[i], phi[i], input_unit='deg').T
C2 = navpy.angle2dcm(psi[i], theta[i], phi[i], input_unit='deg').T
dC = C2.dot(C1.T) - np.eye(3)
# dC contains delta angle. To match delta quaternion, divide by 2.
delta_att[i, :] = [-dC[1, 2] / 2.0, dC[0, 2] / 2.0, -dC[0, 1] / 2.0]
lat_ref = estPOS[idx_init[0], 0]
lon_ref = estPOS[idx_init[0], 1]
alt_ref = estPOS[idx_init[0], 2]
ecef_ref = navpy.lla2ecef(lat_ref, lon_ref, alt_ref)
ecef = navpy.lla2ecef(estPOS[:, 0], estPOS[:, 1], estPOS[:, 2])
filter_ned = navpy.ecef2ned(ecef - ecef_ref, lat_ref, lon_ref, alt_ref)
# when processing real data, we have no truth reference
#ecef = navpy.lla2ecef(flight_data.lat,flight_data.lon,flight_data.alt)
#ecef = navpy.lla2ecef(estPOS[:,0],estPOS[:,1],estPOS[:,2])
#gnss_ned = navpy.ecef2ned(ecef-ecef_ref,lat_ref,lon_ref,alt_ref)
#gnss_ned[0:idx_init[0],:] = np.nan
rawgps = np.nan * np.ones((len(gps_data), 3))
for i, gps in enumerate(gps_data):
rawgps[i, :] = [gps.lat, gps.lon, gps.alt]
ecef = navpy.lla2ecef(rawgps[:, 0],
rawgps[:, 1],
rawgps[:, 2],
latlon_unit='deg')
ref_ned = navpy.ecef2ned(ecef - ecef_ref, lat_ref, lon_ref, alt_ref)
ref_ned[0:idx_init[0], :] = np.nan
return estPOS, estVEL, estATT, estAB, estGB, Pp, Pvel, Patt, Pab, Pgb, stateInnov, plot_time, idx_init, filter_ned, ref_ned, psi, theta, phi, delta_att