3

I am working with data coming from a position sensor that reports in angular coordinates (pitch, roll, and yaw). The sensors readout is such that the maximum reading is 180 degrees. Going past that, the readings jump to negative degrees. For example, a slightly oscillating series of readings might go something like:

 178.8
 178.9
-178.8
-178.0
-179.0
 180.0
 179.0

I'm presently handling this by checking all the values sequentially in a loop. I get the delta between two sequential readings and keep a running total to get my current angular position. If I cross from the 3rd quadrant (90 <= previous value <= 180) into the fourth (-180 <= present value <= -90), I add 360 to the present value before taking the delta. If I'm going the fourth quadrant into the 3rd, I subtract 360 before taking the delta. For example:

    Yaw = inData['Yaw']
    cum_sum = Yaw[0]
    for i in range(1, Yaw)):
        x_prev = Yaw[i-1]
        x = Yaw[i]
        if 90 <= x_prev and x_prev <= 180 and -180 <= x and x <= -90:
            x = x + 360
        elif -180 <= x_prev and x_prev <= -90 and 90 <= x and x <= 180:
            x = x - 360
        else:
            pass
        delta = x - x_prev
        cum_sum += delta

Here, inData is a dataframe containing angular positions from multiple axes, and 'Yaw' is a series in that dataframe.

If I could do a consistent correction to the angle values, I could generate a series of deltas and do a cumulative sum to get position. However, adding 360 to all readings or working with the angles as modulo 360 only moves the roll-over point from crossing between +/-180 to crossing between 0 and 360.

Given that I already have these readings in a DataFrame/Series, I was wondering if there is a more Pythonic (and faster) way to do this.

Pat B.
  • 419
  • 3
  • 12
  • Why don't you add 360 degrees only to the values that are less than zero? – BrenBarn Feb 19 '16 at 20:10
  • 1
    Something like: `inData.Yaw[inData.Yaw<0.0] += 360.` – screenpaver Feb 19 '16 at 20:14
  • 1
    Note that this also works and is more readable: `-180 <= x <= -90`. Given that -180 is the min reading, you can simply use `x <= -90`. But I wouldn't do this calculation in a loop anyway. Have a look at my vectorized solution below. – Alexander Feb 19 '16 at 20:59
  • Adding 360 to the negative numbers simply moves the roll-over from between the 3rd and 4th quadrant to the 1st and 4th. Think of being at 1 degree and then moving two degrees in the negative direction. If you add 360 to the second value, then instead of going from 1 degree to -1 degree, you would go to -359 degrees. – Pat B. Feb 22 '16 at 01:18
  • Yes, the compound conditional is more readable. Just a holdover from my C days, I guess. Thanks for the suggestion. – Pat B. Feb 22 '16 at 01:20
  • Oops! Just noted an error in my comment: adding 360 would make a jump from 1 degree to 359 degrees. – Pat B. Feb 22 '16 at 01:40

3 Answers3

2

I think np.unwrap() does what you want without having to mess with deltas and cumsum. Docs

In [120]: data = pd.Series([178.8, 178.9, -178.8, -178.0, -179.0, 180.0, 179.0])

In [121]: np.degrees(np.unwrap(np.radians(data)))
Out[121]: array([ 178.8,  178.9,  181.2,  182. ,  181. ,  180. ,  179. ])

It handles multiple wrap arounds:

In [124]: np.degrees(np.unwrap(np.radians(data + 540)))
Out[124]: array([ 718.8,  718.9,  721.2,  722. ,  721. ,  720. ,  719. ])
RootTwo
  • 4,288
  • 1
  • 11
  • 15
1

All vectorized, so it should be fast.

s = pd.Series([-178.8, 178.9, -178., -179., 180., 179.])

delta = (s.diff() + 
         (((s.shift() >= 90) & (s <= -90)).astype(int) - 
          ((s.shift() <= -90) & (s >= 90)).astype(int)) * 360)

>>> delta.cumsum()
0    NaN
1   -2.3
2    0.8
3   -0.2
4   -1.2
5   -2.2
dtype: float64

Regarding the logic, ((s.shift() >= 90) & (s <= -90)).astype(int) will evaluate to 1 if the measurement is in the upper left quadrant and zero otherwise. -((s.shift() <= -90) & (s >= 90)).astype(int) will evaluate to minus one if the measurement is in the bottom right quadrant and zero otherwise. This is a vectorized way to say that you should add 360 to the difference if you are in the upper left quadrant and subtract 360 from the difference if you are in the bottom right quadrant, otherwise just take the difference.

Alexander
  • 105,104
  • 32
  • 201
  • 196
0

If I understand you well, the quadrants are like this (going clockwise):

  • 1st: -90 to 0 degrees.
  • 2nd: from 0 to 90 degrees
  • 3rd: from 90 to 180 degrees.
  • 4th: from -180 to 90 degrees.

If that is the case, I suggest you first transform your lectures to conventional 360 degree measurements:

def parseSensorReading(readingDegrees):
    if readingDegrees < 0:
        degrees360 = readingDegrees + 360
    degrees360 = readingDegrees
    return degrees360

Then detect the quadrant you are reading from:

def getQuadrant(degrees360):
    if degrees360 < 360:
        quadrant = 1
    if degrees360 < 270:
        quadrant = 4
    if degrees360 < 180:
        quadrant = 3
    if degrees360 < 90:
        quadrant = 2
    return quadrant

Having the quadrants, now you can calculate your deltas:

def getDelta(x_old, x_new):
    q_old = getQuadrant(x_old)
    q_new = getQuadrant(x_new)
    if q_old == 1 and q_new == 2:
        delta = (x_new + 360) - x_old
    else:
        delta = x_new - x_old

And finally, your code becomes:

yaw = inData["Yaw"]
cumSum = yaw[0]
for i in range(1, len(yaw) + 1)):
    x_old = parseSensorReading(yaw[i-1])
    x_new = parseSensorReading(yaw[i])
    delta = getDelta(x_old, x_new)
    cumSum += delta

Note: I changed the "range" expression in the for loop as it was using the list "yaw" as its second parameter. I assume you wanted to say len(yaw) + 1, so "i" gets values from 1 to len(yaw).

Ricardo Alejos
  • 402
  • 4
  • 9
  • Thanks! I was counting quadrants counter-clockwise, however. – Pat B. Feb 22 '16 at 02:01
  • I may have mis-stated in my initial problem statement. Sorry. Also, this doesn't seem to be subject to vectorization. I'm trying to find some way of taking advantage of numpy/pandas vector operations. – Pat B. Feb 22 '16 at 02:05