elif not est.valid:
nav_init = False
estPOS[i, :] = est.estPOS[:]
estVEL[i, :] = est.estVEL[:]
estATT[i, :] = est.estATT[:]
estAB[i, :] = est.estAB[:]
estGB[i, :] = est.estGB[:]
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):
C1 = navpy.angle2dcm(flight_data.psi[i], flight_data.theta[i],
flight_data.phi[i]).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]
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
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]
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,
)
