def ecef2ned(gps,refLla): ecefOrigin1D = navpy.lla2ecef(refLla.lat,refLla.lon,refLla.alt) ecefOrigin = np.array([ecefOrigin1D]).T ecefPositionLocal = gps.position - ecefOrigin gpsPositionNed = navpy.ecef2ned(ecefPositionLocal,refLla.lat,refLla.lon,refLla.alt) gpsVelocityNed = navpy.ecef2ned(gps.velocity,refLla.lat,refLla.lon,refLla.alt) gpsNed = GpsNed(gps.time,gpsPositionNed,gpsVelocityNed) return gpsNed
def base_gps_callback(self, gps):
if gps.fix != 3:
print('base gps not in fix. Fix = ', gps.fix)
return
if not self.refLlaSet:
return
velocityNed1D = navpy.ecef2ned(gps.velocityEcef, self.latRef,
self.lonRef, self.altRef)
velocityNed = np.array([velocityNed1D]).T
zt = velocityNed
ht = ekf.update_base_gps_velocity_model(self.baseStates.euler,
self.belief.vb,
self.baseStates.wLpf,
self.params.antennaOffset)
Ct = ekf.get_jacobian_C_base_velocity(self.baseStates)
ekf.update(self.belief, self.params.QtGpsVelocity, zt, ht, Ct)
def test_ecef2ned(self):
"""
Test conversion from ECEF to NED.
Data Source: Examples 2.1 and 2.4 of Aided Navigation: GPS with High
Rate Sensors, Jay A. Farrel 2008
Note: N, E (+) and S, W (-)
"""
# A point near Los Angeles, CA, given from equation 2.12 [degrees]
lat = +( 34. + 0./60 + 0.00174/3600) # North
lon = -(117. + 20./60 + 0.84965/3600) # West
alt = 251.702 # [meters]
# Define example ECEF vector and associated NED, given in Example 2.4
ecef = np.array([0.3808, 0.7364, -0.5592])
ned = np.array([0, 0, 1]) # local unit gravity
# Do conversion and check result
# Note: default assumption on units is deg and meters
ned_computed = navpy.ecef2ned(ecef, lat, lon, alt)
for e1, e2 in zip(ned_computed, ned):
self.assertAlmostEqual(e1, e2, places=3)
def test_ecef2ned(self):
"""
Test conversion from ECEF to NED.
Data Source: Examples 2.1 and 2.4 of Aided Navigation: GPS with High
Rate Sensors, Jay A. Farrel 2008
Note: N, E (+) and S, W (-)
"""
# A point near Los Angeles, CA, given from equation 2.12 [degrees]
lat = +(34. + 0. / 60 + 0.00174 / 3600) # North
lon = -(117. + 20. / 60 + 0.84965 / 3600) # West
alt = 251.702 # [meters]
# Define example ECEF vector and associated NED, given in Example 2.4
ecef = np.array([0.3808, 0.7364, -0.5592])
ned = np.array([0, 0, 1]) # local unit gravity
# Do conversion and check result
# Note: default assumption on units is deg and meters
ned_computed = navpy.ecef2ned(ecef, lat, lon, alt)
for e1, e2 in zip(ned_computed, ned):
self.assertAlmostEqual(e1, e2, places=3)
def rover_gps_callback(self, gps):
if gps.fix != 3:
print('rover gps not in fix. Fix = ', gps.fix)
return
if not self.refLlaSet:
self.latRef = gps.lla[0]
self.lonRef = gps.lla[1]
self.altRef = gps.lla[2]
refEcef1D = navpy.lla2ecef(self.latRef, self.lonRef, self.altRef)
self.refEcef = np.array([refEcef1D]).T
self.refLlaSet = True
print('ref lla = ', self.latRef, ' ', self.lonRef, ' ',
self.altRef)
velocityNed1D = navpy.ecef2ned(gps.velocityEcef, self.latRef,
self.lonRef, self.altRef)
velocityNed = np.array([velocityNed1D]).T
zt = velocityNed
ht = ekf.update_rover_gps_velocity_model(self.belief.vr)
Ct = ekf.get_jacobian_C_rover_velocity()
ekf.update(self.belief, self.params.QtGpsVelocity, zt, ht, Ct)
def load(flight_dir):
result = {}
gps_data = []
filter_data = []
# load imu/gps data files
imu_file = flight_dir + "/imu.csv"
gps_file = flight_dir + "/gps.csv"
filter_post = flight_dir + "/filter-post-ins.txt"
# calibration by plotting and eye-balling (just finding center point, no
# normalization cooked into calibration.)
#hx_coeffs = np.array([ 1.0, -1.5], dtype=np.float64)
#hy_coeffs = np.array([ 1.0, -78.5], dtype=np.float64)
#hz_coeffs = np.array([ 1.0, -156.5], dtype=np.float64)
#~/Projects/PILLS/Phantom\ 3\ Flight\ Data/2016-03-22\ --\ imagery_0012\ -\ 400\ ft\ survey
#hx_coeffs = np.array([ 0.01857771, -0.18006661], dtype=np.float64)
#hy_coeffs = np.array([ 0.01856938, -1.20854406], dtype=np.float64)
#hz_coeffs = np.array([ 0.01559645, 2.81011976], dtype=np.float64)
# ~/Projects/PILLS/Phantom\ 3\ Flight\ Data/imagery_0009 - 0012
#hx_coeffs = np.array([ 0.01789447, 3.70605872], dtype=np.float64)
#hy_coeffs = np.array([ 0.017071, 0.7125617], dtype=np.float64)
#hz_coeffs = np.array([ 0.01447557, -6.54621951], dtype=np.float64)
# ~/Projects/PILLS/2016-04-04\ --\ imagery_0002
# ~/Projects/PILLS/2016-04-14\ --\ imagery_0003
# ~/Projects/PILLS/2016-04-14\ --\ imagery_0004
#hx_coeffs = np.array([ 0.01658555, -0.07790598], dtype=np.float64)
#hy_coeffs = np.array([ 0.01880532, -1.26548151], dtype=np.float64)
#hz_coeffs = np.array([ 0.01339084, 2.61905809], dtype=np.float64)
# ~/Projects/PILLS/2016-05-12\ --\ imagery_0004
#hx_coeffs = np.array([ 0.01925678, 0.01527908], dtype=np.float64)
#hy_coeffs = np.array([ 0.01890112, -1.18040666], dtype=np.float64)
#hz_coeffs = np.array([ 0.01645011, 2.87769626], dtype=np.float64)
#hx_func = np.poly1d(hx_coeffs)
#hy_func = np.poly1d(hy_coeffs)
#hz_func = np.poly1d(hz_coeffs)
# ~/Projects/PILLS/2016-06-29\ --\ calibration_0002/
# mag_affine = np.array(
# [[ 0.0223062041, -0.0002700799, -0.0001325525, 1.2016235718],
# [-0.0002700799, 0.0229484854, 0.0000356172, 0.1177744077],
# [-0.0001325525, 0.0000356172, 0.0206129279, -3.2713740483],
# [ 0. , 0. , 0. , 1. ]]
# )
# Phantom 3 - Aug 2016 (ellipse cal)
# mag_affine = np.array(
# [[ 0.0189725067, 0.0000203615, 0.0002139272, -0.0134053645],
# [ 0.0000760692, 0.0180178765, 0.0000389461, -1.044762755 ],
# [ 0.0002417847, 0.0000458039, 0.0171450614, 2.647911793 ],
# [ 0. , 0. , 0. , 1. ]]
# )
# Phantom 3 - Aug 2016 (ekf cal)
# mag_affine = np.array(
# [[ 0.0181297161, 0.000774339, -0.002037224 , -0.2576406372],
# [ 0.0002434548, 0.018469032, 0.0016475328, -0.8452362072],
# [ 0.0000145964, 0.000267444, 0.0159433791, 2.5630653789],
# [ 0. , 0. , 0. , 1. ]]
# )
# 2017-06-07_23-43-50
mag_affine = np.array(
[[0.0182094965, 0.0001891445, 0.0005079058, -1.0275778093],
[0.0001891445, 0.0188836673, 0.0003014306, -0.7472003813],
[0.0005079058, 0.0003014306, 0.0176589615, 0.9988130618],
[0., 0., 0., 1.]])
print(mag_affine)
result['imu'] = []
fimu = fileinput.input(imu_file)
for line in fimu:
#print line
if not re.search('Time', line):
tokens = line.split(',')
#print len(tokens)
if len(tokens) == 11 and isFloat(tokens[10]):
#print '"' + tokens[10] + '"'
(time, p, q, r, ax, ay, az, hx, hy, hz, temp) = tokens
# remap axis before applying mag calibration
p = -float(p)
q = float(q)
r = -float(r)
ax = -float(ax) * g
ay = float(ay) * g
az = -float(az) * g
hx = -float(hx)
hy = float(hy)
hz = -float(hz)
mag_orientation = 'random1'
if mag_orientation == 'older':
#hx_new = hx_func(float(hx))
#hy_new = hy_func(float(hy))
#hz_new = hz_func(float(hz))
s = [hx, hy, hz]
elif mag_orientation == 'newer':
# remap for 2016-05-12 (0004) data set
#hx_new = hx_func(float(-hy))
#hy_new = hy_func(float(-hx))
#hz_new = hz_func(float(-hz))
s = [-hy, -hx, -hz]
elif mag_orientation == 'random1':
# remap for 2016-05-12 (0004) data set
#hx_new = hx_func(float(-hy))
#hy_new = hy_func(float(-hx))
#hz_new = hz_func(float(-hz))
s = [-hy, -hx, hz]
# mag calibration mapping via mag_affine matrix
hs = np.hstack([s, 1.0])
hf = np.dot(mag_affine, hs)
#print hf[:3]
#norm = np.linalg.norm([hx_new, hy_new, hz_new])
#hx_new /= norm
#hy_new /= norm
#hz_new /= norm
imu = Record()
imu.time = float(time) / 1000000.0
imu.p = p
imu.q = q
imu.r = r
imu.ax = ax
imu.ay = ay
imu.az = az
do_mag_transform = True
if do_mag_transform:
imu.hx = hf[0]
imu.hy = hf[1]
imu.hz = hf[2]
else:
imu.hx = s[0]
imu.hy = s[1]
imu.hz = s[2]
#float(hf[0]), float(hf[1]), float(hf[2]),
imu.temp = float(temp)
result['imu'].append(imu)
result['gps'] = []
fgps = fileinput.input(gps_file)
for line in fgps:
if not re.search('Timestamp', line):
# print line
tokens = line.split(',')
# print 'gps tokens:', len(tokens)
if len(tokens) == 14:
(time, itow, ecefx, ecefy, ecefz, ecefvx, ecefvy, ecefvz,
fixtype, posacc, spdacc, posdop, numsvs, flags) = tokens
elif len(tokens) == 19:
(time, itow, lat, lon, alt, ecefx, ecefy, ecefz, ecefvx,
ecefvy, ecefvz, fixtype, posacc, spdacc, posdop, numsvs,
flags, diff_status, carrier_status) = tokens
# wgs84 position
pos_source = 'llh' # 'llh' or 'ecef'
llh = navpy.ecef2lla([
float(ecefx) / 100.0,
float(ecefy) / 100.0,
float(ecefz) / 100.0
], "deg")
# velocity
ned = navpy.ecef2ned([
float(ecefvx) / 100.0,
float(ecefvy) / 100.0,
float(ecefvz) / 100.0
], llh[0], llh[1], llh[2])
if int(numsvs) >= 4:
gps = Record()
gps.time = float(time) / 1000000.0
#gps.status = int(status)
gps.unix_sec = float(time) / 1000000.0 # make filter happy
gps.lat = float(lat)
gps.lon = float(lon)
gps.alt = float(alt)
gps.vn = ned[0]
gps.ve = ned[1]
gps.vd = ned[2]
result['gps'].append(gps)
result['filter'] = []
# load filter (post process) records if they exist (for comparison
# purposes)
if os.path.exists(filter_post):
print('found filter-post-ins.txt, using that for ekf results')
result['filter'] = []
ffilter = fileinput.input(filter_post)
for line in ffilter:
tokens = re.split('[,\s]+', line.rstrip())
lat = float(tokens[1])
lon = float(tokens[2])
if abs(lat) > 0.0001 and abs(lon) > 0.0001:
filterpt = Record()
filterpt.time = float(tokens[0])
filterpt.lat = lat * d2r
filterpt.lon = lon * d2r
filterpt.alt = float(tokens[3])
filterpt.vn = float(tokens[4])
filterpt.ve = float(tokens[5])
filterpt.vd = float(tokens[6])
filterpt.phi = float(tokens[7]) * d2r
filterpt.the = float(tokens[8]) * d2r
psi = float(tokens[9])
if psi > 180.0:
psi = psi - 360.0
if psi < -180.0:
psi = psi + 360.0
filterpt.psi = psi * d2r
result['filter'].append(filterpt)
return result
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]
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)
ecef = navpy.lla2ecef(flight_data.lat, flight_data.lon, flight_data.alt)
gnss_ned = navpy.ecef2ned(ecef - ecef_ref, lat_ref, lon_ref, alt_ref)
gnss_ned[0:idx_init[0], :] = np.nan
ecef = navpy.lla2ecef(flight_data.navlat,
flight_data.navlon,
flight_data.navalt,
latlon_unit='rad')
ref_ned = navpy.ecef2ned(ecef - ecef_ref, lat_ref, lon_ref, alt_ref)
ref_ned[0:idx_init[0], :] = np.nan
# ============================= INS PLOTS ======================================
nsig = 3
istart = idx_init[0]
# 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
# ============================= INS PLOTS ======================================
def load(flight_dir):
result = {}
gps_data = []
filter_data = []
# load imu/gps data files
imu_file = os.path.join(flight_dir, "imu.csv")
gps_file = os.path.join(flight_dir, "gps.csv")
filter_post = os.path.join(flight_dir, "filter-post-ins.txt")
# calibration by plotting and eye-balling (just finding center point, no
# normalization cooked into calibration.)
#hx_coeffs = np.array([ 1.0, -1.5], dtype=np.float64)
#hy_coeffs = np.array([ 1.0, -78.5], dtype=np.float64)
#hz_coeffs = np.array([ 1.0, -156.5], dtype=np.float64)
#~/Projects/PILLS/Phantom\ 3\ Flight\ Data/2016-03-22\ --\ imagery_0012\ -\ 400\ ft\ survey
#hx_coeffs = np.array([ 0.01857771, -0.18006661], dtype=np.float64)
#hy_coeffs = np.array([ 0.01856938, -1.20854406], dtype=np.float64)
#hz_coeffs = np.array([ 0.01559645, 2.81011976], dtype=np.float64)
# ~/Projects/PILLS/Phantom\ 3\ Flight\ Data/imagery_0009 - 0012
#hx_coeffs = np.array([ 0.01789447, 3.70605872], dtype=np.float64)
#hy_coeffs = np.array([ 0.017071, 0.7125617], dtype=np.float64)
#hz_coeffs = np.array([ 0.01447557, -6.54621951], dtype=np.float64)
# ~/Projects/PILLS/2016-04-04\ --\ imagery_0002
# ~/Projects/PILLS/2016-04-14\ --\ imagery_0003
# ~/Projects/PILLS/2016-04-14\ --\ imagery_0004
#hx_coeffs = np.array([ 0.01658555, -0.07790598], dtype=np.float64)
#hy_coeffs = np.array([ 0.01880532, -1.26548151], dtype=np.float64)
#hz_coeffs = np.array([ 0.01339084, 2.61905809], dtype=np.float64)
# ~/Projects/PILLS/2016-05-12\ --\ imagery_0004
#hx_coeffs = np.array([ 0.01925678, 0.01527908], dtype=np.float64)
#hy_coeffs = np.array([ 0.01890112, -1.18040666], dtype=np.float64)
#hz_coeffs = np.array([ 0.01645011, 2.87769626], dtype=np.float64)
#hx_func = np.poly1d(hx_coeffs)
#hy_func = np.poly1d(hy_coeffs)
#hz_func = np.poly1d(hz_coeffs)
# ~/Projects/PILLS/2016-06-29\ --\ calibration_0002/
# mag_affine = np.array(
# [[ 0.0223062041, -0.0002700799, -0.0001325525, 1.2016235718],
# [-0.0002700799, 0.0229484854, 0.0000356172, 0.1177744077],
# [-0.0001325525, 0.0000356172, 0.0206129279, -3.2713740483],
# [ 0. , 0. , 0. , 1. ]]
# )
# Phantom 3 - Aug 2016 (ellipse cal)
# mag_affine = np.array(
# [[ 0.0189725067, 0.0000203615, 0.0002139272, -0.0134053645],
# [ 0.0000760692, 0.0180178765, 0.0000389461, -1.044762755 ],
# [ 0.0002417847, 0.0000458039, 0.0171450614, 2.647911793 ],
# [ 0. , 0. , 0. , 1. ]]
# )
# Phantom 3 - Aug 2016 (ekf cal)
# mag_affine = np.array(
# [[ 0.0181297161, 0.000774339, -0.002037224 , -0.2576406372],
# [ 0.0002434548, 0.018469032, 0.0016475328, -0.8452362072],
# [ 0.0000145964, 0.000267444, 0.0159433791, 2.5630653789],
# [ 0. , 0. , 0. , 1. ]]
# )
# 2017-06-07_23-43-50
mag_affine = np.array(
[[ 0.0182094965, 0.0001891445, 0.0005079058, -1.0275778093],
[ 0.0001891445, 0.0188836673, 0.0003014306, -0.7472003813],
[ 0.0005079058, 0.0003014306, 0.0176589615, 0.9988130618],
[ 0. , 0. , 0. , 1. ]]
)
print(mag_affine)
result['imu'] = []
with open(imu_file, 'r') as fimu:
reader = csv.DictReader(fimu)
for row in reader:
# print(row)
imu = Record()
try:
imu.time = float(row['Time Stamp (ns since boot)']) / 1000000000.0
imu.p = float(row['xGyro (rad/s)'])
imu.q = float(row['yGyro (rad/s)'])
imu.r = float(row['zGyro (rad/s)'])
imu.ax = float(row[' xAccel (g)'])*g
imu.ay = float(row[' yAccel (g)'])*g
imu.az = float(row[' zAccel (g)'])*g
imu.hx = float(row[' xMag (uT)']) # not logged
imu.hy = float(row[' xMag (uT)']) # not logged
imu.hz = float(row[' xMag (uT)']) # not logged
temp = row[' Temp (C)']
if temp != 'N/A':
imu.temp = float(row[' Temp (C)']) # not logged
else:
imu.temp = 0.0
result['imu'].append( imu )
except:
print('[IMU] failed to parse incomplete row:', row)
result['gps'] = []
with open(gps_file, 'r') as fgps:
reader = csv.DictReader(fgps)
for row in reader:
#print(row)
gps = Record()
try:
gps.time = float(row['Timestamp (ns since boot)']) / 1000000000.0
gps.unix_sec = gps.time # hack
#gps.lat = float(row['Lat (deg)'])
#gps.lon = float(row['Lon (deg)'])
#gps.alt = float(row['Alt Geoid EGM 96 (m)'])
ecefx = float(row['ecefX (cm)'])
ecefy = float(row['ecefY (cm)'])
ecefz = float(row['ecefZ (cm)'])
ecefvx = float(row['ecefVX (cm/s)'])
ecefvy = float(row['ecefVY (cm/s)'])
ecefvz = float(row['ecefVZ (cm/s)'])
gps.sats = int(row['Num SVs Used'])
# wgs84 position
pos_source = 'llh' # 'llh' or 'ecef'
llh = navpy.ecef2lla([float(ecefx)/100.0,
float(ecefy)/100.0,
float(ecefz)/100.0], "deg")
gps.lat = llh[0]
gps.lon = llh[1]
gps.alt = llh[2]
# velocity
ned = navpy.ecef2ned([float(ecefvx)/100.0,
float(ecefvy)/100.0,
float(ecefvz)/100.0],
llh[0], llh[1], llh[2])
gps.vn = ned[0]
gps.ve = ned[1]
gps.vd = ned[2]
if int(row['Fix Type']) == 3:
result['gps'].append(gps)
except:
print('[GPS] failed to parse incomplete row:', row)
result['filter'] = []
# load filter (post process) records if they exist (for comparison
# purposes)
if os.path.exists(filter_post):
print('found filter-post-ins.txt, using that for ekf results')
result['filter'] = []
ffilter = fileinput.input(filter_post)
for line in ffilter:
tokens = re.split('[,\s]+', line.rstrip())
lat = float(tokens[1])
lon = float(tokens[2])
if abs(lat) > 0.0001 and abs(lon) > 0.0001:
filterpt = Record()
filterpt.time = float(tokens[0])
filterpt.lat = lat*d2r
filterpt.lon = lon*d2r
filterpt.alt = float(tokens[3])
filterpt.vn = float(tokens[4])
filterpt.ve = float(tokens[5])
filterpt.vd = float(tokens[6])
filterpt.phi = float(tokens[7])*d2r
filterpt.the = float(tokens[8])*d2r
psi = float(tokens[9])
if psi > 180.0:
psi = psi - 360.0
if psi < -180.0:
psi = psi + 360.0
filterpt.psi = psi*d2r
result['filter'].append(filterpt)
return result
def load(flight_dir):
imu_data = []
gps_data = []
filter_data = []
# load imu/gps data files
imu_file = flight_dir + "/imu.csv"
gps_file = flight_dir + "/gps.csv"
# calibration by plotting and eye-balling (just finding center point, no
# normalization cooked into calibration.)
#hx_coeffs = np.array([ 1.0, -1.5], dtype=np.float64)
#hy_coeffs = np.array([ 1.0, -78.5], dtype=np.float64)
#hz_coeffs = np.array([ 1.0, -156.5], dtype=np.float64)
#~/Projects/PILLS/Phantom\ 3\ Flight\ Data/2016-03-22\ --\ imagery_0012\ -\ 400\ ft\ survey
#hx_coeffs = np.array([ 0.01857771, -0.18006661], dtype=np.float64)
#hy_coeffs = np.array([ 0.01856938, -1.20854406], dtype=np.float64)
#hz_coeffs = np.array([ 0.01559645, 2.81011976], dtype=np.float64)
# ~/Projects/PILLS/Phantom\ 3\ Flight\ Data/imagery_0009 - 0012
#hx_coeffs = np.array([ 0.01789447, 3.70605872], dtype=np.float64)
#hy_coeffs = np.array([ 0.017071, 0.7125617], dtype=np.float64)
#hz_coeffs = np.array([ 0.01447557, -6.54621951], dtype=np.float64)
# ~/Projects/PILLS/2016-04-04\ --\ imagery_0002
# ~/Projects/PILLS/2016-04-14\ --\ imagery_0003
# ~/Projects/PILLS/2016-04-14\ --\ imagery_0004
#hx_coeffs = np.array([ 0.01658555, -0.07790598], dtype=np.float64)
#hy_coeffs = np.array([ 0.01880532, -1.26548151], dtype=np.float64)
#hz_coeffs = np.array([ 0.01339084, 2.61905809], dtype=np.float64)
# ~/Projects/PILLS/2016-05-12\ --\ imagery_0004
#hx_coeffs = np.array([ 0.01925678, 0.01527908], dtype=np.float64)
#hy_coeffs = np.array([ 0.01890112, -1.18040666], dtype=np.float64)
#hz_coeffs = np.array([ 0.01645011, 2.87769626], dtype=np.float64)
#hx_func = np.poly1d(hx_coeffs)
#hy_func = np.poly1d(hy_coeffs)
#hz_func = np.poly1d(hz_coeffs)
# ~/Projects/PILLS/2016-06-29\ --\ calibration_0002/
# mag_affine = np.array(
# [[ 0.0223062041, -0.0002700799, -0.0001325525, 1.2016235718],
# [-0.0002700799, 0.0229484854, 0.0000356172, 0.1177744077],
# [-0.0001325525, 0.0000356172, 0.0206129279, -3.2713740483],
# [ 0. , 0. , 0. , 1. ]]
# )
# Phantom 3 - Aug 2016 (ellipse cal)
mag_affine = np.array(
[[0.0189725067, 0.0000203615, 0.0002139272, -0.0134053645],
[0.0000760692, 0.0180178765, 0.0000389461, -1.044762755],
[0.0002417847, 0.0000458039, 0.0171450614, 2.647911793],
[0., 0., 0., 1.]])
# Phantom 3 - Aug 2016 (ekf cal)
# mag_affine = np.array(
# [[ 0.0181297161, 0.000774339, -0.002037224 , -0.2576406372],
# [ 0.0002434548, 0.018469032, 0.0016475328, -0.8452362072],
# [ 0.0000145964, 0.000267444, 0.0159433791, 2.5630653789],
# [ 0. , 0. , 0. , 1. ]]
# )
print mag_affine
fimu = fileinput.input(imu_file)
for line in fimu:
#print line
if not re.search('Time', line):
tokens = line.split(',')
#print len(tokens)
if len(tokens) == 11 and isFloat(tokens[10]):
#print '"' + tokens[10] + '"'
(time, p, q, r, ax, ay, az, hx, hy, hz, temp) = tokens
# remap axis before applying mag calibration
p = -float(p)
q = float(q)
r = -float(r)
ax = -float(ax) * g
ay = float(ay) * g
az = -float(az) * g
hx = -float(hx)
hy = float(hy)
hz = -float(hz)
mag_orientation = 'newer'
if mag_orientation == 'older':
#hx_new = hx_func(float(hx))
#hy_new = hy_func(float(hy))
#hz_new = hz_func(float(hz))
s = [hx, hy, hz]
elif mag_orientation == 'newer':
# remap for 2016-05-12 (0004) data set
#hx_new = hx_func(float(-hy))
#hy_new = hy_func(float(-hx))
#hz_new = hz_func(float(-hz))
s = [-hy, -hx, -hz]
# mag calibration mapping via mag_affine matrix
hs = np.hstack([s, 1.0])
hf = np.dot(mag_affine, hs)
#print hf[:3]
#norm = np.linalg.norm([hx_new, hy_new, hz_new])
#hx_new /= norm
#hy_new /= norm
#hz_new /= norm
imu = nav.structs.IMUdata()
imu.time = float(time) / 1000000.0
imu.p = p
imu.q = q
imu.r = r
imu.ax = ax
imu.ay = ay
imu.az = az
imu.hx = hf[0]
imu.hy = hf[1]
imu.hz = hf[2]
#float(hf[0]), float(hf[1]), float(hf[2]),
imu.temp = float(temp)
imu_data.append(imu)
fgps = fileinput.input(gps_file)
for line in fgps:
if not re.search('Timestamp', line):
#print line
tokens = line.split(',')
#print len(tokens)
if len(tokens) == 14:
(time, itow, ecefx, ecefy, ecefz, ecefvx, ecefvy, ecefvz,
fixtype, posacc, spdacc, posdop, numsvs, flags) = tokens
elif len(tokens) == 19:
(time, itow, lat, lon, alt, ecefx, ecefy, ecefz, ecefvx,
ecefvy, ecefvz, fixtype, posacc, spdacc, posdop, numsvs,
flags, diff_status, carrier_status) = tokens
# wgs84 position
pos_source = 'llh' # 'llh' or 'ecef'
llh = navpy.ecef2lla([
float(ecefx) / 100.0,
float(ecefy) / 100.0,
float(ecefz) / 100.0
], "deg")
# velocity
ned = navpy.ecef2ned([
float(ecefvx) / 100.0,
float(ecefvy) / 100.0,
float(ecefvz) / 100.0
], llh[0], llh[1], llh[2])
if int(numsvs) >= 4:
gps = nav.structs.GPSdata()
gps.time = float(time) / 1000000.0
#gps.status = int(status)
gps.unix_sec = float(time) / 1000000.0 # make filter happy
gps.lat = float(lat)
gps.lon = float(lon)
gps.alt = float(alt)
gps.vn = ned[0]
gps.ve = ned[1]
gps.vd = ned[2]
gps_data.append(gps)
return imu_data, gps_data, filter_data
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
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,
)
def update(self, imu, gps, verbose=True):
# Check for validity of GNSS at this time
self.tcpu = imu.time
self.tow = gps.tow
# Test 1: navValid flag and the data is indeed new (new Time of Week)
# Execute Measurement Update if this is true and Test 2 passes
NEW_GNSS_FLAG = ((gps.valid == 0) &
(abs(self.tow - self.last_tow) > 1e-3))
# Test 2: Check if the delta time of the Time of Week and the
# CPU time are consistent
# If this fails, re-initialize the filter. There must have been a glitch
if NEW_GNSS_FLAG:
#print self.tcpu, self.last_tcpu, self.tcpu-self.last_tcpu
#print self.tow, self.last_tow, self.tow-self.last_tow
if abs((self.tcpu - self.last_tcpu) -
(self.tow - self.last_tow)) > 0.5:
self.TU_COUNT = 0
self.NAV_INIT = False
if verbose:
print("Time Sync Error -- Request reinitialization")
# Record the tow and tcpu at this new update
self.last_tow = self.tow
self.last_tcpu = self.tcpu
# Different subroutine executed in the filter
if not self.NAV_INIT:
if not self.IMU_CAL_INIT:
# SUBROUTINE: IMU CALIBRATION,
# This only happens first time round on the ground. Inflight
# reinitialization is not the same.
self.estAB[:] = [0, 0, 0]
if self.very_first_time:
self.very_first_time = False
self.estGB[:] = [imu.p, imu.q, imu.r]
self.phi = 0
self.theta = 0
else:
self.estGB[:] = self.last_estGB[:]*self.TU_COUNT \
+ [imu.p, imu.q, imu.r]
# Simple AHRS values
self.phi = self.phi*self.TU_COUNT \
+ np.arctan2(-imu.ay, -imu.az)
self.theta = self.theta*self.TU_COUNT \
+ np.arctan2(imu.ax, np.sqrt(imu.ay**2+imu.az**2))
self.phi /= (self.TU_COUNT + 1)
self.theta /= (self.TU_COUNT + 1)
self.estGB[:] /= (self.TU_COUNT + 1)
self.estATT[0], self.estATT[1:] = navpy.angle2quat(
0, self.theta, self.phi)
"""
print("t = %7.3f, Gyro Bias Value: [%6.2f, %6.2f, %6.2f] deg/sec" %\
(imu.time, np.rad2deg(self.estGB[0]), np.rad2deg(self.estGB[1]), np.rad2deg(self.estGB[2]) ))
print("t = %7.3f, phi = %6.2f, theta = %6.2f" % (imu.time,np.rad2deg(self.phi),np.rad2deg(self.theta)) )
"""
self.TU_COUNT += 1
if self.TU_COUNT >= 35:
self.TU_COUNT = 0
self.IMU_CAL_INIT = True
if verbose:
print("t = %7.3f, IMU Calibrated!" % (imu.time))
del (self.phi)
del (self.theta)
else:
if not NEW_GNSS_FLAG:
# SUBROUTINE 1: BACKUP NAVIGATION or IN-FLIGHT INITIALIZATION
# >>>> AHRS CODE GOES HERE
self.estATT[:] = self.last_estATT[:] # This should be some backup nav mode
# <<<<
# When still there is no GNSS signal, continue propagating bias
self.estAB[:] = self.last_estAB[:]
self.estGB[:] = self.last_estGB[:]
else:
# When there is GNSS fix available, initialize all the states
# and renew covariance
self.estPOS[:] = [gps.lat, gps.lon, gps.alt]
self.estVEL[:] = [gps.vn, gps.ve, gps.vd]
self.estATT[:] = self.last_estATT[:]
#self.estATT[0], self.estATT[1:] = navpy.angle2quat(flight_data.psi[i],flight_data.theta[i],flight_data.phi[i])
self.estAB[:] = self.last_estAB[:]
self.estGB[:] = self.last_estGB[:]
self.init_covariance()
#idx_init.append(i)
self.NAV_INIT = True
if verbose:
print("t = %7.3f, Filter (Re-)initialized" %
(imu.time))
elif self.NAV_INIT:
# SUBROUTINE 2: MAIN FILTER ALGORITHM, INS + GNSS
# ==== Time-Update ====
dt = imu.time - self.last_imu.time
q0, qvec = self.last_estATT[0], self.last_estATT[1:4]
C_B2N = navpy.quat2dcm(q0, qvec).T
# 0. Data Acquisition
f_b=np.array([0.5*(self.last_imu.ax+imu.ax)-self.last_estAB[0],\
0.5*(self.last_imu.ay+imu.ay)-self.last_estAB[1],\
0.5*(self.last_imu.az+imu.az)-self.last_estAB[2]])
om_ib=np.array([0.5*(self.last_imu.p+imu.p)-self.last_estGB[0],\
0.5*(self.last_imu.q+imu.q)-self.last_estGB[1],\
0.5*(self.last_imu.r+imu.r)-self.last_estGB[2]])
# 1. Attitude Update
# --> Need to compensate for navrate and earthrate
dqvec = 0.5 * om_ib * dt
self.estATT[0], self.estATT[1:4] = navpy.qmult(
q0, qvec, 1.0, dqvec)
self.estATT[0] /= np.sqrt(self.estATT[0]**2 \
+ la.norm(self.estATT[1:4])**2)
self.estATT[1:4] /= np.sqrt(self.estATT[0]**2 \
+ la.norm(self.estATT[1:4])**2)
# 2. Velocity Update
# --> Need to compensate for coriolis effect
g = np.array([0, 0, 9.81])
f0, fvec = navpy.qmult(q0, qvec, 0, f_b)
f_n0, f_nvec = navpy.qmult(f0, fvec, q0, -qvec)
self.estVEL[:] = self.last_estVEL[:] + (f_nvec + g) * dt
# 3. Position Update
dPOS = navpy.llarate(self.last_estVEL[0], self.last_estVEL[1],
self.last_estVEL[2], self.last_estPOS[0],
self.last_estPOS[2])
dPOS *= dt
self.estPOS[:] = self.last_estPOS[:] + dPOS
# 4. Biases are constant
self.estAB[:] = self.last_estAB[:]
self.estGB[:] = self.last_estGB[:]
# 5. Jacobian
pos2pos = np.zeros((3, 3))
pos2gs = np.eye(3)
pos2att = np.zeros((3, 3))
pos2acc = np.zeros((3, 3))
pos2gyr = np.zeros((3, 3))
gs2pos = np.zeros((3, 3))
gs2pos[2, 2] = -2 * 9.81 / wgs84.R0
gs2gs = np.zeros((3, 3))
gs2att = -2 * C_B2N.dot(navpy.skew(f_b))
gs2acc = -C_B2N
gs2gyr = np.zeros((3, 3))
att2pos = np.zeros((3, 3))
att2gs = np.zeros((3, 3))
att2att = -navpy.skew(om_ib)
att2acc = np.zeros((3, 3))
att2gyr = -0.5 * np.eye(3)
F = np.zeros((15, 15))
F[0:3, 0:3] = pos2pos
F[0:3, 3:6] = pos2gs
F[0:3, 6:9] = pos2att
F[0:3, 9:12] = pos2acc
F[0:3, 12:15] = pos2gyr
F[3:6, 0:3] = gs2pos
F[3:6, 3:6] = gs2gs
F[3:6, 6:9] = gs2att
F[3:6, 9:12] = gs2acc
F[3:6, 12:15] = gs2gyr
F[6:9, 0:3] = att2pos
F[6:9, 3:6] = att2gs
F[6:9, 6:9] = att2att
F[6:9, 9:12] = att2acc
F[6:9, 12:15] = att2gyr
F[9:12, 9:12] = -1.0 / self.tau_a * np.eye(3)
F[12:15, 12:15] = -1.0 / self.tau_g * np.eye(3)
PHI = np.eye(15) + F * dt
# 6. Process Noise
G = np.zeros((15, 12))
G[3:6, 0:3] = -C_B2N
G[6:9, 3:6] = att2gyr
G[9:12, 6:9] = np.eye(3)
G[12:15, 9:12] = np.eye(3)
Q = G.dot(self.Rw.dot(G.T)) * dt
# 7. Covariance Update
self.P = PHI.dot(self.P.dot(PHI.T)) + Q
self.TU_COUNT += 1
if self.TU_COUNT >= 500:
# Request reinitialization after 12 seconds of no GNSS updates
self.TU_COUNT = 0
self.NAV_INIT = False
if NEW_GNSS_FLAG:
# ==== Measurement-Update ====
# 0. Get measurement and make innovations
ecef_ref = navpy.lla2ecef(self.estPOS[0], self.estPOS[1], 0)
ins_ecef = navpy.lla2ecef(self.estPOS[0], self.estPOS[1],
self.estPOS[2])
gnss_ecef = navpy.lla2ecef(gps.lat, gps.lon, gps.alt)
ins_ned = navpy.ecef2ned(ins_ecef - ecef_ref, self.estPOS[0],
self.estPOS[1], self.estPOS[2])
gnss_ned = navpy.ecef2ned(gnss_ecef - ecef_ref, self.estPOS[0],
self.estPOS[1], self.estPOS[2])
dpos = gnss_ned - ins_ned
gnss_vel = np.array([gps.vn, gps.ve, gps.vd])
dvel = gnss_vel - self.estVEL[:]
dy = np.hstack((dpos, dvel))
self.stateInnov[:] = dy
# 1. Kalman Gain
K = self.P.dot(self.H.T)
K = K.dot(la.inv(self.H.dot(self.P.dot(self.H.T)) + self.R))
# 2. Covariance Update
ImKH = np.eye(15) - K.dot(self.H)
KRKt = K.dot(self.R.dot(K.T))
self.P = ImKH.dot(self.P.dot(ImKH.T)) + KRKt
# 3. State Update
dx = K.dot(dy)
Rew, Rns = navpy.earthrad(self.estPOS[0])
self.estPOS[2] -= dx[2]
self.estPOS[0] += np.rad2deg(dx[0] / (Rew + self.estPOS[2]))
self.estPOS[1] += np.rad2deg(
dx[1] / (Rns + self.estPOS[2]) /
np.cos(np.deg2rad(self.estPOS[0])))
self.estVEL[:] += dx[3:6]
self.estATT[0], self.estATT[1:4] = navpy.qmult(
self.estATT[0], self.estATT[1:4], 1, dx[6:9])
self.estATT[0] /= np.sqrt(self.estATT[0]**2 \
+ la.norm(self.estATT[1:4])**2)
self.estATT[1:4] /= np.sqrt(self.estATT[0]**2 \
+ la.norm(self.estATT[1:4])**2)
self.estAB[:] += dx[9:12]
self.estGB[:] += dx[12:15]
if verbose:
print("t = %7.3f, GNSS Update, self.TU_COUNT = %d" %\
(gps.time, self.TU_COUNT) )
self.TU_COUNT = 0
self.last_estPOS[:] = self.estPOS[:]
self.last_estVEL[:] = self.estVEL[:]
self.last_estATT[:] = self.estATT[:]
self.last_estAB[:] = self.estAB[:]
self.last_estGB[:] = self.estGB[:]
else:
# SUBROUTINE 3: e.g. BACKUP NAVIGATION MODE
pass
self.last_imu = imu
self.last_gps = gps
result = INSGPS(self.NAV_INIT, imu.time, self.estPOS, self.estVEL,
self.estATT, self.estAB, self.estGB, self.P,
self.stateInnov)
return result
def load(flight_dir):
imu_data = []
gps_data = []
filter_data = []
# load imu/gps data files
imu_file = flight_dir + "/imu.csv"
gps_file = flight_dir + "/gps.csv"
# calibration by plotting and eye-balling (just finding center point, no
# normalization cooked into calibration.)
#hx_coeffs = np.array([ 1.0, -1.5], dtype=np.float64)
#hy_coeffs = np.array([ 1.0, -78.5], dtype=np.float64)
#hz_coeffs = np.array([ 1.0, -156.5], dtype=np.float64)
#~/Projects/PILLS/Phantom\ 3\ Flight\ Data/2016-03-22\ --\ imagery_0012\ -\ 400\ ft\ survey
#hx_coeffs = np.array([ 0.01857771, -0.18006661], dtype=np.float64)
#hy_coeffs = np.array([ 0.01856938, -1.20854406], dtype=np.float64)
#hz_coeffs = np.array([ 0.01559645, 2.81011976], dtype=np.float64)
# ~/Projects/PILLS/Phantom\ 3\ Flight\ Data/imagery_0009 - 0012
#hx_coeffs = np.array([ 0.01789447, 3.70605872], dtype=np.float64)
#hy_coeffs = np.array([ 0.017071, 0.7125617], dtype=np.float64)
#hz_coeffs = np.array([ 0.01447557, -6.54621951], dtype=np.float64)
# ~/Projects/PILLS/2016-04-04\ --\ imagery_0002
# ~/Projects/PILLS/2016-04-14\ --\ imagery_0003
# ~/Projects/PILLS/2016-04-14\ --\ imagery_0004
#hx_coeffs = np.array([ 0.01658555, -0.07790598], dtype=np.float64)
#hy_coeffs = np.array([ 0.01880532, -1.26548151], dtype=np.float64)
#hz_coeffs = np.array([ 0.01339084, 2.61905809], dtype=np.float64)
# ~/Projects/PILLS/2016-05-12\ --\ imagery_0004
#hx_coeffs = np.array([ 0.01925678, 0.01527908], dtype=np.float64)
#hy_coeffs = np.array([ 0.01890112, -1.18040666], dtype=np.float64)
#hz_coeffs = np.array([ 0.01645011, 2.87769626], dtype=np.float64)
#hx_func = np.poly1d(hx_coeffs)
#hy_func = np.poly1d(hy_coeffs)
#hz_func = np.poly1d(hz_coeffs)
# ~/Projects/PILLS/2016-06-29\ --\ calibration_0002/
# mag_affine = np.array(
# [[ 0.0223062041, -0.0002700799, -0.0001325525, 1.2016235718],
# [-0.0002700799, 0.0229484854, 0.0000356172, 0.1177744077],
# [-0.0001325525, 0.0000356172, 0.0206129279, -3.2713740483],
# [ 0. , 0. , 0. , 1. ]]
# )
# Phantom 3 - Aug 2016 (ellipse cal)
mag_affine = np.array(
[[ 0.0189725067, 0.0000203615, 0.0002139272, -0.0134053645],
[ 0.0000760692, 0.0180178765, 0.0000389461, -1.044762755 ],
[ 0.0002417847, 0.0000458039, 0.0171450614, 2.647911793 ],
[ 0. , 0. , 0. , 1. ]]
)
# Phantom 3 - Aug 2016 (ekf cal)
# mag_affine = np.array(
# [[ 0.0181297161, 0.000774339, -0.002037224 , -0.2576406372],
# [ 0.0002434548, 0.018469032, 0.0016475328, -0.8452362072],
# [ 0.0000145964, 0.000267444, 0.0159433791, 2.5630653789],
# [ 0. , 0. , 0. , 1. ]]
# )
print mag_affine
fimu = fileinput.input(imu_file)
for line in fimu:
#print line
if not re.search('Time', line):
tokens = line.split(',')
#print len(tokens)
if len(tokens) == 11 and isFloat(tokens[10]):
#print '"' + tokens[10] + '"'
(time, p, q, r, ax, ay, az, hx, hy, hz, temp) = tokens
# remap axis before applying mag calibration
p = -float(p)
q = float(q)
r = -float(r)
ax = -float(ax)*g
ay = float(ay)*g
az = -float(az)*g
hx = -float(hx)
hy = float(hy)
hz = -float(hz)
mag_orientation = 'newer'
if mag_orientation == 'older':
#hx_new = hx_func(float(hx))
#hy_new = hy_func(float(hy))
#hz_new = hz_func(float(hz))
s = [hx, hy, hz]
elif mag_orientation == 'newer':
# remap for 2016-05-12 (0004) data set
#hx_new = hx_func(float(-hy))
#hy_new = hy_func(float(-hx))
#hz_new = hz_func(float(-hz))
s = [-hy, -hx, -hz]
# mag calibration mapping via mag_affine matrix
hs = np.hstack( [s, 1.0] )
hf = np.dot(mag_affine, hs)
#print hf[:3]
#norm = np.linalg.norm([hx_new, hy_new, hz_new])
#hx_new /= norm
#hy_new /= norm
#hz_new /= norm
imu = nav.structs.IMUdata()
imu.time = float(time)/1000000.0
imu.p = p
imu.q = q
imu.r = r
imu.ax = ax
imu.ay = ay
imu.az = az
imu.hx = hf[0]
imu.hy = hf[1]
imu.hz = hf[2]
#float(hf[0]), float(hf[1]), float(hf[2]),
imu.temp = float(temp)
imu_data.append( imu )
fgps = fileinput.input(gps_file)
for line in fgps:
if not re.search('Timestamp', line):
#print line
tokens = line.split(',')
#print len(tokens)
if len(tokens) == 14:
(time, itow, ecefx, ecefy, ecefz, ecefvx, ecefvy, ecefvz,
fixtype, posacc, spdacc, posdop, numsvs, flags) = tokens
elif len(tokens) == 19:
(time, itow, lat, lon, alt, ecefx, ecefy, ecefz,
ecefvx, ecefvy, ecefvz,
fixtype, posacc, spdacc, posdop, numsvs, flags,
diff_status, carrier_status) = tokens
# wgs84 position
pos_source = 'llh' # 'llh' or 'ecef'
llh = navpy.ecef2lla([float(ecefx)/100.0,
float(ecefy)/100.0,
float(ecefz)/100.0], "deg")
# velocity
ned = navpy.ecef2ned([float(ecefvx)/100.0,
float(ecefvy)/100.0,
float(ecefvz)/100.0],
llh[0], llh[1], llh[2])
if int(numsvs) >= 4:
gps = nav.structs.GPSdata()
gps.time = float(time)/1000000.0
#gps.status = int(status)
gps.unix_sec = float(time)/1000000.0 # make filter happy
gps.lat = float(lat)
gps.lon = float(lon)
gps.alt = float(alt)
gps.vn = ned[0]
gps.ve = ned[1]
gps.vd = ned[2]
gps_data.append(gps)
return imu_data, gps_data, filter_data
def update(self, imu, gps, verbose=True):
# Check for validity of GNSS at this time
self.tcpu = imu.time
self.tow = gps.tow
# Test 1: navValid flag and the data is indeed new (new Time of Week)
# Execute Measurement Update if this is true and Test 2 passes
NEW_GNSS_FLAG = ((gps.valid==0) & (abs(self.tow-self.last_tow) > 1e-3))
# Test 2: Check if the delta time of the Time of Week and the
# CPU time are consistent
# If this fails, re-initialize the filter. There must have been a glitch
if NEW_GNSS_FLAG:
#print self.tcpu, self.last_tcpu, self.tcpu-self.last_tcpu
#print self.tow, self.last_tow, self.tow-self.last_tow
if abs((self.tcpu-self.last_tcpu) - (self.tow-self.last_tow)) > 0.5:
self.TU_COUNT = 0
self.NAV_INIT = False
if verbose:
print("Time Sync Error -- Request reinitialization")
# Record the tow and tcpu at this new update
self.last_tow = self.tow
self.last_tcpu = self.tcpu
# Different subroutine executed in the filter
if not self.NAV_INIT:
if not self.IMU_CAL_INIT:
# SUBROUTINE: IMU CALIBRATION,
# This only happens first time round on the ground. Inflight
# reinitialization is not the same.
self.estAB[:] = [0,0,0]
if self.very_first_time:
self.very_first_time = False
self.estGB[:] = [imu.p, imu.q, imu.r]
self.phi = 0
self.theta = 0
else:
self.estGB[:] = self.last_estGB[:]*self.TU_COUNT \
+ [imu.p, imu.q, imu.r]
# Simple AHRS values
self.phi = self.phi*self.TU_COUNT \
+ np.arctan2(-imu.ay, -imu.az)
self.theta = self.theta*self.TU_COUNT \
+ np.arctan2(imu.ax, np.sqrt(imu.ay**2+imu.az**2))
self.phi /= (self.TU_COUNT + 1)
self.theta /= (self.TU_COUNT + 1)
self.estGB[:] /= (self.TU_COUNT + 1)
self.estATT[0], self.estATT[1:] = navpy.angle2quat(0, self.theta, self.phi)
"""
print("t = %7.3f, Gyro Bias Value: [%6.2f, %6.2f, %6.2f] deg/sec" %\
(imu.time, np.rad2deg(self.estGB[0]), np.rad2deg(self.estGB[1]), np.rad2deg(self.estGB[2]) ))
print("t = %7.3f, phi = %6.2f, theta = %6.2f" % (imu.time,np.rad2deg(self.phi),np.rad2deg(self.theta)) )
"""
self.TU_COUNT += 1
if self.TU_COUNT >= 35:
self.TU_COUNT = 0
self.IMU_CAL_INIT = True
if verbose:
print("t = %7.3f, IMU Calibrated!" % (imu.time))
del(self.phi)
del(self.theta)
else:
if not NEW_GNSS_FLAG:
# SUBROUTINE 1: BACKUP NAVIGATION or IN-FLIGHT INITIALIZATION
# >>>> AHRS CODE GOES HERE
self.estATT[:] = self.last_estATT[:] # This should be some backup nav mode
# <<<<
# When still there is no GNSS signal, continue propagating bias
self.estAB[:] = self.last_estAB[:]
self.estGB[:] = self.last_estGB[:]
else:
# When there is GNSS fix available, initialize all the states
# and renew covariance
self.estPOS[:] = [gps.lat, gps.lon, gps.alt]
self.estVEL[:] = [gps.vn, gps.ve, gps.vd]
self.estATT[:] = self.last_estATT[:]
#self.estATT[0], self.estATT[1:] = navpy.angle2quat(flight_data.psi[i],flight_data.theta[i],flight_data.phi[i])
self.estAB[:] = self.last_estAB[:]
self.estGB[:] = self.last_estGB[:]
self.init_covariance()
#idx_init.append(i)
self.NAV_INIT = True
if verbose:
print("t = %7.3f, Filter (Re-)initialized" % (imu.time) )
elif self.NAV_INIT:
# SUBROUTINE 2: MAIN FILTER ALGORITHM, INS + GNSS
# ==== Time-Update ====
dt = imu.time - self.last_imu.time
q0, qvec = self.last_estATT[0], self.last_estATT[1:4]
C_B2N = navpy.quat2dcm(q0,qvec).T
# 0. Data Acquisition
f_b=np.array([0.5*(self.last_imu.ax+imu.ax)-self.last_estAB[0],\
0.5*(self.last_imu.ay+imu.ay)-self.last_estAB[1],\
0.5*(self.last_imu.az+imu.az)-self.last_estAB[2]])
om_ib=np.array([0.5*(self.last_imu.p+imu.p)-self.last_estGB[0],\
0.5*(self.last_imu.q+imu.q)-self.last_estGB[1],\
0.5*(self.last_imu.r+imu.r)-self.last_estGB[2]])
# 1. Attitude Update
# --> Need to compensate for navrate and earthrate
dqvec = 0.5*om_ib*dt
self.estATT[0], self.estATT[1:4] = navpy.qmult(q0,qvec,1.0,dqvec)
self.estATT[0] /= np.sqrt(self.estATT[0]**2 \
+ la.norm(self.estATT[1:4])**2)
self.estATT[1:4] /= np.sqrt(self.estATT[0]**2 \
+ la.norm(self.estATT[1:4])**2)
# 2. Velocity Update
# --> Need to compensate for coriolis effect
g = np.array([0,0,9.81])
f0, fvec = navpy.qmult(q0,qvec,0,f_b)
f_n0,f_nvec = navpy.qmult(f0,fvec,q0,-qvec)
self.estVEL[:] = self.last_estVEL[:] + (f_nvec+g)*dt
# 3. Position Update
dPOS = navpy.llarate(self.last_estVEL[0], self.last_estVEL[1],
self.last_estVEL[2], self.last_estPOS[0],
self.last_estPOS[2])
dPOS *= dt
self.estPOS[:] = self.last_estPOS[:] + dPOS
# 4. Biases are constant
self.estAB[:] = self.last_estAB[:]
self.estGB[:] = self.last_estGB[:]
# 5. Jacobian
pos2pos = np.zeros((3,3))
pos2gs = np.eye(3)
pos2att = np.zeros((3,3))
pos2acc = np.zeros((3,3))
pos2gyr = np.zeros((3,3))
gs2pos = np.zeros((3,3))
gs2pos[2,2] = -2*9.81/wgs84.R0
gs2gs = np.zeros((3,3))
gs2att = -2*C_B2N.dot(navpy.skew(f_b))
gs2acc = -C_B2N
gs2gyr = np.zeros((3,3))
att2pos = np.zeros((3,3))
att2gs = np.zeros((3,3))
att2att = -navpy.skew(om_ib)
att2acc = np.zeros((3,3))
att2gyr = -0.5*np.eye(3)
F = np.zeros((15,15))
F[0:3,0:3] = pos2pos
F[0:3,3:6] = pos2gs
F[0:3,6:9] = pos2att
F[0:3,9:12] = pos2acc
F[0:3,12:15] = pos2gyr
F[3:6,0:3] = gs2pos
F[3:6,3:6] = gs2gs
F[3:6,6:9] = gs2att
F[3:6,9:12] = gs2acc
F[3:6,12:15] = gs2gyr
F[6:9,0:3] = att2pos
F[6:9,3:6] = att2gs
F[6:9,6:9] = att2att
F[6:9,9:12] = att2acc
F[6:9,12:15] = att2gyr
F[9:12,9:12] = -1.0/self.tau_a*np.eye(3)
F[12:15,12:15] = -1.0/self.tau_g*np.eye(3)
PHI = np.eye(15) + F*dt
# 6. Process Noise
G = np.zeros((15,12))
G[3:6,0:3] = -C_B2N
G[6:9,3:6] = att2gyr
G[9:12,6:9] = np.eye(3)
G[12:15,9:12] = np.eye(3)
Q = G.dot(self.Rw.dot(G.T))*dt
# 7. Covariance Update
self.P = PHI.dot(self.P.dot(PHI.T)) + Q
self.TU_COUNT += 1
if self.TU_COUNT >= 500:
# Request reinitialization after 12 seconds of no GNSS updates
self.TU_COUNT = 0
self.NAV_INIT = False
if NEW_GNSS_FLAG:
# ==== Measurement-Update ====
# 0. Get measurement and make innovations
ecef_ref = navpy.lla2ecef(self.estPOS[0], self.estPOS[1], 0)
ins_ecef = navpy.lla2ecef(self.estPOS[0], self.estPOS[1],
self.estPOS[2])
gnss_ecef = navpy.lla2ecef(gps.lat, gps.lon, gps.alt)
ins_ned = navpy.ecef2ned(ins_ecef-ecef_ref, self.estPOS[0],
self.estPOS[1], self.estPOS[2])
gnss_ned = navpy.ecef2ned(gnss_ecef-ecef_ref, self.estPOS[0],
self.estPOS[1], self.estPOS[2])
dpos = gnss_ned - ins_ned
gnss_vel = np.array([gps.vn, gps.ve, gps.vd])
dvel = gnss_vel - self.estVEL[:]
dy = np.hstack((dpos, dvel))
self.stateInnov[:] = dy
# 1. Kalman Gain
K = self.P.dot(self.H.T)
K = K.dot( la.inv(self.H.dot(self.P.dot(self.H.T)) + self.R) )
# 2. Covariance Update
ImKH = np.eye(15) - K.dot(self.H)
KRKt = K.dot(self.R.dot(K.T))
self.P = ImKH.dot(self.P.dot(ImKH.T)) + KRKt
# 3. State Update
dx = K.dot(dy)
Rew, Rns = navpy.earthrad(self.estPOS[0])
self.estPOS[2] -= dx[2]
self.estPOS[0] += np.rad2deg(dx[0]/(Rew + self.estPOS[2]))
self.estPOS[1] += np.rad2deg(dx[1]/(Rns + self.estPOS[2])/np.cos(np.deg2rad(self.estPOS[0])))
self.estVEL[:] += dx[3:6]
self.estATT[0], self.estATT[1:4] = navpy.qmult(self.estATT[0], self.estATT[1:4], 1, dx[6:9])
self.estATT[0] /= np.sqrt(self.estATT[0]**2 \
+ la.norm(self.estATT[1:4])**2)
self.estATT[1:4] /= np.sqrt(self.estATT[0]**2 \
+ la.norm(self.estATT[1:4])**2)
self.estAB[:] += dx[9:12]
self.estGB[:] += dx[12:15]
if verbose:
print("t = %7.3f, GNSS Update, self.TU_COUNT = %d" %\
(gps.time, self.TU_COUNT) )
self.TU_COUNT = 0
self.last_estPOS[:] = self.estPOS[:]
self.last_estVEL[:] = self.estVEL[:]
self.last_estATT[:] = self.estATT[:]
self.last_estAB[:] = self.estAB[:]
self.last_estGB[:] = self.estGB[:]
else:
# SUBROUTINE 3: e.g. BACKUP NAVIGATION MODE
pass
self.last_imu = imu
self.last_gps = gps
result = INSGPS( self.NAV_INIT, imu.time, self.estPOS, self.estVEL,
self.estATT, self.estAB, self.estGB,
self.P, self.stateInnov )
return result
