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 = ?
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))