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]
