def AddPoint(self, sensor, down=False): if not self.lastpoint: self.lastpoint = SigmaPoint(sensor, down) return if self.lastpoint.count < self.min_count: # require x measurements if vector.dist2(self.lastpoint.sensor, sensor) < self.sigma: self.lastpoint.add_measurement(sensor, down) return self.lastpoint = False return # use lastpoint as better sample sensor, down = self.lastpoint.sensor, self.lastpoint.down self.lastpoint = False ind = 0 for point in self.sigma_points: if point.count > 100: continue if vector.dist2(point.sensor, sensor) < self.sigma: point.add_measurement(sensor, down) if ind > 0: # move toward front of list to speed up future tests self.sigma_points = self.sigma_points[:ind-1] + [point] + [self.sigma_points[ind-1]] + self.sigma_points[ind+1:] return ind += 1 self.updated = True index = len(self.sigma_points) p = SigmaPoint(sensor, down) if index < self.max_sigma_points: # push to front of list self.sigma_points = [p] + self.sigma_points return # replace point that is closest to other points mindi = 0 mind = 1e20 for i in range(len(self.sigma_points)): dt = time.monotonic() - self.sigma_points[i].time d = [] for j in range(len(self.sigma_points)): if i == j: continue dist = vector.dist(self.sigma_points[i].sensor, self.sigma_points[j].sensor) count = min(self.sigma_points[i].count, 100) d.append(dist) d.sort() # weight based on distance to closest 2 points and time total = (d[0]+d[1])*1/dt**.2 if total < mind: mindi = i mind = total self.sigma_points[mindi] = p
def AddPoint(self, compass, down): if not self.lastpoint: self.lastpoint = SigmaPoint(compass, down) return if vector.dist2(self.lastpoint.compass, compass) < SigmaPoints.sigma: fac = .02 for i in range(3): self.lastpoint.compass[i] = fac * compass[i] + ( 1 - fac) * self.lastpoint.compass[i] self.lastpoint.down[i] += down[i] self.lastpoint.count += 1 return if self.lastpoint.count < 3: # require 3 measurements self.lastpoint = False return compass, down = self.lastpoint.compass, self.lastpoint.down for i in range(3): down[i] /= self.lastpoint.count self.lastpoint = False ind = 0 for point in self.sigma_points: if vector.dist2(point.compass, compass) < SigmaPoints.sigma: point.add_measurement(compass, down) if ind > 0: # put at front of list to speed up future tests self.sigma_points = self.sigma_points[:ind-1] + [point] + \ [self.sigma_points[ind-1]] + self.sigma_points[ind+1:] return ind += 1 index = len(self.sigma_points) p = SigmaPoint(compass, down) if index == SigmaPoints.max_sigma_points: # replace point that is closest to other points minweighti = 0 minweight = 1e20 for i in range(len(self.sigma_points)): for j in range(len(self.sigma_points)): if i == j: continue dist = vector.dist(self.sigma_points[i].compass, self.sigma_points[j].compass) count = min(self.sigma_points[i].count, 100) dt = time.time() - self.sigma_points[i].time weight = dist * count**.2 * 1 / dt**.1 #print 'ij', i, j, dist, count, dt, weight if weight < minweight: minweighti = i minweight = weight #print 'replace', minweighti, self.sigma_points[minweighti].count, time.time() - self.sigma_points[minweighti].time self.sigma_points[minweighti] = p else: self.sigma_points.append(p)
def AddPoint(self, sensor, down): if not self.lastpoint: self.lastpoint = SigmaPoint(sensor, down) return if self.lastpoint.count < self.min_count: # require x measurements if vector.dist2(self.lastpoint.sensor, sensor) < self.sigma: self.lastpoint.add_measurement(sensor, down) return self.lastpoint = False return # use lastpoint as better sample sensor, down = self.lastpoint.sensor, self.lastpoint.down for i in range(3): down[i] /= self.lastpoint.count self.lastpoint = False ind = 0 for point in self.sigma_points: if vector.dist2(point.sensor, sensor) < self.sigma: point.add_measurement(sensor, down) if ind > 0: # put at front of list to speed up future tests self.sigma_points = self.sigma_points[:ind-1] + [point] + \ [self.sigma_points[ind-1]] + self.sigma_points[ind+1:] return ind += 1 index = len(self.sigma_points) p = SigmaPoint(sensor, down) if index == self.max_sigma_points: # replace point that is closest to other points minweighti = 0 minweight = 1e20 for i in range(len(self.sigma_points)): for j in range(len(self.sigma_points)): if i == j: continue dist = vector.dist(self.sigma_points[i].sensor, self.sigma_points[j].sensor) count = min(self.sigma_points[i].count, 100) dt = time.time() - self.sigma_points[i].time weight = dist * count**.2 * 1 / dt**.1 #print('ij', i, j, dist, count, dt, weight) if weight < minweight: minweighti = i minweight = weight #print('replace', minweighti, self.sigma_points[minweighti].count, time.time() - self.sigma_points[minweighti].time) self.sigma_points[minweighti] = p return self.sigma_points.append(p)
def FitCompass(debug, compass_points, compass_calibration, norm): p = compass_points.Points(True) if len(p) < 8: return fit = FitPointsCompass(debug, p, compass_calibration, norm) if not fit: return #debug('FitCompass', fit) g_required_dev = .25 # must have more than this to allow 1d or 3d fit gpoints = [] for q in p: gpoints.append(q[3:]) avg, g_dev, g_max_dev = PointFit(gpoints) #debug('gdev', g_dev, g_max_dev) c = fit[1] # use 2d fit if g_max_dev < g_required_dev: debug('sigmapoints flat, 2D fit only', g_max_dev, g_required_dev) else: if fit[2]: c = fit[2] # 3d fit # for now do not allow 1d fit if not c: debug('would be using 1d fit!') #c = fit[0] # 1d fit only possible if not c: debug('No Fit available', fit) return coverage = ComputeCoverage(p, c[0][:3], norm) #debug('coverage', coverage) if coverage < 14: # require 280 degrees debug('insufficient coverage:', coverage, ' need 14') if c == fit[1]: # must have had 3d fit to use 1d fit return c = fit[0] return # for now disallow 1d fit debug('using 1d fit') # make sure the magnitude is sane mag = c[0][3] if mag < 7 or mag > 120: debug('fit found field outside of normal earth field strength', mag) return # require inclination less than 82 degrees, with so much inclination, # the fit is inaccurate (near magnetic pole?) inc = c[0][4] if abs(inc) > 82: debug('incline greater than 82 degrees, no fit',) return # test points for deviation, all must fall on a sphere deviation = c[1] if deviation[0] > .15 or deviation[1] > 3: curdeviation = ComputeDeviation(p, compass_calibration) debug('bad fit:', deviation, 'cur dev:', curdeviation) # if compass_calibration calibration is really terrible if deviation[0]/curdeviation[0] + deviation[1]/curdeviation[1] < 2.5 or curdeviation[0] > .2 or curdeviation[1] > 10: debug('allowing bad fit') else: compass_points.RemoveOldest() # remove oldest point if too much deviation return # don't use this fit # if the bias has not sufficiently changed, # the fit didn't change much, so don't bother to report this update if vector.dist2(c[0], compass_calibration) < .1: debug('new calibration same as previous') return c[1].append(coverage) return c
def FitCompass(compass_cal, compass_calibration, norm): p = compass_cal.Points() #print('compassfit count', len(p)) if len(p) < 8: return False debug('FitPointsCompass', p, compass_calibration, norm) fit = FitPointsCompass(p, compass_calibration, norm) if not fit: return debug('compass fit', fit) g_required_dev = .5 # must have more than this to allow 1d or 3d fit gpoints = [] for q in p: gpoints.append(q[3:]) avg, g_dev, g_max_dev = PointFit(gpoints) debug('gdev', g_dev, g_max_dev) c = fit[1] # use 2d fit if g_max_dev < g_required_dev: debug('sigmapoints flat, 2D fit only') else: if fit[2]: c = fit[2] # 3d fit # for now do not allow 1d fit ##if not c: ## c = fit[0] # 1d fit only possible if not c: return coverage = ComputeCoverage(p, c[0][:3], norm) if coverage < 12: # require 240 degrees debug('calibration: not enough coverage:', coverage) if c == fit[1]: # must have had 3d fit to use 1d fit return debug('insufficient coverage, use 1d fit') return # no 1d fit # make sure the magnitude is sane mag = c[0][3] if mag < 7 or mag > 120: debug('fit found field outside of normal earth field strength', mag) return # require inclination less than 82 degrees, with so much inclination, # the fit is inaccurate (near magnetic pole?) inc = c[0][4] if abs(inc) > 82: debug('incline greater than 82 degrees, no fit',) return # test points for deviation, all must fall on a sphere deviation = c[1] if deviation[0] > .15 or deviation[1] > 3: debug('bad fit:', deviation) curdeviation = ComputeDeviation(p, compass_calibration) debug('cur dev', curdeviation) # if compass_calibration calibration is really terrible if deviation[0]/curdeviation[0] + deviation[1]/curdeviation[1] < 2.5 or curdeviation[0] > .2 or curdeviation[1] > 10: debug('allowing bad fit') else: compass_cal.RemoveOldest() # remove oldest point if too much deviation return # don't use this fit # if the bias has not sufficiently changed, # the fit didn't change much, so don't bother to report this update if vector.dist2(c[0], compass_calibration) < .1: debug('insufficient change in bias, calibration already ok') debug('coverage', coverage, 'new fit:', c) return c
def CalibrationProcess(points, norm_pipe, fit_output, current): import os if os.system('sudo chrt -pi 0 %d 2> /dev/null > /dev/null' % os.getpid()): print 'warning, failed to make calibration process idle, trying renice' if os.system("renice 20 %d" % os.getpid()): print 'warning, failed to renice calibration process' cal = SigmaPoints() norm = [0, 0, 0] while True: # each iteration remove oldest point if we have more than 12 #if len(cal.sigma_points) > 1: # cal.RemoveOldest() t = time.time() addedpoint = False while time.time() - t < calibration_fit_period: p = points.recv(1) if p: cal.AddPoint(p[:3], p[3:6]) addedpoint = True while True: n = norm_pipe.recv() if not n: break norm = n cal.sigma_points = [] #print 'set norm', norm if not addedpoint: # don't bother to run fit if no new data continue # remove points older than 1 hour p = [] for sigma in cal.sigma_points: # only use measurements in last hour if time.time() - sigma.time < 3600: p.append(sigma) cal.sigma_points = p #inject if False: p = [] # put points here cal.sigma_points = [] for q in p: cal.sigma_points.append(SigmaPoint(q[:3], q[3:6])) # attempt to perform least squares fit p = [] for sigma in cal.sigma_points: p.append(sigma.compass + sigma.down) if debug: print 'FitPoints', p, norm # for now, require at least 6 points to agree well for update if len(p) < 6: continue gpoints = [] for q in p: gpoints.append(q[3:]) fit = FitPoints(p, current, norm) if not fit: continue if debug: print 'fit', fit g_required_dev = .2 # must have more than this to allow 1d or 3d fit avg, g_dev, g_max_dev = PointFit(gpoints) if debug: print 'gdev', g_dev, g_max_dev if g_max_dev < g_required_dev: c = fit[1] # use 2d fit if debug: print 'sigmapoints flat, 2d fit only' else: c = fit[2] # 3d fit if not c: c = fit[0] # 1d fit only possible if not c: continue coverage = 360 - math.degrees( ComputeCoverage(cal.sigma_points, c[0][:3])) if coverage < 120: # require 120 degrees if debug: print 'calibration: not enough coverage', coverage, 'degrees' if c == fit[1]: # must have had 3d fit to use 1d fit continue c = fit[0] # 1d fit ok with insufficient coverage # make sure the magnitude is sane mag = c[0][3] if mag < 7 or mag > 80: if debug: print 'fit found field outside of normal earth field strength', mag continue # sphere fit should basically agree with new bias ''' sbd = 0 if fit[0] != fit[1]: spherebias = fit[2][:3] bias = fit[0][:3] sbd = vector.norm(vector.sub(bias, spherebias)) if sbd > 6: if debug: print 'sphere and newbias disagree', sbd fit[0] = fit[1] print 'sphere bias difference', sbd ''' # test points for deviation, all must fall on a sphere deviation = c[1] if deviation[0] > .15 or deviation[1] > 3: if debug: print 'bad fit:', deviation cal.RemoveOldest() # remove oldest point if too much deviation continue # don't use this fit # if the bias has not sufficiently changed, # the fit didn't change much, so don't bother to report this update if vector.dist2(c[0], current) < .1: if debug: print 'insufficient change in bias, calibration ok' continue if debug: print 'coverage', coverage, 'new fit:', c fit_output.send( (c, map(lambda p: p.compass + p.down, cal.sigma_points)), False) current = c[0]
def CalibrationProcess(points, norm_pipe, fit_output, current): import os if os.system('sudo chrt -pi 0 %d 2> /dev/null > /dev/null' % os.getpid()): print 'warning, failed to make calibration process idle, trying renice' if os.system("renice 20 %d" % os.getpid()): print 'warning, failed to renice calibration process' cal = SigmaPoints() norm = [0, 0, 1] while True: # each iteration remove oldest point if we have more than 12 #if len(cal.sigma_points) > 1: # cal.RemoveOldest() t = time.time() addedpoint = False while time.time() - t < calibration_fit_period: p = points.recv(1) if p: cal.AddPoint(p[:3], p[3:6]) addedpoint = True while True: n = norm_pipe.recv() if not n: break norm = n cal.sigma_points = [] #print 'set norm', norm if not addedpoint: # don't bother to run fit if no new data continue # remove points older than 1 hour p = [] for sigma in cal.sigma_points: # only use measurements in last hour if time.time() - sigma.time < 3600: p.append(sigma) cal.sigma_points = p #inject if False: p = [[ 99.61022395739637, -32.0896372233238, -46.19833893428187, 0.9959545553521548, 0.01881086049517002, -0.08709012682673913 ], [ 99.41269965838998, -29.18933915381635, -50.87318433884822, 0.9959240391187345, 0.025598832598336896, -0.08624850966044939 ], [ 99.63371012917594, -30.564020033256263, -49.51652671143861, 0.9959725515496398, 0.021270918093455812, -0.08636070074997991 ], [ 99.55269985551303, -31.555431659129688, -48.67149985377176, 0.996239375494907, 0.008675418089256124, -0.08608916000100958 ], [ 99.85053925286313, -32.804657891397724, -43.07924450281722, 0.9958921542039714, 0.020566397536647005, -0.08774229505974987 ], [ 99.09234339278912, -33.6539144001473, -39.992952093580584, 0.9960624906154125, 0.012102753257664767, -0.08734126093468247 ], [ 99.36440514827613, -27.83050871518071, -23.86624704383829, 0.9961120564762204, 0.010738915365523884, -0.08721644126707587 ], [ 99.27881542065676, -31.07749102140572, -29.11323456675227, 0.9959733742690939, 0.018797671301392187, -0.08734021276986086 ], [ 98.66466785647648, -33.77910428722979, -35.88069749493785, 0.9960232490868438, 0.00878364968675116, -0.08807145988954777 ], [ 98.25940791866283, -33.38075607725948, -43.511240257178336, 0.9959494378584233, 0.008111556392145083, -0.08921317309205794 ], [ 99.23090625079405, -31.96668639422272, -31.79659116533928, 0.995697680058246, 0.021258988302123628, -0.08989484314049596 ], [ 98.05279227664792, -34.04676396012794, -38.32184662393996, 0.9959648038133849, 0.007050180604718743, -0.08913339420101368 ], [ 99.3094944746132, -33.063143800282035, -37.07277486136846, 0.9959314585521511, 0.021396914991636075, -0.0870363232937691 ], [ 99.46857288800534, -25.70920675991828, -22.212079862195036, 0.9960765103312297, 0.022150356953854627, -0.0853784440140581 ], [ 98.06189470192125, -33.6074115180095, -33.95455487369483, 0.9959882312382028, 0.008958823339376289, -0.08858464837243196 ], [ 97.91227795273693, -31.233531578042207, -27.844580754671917, 0.9959462962950375, 0.009456129369926113, -0.08901938128629384 ], [ 97.4962095079903, -33.224780551340096, -31.15958596160746, 0.9959328787036706, 0.003916703460782957, -0.0896401065300612 ], [ 97.94596614832525, -32.33912173753667, -29.559069049234942, 0.9959474803084559, 0.007237095190038474, -0.08914248460492485 ]] norm = [ 0.9957405037490841, 0.010161766950568168, -0.09163835270214449 ] cal.sigma_points = [] for q in p: cal.sigma_points.append(SigmaPoint(q[:3], q[3:6])) # attempt to perform least squares fit p = [] for sigma in cal.sigma_points: p.append(sigma.compass + sigma.down) # for now, require at least 6 points to agree well for update if len(p) < 6: continue gpoints = [] for q in p: gpoints.append(q[3:]) debug('FitPoints', p, current, norm) fit = FitPoints(p, current, norm) if not fit: continue debug('fit', fit) g_required_dev = .15 # must have more than this to allow 1d or 3d fit avg, g_dev, g_max_dev = PointFit(gpoints) debug('gdev', g_dev, g_max_dev) c = fit[1] # use 2d fit if g_max_dev < g_required_dev: debug('sigmapoints flat, 2D fit only') else: if fit[2]: c = fit[2] # 3d fit # for now do not allow 1d fit ##if not c: ## c = fit[0] # 1d fit only possible if not c: continue coverage = 360 - ComputeCoverage(cal.sigma_points, c[0][:3], norm) if coverage < 180: # require 180 degrees debug('calibration: not enough coverage', coverage, 'degrees') if c == fit[1]: # must have had 3d fit to use 1d fit continue debug('insufficient coverage, use 1d fit') continue # no 1d fit #c = fit[0] # 1d fit ok with insufficient coverage #if c == fit[2] and coverage < 240: # debug('not enough coverage for 3d fit') # continue # make sure the magnitude is sane mag = c[0][3] if mag < 7 or mag > 120: debug('fit found field outside of normal earth field strength', mag) continue # require inclination less than 82 degrees, with so much inclination, # the fit is inaccurate (near magnetic pole?) inc = c[0][4] if abs(inc) > 82: debug('incline greater than 82 degrees, no fit', ) continue # sphere fit should basically agree with new bias ''' sbd = 0 if fit[0] != fit[1]: spherebias = fit[2][:3] bias = fit[0][:3] sbd = vector.norm(vector.sub(bias, spherebias)) if sbd > 6: debug('sphere and newbias disagree', sbd) fit[0] = fit[1] print 'sphere bias difference', sbd ''' # test points for deviation, all must fall on a sphere deviation = c[1] if deviation[0] > .15 or deviation[1] > 3: debug('bad fit:', deviation) curdeviation = ComputeDeviation(p, current) debug('cur dev', curdeviation) # if current calibration is really terrible if deviation[0] < curdeviation[0] / 2 or curdeviation[0] > 1: debug('allowing bad fit') else: cal.RemoveOldest() # remove oldest point if too much deviation continue # don't use this fit # if the bias has not sufficiently changed, # the fit didn't change much, so don't bother to report this update if vector.dist2(c[0], current) < .1: debug('insufficient change in bias, calibration already ok') debug('coverage', coverage, 'new fit:', c) fit_output.send( (c, map(lambda p: p.compass + p.down, cal.sigma_points)), False) current = c[0]