ISToolsPlot.py
Go to the documentation of this file.
1 import numpy as np
2 import os
3 # import sys
4 import simplekml
5 # import pdb
6 
7 import pylib.pose as ps
8 import pylib.ISToolsDataSorted as itd
9 import pylib.filterTools as ft
10 import pylib.plotTools as plotTools
11 import matplotlib.pyplot as plt
12 import matplotlib
13 import datetime
14 
15 import time as systime
16 from pylib.ISToolsDataSorted import cObj, refLla, getTimeFromTowMs, getTimeFromTow
17 from pylib import pose
18 
19 RAD2DEG = 180/np.pi
20 DEG2RAD = np.pi/180
21 
22 
23 pt = plotTools.cPlot()
24 pt.showUtcTime = 0 # 1 = UTC Time, 2 = decimal seconds, 0 = time starts at zero seconds (NOT UTC)
25 
26 g_imu1BiasPqr = []
27 g_imu2BiasPqr = []
28 g_imu1BiasAcc = []
29 g_imu2BiasAcc = []
30 timesCalled = 0
31 serialNumbers = []
32 
33 def log2Data(log, dataClass, key):
34  if key in log.data.keys():
35  d = dataClass(log.data[key])
36  return d;
37  else:
38  return 0;
39 
40 
41 def IsLoggerPlot( pe, log, tru=None, startFigure=None, referencePlot=False, saveFigs=False, saveFigsDirectory='', numDevs=None ):
42  global pt
43  global g_imu1BiasPqr
44  global g_imu2BiasPqr
45  global g_imu1BiasAcc
46  global g_imu2BiasAcc
47  global timesCalled
48  global serialNumbers
49  timesCalled += 1
50  try:
51  serialNumbers.append(log.data['devInfo']['serialNumber'][0])
52  except:
53  print ("something is wrong")
54 
55  def saveFigures(filename, f=None, fig=None):
56  if saveFigs:
57  if fig is None:
58  if f is None:
59  fig = plt.gcf()
60  else:
61  fig = plt.figure(f)
62  fsize = fig.get_size_inches()
63  # fig.set_size_inches(16,16)
64  fig.set_size_inches(20,20)
65  fig.savefig(os.path.join(saveFigsDirectory,filename), bbox_inches='tight')
66  fig.set_size_inches(fsize)
67 
68  def peCheck(key):
69  return key in pe and pe[key]
70 
71  if pe['showUtcTime']:
72  pt.timeIsUtc = pe['showUtcTime']
73  itd.setShowUtcTime(1)
74 
75  # Print all data structures and their variables
76  print("=============== IsLoggerPlot() - datalog keys ===============")
77  for key in log.data.keys():
78  try:
79  print(key, log.data[key].dtype.names)
80  except:
81  pass
82 
83 
84  key = 'ins2'
85  if not key in log.data.keys(): # Use whichever ins is available
86  key = 'ins1'
87  if not key in log.data.keys():
88  key = 'insDev1'
89  if not key in log.data.keys():
90  print("INS data not found!!!")
91  return
92  print("==============================================================")
93 
94  ins = log2Data( log, itd.cINS, key )
95  log.ins = ins
96  if ins:
97  itd.setGpsWeek( ins.v['week'] )
98 
99  gps1Pos = log2Data( log, itd.cGPS, 'gps1Pos' )
100  # gps1Vel = log2Data( log, itd.cGPS, 'gps1Vel' )
101  gps1Ubx = log2Data( log, itd.cGPS, 'gps1UbxPos' )
102  gps1RtkPos = log2Data( log, itd.cGPS, 'gps1RtkPos' )
103  gps1Raw = log2Data( log, itd.cGPSRaw, 'GPS1Raw')
104  gps2Raw = log2Data( log, itd.cGPSRaw, 'GPS2Raw')
105  gpsBaseRaw = log2Data( log, itd.cGPSRaw, 'GPSBaseRaw')
106  imu1 = log2Data( log, itd.cIMU, 'imu1' )
107  dimu = log2Data( log, itd.cIMU, 'dualImu' )
108  dimu = log2Data( log, itd.cIMU, 'preintegratedImu' )
109  mag1 = log2Data( log, itd.cIMU, 'magnetometer1' )
110  mag2 = log2Data( log, itd.cIMU, 'magnetometer2' )
111  magInfo = log2Data( log, itd.cSIMPLE, 'inl2MagObs')
112  varInfo = log2Data( log, itd.cSIMPLE, 'inl2Variance')
113  baro = log2Data( log, itd.cIMU, 'barometer' )
114 
115  print("Timestamps:")
116  if ins:
117  gmt1 = systime.gmtime( ins.time[0] - systime.timezone )
118  gmt2 = systime.gmtime( ins.time[-1] - systime.timezone )
119  dgmt = systime.gmtime( ins.time[-1] - ins.time[0] )
120  print(" INS: %d-%d-%d %d:%d:%d - %d:%d:%d (%d:%d:%d)" % (gmt1.tm_year, gmt1.tm_mon, gmt1.tm_mday,
121  gmt1.tm_hour, gmt1.tm_min, gmt1.tm_sec,
122  gmt2.tm_hour, gmt2.tm_min, gmt2.tm_sec,
123  dgmt.tm_hour, dgmt.tm_min, dgmt.tm_sec))
124  if imu1:
125  gmt1 = systime.gmtime( imu1.time[0] - systime.timezone )
126  gmt2 = systime.gmtime( imu1.time[-1] - systime.timezone )
127  dgmt = systime.gmtime( imu1.time[-1] - imu1.time[0] )
128  print(" IMU: %d-%d-%d %d:%d:%d - %d:%d:%d (%d:%d:%d)" % (gmt1.tm_year, gmt1.tm_mon, gmt1.tm_mday,
129  gmt1.tm_hour, gmt1.tm_min, gmt1.tm_sec,
130  gmt2.tm_hour, gmt2.tm_min, gmt2.tm_sec,
131  dgmt.tm_hour, dgmt.tm_min, dgmt.tm_sec))
132  if gps1Pos:
133  gmt1 = systime.gmtime( gps1Pos.time[0] - systime.timezone )
134  gmt2 = systime.gmtime( gps1Pos.time[-1] - systime.timezone )
135  dgmt = systime.gmtime( gps1Pos.time[-1] - gps1Pos.time[0] )
136  print(" GPS: %d-%d-%d %d:%d:%d - %d:%d:%d (%d:%d:%d)" % (gmt1.tm_year, gmt1.tm_mon, gmt1.tm_mday,
137  gmt1.tm_hour, gmt1.tm_min, gmt1.tm_sec,
138  gmt2.tm_hour, gmt2.tm_min, gmt2.tm_sec,
139  dgmt.tm_hour, dgmt.tm_min, dgmt.tm_sec))
140 
141  sysp = log2Data( log, itd.cSysParams, 'sysParams' )
142  sysp = log2Data( log, itd.cSysParams, 'ins2' )
143  if sysp == 0:
144  sysp = log2Data( log, itd.cSysParams, 'sysParams' )
145  ires = log2Data( log, itd.cInsRes, 'insResources')
146  info = log2Data( log, itd.cDevInfo, 'devInfo')
147 
148  if info:
149  print('Index:', log.index, ' Device S/N: ',info.v['serialNumber'][-1])
150  hver = info.v['hardwareVer'][-1]
151  cver = info.v['commVer'][-1]
152  fver = info.v['firmwareVer'][-1]
153  print(' Hardware: %d.%d.%d.%d Com: %d.%d.%d.%d Firmware: %d.%d.%d.%d b%d r%d' %( hver[0], hver[1], hver[2], hver[3], cver[0], cver[1], cver[2], cver[3], fver[0], fver[1], fver[2], fver[3], info.v['build'][-1], info.v['repoRevision'][-1] ))
154  else:
155  print('Index:', log.index)
156 
157  if 'debugArray' in log.data.keys():
158  pe['dbg'] = 1
159  dbg = log2Data( log, itd.cSIMPLE, 'debugArray')
160  dbg.time = dbg.v['i'][:,0]*0.001
161  else:
162  pe['dbg'] = 0
163  dbg = 0
164 
165  if tru:
166  biasPqr = np.r_[ 0.0, 0.44, -0.15 ]*DEG2RAD
167  biasAcc = np.r_[ -0.038, 0.085, 0.065 ]
168  rot = np.r_[ 0.32, 0.18, 0.5 ]*DEG2RAD
169 
170  rImu = itd.cRIMU(tru.data['IMU'])
171  rGps = itd.cRGPS(tru.data['GPS'])
172  rIns = itd.cRINS(tru.data['INS'])
173 
174  print("RINS keys:", rIns.v.dtype.names)
175  print("RIMU keys:", rImu.v.dtype.names)
176  print("RGPS keys:", rGps.v.dtype.names)
177  else:
178  rImu = 0
179  rGps = 0
180  rIns = 0
181 
182  # if imu1:
183  # print("IMU keys:", imu1.v.dtype.names)
184  # if dimu:
185  # print("Dual IMU keys:", dimu.v.dtype.names)
186  # if mag1:
187  # print("Mag keys:", mag1.v.dtype.names)
188  # if mag2:
189  # print("Mag keys:", mag2.v.dtype.names)
190  # if baro:
191  # print("Baro keys:", baro.v.dtype.names)
192  # if gps1Pos:
193  # print("GPS keys:", gps1Pos.v.dtype.names)
194  # if ins:
195  # print("INS keys:", ins.v.dtype.names)
196  # if sysp:
197  # print("SPR keys:", sysp.v.dtype.names)
198 
199  # Debug. Print all of numpy array
200  np.set_printoptions(threshold='nan')
201 
202 
203  # Start time at zero seconds. Don't use UTC time.
204  meanTowOffset = 0
205  if pt.timeIsUtc == 0:
206  startTow = ins.time[0]
207  ins.time = ins.time - startTow
208  if gps1Pos!=0:
209  meanTowOffset = np.mean(gps1Pos.v['towOffset'])
210 
211  if gps1Pos:
212  gps1Pos.time = gps1Pos.time - startTow
213  if gps1Ubx:
214  gps1Ubx.time = gps1Ubx.time - startTow
215  if gps1RtkPos:
216  gps1RtkPos.time = gps1RtkPos.time - startTow
217 
218  if sysp:
219  sysp.time = sysp.time - startTow
220  if ires and 'time' in ires.v:
221  ires.time = ires.time - startTow
222  if imu1:
223  if gps1Pos!=0:
224  imu1.time = imu1.time + meanTowOffset - startTow
225  else:
226  imu1.time = imu1.time - imu1.time[0]
227 
228  if dimu:
229  if gps1Pos!=0:
230  dimu.time = dimu.time + meanTowOffset - startTow
231  else:
232  dimu.time = dimu.time - dimu.time[0]
233 
234  if mag1:
235  mag1.time = mag1.time + meanTowOffset - startTow
236  if mag2:
237  mag2.time = mag2.time + meanTowOffset - startTow
238  if baro:
239  baro.time = baro.time + meanTowOffset - startTow
240 
241  if rIns:
242  rIns.v['tow'] = rIns.v['tow'] - startTow
243  rImu.v['time'] = rImu.v['time'] - startTow
244  rGps.v['tow'] = rGps.v['tow'] - startTow
245 
246  if dbg:
247  dbg.time = dbg.time - startTow
248 
249  if 'ekfStates' in log.data:
250  if gps1Pos!=0:
251  log.data['ekfStates']['time'] = log.data['ekfStates']['time'] + meanTowOffset - startTow
252  else:
253  log.data['ekfStates']['time'] = log.data['ekfStates']['time'] - log.data['ekfStates']['time'][0]
254 
255  # Fix data
256  if 'ned' in ins.v.dtype.names:
257  ins.v['ned'] = ins.v['ned'] - ins.v['ned'][0,:]
258 
259 
260 
263  if 'eulerErr' in ins.v.dtype.names and rIns:
264  # start = 0 # All of data
265  start = np.shape(ins.time)[0]/2 # Use last half of data
266  print(" ========== STATISTICS ========== ")
267 
268  # RMS Accuracy - Attitude
269  insEuler = ins.v['eulerErr'][start:,:]
270  rIns.offset = np.r_[np.mean(insEuler[:,0]),
271  np.mean(insEuler[:,1]),
272  np.mean(insEuler[:,2])]
273  eulerErr = np.c_[insEuler[:,0] - rIns.offset[0],
274  insEuler[:,1] - rIns.offset[1],
275  insEuler[:,2] - rIns.offset[2]]
276  ins.eulerErrRms = np.sqrt( np.r_[np.mean(eulerErr[:,0]**2),
277  np.mean(eulerErr[:,1]**2),
278  np.mean(eulerErr[:,2]**2)] )
279  print("Euler Angles (deg):")
280  print(" Offset from Truth: \t", rIns.offset*RAD2DEG)
281  print(" RMS Accuracy: \t", ins.eulerErrRms*RAD2DEG)
282 
283 
284  # RMS Accuracy - Velocity
285  uvwErr = ins.v['uvwErr'][start:,:]
286  ins.uvwErrRms = np.sqrt( np.r_[np.mean(uvwErr[:,0]**2),
287  np.mean(uvwErr[:,1]**2),
288  np.mean(uvwErr[:,2]**2)] )
289  print("UVW RMS Accuracy (m/s): \t", ins.uvwErrRms)
290 
291 
292  # RMS Accuracy - Position
293  nedErr = ins.v['nedErr'][start:,:]
294  ins.nedErrRms = np.sqrt( np.r_[np.mean(nedErr[:,0]**2),
295  np.mean(nedErr[:,1]**2),
296  np.mean(nedErr[:,2]**2)] )
297  print("NED RMS Accuracy (m): \t", ins.nedErrRms)
298 
299  if peCheck('rawGPSStats'):
300  if gps1Raw is not 0:
301  print("GPS 1 Corrupt Data Count", gps1Raw.corruptCount, "/", len(gps1Raw.count))
302  if gps2Raw is not 0:
303  print("GPS 2 Corrupt Data Count", gps2Raw.corruptCount, "/", len(gps2Raw.count))
304  if gpsBaseRaw is not 0:
305  print("GPS Base Corrupt Data Count", gpsBaseRaw.corruptCount, "/", len(gpsBaseRaw.count))
306 
307 
308 
311  insColor = 'b'
312  refColor = 'r' rtkColor = 'c'
313  ubxColor = 'y'
314  rInsColor = 'k'
315  rGpsColor = 'm'
316  errColor = 'm'
317  magColor = 'g'
318  barColor = 'c'
319 
320  if referencePlot:
321  insColor = 'c'
322 
323  f = 0
324  if startFigure!=None:
325  f = startFigure
326 
327  # Create list of serial numbers
328  IsLoggerPlot.serialNumbers += [log.serialNumber]
329 
330  pt.setPreTitle("%s__%s" % (os.path.basename(saveFigsDirectory), log.serialNumber))
331 
332 
335  if peCheck('temp'):
336  f += 1
337  if not 'sysParams' in log.data.keys():
338  print("Unable to plot sysParams (because it's not there)")
339  else:
340  sysP = log.data['sysParams']
341  time = (sysP['towMs'] - sysP['towMs'][0])/1000.0
342  fig, ax = pt.subplots(f, 2, 'Temperature')
343  pt.subplotSingle(ax[0], getTimeFromTowMs(sysP['towMs']), sysP['imuTemp'], title='Temperature: IMU')
344  pt.subplotSingle(ax[1], getTimeFromTowMs(sysP['towMs']), sysP['baroTemp'], title='Temperature: Baro')
345  saveFigures('Temperature', f)
346 
347 
348 
351  if peCheck('debugArray_i'):
352  f += 1
353  if dbg == 0:
354  print("Unable to print debug array - messages missing")
355  else:
356  fig, ax = pt.subplots(f, 1, 'debugArray_i', sharex=True)
357  for j in range(9):
358  pt.subplotSingle(ax, dbg.time, dbg.v['i'][:,j], title= 'i', options={'label': j})
359  plt.legend()
360  saveFigures('debugArray_i.svg', f)
361  if peCheck('debugArray_f') and dbg:
362  f += 1
363  if dbg == 0:
364  print("Unable to print debug array - messages missing")
365  else:
366  fig, ax = pt.subplots(f, 1, 'debugArray_f', sharex=True)
367  for j in range(9):
368  pt.subplotSingle(ax, dbg.time, dbg.v['f'][:,j], title= 'f', options={'label': j})
369  plt.legend()
370  saveFigures('debugArray_f.svg', f)
371  if peCheck('debugArray_lf') and dbg:
372  f += 1
373  if dbg == 0:
374  print("Unable to print debug array - messages missing")
375  else:
376  fig, ax = pt.subplots(f, 1, 'debugArray_lf', sharex=True)
377  for j in range(3):
378  pt.subplotSingle(ax, dbg.time, dbg.v['lf'][:,j], title ='lf', options={'label': j})
379  plt.legend()
380  saveFigures('debugArray_lf.svg', f)
381 
382 
385  def inliers(data, m=4):
386  d = np.abs(data - np.median(data))
387  mdev = np.median(d)
388  s = d / mdev if mdev else 0.
389  return s < m
390 
391  def plotSNR(raw_gps, f, name):
392  f += 1
393  if raw_gps is None:
394  print("unable to plot " + name + "SNR because data is missing")
395  return
396  satellites = np.unique(raw_gps.obs['sat'])
397  fig, ax = pt.subplots(f, len(satellites), 'raw' + name + 'SNR', sharex=True)
398 
399  for i, sat in enumerate(satellites):
400  valid_indexes = (raw_gps.obs['sat'] == sat)
401  pt.subplotSingle(ax[i], raw_gps.obstime[valid_indexes].astype(datetime.datetime), raw_gps.obs['sat'][valid_indexes], title="sat " + str(sat), options='x')
402  saveFigures('raw' + name + 'SNR.svg', f)
403  return f
404 
405  def plotPrange(raw_gps, f, name):
406  f += 1
407  if raw_gps is None:
408  print("unable to plot " + name + "Prange because data is missing")
409  return
410  satellites = np.unique(raw_gps.obs['sat'])
411  fig, ax = pt.subplots(f, len(satellites), 'raw' + name + 'Prange', sharex=True)
412 
413  for i, sat in enumerate(satellites):
414  valid_indexes = raw_gps.obs['sat'] == sat
415  try:
416  pt.subplotSingle(ax[i], raw_gps.obstime[valid_indexes].astype(datetime.datetime), raw_gps.obs['P'][valid_indexes], title="sat " + str(sat), options='x')
417  except:
418  pass
419  saveFigures('raw' + name + 'Prange.svg', f)
420  return f
421 
422  def plotCPhase(raw_gps, f, name):
423  f += 1
424  if raw_gps is None:
425  print("unable to plot " + name + "Cphase because data is missing")
426  return
427  satellites = np.unique(raw_gps.obs['sat'])
428  fig, ax = pt.subplots(f, len(satellites), 'raw' + name + 'CarrierPhase', sharex=True)
429 
430  for i, sat in enumerate(satellites):
431  valid_indexes = (raw_gps.obs['sat'] == sat) & (raw_gps.obs['L'] != 0)
432  try:
433  pt.subplotSingle(ax[i], raw_gps.obstime[valid_indexes].astype(datetime.datetime), raw_gps.obs['L'][valid_indexes],
434  title="sat " + str(sat), options='x')
435  except:
436  print("problem plotting cphase")
437  saveFigures('raw' + name + 'CarrierPhase.svg', f)
438  return f
439 
440  if peCheck('rawBaseGpsSNR') and gpsBaseRaw:
441  f = plotSNR(gpsBaseRaw, f, 'BaseRaw')
442 
443  if peCheck('rawBaseGpsPrange') and gpsBaseRaw:
444  f = plotPrange(gpsBaseRaw, f, 'BaseRaw')
445 
446  if peCheck('rawBaseCarrierPhase') and gpsBaseRaw:
447  f = plotCPhase(gpsBaseRaw, f, 'BaseRaw')
448 
449 
452  if peCheck('rawGps1SNR') and gps1Raw:
453  f = plotSNR(gps1Raw, f, 'Gps1Raw')
454 
455  if peCheck('rawGps1Prange') and gps1Raw:
456  f = plotPrange(gps1Raw, f, 'Gps1Raw')
457 
458  if peCheck('rawGps1CarrierPhase') and gps1Raw:
459  f = plotCPhase(gps1Raw, f, 'Gps1Raw')
460 
461 
464  if peCheck('rawGps2SNR') and gps2Raw:
465  f = plotSNR(gps2Raw, f, 'GPS2Raw')
466 
467  if peCheck('rawGps2Prange') and gps2Raw:
468  f = plotPrange(gps2Raw, f, 'GPS2Raw')
469 
470  if peCheck('rawGps2CarrierPhase') and gps2Raw:
471  f = plotCPhase(gps2Raw, f, 'GPS2Raw')
472 
473 
475  if peCheck('compassingAngle'):
476  f += 1
477  if 'gps1RtkCmpRel' not in log.data.keys():
478  print("no gps1RtkCmpRel, cannot plot compassingAngle")
479  else:
480  legend = []
481  fig, ax = pt.subplots(f, 2, 'Heading', sharex=True)
482 
483  rtkRel = log.data['gps1RtkCmpRel']
484 
485  pt.subplotSingle(ax[0], getTimeFromTowMs(rtkRel['timeOfWeekMs']), rtkRel['headingToBase'] * RAD2DEG, title="RTK")
486  pt.subplotSingle(ax[1], getTimeFromTow(ins.v['tow']), ins.v['euler'][:, 2] * RAD2DEG, title="INS")
487  saveFigures('compassingAngle.svg', f)
488 
489 
491  if peCheck('vectorToBase'):
492  # pt.labels('NED', 'm')
493  f += 1
494  if 'gps1RtkCmpRel' not in log.data.keys():
495  print("no gps1RtkCmpRel, cannot plot vectorToBase")
496  else:
497  legend = []
498  fig, ax = pt.subplots(f, 4, 'vectorToBase', sharex=True)
499 
500  rtkRel = log.data['gps1RtkCmpRel']
501 
502  # vec2basefix = rtkRel['vectorToBase'].copy()
503  # vec2basefloat = rtkRel['vectorToBase'].copy()
504  # vec2basesingle = rtkRel['vectorToBase'].copy()
505  # dist2basefix = rtkRel['distanceToBase'].copy()
506  # dist2basefloat = rtkRel['distanceToBase'].copy()
507  # dist2basesingle = rtkRel['distanceToBase'].copy()
508  #
509  # vec2basefix[rtkRel['arRatio'] < 3.0, :] = np.nan
510  # vec2basefloat[(rtkRel['arRatio'] < 0.0) | (rtkRel['arRatio'] > 3.0), :] = np.nan
511  # vec2basesingle[rtkRel['arRatio'] > 0.001, :] = np.nan
512  # dist2basefix[rtkRel['arRatio'] < 3.0] = np.nan
513  # dist2basefloat[(rtkRel['arRatio'] < 0.0) | (rtkRel['arRatio'] > 3.0)] = np.nan
514  # dist2basesingle[rtkRel['arRatio'] > 0.001] = np.nan
515 
516  time = getTimeFromTowMs(rtkRel['timeOfWeekMs'])
517 
518  cmap = matplotlib.cm.get_cmap('Spectral')
519  colors = [cmap(i/numDevs) for i in range(numDevs)]
520 
521  titles = ["North", "East", "Down"]
522  for i in range(3):
523  pt.subplotSingle(ax[i], time, rtkRel['vectorToBase'][:,i],title=titles[i], ylabel='m')
524  # pt.subplotSingle(ax[i], time, vec2basefix[:,i],title=titles[i], ylabel='m')
525  # pt.subplotSingle(ax[i], time, vec2basefloat[:, i], ylabel='m')
526  # pt.subplotSingle(ax[i], time, vec2basesingle[:, i], ylabel='m')
527  if i == 0:
528  ax[0].legend([str(ser) for ser in serialNumbers])
529 
530  pt.subplotSingle(ax[3], time, rtkRel['distanceToBase']
531  , title="DistanceToBase", ylabel='m')
532  # pt.subplotSingle(ax[3], time, dist2basefix, title="DistanceToBase", ylabel='m')
533  # pt.subplotSingle(ax[3], time, dist2basesingle)
534  # pt.subplotSingle(ax[3], time, dist2basefloat)
535  saveFigures('vectorToBase.svg', f)
536 
537 
539  if peCheck('compassingCircle'):
540  f += 1
541  if 'gps1RtkCmpRel' not in log.data.keys():
542  print("no RTK_Rel, cannot plot compassingCircle")
543  else:
544  legend = []
545 
546  rtkRel = log.data['gps1RtkCmpRel']
547 
548  vec2basefix = rtkRel['vectorToBase'].copy()
549  vec2basefloat = rtkRel['vectorToBase'].copy()
550  vec2basesingle = rtkRel['vectorToBase'].copy()
551 
552  vec2basefix[rtkRel['arRatio'] < 3.0, :] = np.nan
553  vec2basefloat[(rtkRel['arRatio'] < 0.0) | (rtkRel['arRatio'] > 3.0), :] = np.nan
554  vec2basesingle[rtkRel['arRatio'] > 0.001, :] = np.nan
555 
556  fig, ax = pt.subplots(f, 2, 'compassingCircle', numCols=2)
557  pt.subplotSingle(ax[0,0], vec2basefix[:, 0], vec2basefix[:, 1], options='m')
558  pt.subplotSingle(ax[0,0], vec2basefloat[:, 0], vec2basefloat[:, 1], options='b')
559  pt.subplotSingle(ax[0,0], vec2basesingle[:, 0], vec2basesingle[:, 1], options='g')
560  plt.axis('equal')
561  plt.xlabel("ECEF x")
562  plt.xlabel("ECEF y")
563 
564  pt.subplotSingle(ax[1,0], vec2basefix[:, 1], vec2basefix[:, 2], options='m')
565  pt.subplotSingle(ax[1,0], vec2basefloat[:, 1], vec2basefloat[:, 2], options='b')
566  pt.subplotSingle(ax[1,0], vec2basesingle[:, 1], vec2basesingle[:, 2], options='g')
567  plt.axis('equal')
568  plt.xlabel("ECEF y")
569  plt.xlabel("ECEF z")
570 
571  pt.subplotSingle(ax[0,1], vec2basefix[:, 0], vec2basefix[:, 2], options='m')
572  pt.subplotSingle(ax[0,1], vec2basefloat[:, 0], vec2basefloat[:, 2], options='b')
573  pt.subplotSingle(ax[0,1], vec2basesingle[:, 0], vec2basesingle[:, 2], options='g')
574  plt.xlabel("ECEF x")
575  plt.xlabel("ECEF z")
576 
577  ax[0,1].legend(['fix', 'float', 'single'])
578  plt.axis('equal')
579  saveFigures('compassingCircle.svg', f)
580 
581  if peCheck('compassingCircle3d'):
582  f += 1
583  if 'gps1RtkCmpRel' not in log.data.keys():
584  print("no RTK_Rel, cannot plot compassingCircle")
585  else:
586  vec2basefix = rtkRel['vectorToBase'].copy()
587  vec2basefloat = rtkRel['vectorToBase'].copy()
588  vec2basesingle = rtkRel['vectorToBase'].copy()
589 
590  vec2basefix[rtkRel['arRatio'] < 3.0, :] = np.nan
591  vec2basefloat[(rtkRel['arRatio'] < 0.0) | (rtkRel['arRatio'] > 3.0), :] = np.nan
592  vec2basesingle[rtkRel['arRatio'] > 0.001, :] = np.nan
593 
594  fig, ax = pt.plot3D(f, 'compassingCircle3d')
595  ax.plot(vec2basefix[:, 0], vec2basefix[:, 1], vec2basefix[:,2], 'm')
596  ax.plot(vec2basefloat[:, 0], vec2basefloat[:, 1], vec2basefloat[:,2], 'b')
597  ax.plot(vec2basesingle[:, 0], vec2basesingle[:, 1], vec2basesingle[:,2], 'g')
598  ax.legend(['fix', 'float', 'single'])
599  plt.axis('equal')
600  saveFigures('compassingCircle3d.svg', f)
601 
602  u, v = np.mgrid[0:2 * np.pi:180j, 0:np.pi:180j]
603  x = np.cos(u) * np.sin(v)
604  y = np.sin(u) * np.sin(v)
605  z = np.cos(v)
606  ax.plot_wireframe(x, y, z, color="k", alpha=0.1, linewidth=1.0)
607 
608 
610  if peCheck('googleEarth'):
611  tStep = 1
612 
613  if pe['googleEarth'] == 2:
614  altMode="absolute"
615  else:
616  altMode="clampToGround"
617 
618  # kmlfile = log.directory + "/" + itd.lla2kml(ins.time[insTCnt:], ins.v['lla'][insTCnt:,:], log.serialNumber, "LOG_"+log.serialNumber+"ins.kml", timeStep=tStep, altitudeMode=altMode )
619  # kmlfile = log.directory + "/" + itd.lla2kml(ins.time, ins.v['lla'], log.serialNumber, "LOG_"+log.serialNumber+"ins.kml", timeStep=tStep, altitudeMode=altMode )
620  kmlfile = itd.lla2kml(ins.time, ins.v['lla'], log.serialNumber, log.directory + "/" + "LOG_"+log.serialNumber+"ins.kml", timeStep=tStep, altitudeMode=altMode )
621  os.startfile( kmlfile )
622  if gps1Pos:
623  # kmlfile = log.directory + "/" + itd.lla2kml(gps1Pos.time, gps1Pos.v['lla'], log.serialNumber, "LOG_"+log.serialNumber+"gps1Pos.kml", timeStep=tStep, altitudeMode=altMode, color=simplekml.Color.red )
624  kmlfile = itd.lla2kml(gps1Pos.time, gps1Pos.v['lla'], log.serialNumber, log.directory + "/" + "LOG_"+log.serialNumber+"gps1Pos.kml", timeStep=tStep, altitudeMode=altMode, color=simplekml.Color.red )
625  os.startfile( kmlfile )
626  if rIns:
627  # kmlfile = log.directory + "/" + itd.lla2kml(rIns.v['time'], rIns.v['lla'], tru.serialNumber, "LOG_"+tru.serialNumber+"refIns.kml", timeStep=tStep, altitudeMode=altMode, color=simplekml.Color.black )
628  kmlfile = itd.lla2kml(rIns.v['time'], rIns.v['lla'], tru.serialNumber, log.directory + "/" + "LOG_"+tru.serialNumber+"refIns.kml", timeStep=tStep, altitudeMode=altMode, color=simplekml.Color.black )
629  os.startfile( kmlfile )
630 
631  if pe['googleEarth'] == 2:
632  return
633 
634 
635 
636 
638  if dimu and 0:
639  pt.labels('IMU Time', 'ms')
640  f += 1; legend = []
641  fig, ax = pt.subplots(f,1,"IMU time")
642  timeCnt = len(dimu.time)
643  dimuTime = []
644  counter = []
645  for i in range(0,timeCnt):
646  counter.append(i)
647  dimuTime.append(np.float64(dimu.time[i]).item())
648 
649  pt.subplotSingle(ax, counter, dimu.v['time'])
650  plt.ylim( -1, 10000 )
651 
652 
654  if peCheck('uvw'):
655  pt.labels('UVW', 'm/s')
656  f += 1; legend = []
657  if rIns:
658  pt.plot3Axes(f, rIns.v['time'], rIns.uvw, options=rInsColor)
659  legend += ['Truth']
660  pt.plot3Axes(f, getTimeFromTow(ins.v['tow']), ins.v['uvw'], options=insColor)
661  legend += ['INS']
662  plt.legend(legend)
663 
664  if peCheck('uvwDot'):
665  f += 1; legend = []
666 
667  if rIns:
668  pt.plot3Axes(f, rIns.v['time'], ft.smooth(ft.derivative(rIns.v['time'], rIns.uvw, delta=2), delta=10), options=rInsColor)
669  legend += ['Truth']
670  pt.plot3Axes(f, ires.time, ft.smooth(ires.v['x_dot.uvw'], delta=20), 'UVW Dot', 'm/s^2', options=insColor)
671  legend += ['INS']
672  plt.legend(legend)
673 
674  saveFigures('uvw.svg', f)
675 
676  if peCheck('uvwErr') and 'uvwErr' in ins.v.dtype.names:
677  f += 1; legend = []
678  pt.plot3Axes(f, getTimeFromTow(ins.v['tow']), ins.v['uvwErr'], 'UVW Error from Truth INS', 'm/s', options=errColor)
679 
680 
681 
683  if peCheck('att'):
684  f += 1; legend = [str(ser) for ser in serialNumbers]
685  fig, ax = pt.subplots(f,3, "Attitude", sharex=True)
686 
687  if rIns:
688  pt.subplotSingle(ax[0], rIns.v['time'], rIns.v['euler'][:,0]*RAD2DEG, options=rInsColor)
689 
690  if referencePlot:
691  pt.subplotSingle(ax[0], getTimeFromTow(ins.v['tow']), ins.v['euler'][:,0]*RAD2DEG, 'Roll', 'deg', options=insColor)
692  else:
693  pt.subplotSingle(ax[0], getTimeFromTow(ins.v['tow']), ins.v['euler'][:,0]*RAD2DEG, 'Roll', 'deg')
694 
695  if rIns:
696  pt.subplotSingle(ax[1], rIns.v['time'], rIns.v['euler'][:,1]*RAD2DEG, options=rInsColor)
697 
698  if referencePlot:
699  pt.subplotSingle(ax[1], getTimeFromTow(ins.v['tow']), ins.v['euler'][:,1]*RAD2DEG, 'Pitch', 'deg', options=insColor)
700  else:
701  pt.subplotSingle(ax[1], getTimeFromTow(ins.v['tow']), ins.v['euler'][:,1]*RAD2DEG, 'Pitch', 'deg')
702 
703  if rIns:
704  pt.subplotSingle(ax[2], rIns.v['time'], rIns.v['euler'][:,2]*RAD2DEG, options=rInsColor)
705  legend += ['Truth']
706 
707  if referencePlot:
708  pt.subplotSingle(ax[2], getTimeFromTow(ins.v['tow']), ins.v['euler'][:,2]*RAD2DEG, 'Yaw', 'deg', options=insColor)
709  else:
710  pt.subplotSingle(ax[2], getTimeFromTow(ins.v['tow']), ins.v['euler'][:,2]*RAD2DEG, 'Yaw', 'deg')
711 
712  legend += ['INS']
713 
714  if pe['att'] >= 2: # Display GPS heading
715  # Ignore GPS if timestamp is not valid
716  if gps1Pos and np.fabs(np.mean(getTimeFromTow(ins.v['tow'])) - np.mean(gps1Pos.time)) < 1000:
717  pt.subplotSingle(ax[2], gps1Pos.time, gps1Pos.v['course']*RAD2DEG, options=refColor)
718  legend += ['GPS']
719 
720  ax[2].legend(legend)
721 
722  saveFigures('attINS.svg', f)
723 
724 
725 
727  if peCheck('magNISThreshold'):
728  f += 1; legend = []
729  fig, ax = pt.subplots(f,1, 'magNISThreshold')
730  pt.labels('Mag NIS')
731  pt.subplotSingle(ax,magInfo.v['towMs'], magInfo.v['nis'] , 'nis' )
732  pt.subplotSingle(ax,magInfo.v['towMs'], magInfo.v['nis_threshold'] , 'nis_threshold' )
733 
734 
736  if peCheck('variance'):
737  f += 1; legend = []
738  fig, ax = pt.subplots(f,3, 'Variance', sharex=True)
739  pt.labels('INL2 Position Variance')
740  pt.subplotSingle(ax[0],varInfo.v['towMs'], varInfo.v['PxyxNED'][:,0] , 'INS Px N (NED)' )
741  pt.subplotSingle(ax[1],varInfo.v['towMs'], varInfo.v['PxyxNED'][:,1] , 'INS Py E (NED)' )
742  pt.subplotSingle(ax[2],varInfo.v['towMs'], varInfo.v['PxyxNED'][:,2] , 'INS Pz D (NED)' )
743 
744  f += 1; legend = []
745  fig, ax = pt.subplots(f,3, 'Vel Variance', sharex=True)
746  pt.labels('INL2 Velocity Variance')
747  pt.subplotSingle(ax[0],varInfo.v['towMs'], varInfo.v['PvelNED'][:,0] , 'INS Pvx N (NED)' )
748  pt.subplotSingle(ax[1],varInfo.v['towMs'], varInfo.v['PvelNED'][:,1] , 'INS Pvy E (NED)' )
749  pt.subplotSingle(ax[2],varInfo.v['towMs'], varInfo.v['PvelNED'][:,2] , 'INS Pvz D (NED)' )
750 
751  f += 1; legend = []
752  fig, ax = pt.subplots(f,3, 'Att Variance', sharex=True)
753  pt.labels('INL2 Attitude Variance')
754  pt.subplotSingle(ax[0],varInfo.v['towMs'], varInfo.v['PattNED'][:,0] , 'INS Pwx (NED)' )
755  pt.subplotSingle(ax[1],varInfo.v['towMs'], varInfo.v['PattNED'][:,1] , 'INS Pwy (NED)' )
756  pt.subplotSingle(ax[2],varInfo.v['towMs'], varInfo.v['PattNED'][:,2] , 'INS Pwz (NED)' )
757 
758  f += 1; legend = []
759  fig, ax = pt.subplots(f,3, 'A Bias Variance', sharex=True)
760  pt.labels('INL2 A Bias Variance')
761  pt.subplotSingle(ax[0],varInfo.v['towMs'], varInfo.v['PABias'][:,0] , 'INS PA Bias x' )
762  pt.subplotSingle(ax[1],varInfo.v['towMs'], varInfo.v['PABias'][:,1] , 'INS PA Bias y' )
763  pt.subplotSingle(ax[2],varInfo.v['towMs'], varInfo.v['PABias'][:,2] , 'INS PA Bias z' )
764 
765  f += 1; legend = []
766  fig, ax = pt.subplots(f,3, 'W Bias Variance', sharex=True)
767  pt.labels('INL2 W Bias Variance')
768  pt.subplotSingle(ax[0],varInfo.v['towMs'], varInfo.v['PWBias'][:,0] , 'INS PW Bias x' )
769  pt.subplotSingle(ax[1],varInfo.v['towMs'], varInfo.v['PWBias'][:,1] , 'INS PW Bias y' )
770  pt.subplotSingle(ax[2],varInfo.v['towMs'], varInfo.v['PWBias'][:,2] , 'INS PW Bias z' )
771 
772  f += 1; legend = []
773  fig, ax = pt.subplots(f,1, 'Baro Bias Variance', sharex=True)
774  pt.labels('INL2 Baro Bias Variance')
775  pt.subplotSingle(ax,varInfo.v['towMs'], varInfo.v['PBaroBias'] , 'Baro Bias' )
776 
777  f += 1; legend = []
778  fig, ax = pt.subplots(f,1, 'Declination Variance', sharex=True)
779  pt.labels('INL2 Declination Variance')
780  pt.subplotSingle(ax,varInfo.v['towMs'], varInfo.v['PDeclination'] , 'Declination' )
781 
782 
783 
785  if peCheck('gpsVel'):
786  pt.labels('GPS Vel NED', 'm/s')
787  f += 1; legend = []
788  if rIns:
789  pt.plot3Axes(f, rIns.v['time'], rIns.v['nedDot'], options=rInsColor )
790  legend += ['Truth']
791  if gps1Vel:
792  pt.plot3Axes(f, gps1Vel.time, gps1Vel.v['velNed'], options=refColor )
793  legend += ['GPS.velNed']
794  gps1Pos.nedDot = ft.derivative(gps1Pos.time, gps1Pos.ned, delta=2)
795  # pt.plot3Axes(f, gps1Vel.time, gps1Vel.nedDot, options='m' )
796  # legend += ['GPS.ned dot']
797  plt.legend(legend)
798  saveFigures('gpsVel.svg', f)
799 
800 
802  if peCheck('ekfStatesMagField') and 'ekfStates' in log.data.keys():
803  f += 1; legend = []
804  fig, ax = pt.subplots(f,2, 'ekfStatesMagField', sharex=True)
805  pt.subplotSingle(ax[0], log.data['ekfStates']['time'], log.data['ekfStates']['magInc']*RAD2DEG, 'EKF States - Mag Inclination', 'deg', options=insColor )
806  pt.subplotSingle(ax[1], log.data['ekfStates']['time'], log.data['ekfStates']['magDec']*RAD2DEG, 'EKF States - Mag Declination', 'deg', options=insColor )
807 
808  saveFigures('ekfStatesMagField.svg', fig=fig)
809 
810 
812  if peCheck('velNED'):
813  pt.labels('Vel NED', 'm/s')
814  f += 1; legend = []
815  crnFreq = 2
816 
817  if rIns:
818  pt.plot3Axes(f, rIns.v['time'], rIns.v['nedDot'], options=rInsColor )
819  legend += ['Truth']
820  if gps1Pos:
821  pt.plot3Axes(f, gps1Pos.time, gps1Pos.v['ned'], options=refColor )
822  legend += ['GPS']
823  pt.plot3Axes(f, ins.time, ins.velNed(), options=insColor )
824  legend += ['INS']
825 
826  plt.legend(legend)
827 
828  if peCheck('velNedDot'):
829  f += 1; legend = []
830 
831  delta = 2
832  gps1Pos.nedVelDot = ft.derivative(gps1Pos.time, gps1Pos.v['ned'], delta)
833  ins.nedVelDot = ft.derivative(ins.time, ins.v['vel'], delta)
834  ins.nedVelDot = ft.lpfNoDelay(ins.nedVelDot, cornerFreqHz=20, time=ins.time)
835 
836  if rIns:
837  pt.plot3Axes(f, rIns.v['time'], rIns.nedDotDot, options=rInsColor )
838  legend += ['Truth']
839  pt.plot3Axes(f, ins.time, ins.nedVelDot )
840  legend += ['ins']
841  pt.plot3Axes(f, gps1Pos.time, gps1Pos.nedVelDot, 'NED Vel Dot', 'm/s^2', options=refColor )
842  legend += ['gps1Pos']
843  plt.legend(legend)
844 
845  saveFigures('velNED.svg', f)
846 
847 
848 
850  if peCheck('ecef') and gps1Pos:
851  f += 1; legend = []
852  fig, ax = pt.subplots(f,3, 'ECEF', sharex=True)
853 
854 
855  instime = getTimeFromTow(ins.v['tow'])
856  gpstime = getTimeFromTowMs(gps1Pos.v['timeOfWeekMs'])
857  pt.subplotSingle(ax[0], instime, ins.ecef()[:,0], 'X', 'm', options=insColor)
858  pt.subplotSingle(ax[0], gpstime, gps1Pos.v['ecef'][:,0], options=refColor )
859 
860  pt.subplotSingle(ax[1], instime, ins.ecef()[:,1], 'Y', 'm', options=insColor )
861  pt.subplotSingle(ax[1], gpstime, gps1Pos.v['ecef'][:,1], options=refColor )
862 
863  pt.labels('Z', 'm')
864  pt.subplotSingle(ax[2], instime, ins.ecef()[:,2], options=insColor )
865  legend += ['INS']
866  pt.subplotSingle(ax[2], gpstime, gps1Pos.v['ecef'][:,2], options=refColor )
867  legend += ['GPS']
868  ax[2].legend(legend)
869 
870  saveFigures('ecef.svg', f)
871 
872 
874  if peCheck('lla') and gps1Pos and hasattr(gps1Pos, 'pos'):
875  f += 1; legend = []
876  fig, ax = pt.subplots(f,3, 'LLA', sharex=True)
877  if rIns:
878  pt.subplotSingle(ax[0], rIns.v['time'], rIns.v['lla'][:,0], options=rInsColor )
879 
880  instime = getTimeFromTow(ins.v['tow'])
881  gpstime = getTimeFromTowMs(gps1Pos.v['timeOfWeekMs'])
882  pt.subplotSingle(ax[0], instime, ins.v['lla'][:,0], 'Latitude', 'deg', options=insColor)
883  pt.subplotSingle(ax[0], gpstime, gps1Pos.v['lla'][:,0], options=refColor )
884 
885  if rIns:
886  pt.subplotSingle(ax[1], rIns.v['time'], rIns.v['lla'][:,1], options=rInsColor )
887 
888  pt.subplotSingle(ax[1], instime, ins.v['lla'][:,1], 'Longitude', 'deg', options=insColor )
889  pt.subplotSingle(ax[1], gpstime, gps1Pos.v['lla'][:,1], options=refColor )
890 
891  if rIns:
892  pt.subplotSingle(ax[2], rIns.v['time'], rIns.v['lla'][:,2], options=rInsColor )
893  legend += ['Truth']
894 
895  pt.labels('Elipsoid Alt', 'm')
896  pt.subplotSingle(ax[2], instime, ins.v['lla'][:,2], options=insColor )
897  legend += ['INS']
898  pt.subplotSingle(ax[2], gpstime, gps1Pos.v['lla'][:,2], options=refColor )
899  legend += ['GPS']
900  if baro:
901  pt.subplotSingle(ax[2], baro.time, baro.v['mslBar'], options='m' )
902  legend += ['mslBar']
903  ax[2].legend(legend)
904 
905  saveFigures('lla.svg', f)
906 
907 
909  if peCheck('nedErr') and 'nedErr' in ins.v.dtype.names:
910  f += 1;
911  pt.plot3Axes(f, ins.time, ins.v['nedErr'], 'NED Error from Truth INS', 'm' )
912 
913 
914 
916  if peCheck('ned'):
917  # pt.labels('NED', 'm')
918  f+=1; legend = []
919  fig, ax = pt.subplots(f,3, 'NED', sharex=True)
920 
921  # if rIns:
922  # pt.plot3Axes(f, rIns.v['time'], rIns.ned, options=rInsColor )
923  # legend += ['Truth']
924  # if pe['ned == 1: # INS & GPS
925  # j = 0
926  # elif pe['ned == 2: # GPS & RTK
927  # fd
928 
929  if ins:
930  instime = getTimeFromTow(ins.v['tow'])
931  if gps1Pos:
932  gpstime = getTimeFromTowMs(gps1Pos.v['timeOfWeekMs'])
933 
934  if 'gps1UbxPos' in log.data.keys():
935  global refLla
936  # ubxNED = pose.lla2ned(refLla, log.data['gps1RtkPos']['lla'])
937  # ubxtime = getTimeFromTowMs(log.data['gps1RtkPos']['timeOfWeekMs'])
938  ubxtime = getTimeFromTowMs(gps1Ubx.v['timeOfWeekMs'])
939  # else:
940  # rtkNED = None
941 
942  if 'gps1RtkPos' in log.data.keys():
943  global refLla
944  # rtkNED = pose.lla2ned(refLla, log.data['gps1RtkPos']['lla'])
945  # rtktime = getTimeFromTowMs(log.data['gps1RtkPos']['timeOfWeekMs'])
946  rtktime = getTimeFromTowMs(gps1RtkPos.v['timeOfWeekMs'])
947  else:
948  rtkNED = None
949 
950  if gps1Ubx:
951  ubxtime = getTimeFromTowMs(gps1Ubx.v['timeOfWeekMs'])
952 
953  pt.subplotSingle(ax[0], instime, ins.ned()[:,0], options=insColor, title="North", ylabel='m')
954  legend = ['ins']
955  if gps1Pos:
956  pt.subplotSingle(ax[0], gpstime, gps1Pos.ned[:,0], options=refColor)
957  legend += ['gps1Pos']
958  if gps1Ubx:
959  pt.subplotSingle(ax[0], ubxtime, gps1Ubx.ned[:,0], options=ubxColor)
960  legend += ['gps1Ubx']
961  if gps1RtkPos:
962  pt.subplotSingle(ax[0], rtktime, gps1RtkPos.ned[:,0], options=rtkColor)
963  legend += ['gps1RtkPos']
964  # if rtkNED is not None:
965  # pt.subplotSingle(ax[0], rtktime, rtkNED[:,0], options=rtkColor)
966  # legend += ['rtk']
967  ax[0].legend(legend)
968 
969  pt.subplotSingle(ax[1], instime, ins.ned()[:,1], options=insColor, title="East", ylabel='m')
970  legend = ['ins']
971  if gps1Pos:
972  pt.subplotSingle(ax[1], gpstime, gps1Pos.ned[:,1], options=refColor)
973  legend += ['gps1Pos']
974  if gps1Ubx:
975  pt.subplotSingle(ax[1], ubxtime, gps1Ubx.ned[:,1], options=ubxColor)
976  legend += ['gps1Ubx']
977  if gps1RtkPos:
978  pt.subplotSingle(ax[1], rtktime, gps1RtkPos.ned[:,1], options=rtkColor)
979  legend += ['gps1RtkPos']
980  # if rtkNED is not None:
981  # pt.subplotSingle(ax[1], rtktime, rtkNED[:,1], options=rtkColor)
982  # legend += ['rtk']
983  ax[1].legend(legend)
984 
985  pt.subplotSingle(ax[2], instime, ins.ned()[:,2], options=insColor, title="Down", ylabel='m')
986  legend = ['ins']
987  if gps1Pos:
988  pt.subplotSingle(ax[2], gpstime, gps1Pos.ned[:,2], options=refColor)
989  legend += ['gps1Pos']
990  if gps1Ubx:
991  pt.subplotSingle(ax[2], ubxtime, gps1Ubx.ned[:,2], options=ubxColor)
992  legend += ['gps1Ubx']
993  if gps1RtkPos:
994  pt.subplotSingle(ax[2], rtktime, gps1RtkPos.ned[:,2], options=rtkColor)
995  legend += ['gps1RtkPos']
996  # if rtkNED is not None:
997  # pt.subplotSingle(ax[2], rtktime, rtkNED[:,2], options=rtkColor)
998  # legend += ['rtk']
999  ax[2].legend(legend)
1000 
1001  # pt.plot3Axes(f, ins.time, ins.ned(), options=insColor )
1002  # legend += ['ins']
1003  # if gps1Pos:
1004  # pt.plot3Axes(f, gps1Pos.time, gps1Pos.ned, options=refColor )
1005  # legend += ['gps1Pos']
1006 
1007  saveFigures('ned.svg', f)
1008 
1009 
1011  if peCheck('nedMap'):
1012  # pt.labels('NED', 'm')
1013  f += 1; legend = []
1014  # if rIns:
1015  # pt.plot3Axes(f, rIns.v['time'], rIns.ned, options=rInsColor )
1016  # legend += ['Truth']
1017 
1018  fig, ax = pt.subplots(f,1, 'NED Map')
1019  pt.subplotSingle(ax, ins.ned()[:,1], ins.ned()[:,0], options=insColor, title="North East Map")
1020  legend += ['ins']
1021  if gps1Pos:
1022  pt.subplotSingle(ax, gps1Pos.ned[:,1], gps1Pos.ned[:,0], options=refColor)
1023  legend += ['gps1Pos']
1024  ax.legend(legend)
1025 
1026  saveFigures('nedMap.svg', f)
1027 
1028 
1030  if peCheck('nedMapRtk'):
1031  # pt.labels('NED', 'm')
1032  f += 1; legend = []
1033 
1034  fig, ax = pt.subplots(f,1, 'RTK NED Map')
1035  if gps1RtkPos:
1036  pt.subplotSingle(ax, gps1RtkPos.ned[:,1], gps1RtkPos.ned[:,0], options=insColor)
1037  legend += ['rtk']
1038  if gps1Ubx:
1039  pt.subplotSingle(ax, gps1Ubx.ned[:,1], gps1Ubx.ned[:,0], options=refColor)
1040  legend += ['gps1Ubx']
1041  ax.legend(legend)
1042  pt.labels('RTK NED Map', 'm')
1043 
1044  saveFigures('nedMapRtk.png', f)
1045 
1046 
1080 
1081  if peCheck('nve') and not peCheck('showUtcTime'):
1082  f += 1; legend = []
1083  fig, ax = pt.subplots(f,1, 'NVE')
1084 
1085  pt.subplotSingle(ax, ins.v['lla'][:,1], ins.v['lla'][:,0], options=insColor, title="Lat Lon Map")
1086  legend += ['ins']
1087  if gps1Pos:
1088  pt.subplotSingle(ax, gps1Pos.v['lla'][:,1], gps1Pos.v['lla'][:,0], options=refColor)
1089  legend += ['gps1Pos']
1090 
1091  saveFigures('LatLonMap.svg', f)
1092 
1093 
1095  if peCheck('nedDot'):
1096 
1097  f += 1; legend = []
1098 
1099  delta = 1
1100  ins.nedDot = ft.derivative(ins.time, ins.ned(), delta, title="NED dot")
1101  if gps1Pos:
1102  gps1Pos.nedDot = ft.derivative(gps1Pos.time, gps1Pos.ned, delta)
1103  if rIns:
1104  rIns.nedDot = ft.derivative(rIns.v['time'], rIns.ned, delta)
1105  pt.plot3Axes(f, rIns.v['time'], rIns.nedDot, options=rInsColor )
1106  legend += ['Truth']
1107 
1108  pt.plot3Axes(f, ins.time, ins.velNED )
1109  legend += ['ins']
1110  if gps1Pos:
1111  pt.plot3Axes(f, gps1Pos.time, gps1Pos.nedDot, 'NED Dot', 'm/s', options=refColor )
1112  legend += ['gps1Pos']
1113  plt.legend(legend)
1114 
1115  if peCheck('staticBiasAttEst'):
1116  f += 1; legend = []
1117  [ imu1.accAtt, imu1.accBias] = ps.acc2AttAndBias( imu1.fltAcc())
1118  if rIns:
1119  [rImu.accAtt, rImu.accBias] = ps.acc2AttAndBias(rImu.fltAcc())
1120  pt.plot3Axes(f, rImu.v['time'], rImu.accBias, options=rInsColor)
1121  legend += ['RefIMU'];
1122  pt.plot3Axes(f, imu1.time, imu1.accBias, 'IMU Accel Bias','m/s^2' )
1123  legend += ['imu1'];
1124  plt.legend(legend)
1125 
1126  f += 1; legend = []
1127  if rIns:
1128  pt.plot3Axes(f, rImu.v['time'], rImu.accAtt*RAD2DEG, options=rInsColor)
1129  legend += ['RefIMU'];
1130  pt.plot3Axes(f, imu1.time, imu1.accAtt*RAD2DEG, 'Attitude from Gravity','deg')
1131  legend += ['imu1'];
1132  plt.legend(legend)
1133 
1134 
1136  if peCheck('altitude'):
1137  f += 1; legend = []
1138  fig, ax = pt.subplots(f,1, 'Altitude')
1139  pt.labels('Altitude', 'm')
1140 
1141  pt.subplotSingle(ax, ins.time, ins.v['lla'][:,2], options=insColor )
1142  legend += ['INS']
1143  if baro:
1144  pt.subplotSingle(ax, baro.time, baro.v['mslBar'], options='g' )
1145  legend += ['Baro MSL']
1146  if gps1Pos:
1147  pt.subplotSingle(ax, gps1Pos.time - gps1Pos.time[0], gps1Pos.v['lla'][:,2], options=refColor )
1148  legend += ['GPS ellipsoid']
1149  pt.subplotSingle(ax, gps1Pos.time - gps1Pos.time[0], gps1Pos.v['hMSL'], options='m' )
1150  legend += ['GPS geoid/MSL']
1151  ax.legend(legend)
1152 
1153 
1156  imu = [cObj(),cObj()]
1157  mag = [cObj(),cObj()]
1158  if dimu:
1159  imu[0].pqr = dimu.i[0].pqr
1160  imu[1].pqr = dimu.i[1].pqr
1161  imu[0].acc = dimu.i[0].acc
1162  imu[1].acc = dimu.i[1].acc
1163  xlim = None
1164  if peCheck('sensorXLim'):
1165  xlim = pe['sensorXLim']
1166 
1167 
1169  if peCheck('sensorPqr'):
1170  f += 1; legend = []
1171  if peCheck('sensorSeparate'):
1172  pt.labels('Sensors: PQR1','deg/s')
1173  else:
1174  pt.labels('Sensors: PQR','deg/s')
1175  if pe['sensorPqr'] != 1:
1176  freq = pe['sensorPqr']
1177  imu[0].pqr = ft.lpfNoDelay(imu[0].pqr, freq, time=dimu.time)
1178  imu[1].pqr = ft.lpfNoDelay(imu[1].pqr, freq, time=dimu.time)
1179 
1180  if rIns:
1181  pt.plot3Axes(f, rImu.v['time'], RAD2DEG * rImu.flt.pqr, options=rInsColor )
1182  legend += ['Truth']
1183 
1184  i = len(dimu.time)/2
1185  # ylim = None
1186  yspan = None
1187  if peCheck('sensorPqrSpan'):
1188  yspan = pe['sensorPqrSpan']
1189  pt.plot3Axes(f, dimu.time, imu[0].pqr * RAD2DEG, xlim=xlim )
1190  pt.plot3setYspan(f, yspan)
1191  legend += ['pqr1']
1192  if peCheck('sensorSeparate'):
1193  plt.legend(legend); f += 1; legend = []
1194  pt.labels('Sensors: PQR2','deg/s')
1195  pt.plot3Axes(f, dimu.time, imu[1].pqr * RAD2DEG, xlim=xlim)
1196  pt.plot3setYspan(f, yspan)
1197  legend += ['pqr2']
1198  plt.legend(legend)
1199 
1200  saveFigures('sensorPqr.svg', f)
1201 
1202  g_imu1BiasPqr = np.mean(imu[0].pqr * RAD2DEG, axis=0)
1203  g_imu2BiasPqr = np.mean(imu[1].pqr * RAD2DEG, axis=0)
1204 
1205 
1207  if peCheck('sensorAcc'):
1208  f += 1; legend = []
1209  offset = [0.0,0.0,0.0]
1210  # offset = [0.0,0.0,9.8]
1211  if peCheck('sensorSeparate'):
1212  pt.labels('Sensors: Accel1','m/s^2')
1213  else:
1214  pt.labels('Sensors: Accel','m/s^2')
1215  if pe['sensorAcc'] != 1:
1216  freq = pe['sensorAcc']
1217  imu[0].acc = ft.lpfNoDelay(imu[0].acc, freq, time=dimu.time)
1218  imu[1].acc = ft.lpfNoDelay(imu[1].acc, freq, time=dimu.time)
1219 
1220  if rIns:
1221  pt.plot3Axes(f, rImu.v['time'], rImu.flt.acc+offset, options=rInsColor )
1222  legend += ['Truth'];
1223 
1224  yspan = None
1225  if peCheck('sensorAccSpan'):
1226  yspan = pe['sensorAccSpan']
1227  pt.plot3Axes(f, dimu.time, imu[0].acc+offset, xlim=xlim )
1228  pt.plot3setYspan(f, yspan)
1229  legend += ['acc1']
1230  if peCheck('sensorSeparate'):
1231  plt.legend(legend); f += 1; legend = []
1232  pt.labels('Sensors: Accel2','m/s^2')
1233  pt.plot3Axes(f, dimu.time, imu[1].acc+offset, xlim=xlim )
1234  pt.plot3setYspan(f, yspan)
1235  legend += ['acc2']
1236  plt.legend(legend)
1237 
1238  saveFigures('sensorAccel.svg', f)
1239 
1240  g_imu1BiasAcc = np.mean(imu[0].acc, axis=0)
1241  g_imu2BiasAcc = np.mean(imu[1].acc, axis=0)
1242 
1243 
1245  if peCheck('fftPqr'):
1246  f += 1; legend = []
1247  fig, ax = pt.subplots(f,3, 'FFT PQR')
1248  ax[0].set_title("FFT: PQR")
1249 
1250  N = np.shape(imu[0].pqr[:,0])[0] # Number of samples
1251  T = np.mean(dimu.time[1:] - dimu.time[0:-1]) # Sample period
1252 
1253  pqrFftX = np.linspace(0.0, 1.0/(2.0*T), N/2)
1254  ylim = 0.006
1255  for i in range(0,3):
1256  pqrFftY = 2.0/N * np.abs(np.fft.fft(imu[0].pqr[:,i])[:N//2])
1257  ax[i].set_xlabel('Freq (Hz)')
1258  ax[i].set_ylim([-0.1*ylim,ylim])
1259  pt.subplotSingle(ax[i], pqrFftX, pqrFftY )
1260 
1261  ax[0].set_ylabel('P')
1262  ax[1].set_ylabel('Q')
1263  ax[2].set_ylabel('R')
1264  saveFigures('fftPqr.svg', f)
1265 
1266 
1267 
1269  if peCheck('fftAcc'):
1270  f += 1; legend = []
1271  fig, ax = pt.subplots(f,3, 'FFT Accel')
1272  ax[0].set_title('FFT: Accel')
1273 
1274  N = np.shape(imu[0].acc[:,0])[0] # Number of samples
1275  T = np.mean(dimu.time[1:] - dimu.time[0:-1]) # Sample period
1276 
1277  accFftX = np.linspace(0.0, 1.0/(2.0*T), N/2)
1278  ylim = 0.25
1279  for i in range(0,3):
1280  accFftY = 2.0/N * np.abs(np.fft.fft(imu[0].acc[:,i])[:N//2])
1281  ax[i].set_xlabel('Freq (Hz)')
1282  ax[i].set_ylim([-0.1*ylim,ylim])
1283  pt.subplotSingle(ax[i], accFftX, accFftY )
1284 
1285  ax[0].set_ylabel('X')
1286  ax[1].set_ylabel('Y')
1287  ax[2].set_ylabel('Z')
1288 
1289  saveFigures('fftAcc.svg', f)
1290 
1291 
1292 
1294  if peCheck('sensorMag'):
1295  f += 1; legend = []
1296  pt.labels('Sensors: Mag','gauss')
1297  if pe['sensorMag'] > 1:
1298  mag_1 = ft.smooth(mag1.v['mag'][:,:], pe['sensorMag'])
1299  mag_2 = ft.smooth(mag2.v['mag'][:,:], pe['sensorMag'])
1300  else:
1301  mag_1 = mag1.v['mag'][:,:]
1302  mag_2 = mag2.v['mag'][:,:]
1303 
1304  pt.plot3Axes(f, mag1.v['time'], mag_1 )
1305  pt.plot3Axes(f, mag2.v['time'], mag_2 )
1306  legend += ['mag 1','mag 2']
1307  plt.legend(legend)
1308 
1309  saveFigures('sensorMagnetometer.svg', f)
1310 
1311 
1313  if peCheck('sensorMag'):
1314  f += 1; legend = []
1315  if peCheck('sensorSeparate'):
1316  pt.labels('Sensors: Mag1','gauss')
1317  else:
1318  pt.labels('Sensors: Mag','gauss')
1319  mag[0].time = mag1.time
1320  mag[0].mag = mag1.v['mag'][:,:]
1321  mag[1].time = mag2.time
1322  mag[1].mag = mag2.v['mag'][:,:]
1323 
1324  if pe['sensorMag'] != 1:
1325  freq = pe['sensorAcc']
1326  mag[0].mag = ft.lpfNoDelay(mag[0].mag, freq, time=mag[0].time)
1327  mag[1].mag = ft.lpfNoDelay(mag[1].mag, freq, time=mag[1].time)
1328 
1329  pt.plot3Axes(f, mag[0].time, mag[0].mag, xlim=xlim )
1330  legend += ['mag1']
1331  if peCheck('sensorSeparate'):
1332  plt.legend(legend); f += 1; legend = []
1333  pt.labels('Sensors: Mag2','gauss')
1334  pt.plot3Axes(f, mag[1].time, mag[1].mag, xlim=xlim )
1335  legend += ['mag2']
1336  plt.legend(legend)
1337 
1338  saveFigures('sensorMag.svg', f)
1339 
1340 
1342  if peCheck('sensorBar'):
1343  f += 1; legend = []
1344  fig, ax = pt.subplots(f,1, 'Sensor Baro')
1345 
1346  pt.labels('Barometer MSL', 'm')
1347  pt.subplotSingle(ax, baro.time, baro.v['mslBar'] )
1348  legend += ['mslBar']
1349  if rIns:
1350  pt.subplotSingle(ax, rIns.v['time'], rIns.v['lla'][:,2], options=rInsColor )
1351  legend += ['Truth']
1352  if pe['sensorBar'] == 2:
1353  pt.subplotSingle(ax, gps1Pos.time, gps1Pos.v['lla'][:,2], options=refColor )
1354  legend += ['GPS']
1355  ax.legend(legend)
1356 
1357  saveFigures('baroMsl.svg', f)
1358 
1359 
1361  if peCheck('sensorIs1PqrVsTemp') or \
1362  peCheck('sensorIs1AccVsTemp') or \
1363  peCheck('sensorIs1Pqr') or \
1364  peCheck('sensorIs1Acc') or \
1365  peCheck('sensorIs1Mag'):
1366  mpu = []
1367  mpu.append(cObj())
1368  mpu.append(cObj())
1369  mpu[0].temp = log.data['sensorsIs1']['mpu'][:,0]['temp']
1370  mpu[1].temp = log.data['sensorsIs1']['mpu'][:,1]['temp']
1371  dt = 0.004
1372  mpuTime = np.arange(0.0, np.shape(mpu[0].temp)[0])*dt
1373 
1374 
1376  if peCheck('sensorIs1PqrVsTemp'):
1377  f += 1; legend = []
1378  pt.labels('SensorsIS1: PQR vs Temperature','deg/s')
1379  mpu[0].pqr = log.data['sensorsIs1']['mpu'][:,0]['pqr']
1380  mpu[1].pqr = log.data['sensorsIs1']['mpu'][:,1]['pqr']
1381  if pe['sensorIs1PqrVsTemp'] != 1:
1382  freq = pe['sensorIs1PqrVsTemp']
1383  mpu[0].pqr = ft.lpfNoDelay(mpu[0].pqr, freq, dt=dt)
1384  mpu[1].pqr = ft.lpfNoDelay(mpu[1].pqr, freq, dt=dt)
1385  pt.plot3Axes(f, mpu[0].temp, mpu[0].pqr*RAD2DEG, xlabel="temp (C)" )
1386  pt.plot3Axes(f, mpu[1].temp, mpu[1].pqr*RAD2DEG, xlabel="temp (C)" )
1387  legend += ['mpu1','mpu2']
1388  plt.legend(legend)
1389 
1390  if peCheck('sensorIs1Pqr'):
1391  mpu[0].pqr = log.data['sensorsIs1']['mpu'][:,0]['pqr']
1392  mpu[1].pqr = log.data['sensorsIs1']['mpu'][:,1]['pqr']
1393  if pe['sensorIs1Pqr'] != 1:
1394  freq = pe['sensorIs1Pqr']
1395  mpu[0].pqr = ft.lpfNoDelay(mpu[0].pqr, freq, dt=dt)
1396  mpu[1].pqr = ft.lpfNoDelay(mpu[1].pqr, freq, dt=dt)
1397 
1398  for m in range(0,2):
1399  if m==0 or peCheck('sensorSeparate'):
1400  f += 1; legend = []
1401  fig, ax = pt.subplots(f,4, 'SensorsIS1: PQR & Temp vs Time', sharex=True)
1402  legend += ['mpu%d' % (m+1)]
1403  pt.subplotSingle(ax[0], mpuTime, mpu[m].pqr[:,0]*RAD2DEG, 'P', 'deg/s' )
1404  pt.subplotSingle(ax[1], mpuTime, mpu[m].pqr[:,1]*RAD2DEG, 'Q', 'deg/s' )
1405  pt.subplotSingle(ax[2], mpuTime, mpu[m].pqr[:,2]*RAD2DEG, 'R', 'deg/s' ) pt.subplotSingle(ax[3], mpuTime, mpu[m].temp, 'Temp', 'C' )
1406  ax[0].legend(legend)
1407 
1408 
1410  if peCheck('sensorIs1AccVsTemp'):
1411  f += 1; legend = []
1412  pt.labels('SensorsIS1: Accel vs Temperature','m/s^2')
1413  mpu[0].acc = log.data['sensorsIs1']['mpu'][:,0]['acc']
1414  mpu[1].acc = log.data['sensorsIs1']['mpu'][:,1]['acc']
1415  if pe['sensorIs1AccVsTemp'] != 1:
1416  freq = pe['sensorIs1AccVsTemp']
1417  mpu[0].acc = ft.lpfNoDelay(mpu[0].acc, freq, dt=dt)
1418  mpu[1].acc = ft.lpfNoDelay(mpu[1].acc, freq, dt=dt)
1419  pt.plot3Axes(f, mpu[0].temp, mpu[0].acc, xlabel="temp (C)" )
1420  pt.plot3Axes(f, mpu[1].temp, mpu[1].acc, xlabel="temp (C)" )
1421  legend += ['mpu1','mpu2']
1422  plt.legend(legend)
1423 
1424  if peCheck('sensorIs1Acc'):
1425  mpu[0].acc = log.data['sensorsIs1']['mpu'][:,0]['acc']
1426  mpu[1].acc = log.data['sensorsIs1']['mpu'][:,1]['acc']
1427  if pe['sensorIs1Acc'] != 1:
1428  freq = pe['sensorIs1Acc']
1429  mpu[0].acc = ft.lpfNoDelay(mpu[0].acc, freq, dt=dt)
1430  mpu[1].acc = ft.lpfNoDelay(mpu[1].acc, freq, dt=dt)
1431 
1432  for m in range(0,2):
1433  if m==0 or peCheck('sensorSeparate'):
1434  f += 1; legend = []
1435  fig, ax = pt.subplots(f,4, 'SensorsIS1: PQR & Temp vs Time', sharex=True)
1436  legend += ["mpu%d"%(m+1)]
1437  pt.subplotSingle(ax[0], mpuTime, mpu[m].acc[:,0], 'Accel X', 'm/s^2' )
1438  pt.subplotSingle(ax[1], mpuTime, mpu[m].acc[:,1], 'Accel Y', 'm/s^2' )
1439  pt.subplotSingle(ax[2], mpuTime, mpu[m].acc[:,2], 'Accel Z', 'm/s^2' )
1440  pt.subplotSingle(ax[3], mpuTime, mpu[m].temp, 'Temp', 'C' )
1441  ax[0].legend(legend)
1442 
1443 
1445  if peCheck('sensorIs1Mag'):
1446  f += 1; legend = []
1447  pt.labels('SensorsIS1: Mag vs Temperature','gauss')
1448  lgd = ['mpu1','mpu2']
1449  mpu[0].mag = log.data['sensorsIs1']['mpu'][:,0]['mag']
1450  mpu[1].mag = log.data['sensorsIs1']['mpu'][:,1]['mag']
1451  if pe['sensorIs1Mag'] != 1:
1452  freq = pe['sensorMag']
1453  mpu[0].mag = ft.smooth(mpu[0].mag, pe['sensorIs1Mag'])
1454  mpu[1].mag = ft.smooth(mpu[1].mag, pe['sensorIs1Mag'])
1455  pt.plot3Axes(f, mpu[0].temp, mpu[0].mag, xlabel="temp (C)" )
1456  pt.plot3Axes(f, mpu[1].temp, mpu[1].mag, xlabel="temp (C)" )
1457  legend += lgd
1458  plt.legend(legend)
1459 
1460 
1461 
1463  if peCheck('gpsStats'):
1464  f += 1; legend = []
1465  if 'gps1Pos' not in log.data.keys():
1466  print("unable to print gpsStats, because we are missing data")
1467  else:
1468  fig, ax = pt.subplots(f,3, 'GPS Stats', sharex=True)
1469 
1470  gpsPos = log.data['gps1Pos']
1471  time = getTimeFromTowMs(gpsPos['timeOfWeekMs'])
1472 
1473  pt.subplotSingle(ax[0], time, gpsPos['status'] & 0xFF, 'Satellites Used in Solution', '' )
1474  pt.subplotSingle(ax[1], time, gpsPos['pDop'], 'Accuracy', 'm', options='m' )
1475  legend = ['pDop']
1476  pt.subplotSingle(ax[1], time, gpsPos['hAcc'], 'Accuracy', 'm', options='r' ) legend += ['Hor']
1477  pt.subplotSingle(ax[1], time, gpsPos['vAcc'], options='b' )
1478  legend += ['Ver']
1479  if 'gps1RtkPos' in log.data.keys():
1480  rtktime = getTimeFromTowMs(log.data['gps1RtkPos']['timeOfWeekMs'])
1481  pt.subplotSingle(ax[1], rtktime, log.data['gps1RtkPos']['vAcc'], options=rtkColor)
1482  legend += ['rtkHor']
1483  ax[1].legend(legend)
1484 
1485  pt.subplotSingle(ax[2], time, gpsPos['cnoMean'], 'CNO', 'dBHz', options='b' )
1486  legend = ['Mean']
1487  ax[2].legend(legend)
1488 
1489  saveFigures('gpsStats.svg', f)
1490 
1491  if gps1Ubx and peCheck('gps1Stats'):
1492  f += 1; legend = []
1493  fig, ax = pt.subplots(f,3, 'GPS1 Stats', sharex=True)
1494 
1495  pt.subplotSingle(ax[0], gps1Ubx.time, gps1Ubx.satsUsed, 'Satellites Used in Solution', '' )
1496  pt.subplotSingle(ax[1], gps1Ubx.time, gps1Ubx.v['pDop'], 'Accuracy', 'm', options='m' )
1497  legend = ['pDop']
1498  pt.subplotSingle(ax[1], gps1Ubx.time, gps1Ubx.v['hAcc'], 'Accuracy', 'm', options='r' )
1499  legend += ['Hor']
1500  pt.subplotSingle(ax[1], gps1Ubx.time, gps1Ubx.v['vAcc'], options='b' )
1501  legend += ['Ver']
1502  ax[1].legend(legend)
1503  pt.subplotSingle(ax[2], gps1Ubx.time, gps1Ubx.v['cno'], 'CNO', 'dBHz', options='r' )
1504  legend = ['Max']
1505  pt.subplotSingle(ax[2], gps1Ubx.time, gps1Ubx.v['cnoMean'], 'CNO', 'dBHz', options='b' )
1506  legend += ['Mean']
1507  ax[2].legend(legend)
1508 
1509  saveFigures('gps1Stats.svg', f)
1510 
1511 
1513  if peCheck('rtkStats'):
1514  f += 1; legend = [str(ser) for ser in serialNumbers]
1515  if 'gps1Pos' not in log.data or 'gps1RtkPosRel' not in log.data:
1516  print("Unable to plot rtkStats - data missing")
1517  else:
1518  n_plots = 5
1519  if 'gps1RktMisc' in log.data.keys():
1520  n_plots += 1
1521  # if 'GPS1Raw' in log.data.keys():
1522  # n_plots += 1
1523  # if 'GPS2Raw' in log.data.keys():
1524  # n_plots += 1
1525  fig, ax = pt.subplots(f,n_plots, 'RTK Stats', sharex=True)
1526 
1527  time = getTimeFromTowMs(log.data['gps1Pos']['timeOfWeekMs'])
1528  compassing = log.data['gps1Pos']['status'][10] & 0x00400000
1529  if not compassing:
1530  fixType = log.data['gps1Pos']['status'] >> 8 & 0xFF
1531  pt.subplotSingle(ax[0], time, fixType, 'GPS Fix Type: 2=2D, 3=3D, 10=Single, 11=Float, 12=Fix', '' )
1532  ax[0].legend(legend)
1533 
1534  time = getTimeFromTowMs(log.data['gps1RtkPosRel']['timeOfWeekMs'])
1535 
1536  if compassing:
1537  fixType = log.data['gps1RtkPosRel']['arRatio'].copy()
1538  iStatus
1539  pt.subplotSingle(ax[0], time, fixType, 'GPS Fix Type: 2=2D, 3=3D, 10=Single, 11=Float, 12=Fix', '' )
1540 
1541  if 'gps1RtkPosRel' in log.data.keys():
1542  disttobase = log.data['gps1RtkPosRel']['distanceToBase']
1543  disttobase[disttobase > 100000] = np.nan
1544  pt.subplotSingle(ax[1], time, log.data['gps1RtkPosRel']['differentialAge'], 'RTK: Age of Differential', 's' )
1545  pt.subplotSingle(ax[2], time, log.data['gps1RtkPosRel']['arRatio'], 'RTK: AR Ratio', 'num' )
1546  pt.subplotSingle(ax[3], time, disttobase, 'Distance to Base', 'm' )
1547 
1548  n_plot = 4
1549  if 'gps1RtkPosMisc' in log.data.keys():
1550  rtkMiscTime = getTimeFromTowMs(log.data['gps1RtkPosMisc']['timeOfWeekMs'])
1551  pt.subplotSingle(ax[n_plot], rtkMiscTime, log.data['gps1RtkPosMisc']['cycleSlipCount'], 'RTK: Slip Counter', ' ' )
1552  n_plot += 1
1553 
1554 
1555 
1556  # if 'GPS1Raw' in log.data.keys():
1557  # Obs = log.data['GPS1Raw']['obs']
1558  # satellites = np.unique(Obs['sat'])
1559  # deltas = np.empty_like(Obs['L'])
1560  # for i, sat in enumerate(satellites):
1561  # valid_indexes = Obs['sat'] == sat
1562  # deltas[valid_indexes] = np.hstack((np.array([0]), np.diff(Obs['L'][valid_indexes])))
1563  # obstime = np.array([np.datetime64(int(np.round((t['time'] + t['sec']) * 1000000)), 'us') for t in Obs['time']]).astype(datetime.datetime)
1564  # pt.subplotSingle(ax[n_plot], obstime,deltas, 'Delta Carrier Phase GPS 1', ' ')
1565  # n_plot +=1
1566  #
1567  # if 'GPS2Raw' in log.data.keys():
1568  # Obs = log.data['GPS2Raw']['obs']
1569  # satellites = np.unique(Obs['sat'])
1570  # delta = np.empty_like(Obs['L'])
1571  # for i, sat in enumerate(satellites):
1572  # valid_indexes = Obs['sat'] == sat
1573  # delta[valid_indexes] = np.hstack((np.array([0]), np.diff(Obs['L'][valid_indexes])))
1574  # obstime = np.array([np.datetime64(int(np.round((t['time'] + t['sec']) * 1000000)), 'us') for t in Obs['time']]).astype(datetime.datetime)
1575  # pt.subplotSingle(ax[n_plot], obstime, delta, 'Delta Carrier Phase GPS 2', ' ')
1576 
1577 
1578 
1579  saveFigures('rtkStats.svg', f)
1580 
1581 
1584  if peCheck('iStatus') and ins:
1585  f += 1; legend = []
1586  fig, ax = pt.subplots(f,1, 'INS Status')
1587 
1588  # if len(IsLoggerPlot.serialNumbers)
1589  pt.labels('INS Status')
1590  cnt = 0
1591 
1592  instime = getTimeFromTow(ins.v['tow'])
1593  iStatus = ins.iStatus()
1594  # ax.text(p1, -cnt*1.5, '___ ALIGNED ___')
1595  # cnt+=1
1596  # pt.subplotSingle(ax, ins.time, -cnt*1.5+ iStatus.align.coarse.att, options='b' )
1597  # ax.text(p1, -cnt*1.5, 'Att Conv. Done')
1598  # cnt+=1
1599  pt.subplotSingle(ax, instime, -cnt*1.5+ iStatus.align.coarse.att, options='b' )
1600  p1 = ax.get_xlim()[0] + 0.02*(ax.get_xlim()[1] - ax.get_xlim()[0])
1601  ax.text(p1, -cnt*1.5, 'Att Coarse')
1602  cnt+=1
1603  pt.subplotSingle(ax, instime, -cnt*1.5+ iStatus.align.good.att, options='b' )
1604  ax.text(p1, -cnt*1.5, 'Att Good')
1605  cnt+=1
1606  pt.subplotSingle(ax, instime, -cnt*1.5+ iStatus.align.fine.att, options='c' )
1607  ax.text(p1, -cnt*1.5, 'Att Fine')
1608  cnt+=1
1609  pt.subplotSingle(ax, instime, -cnt*1.5+ iStatus.align.coarse.vel, options='g' )
1610  ax.text(p1, -cnt*1.5, 'Vel Coarse')
1611  cnt+=1
1612  pt.subplotSingle(ax, instime, -cnt*1.5+ iStatus.align.good.vel, options='g' )
1613  ax.text(p1, -cnt*1.5, 'Vel Good')
1614  cnt+=1
1615  pt.subplotSingle(ax, instime, -cnt*1.5+ iStatus.align.coarse.pos, options='r' ) ax.text(p1, -cnt*1.5, 'Pos Coarse')
1616  cnt+=1
1617  pt.subplotSingle(ax, instime, -cnt*1.5+ iStatus.align.good.pos, options='r' )
1618  ax.text(p1, -cnt*1.5, 'Pos Good')
1619  cnt+=1
1620  cnt+=1
1621 
1622  # ax.text(p1, -cnt*1.5, '___ ALIGNING ___')
1623  pt.subplotSingle(ax, instime, -cnt*1.5+ iStatus.usingGps, options='b' )
1624  ax.text(p1, -cnt*1.5, 'Using GPS')
1625  cnt+=1
1626  pt.subplotSingle(ax, instime, -cnt*1.5+ iStatus.usingMag, options='m' )
1627  ax.text(p1, -cnt*1.5, 'Using MAG')
1628  cnt+=1
1629  cnt+=1
1630  pt.subplotSingle(ax, instime, -cnt*1.5+ iStatus.navMode )
1631  ax.text(p1, -cnt*1.5, 'Nav Mode')
1632  cnt+=1
1633  cnt+=1
1634  pt.subplotSingle(ax, instime, -cnt*1.5+ (iStatus.solutionStatus)/4.0 )
1635  ax.text(p1, -cnt*1.5, 'Solution Status')
1636  cnt+=1
1637  cnt+=1
1638 
1639  pt.subplotSingle(ax, instime, -cnt*1.5+ iStatus.magActiveCalSet )
1640  ax.text(p1, -cnt*1.5, 'Mag Active Cal Set')
1641  cnt+=1
1642  pt.subplotSingle(ax, instime, -cnt*1.5+ iStatus.magRecalibrating )
1643  ax.text(p1, -cnt*1.5, 'Mag Recalibrating')
1644  cnt+=1
1645  pt.subplotSingle(ax, instime, -cnt*1.5+ iStatus.magInterOrBadCal )
1646  ax.text(p1, -cnt*1.5, 'Mag Inter. or Bad Cal')
1647  cnt+=1
1648  cnt+=1
1649 
1650  pt.subplotSingle(ax, instime, -cnt*1.5+ iStatus.rtosTaskPeriodOverrun )
1651  ax.text(p1, -cnt*1.5, 'RTOS Task Period Overrun')
1652  cnt+=1
1653  pt.subplotSingle(ax, instime, -cnt*1.5+ iStatus.generalFault )
1654  ax.text(p1, -cnt*1.5, 'General Fault')
1655  cnt+=1
1656 
1657  saveFigures('iStatus.svg', f)
1658 
1659 
1661  f += 1; legend = []
1662  fig, ax = pt.subplots(f,1, 'Solution Status')
1663  pt.labels('Solution Status', ' ')
1664  pt.subplotSingle(ax, instime, iStatus.solutionStatus )
1665 
1666  saveFigures('SolutionStatus.svg', f)
1667 
1668 
1669 
1672  if peCheck('hStatus') and ins:
1673  f += 1; legend = []
1674  fig, ax = pt.subplots(f,1, 'Hdw Status')
1675 
1676  hStatus = ins.hStatus()
1677 
1678  pt.labels('Hardware Status')
1679  cnt = 0
1680  p1 = ins.time[0]
1681 
1682  pt.subplotSingle(ax, ins.time, -cnt*1.5+ hStatus.motionGyrSig )
1683  ax.text(p1, -cnt*1.5, 'Motion Gyr Sig')
1684  cnt+=1
1685  pt.subplotSingle(ax, ins.time, -cnt*1.5+ hStatus.motionAccSig )
1686  ax.text(p1, -cnt*1.5, 'Motion Acc Sig')
1687  cnt+=1
1688  pt.subplotSingle(ax, ins.time, -cnt*1.5+ hStatus.motionGyrDev )
1689  ax.text(p1, -cnt*1.5, 'Motion Gyr Dev')
1690  cnt+=1
1691  pt.subplotSingle(ax, ins.time, -cnt*1.5+ hStatus.motionAccDev )
1692  ax.text(p1, -cnt*1.5, 'Motion Acc Dev')
1693  cnt+=1
1694  cnt+=1
1695 
1696  pt.subplotSingle(ax, ins.time, -cnt*1.5+ hStatus.satellite_rx )
1697  ax.text(p1, -cnt*1.5, 'Satellite Rx')
1698  cnt+=1
1699  cnt+=1
1700 
1701  pt.subplotSingle(ax, ins.time, -cnt*1.5+ hStatus.saturationGyr )
1702  ax.text(p1, -cnt*1.5, 'Saturation Gyr')
1703  cnt+=1
1704  pt.subplotSingle(ax, ins.time, -cnt*1.5+ hStatus.saturationAcc )
1705  ax.text(p1, -cnt*1.5, 'Saturation Acc')
1706  cnt+=1
1707  pt.subplotSingle(ax, ins.time, -cnt*1.5+ hStatus.saturationMag )
1708  ax.text(p1, -cnt*1.5, 'Saturation Mag')
1709  cnt+=1
1710  pt.subplotSingle(ax, ins.time, -cnt*1.5+ hStatus.saturationBaro )
1711  ax.text(p1, -cnt*1.5, 'Saturation Baro')
1712  cnt+=1
1713  pt.subplotSingle(ax, ins.time, -cnt*1.5+ hStatus.saturationHistory )
1714  ax.text(p1, -cnt*1.5, 'Saturation History')
1715  cnt+=1
1716  cnt+=1
1717 
1718  pt.subplotSingle(ax, ins.time, -cnt*1.5+ hStatus.errComTxLimited )
1719  ax.text(p1, -cnt*1.5, 'Err Com Tx Limited')
1720  cnt+=1
1721  pt.subplotSingle(ax, ins.time, -cnt*1.5+ hStatus.errComRxOverrun )
1722  ax.text(p1, -cnt*1.5, 'Err Com Rx Overrun')
1723  cnt+=1
1724  pt.subplotSingle(ax, ins.time, -cnt*1.5+ hStatus.errGpsTxLimited )
1725  ax.text(p1, -cnt*1.5, 'Err GPS Tx Limited')
1726  cnt+=1
1727  pt.subplotSingle(ax, ins.time, -cnt*1.5+ hStatus.errGpsRxOverrun )
1728  ax.text(p1, -cnt*1.5, 'Err GPS Rx Overrun')
1729  cnt+=1
1730  cnt+=1
1731 
1732  pt.subplotSingle(ax, ins.time, -cnt*1.5+ (hStatus.comParseErrCount)/4 )
1733  ax.text(p1, -cnt*1.5, 'Com Parse Error Count')
1734  cnt+=1
1735  pt.subplotSingle(ax, ins.time, -cnt*1.5+ hStatus.selfTestFault )
1736  ax.text(p1, -cnt*1.5, 'Self Test Fault')
1737  cnt+=1
1738  pt.subplotSingle(ax, ins.time, -cnt*1.5+ hStatus.errTemperature )
1739  ax.text(p1, -cnt*1.5, 'Temperature error')
1740  cnt+=1
1741  cnt+=1
1742 
1743  pt.subplotSingle(ax, ins.time, -cnt*1.5+ hStatus.faultWatchdogReset )
1744  ax.text(p1, -cnt*1.5, 'Watchdog Reset')
1745  cnt+=1
1746  pt.subplotSingle(ax, ins.time, -cnt*1.5+ hStatus.faultBODReset )
1747  ax.text(p1, -cnt*1.5, 'Brownout Reset')
1748  cnt+=1
1749  pt.subplotSingle(ax, ins.time, -cnt*1.5+ hStatus.faultPORReset )
1750  ax.text(p1, -cnt*1.5, 'Power-on Reset')
1751  cnt+=1
1752  pt.subplotSingle(ax, ins.time, -cnt*1.5+ hStatus.faultCPUErrReset )
1753  ax.text(p1, -cnt*1.5, 'CPU Error Reset')
1754  cnt+=1
1755  cnt+=1
1756 
1757  saveFigures('hStatus.svg', f)
1758 
1759 
1760 
1762  if peCheck('Timestamp'):
1763  f += 1; legend = [];
1764  fig, ax = pt.subplots(f, 3, 'Timestamp', sharex=True)
1765 
1766  # yspan = 0.05
1767 
1768  dtIns = ins.time[1:] - ins.time[0:-1]
1769  timeIns = ins.time[1:]
1770  pt.subplotSingle(ax[0], timeIns, dtIns, 'INS dt', 's')
1771  # pt.subplotSetYspan(ax[0], yspan);
1772  print("INS dt (min, max, mean):", np.min(dtIns), np.max(dtIns), np.mean(dtIns))
1773 
1774  if imu1:
1775  dtImu = imu1.time[1:] - imu1.time[0:-1]
1776  timeImu = imu1.time[1:]
1777  pt.subplotSingle(ax[1], timeImu, dtImu, 'IMU dt', 's')
1778  # pt.subplotSetYspan(ax[1], yspan);
1779  print("IMU dt (min, max, mean):", np.min(dtImu), np.max(dtImu), np.mean(dtImu))
1780 
1781  if dimu:
1782  dtDImu = dimu.time[1:] - dimu.time[0:-1]
1783  timeDImu = dimu.time[1:]
1784  pt.subplotSingle(ax[1], timeDImu, dtDImu, 'IMU dt', 's')
1785  # pt.subplotSetYspan(ax[1], yspan);
1786  print("IMU dt (min, max, mean):", np.min(dtDImu), np.max(dtDImu), np.mean(dtDImu))
1787 
1788  if gps1Pos:
1789  dtGps = gps1Pos.time[1:] - gps1Pos.time[0:-1]
1790  timeGps = gps1Pos.time[1:]
1791  pt.subplotSingle(ax[2], timeGps, dtGps, 'GPS dt', 's')
1792  # pt.subplotSetYspan(ax[2], yspan);
1793  print("GPS dt (min, max, mean):", np.min(dtGps), np.max(dtGps), np.mean(dtGps))
1794 
1795  saveFigures('Timestamp.svg', f)
1796 
1797  # return last figure number
1798  return f
1799 
1800 # Initialize function static variables after the function definition
1801 IsLoggerPlot.serialNumbers = []
1802 
1803 
1804 
1805 
1806 
python.pylib.ISToolsPlot.IsLoggerPlot
def IsLoggerPlot(pe, log, tru=None, startFigure=None, referencePlot=False, saveFigs=False, saveFigsDirectory='', numDevs=None)
Definition: ISToolsPlot.py:41
Catch::Generators::range
GeneratorWrapper< T > range(T const &start, T const &end, T const &step)
Definition: catch.hpp:4141
python.pylib.ISToolsPlot.log2Data
def log2Data(log, dataClass, key)
Definition: ISToolsPlot.py:33

inertial_sense_ros
Author(s):
autogenerated on Sat Sep 19 2020 03:19:04