newISDataAnalytics.py
Go to the documentation of this file.
1 '''
2 Created on Feb 17, 2017
3 
4 '''
5 # from numbers import Number
6 import numpy as np
7 import os
8 import sys
9 # import ctypes as ct
10 import math
11 import pylib.ISToolsDataSorted as itd
12 import subprocess as subprocess
13 import pdb
14 
15 from pylab import plt
16 from scipy.interpolate import interp1d
17 from pylib.pose import norm, qlog, qexp, lla2ned, quat2eulerArray, meanOfQuatArray, qboxminus, qboxplus, qmult, qinv
18 import yaml
19 
20 RAD2DEG = 180 / np.pi
21 DEG2RAD = np.pi / 180
22 
23 
24 def checkRawDataDrop(log):
25  for dev in log.devices:
26  if 'debugArray' in dev.data.keys() and 'GPS1Raw' in dev.data.keys():
27  dbg = dev.data['debugArray']['i']
28  counts = dev.data['GPS1Raw']['count'][dev.data['GPS1Raw']['type'] == 1]
29  total_obs_in_log = np.sum(counts)
30  obs_at_start = dbg[0, :]
31  obs_at_end = dbg[-1, :]
32  print("===================== Raw GPS Message Statistics ======================")
33  print("uINS - %d", dev.serialNumber)
34  print("recorded observations: %d, " % total_obs_in_log)
35  print("observations reported by firmware %d, " % obs_at_end[3])
36 
37 def checkTempOfLOF(log):
38  for dev in log.devices:
39  if 'gpsRtkNav' not in dev.data.keys() or 'sysParams' not in dev.data.keys():
40  continue
41  fix_status = dev.data['gpsRtkNav']['status'] & 0xFF00 == 0xC00
42  time_of_lost_fix = dev.data['gpsRtkNav']['towMs'][np.argmax(~fix_status)]
43  imu_temp_at_time_of_lost_fix = dev.data['sysParams']['imuTemp'][np.argmax(dev.data['sysParams']['towMs'] > time_of_lost_fix)]
44  baro_temp_at_time_of_lost_fix = dev.data['sysParams']['baroTemp'][np.argmax(dev.data['sysParams']['towMs'] > time_of_lost_fix)]
45  time_of_regain_fix = dev.data['gpsRtkNav']['towMs'][len(fix_status) - np.argmax(~fix_status[::-1]) - 1]
46  imu_temp_at_time_of_regain_fix = dev.data['sysParams']['imuTemp'][np.argmax(dev.data['sysParams']['towMs'] > time_of_regain_fix)]
47  baro_temp_at_time_of_regain_fix = dev.data['sysParams']['baroTemp'][np.argmax(dev.data['sysParams']['towMs'] > time_of_regain_fix)]
48  print ("dev: {0} lost: IMU = {1}, baro = {2}, regain: IMU={3}, baro={4}".format(dev.data['devInfo']['serialNumber'][0], imu_temp_at_time_of_lost_fix,
49  baro_temp_at_time_of_lost_fix, imu_temp_at_time_of_regain_fix, baro_temp_at_time_of_regain_fix))
50 
51 def calcRMS(log, directory, subdir):
52  file = open("/home/superjax/Documents/inertial_sense/config.yaml", 'r')
53  config = yaml.load(file)
54  directory = config["directory"]
55  serials = config['serials']
56 
57  numDev = len(log.devices)
58  debug = True
59  np.set_printoptions(linewidth=200)
60  averageRMS = []
61  compassing = False
62  navMode = (log.devices[0].data['ins2']['iStatus'] & 0x1000)[-1]
63  if numDev > 1:
64 
65  print("\nComputing RMS Accuracies: (%d devices)" % (numDev))
66 
67  # Build a 3D array of the data. idx 0 = Device, idx 1 = t, idx 2 = [t, lla, uvw, log(q)]
68  data = [np.hstack((log.devices[i].data['ins2']['tow'][:,None],
69  log.devices[i].data['ins2']['lla'],
70  log.devices[i].data['ins2']['uvw'],
71  log.devices[i].data['ins2']['q'])) for i in range(numDev)]
72 
73  # Make sure that the time stamps are realistic
74  for dev in range(numDev):
75  if (np.diff(data[dev][:,0]) > 10.0).any():
76  print("large gaps in data for dev", dev, "chopping off data before gap".format(dev))
77  idx = np.argmax(np.diff(data[dev][:,0])) + 1
78  data[dev] = data[dev][idx:,:]
79 
80  min_time = max([np.min(data[i][:,0]) for i in range(numDev)])
81  max_time = min([np.max(data[i][:,0]) for i in range(numDev)])
82 
83 
84  # If we are in compassing mode, then only calculate RMS after all devices have fix
85  if log.devices[0].data['flashConfig']['RTKCfgBits'][-1] == 8:
86  compassing = True
87  time_of_fix_ms = [dev.data['gps1RtkCmpRel']['timeOfWeekMs'][np.argmax(dev.data['gps1RtkCmpRel']['arRatio'] > 3.0)] / 1000.0 for dev in log.devices]
88  # print time_of_fix_ms
89  min_time = max(time_of_fix_ms)
90 
91 
92  # only take the second half of the data
93  min_time = max_time - (max_time - min_time)/2.0
94 
95  # Resample at a steady 100 Hz
96  dt = 0.01
97  t = np.arange(1.0, max_time - min_time - 1.0, dt)
98  for i in range(numDev):
99  # Chop off extra data at beginning and end
100  data[i] = data[i][data[i][:, 0] > min_time]
101  data[i] = data[i][data[i][:, 0] < max_time]
102 
103  # Chop off the min time so everything is wrt to start
104  data[i][:,0] -= min_time
105 
106  # Interpolate data so that it has all the same timestamps
107  fi = interp1d(data[i][:,0], data[i][:,1:].T, kind='cubic', fill_value='extrapolate', bounds_error=False)
108  data[i] = np.hstack((t[:,None], fi(t).T))
109 
110  # Normalize Quaternions
111  data[i][:,7:] /= norm(data[i][:,7:], axis=1)[:,None]
112 
113  # Make a big 3D numpy array we can work with [dev, sample, data]
114  data = np.array(data)
115 
116 
117  # Convert lla to ned using first device lla at center of data as reference
118  refLla = data[0, int(round(len(t) / 2.0)), 1:4].copy()
119  for i in range(numDev):
120  data[i, :, 1:4] = lla2ned(refLla, data[i, :, 1:4])
121 
122  # Find Mean Data
123  means = np.empty((len(data[0]), 10))
124  means[:,:6] = np.mean(data[:,:,1:7], axis=0) # calculate mean position and velocity across devices
125  means[:,6:] = meanOfQuatArray(data[:,:,7:].transpose((1,0,2))) # Calculate mean attitude of all devices at each timestep
126 
127  # calculate the attitude error for each device
128  att_error = np.array([qboxminus(data[dev,:, 7:], means[:, 6:]) for dev in range(numDev)])
129  # Calculate the Mounting Bias for all devices (assume the mounting bias is the mean of the attitude error)
130  mount_bias = np.mean(att_error, axis=1)
131  if compassing:
132  # When in compassing, assume all units are sharing the same GPS antennas and should therefore have
133  # no mounting bias in heading
134  mount_bias[:,2] = 0
135 
136  # Adjust all attitude errors to the mean by the mounting bias
137  # TODO: Talk to Walt about the mount bias - because this probably includes more biases than just the mount bias
138  att_error -= mount_bias[:,None,:]
139 
140  if debug:
141  colors = ['r', 'g', 'b', 'm']
142  plt.figure()
143  plt.subplot(3,1,1) # Position
144  plt.title("position error")
145  for m in range(3):
146  for n in range(numDev):
147  plt.plot(data[n,:,0], data[n, :, m+1], color = colors[m])
148  plt.plot(data[0,:,0], means[:, m], linewidth=2, color = colors[m])
149  plt.subplot(3,1,2)
150  plt.title("velocity error")
151  for m in range(3):
152  for n in range(numDev):
153  plt.plot(data[n,:,0], data[n, :, m+4], color = colors[m] )
154  plt.plot(data[0,:,0], means[:, m+3], linewidth=2, color = colors[m])
155  plt.subplot(3,1,3)
156  plt.title("attitude")
157  for m in range(4):
158  for n in range(numDev):
159  plt.plot(data[n,:,0], data[n, :, m+7], color = colors[m])
160  plt.plot(data[0,:,0], means[:, m+6], linewidth=2, color = colors[m])
161 
162  plt.figure()
163  for m in range(3):
164  plt.subplot(3, 1, m +1)
165  for n in range(numDev):
166  plt.plot(att_error[n, :, m])
167  plt.show()
168 
169  # RMS = sqrt ( 1/N sum(e^2) )
170  RMS = np.empty((numDev, 9))
171  # Calculate RMS for position and velocity
172  RMS[:,:6] = np.sqrt(np.mean(np.square(data[:, :, 1:7] - means[:,0:6]), axis=1))
173  # Calculate RMS for attitude
174  RMS[:,6:] = np.sqrt(np.mean(np.square(att_error[:, :, :]), axis=1))
175 
176  # Average RMS across devices
177  averageRMS = np.mean(RMS, axis=0)
178 
179  print("average RMS = ", averageRMS)
180 
181  # Convert Attitude Error To Euler Angles
182  RMS_euler = RMS[:,6:] # quat2eulerArray(qexp(RMS[:,6:]))
183  averageRMS_euler = averageRMS[6:] #quat2eulerArray(qexp(averageRMS[None,6:]))[0]
184  mount_bias_euler = mount_bias #quat2eulerArray(qexp(mount_bias))
185 
186 
187  # Below is creating the RMS report
188  thresholds = np.array([0.2, 0.2, 0.2, # LLA
189  0.2, 0.2, 0.2, # UVW
190  0.1, 0.1, 2.0]) # ATT (rpy) - (deg)
191  if navMode or compassing:
192  thresholds[8] = 0.3 # Higher heading accuracy
193  else:
194  thresholds[:6] = np.inf
195 
196  thresholds[6:] *= DEG2RAD # convert degrees threshold to radians
197 
198 
199 
200  specRatio = averageRMS / thresholds
201 
202  filename = os.path.join(directory, 'RMS_report_new.txt');
203  f = open(filename, 'w')
204  f.write('***** Performance Analysis Report - %s *****\n' % (subdir))
205  f.write('\n')
206  f.write('Directory: %s\n' % (directory))
207  mode = "AHRS"
208  if navMode: mode = "NAV"
209  if compassing: mode = "DUAL GNSS"
210  f.write("\n")
211 
212  # Print Table of RMS accuracies
213  line = 'Device '
214  if navMode:
215  f.write(
216  '--------------------------------------------------- RMS Accuracy -------------------------------------------\n')
217  line = line + 'UVW[ (m/s) (m/s) (m/s) ], NED[ (m) (m) (m) ],'
218  else: # AHRS mode
219  f.write('-------------- RMS Accuracy --------------\n')
220  line = line + ' Att [ (deg) (deg) (deg) ]\n'
221  f.write(line)
222 
223  for n in range(0, numDev):
224  devInfo = itd.cDevInfo(log.devices[n].data['devInfo'])
225  line = '%2d SN%d ' % (n, devInfo.v['serialNumber'][-1])
226  if navMode:
227  line = line + '[ %6.4f %6.4f %6.4f ], ' % ( RMS[n, 3], RMS[n, 4], RMS[n, 5])
228  line = line + '[ %6.4f %6.4f %6.4f ], ' % ( RMS[n, 0], RMS[n, 1], RMS[n, 2])
229  line = line + '[ %6.4f %6.4f %6.4f ]\n' % (RMS_euler[n, 0] * RAD2DEG, RMS_euler[n, 1] * RAD2DEG, RMS_euler[n, 2] * RAD2DEG)
230  f.write(line)
231 
232  line = 'AVERAGE: '
233  if navMode:
234  f.write('------------------------------------------------------------------------------------------------------------\n')
235  line = line + '[%7.4f %7.4f %7.4f ], ' % (averageRMS[3], averageRMS[4], averageRMS[5])
236  line = line + '[%7.4f %7.4f %7.4f ], ' % (averageRMS[0], averageRMS[1], averageRMS[2])
237  else: # AHRS mode
238  f.write('------------------------------------------\n')
239  line = line + '[%7.4f %7.4f %7.4f ]\n' % (averageRMS_euler[0] * RAD2DEG, averageRMS_euler[1] * RAD2DEG, averageRMS_euler[2] * RAD2DEG)
240  f.write(line)
241 
242  line = 'THRESHOLD: '
243  if navMode:
244  line = line + '[%7.4f %7.4f %7.4f ], ' % (thresholds[3], thresholds[4], thresholds[5])
245  line = line + '[%7.4f %7.4f %7.4f ], ' % (thresholds[0], thresholds[1], thresholds[2])
246  line = line + '[%7.4f %7.4f %7.4f ]\n' % (thresholds[6] * RAD2DEG, thresholds[7] * RAD2DEG, thresholds[8] * RAD2DEG)
247  f.write(line)
248 
249  line = 'RATIO: '
250  if navMode:
251  f.write('------------------------------------------------------------------------------------------------------------\n')
252  line = line + '[%7.4f %7.4f %7.4f ], ' % (specRatio[3], specRatio[4], specRatio[5])
253  line = line + '[%7.4f %7.4f %7.4f ], ' % (specRatio[0], specRatio[1], specRatio[2])
254  else: # AHRS mode
255  f.write('------------------------------------------\n')
256  line = line + '[%7.4f %7.4f %7.4f ]\n' % (specRatio[6], specRatio[7], specRatio[8])
257  f.write(line)
258 
259  def pass_fail(ratio): return 'FAIL' if ratio > 1.0 else 'PASS'
260 
261  line = 'PASS/FAIL: '
262  if navMode:
263  line = line + '[ %s %s %s ], ' % (pass_fail(specRatio[3]),pass_fail(specRatio[4]),pass_fail(specRatio[5])) # LLA
264  line = line + '[ %s %s %s ], ' % (pass_fail(specRatio[0]),pass_fail(specRatio[1]),pass_fail(specRatio[2])) # UVW
265  line = line + '[ %s %s %s ]\n' % (pass_fail(specRatio[6]),pass_fail(specRatio[7]),pass_fail(specRatio[8])) # ATT
266  f.write(line)
267 
268  if navMode:
269  f.write(' ')
270  else: # AHRS mode
271  f.write(' ')
272  f.write('('+mode +' mode)\n\n')
273 
274  # Print Mounting Biases
275  f.write('--------------- Angular Mounting Biases ----------------\n')
276  f.write('Device Euler Biases[ (deg) (deg) (deg) ]\n')
277  for n in range(0, numDev):
278  devInfo = itd.cDevInfo(log.devices[n].data['devInfo'])
279  f.write('%2d SN%d [ %7.4f %7.4f %7.4f ]\n' % (
280  n, devInfo.v['serialNumber'][-1], mount_bias_euler[n, 0] * RAD2DEG, mount_bias_euler[n, 1] * RAD2DEG, mount_bias_euler[n, 2] * RAD2DEG))
281  f.write('\n')
282 
283  # Print Device Version Information
284  f.write(
285  '------------------------------------------- Device Info -------------------------------------------------\n')
286  for n in range(0, numDev):
287  devInfo = itd.cDevInfo(log.devices[n].data['devInfo'])
288  hver = devInfo.v['hardwareVer'][-1]
289  cver = devInfo.v['commVer'][-1]
290  fver = devInfo.v['firmwareVer'][-1]
291  buld = devInfo.v['build'][-1]
292  repo = devInfo.v['repoRevision'][-1]
293  date = devInfo.v['buildDate'][-1]
294  time = devInfo.v['buildTime'][-1]
295  addi = devInfo.v['addInfo'][-1]
296  f.write(
297  '%2d SN%d HW: %d.%d.%d.%d FW: %d.%d.%d.%d build %d repo %d Proto: %d.%d.%d.%d Date: %04d-%02d-%02d %02d:%02d:%02d %s\n' % (
298  n, devInfo.v['serialNumber'][-1],
299  hver[3], hver[2], hver[1], hver[0],
300  fver[3], fver[2], fver[1], fver[0], buld, repo,
301  cver[3], cver[2], cver[1], cver[0],
302  2000 + date[2], date[1], date[0],
303  time[3], time[2], time[1],
304  addi))
305  f.write('\n')
306 
307  f.close()
308 
309  # Automatically open report in Windows
310  if 'win' in sys.platform:
311  subprocess.Popen(["notepad.exe", filename]) # non-blocking call
312  if 'linux' in sys.platform:
313  subprocess.Popen(['gedit', filename])
314 
315  print("Done.")
316 
317  # TODO: Pass out the union of the test errors
318  return averageRMS
319 
python.pylib.pose.lla2ned
def lla2ned(llaRef, lla)
Definition: pose.py:429
python.pylib.pose.meanOfQuatArray
def meanOfQuatArray(q)
Definition: pose.py:796
Catch::Generators::range
GeneratorWrapper< T > range(T const &start, T const &end, T const &step)
Definition: catch.hpp:4141
max
#define max(a, b)
Takes the maximal value of a and b.
Definition: compiler.h:823
python.pylib.pose.qboxminus
def qboxminus(q1, q2)
Definition: pose.py:778
python.pylib.newISDataAnalytics.calcRMS
def calcRMS(log, directory, subdir)
Definition: newISDataAnalytics.py:51
python.pylib.pose.norm
def norm(v, axis=None)
Definition: pose.py:695

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