Circular interpolation in Python

Question

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:

1. Interpolating from List A to list B.

2. 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 = ?

Answer 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))

Answer 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.

Answer 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.

Answer 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.