问题
I have two systems, each of which has a direction sensor (0-360 degrees), but the sensors can provide wildly different values depending on the orientation of each system and the linearity of each sensor. I have a mechanical reference that I can use to generate a table of where each system is actually pointing. This yields a table with three columns:
Physical SystemA SystemB
-------- ------- -------
000.0 005.7 182.3
005.0 009.8 178.4
... ... ...
From just the data shown, we can see that SystemA isn't far from the physical reference, but SystemB is about 180 degrees off, and goes in the opposite direction (imagine it is mounted upside-down).
I need to be able to map back and forth between all three values: If SystemA reports something is at 105.7, I need to tell the user what physical direction that is, then tell SystemB to point to the same location. The same if SystemB makes the initial report. And the user can request both systems to point to a desired physical direction, so SystemA and SystemB would need to be told where to point.
Linear interpolation isn't hard, but I'm having trouble when data is going in opposite directions, and is modular/cyclical.
Is there a Pythonic way to do all these mappings?
EDIT: Let's focus on the most difficult case, where we have two paired lists of values:
A B
----- -----
0.0 182.5
10.0 172.3
20.0 161.4
... ...
170.0 9.7
180.0 359.1
190.0 348.2
... ...
340.0 163.6
350.0 171.8
Let's say the lists come from two different radars with pointers that aren't aligned to North or anything else, but we did manually take the above data by moving a target around and seeing where each radar had to point to see it.
When Radar A says "I have a target at 123.4!", where do I need to aim Radar B to see it? If Radar B finds a target, where do I tell Radar A to point?
List A wraps between the last and first elements, but list B wraps nearer to the middle of the list. List A increases monotonically, while list B decreases monotonically. Notice that the size of a degree on A is generally not the same size as a degree on B.
Is there a simple interpolator that will wrap correctly when:
Interpolating from List A to list B.
Interpolating from List B to list A.
It is OK to use two separate interpolator instantiations, one for going in each direction. I'll assume a linear (first-order) interpolator is OK, but I may want to use higher-order or spline interpolation in the future.
Some test cases:
A = 356.7, B = ?
A = 179.2, B = ?
回答1:
This is what works for me. Could probably use some clean-up.
class InterpolatedArray(object):
""" An array-like object that provides interpolated values between set points.
"""
points = None
wrap_value = None
offset = None
def _mod_delta(self, a, b):
""" Perform a difference within a modular domain.
Return a value in the range +/- wrap_value/2.
"""
limit = self.wrap_value / 2.
val = a - b
if val < -limit: val += self.wrap_value
elif val > limit: val -= self.wrap_value
return val
def __init__(self, points, wrap_value=None):
"""Initialization of InterpolatedArray instance.
Parameter 'points' is a list of two-element tuples, each of which maps
an input value to an output value. The list does not need to be sorted.
Optional parameter 'wrap_value' is used when the domain is closed, to
indicate that both the input and output domains wrap. For example, a
table of degree values would provide a 'wrap_value' of 360.0.
After sorting, a wrapped domain's output values must all be monotonic
in either the positive or negative direction.
For tables that don't wrap, attempts to interpolate values outside the
input range cause a ValueError exception.
"""
if wrap_value is None:
points.sort() # Sort in-place on first element of each tuple
else: # Force values to positive modular range
points = sorted([(p[0]%wrap_value, p[1]%wrap_value) for p in points])
# Wrapped domains must be monotonic, positive or negative
monotonic = [points[x][1] < points[x+1][1] for x in xrange(0,len(points)-1)]
num_pos_steps = monotonic.count(True)
num_neg_steps = monotonic.count(False)
if num_pos_steps > 1 and num_neg_steps > 1: # Allow 1 wrap point
raise ValueError("Table for wrapped domains must be monotonic.")
self.wrap_value = wrap_value
# Pre-compute inter-value slopes
self.x_list, self.y_list = zip(*points)
if wrap_value is None:
intervals = zip(self.x_list, self.x_list[1:], self.y_list, self.y_list[1:])
self.slopes = [(y2 - y1)/(x2 - x1) for x1, x2, y1, y2 in intervals]
else: # Create modular slopes, including wrap element
x_rot = list(self.x_list[1:]); x_rot.append(self.x_list[0])
y_rot = list(self.y_list[1:]); y_rot.append(self.y_list[0])
intervals = zip(self.x_list, x_rot, self.y_list, y_rot)
self.slopes = [self._mod_delta(y2, y1)/self._mod_delta(x2, x1) for x1, x2, y1, y2 in intervals]
def __getitem__(self, x): # Works with indexing operator []
result = None
if self.wrap_value is None:
if x < self.x_list[0] or x > self.x_list[-1]:
raise ValueError('Input value out-of-range: %s'%str(x))
i = bisect.bisect_left(self.x_list, x) - 1
result = self.y_list[i] + self.slopes[i] * (x - self.x_list[i])
else:
x %= self.wrap_value
i = bisect.bisect_left(self.x_list, x) - 1
result = self.y_list[i] + self.slopes[i] * self._mod_delta(x, self.x_list[i])
result %= self.wrap_value
return result
And a test:
import nose
def xfrange(start, stop, step=1.):
""" Floating point equivalent to xrange()."""
while start < stop:
yield start
start += step
# Test simple inverted mapping for non-wrapped domain
pts = [(x,-x) for x in xfrange(1.,16., 1.)]
a = InterpolatedArray(pts)
for i in xfrange(1., 15., 0.1):
nose.tools.assert_almost_equal(a[i], -i)
# Cause expected over/under range errors
result = False # Assume failure
try: x = a[0.5]
except ValueError: result = True
assert result
result = False
try: x = a[15.5]
except ValueError: result = True
assert result
# Test simple wrapped domain
wrap = 360.
offset = 1.234
pts = [(x,((wrap/2.) - x)) for x in xfrange(offset, wrap+offset, 10.)]
a = InterpolatedArray(pts, wrap)
for i in xfrange(0.5, wrap, 0.1):
nose.tools.assert_almost_equal(a[i], (((wrap/2.) - i)%wrap))
回答2:
But you can use linear interpolation. If your sample A value is e.g. 7.75, that resembles 2.5 degrees. If the sample B value is 180.35, it also resembles 2.5 degrees. The tricky part is when the values overflow, if that is possible at all. Just set up a bunch of unittests to check if your algorithm works and you should quickly be going.
回答3:
Answer to part 1: translation table containing the calibration values + drift value.
Basically, if DialA reports 5.7 when it is physically at 0, 9.7 when it is at 5, then I would set the drift value to be +/- .25 of the distance between each readout position to account for mechanical and readout drift.
Answer to part 2: keeping the same values on both dials, while displaying the expected position.
If you are not direction dependent, then just spin the output dial until it is in the correct position as per your calibration table.
If you are direction dependent, then you will need to track the last 1-2 values to determine direction. Once you have determined direction, you can then move the dependent dial in the direction you require, until the destination position is reached.
Your calibration table should include direction as well(positive or negative, for instance).
With the above two parts in mind, you will be able to compensate for rotational offset and directional flips, and produce an accurate position and direction reading.
Here is some code that given a calibration table, will yield position and direction, which will solve the problem of display and making the dependent dial match up with the primary dial:
#!/usr/bin/env python
# Calibration table
# calibrations[ device ][physical position]=recorded position
calibrations={}
calibrationsdrift=1.025
calibrations["WheelA"]={}
calibrations["WheelA"]={}
calibrations["WheelA"]["000.0"]=5.7
calibrations["WheelA"]["005.0"]=9.8
calibrations["WheelA"]["010.0"]=13.9
calibrations["WheelA"]["015.0"]=18.0
calibrations["WheelB"]={}
calibrations["WheelB"]["000.0"]=182.3
calibrations["WheelB"]["005.0"]=178.4
calibrations["WheelB"]["010.0"]=174.4
calibrations["WheelB"]["015.0"]=170.3
def physicalPosition( readout , device ):
calibration=calibrations[device]
for physicalpos,calibratedpos in calibration.items():
if readout < ( calibratedpos + calibrationsdrift ):
if readout > ( calibratedpos - calibrationsdrift ):
return physicalpos
return -0
print physicalPosition( 5.8 , "WheelA")
print physicalPosition( 9.8 , "WheelA")
print physicalPosition( 10.8 , "WheelA")
def physicalDirection( lastreadout, currentreadout, device ):
lastposition=physicalPosition( lastreadout, device)
currentposition=physicalPosition( currentreadout, device)
# Assumes 360 = 0, so 355 is the last position before 0
if lastposition < currentposition:
if lastposition == 000.0:
if currentposition == 355:
return -1
return 1
else:
return -1
print physicalDirection( 5.8, 10.8, "WheelA")
print physicalDirection( 10.8, 2.8, "WheelA")
print physicalDirection( 182, 174, "WheelB")
print physicalDirection( 170, 180, "WheelB")
Running the program shows that the direction is determined correctly, even for WheelB, which is mounted backwards on the panel/device/etc:
$ ./dials
000.0
005.0
005.0
1
-1
1
-1
Note that some of the "readout" values fed to the functions are off. That is compensated for, by the drift value. Whether you need one depends on the equipment you are interfacing with.
回答4:
The easiest solution is to make all of your table elements be increasing (or decreasing as the case may be), adding or subtracting 360 to individual elements to make it so. Double up the table back to back so that it covers the entire range of 0 to 360 even after all the additions and subtractions. This makes a simple linear interpolation possible. Then you can take a modulo 360 after the calculation to bring it back into range.
来源：https://stackoverflow.com/questions/15888927/circular-interpolation-in-python