Fixed-point atan2

This was an unfinished draft meant to be published on February 6, 2012. Enjoy?

atan2 is a freaking useful function: given some y and x12, it computes their “four-quadrant” arctangent, which is basically that coordinate’s angle in polar form.

These two beautiful Wikipedia diagrams explain why it’s so much better than the naïve atan(y/x):

Atan2DiagramAtanDiagram

It also shows the two reasons why I suffered a stroke of unsanity and whipped up my own atan2 function in Q15 fixed point/1.0.15/1.15/whycan’twehaveonenameforthis format that’s so commonly used by DSP chips:

  1. You see how smooth that thing is? That’s because it was generated in some high-falutin’ floating point on a fancy machine with a sweet FPU. Well, I program itty-bitty embedded microcontrollers with nary a hardware integer divider, so I use fixed-point arithmetic for my math. One of the more popular fixed-point formats is Q15, which is taking a 16-bit signed two’s complement integer3 and instead of having it represent [-32768, 32767], have it represent [-1, 1 - 2^{-15}]4. I used it out of convenience since I knew a lot of the tips & tricks for making Q15 computations go fast.
  2. The range on atan2 output is [-\pi, \pi]. That’s nice and mathemagicatical, but not so useful an angle representation on a microcontroller. See, first, that doesn’t fit inside of a Q15 number. Then even if it did, it’d still be ridiculous to have your angle go from some arbitrary constant to some bigger arbitrary constant. You got only 16 bits! You want every possible 16-bit number to be a valid and useful angle, not just some subrange. So let’s map our 16-bit range [0, 2^{16} - 1] to a full turn of the circle [0 \text{ turns}, \frac{2^{15}}{65536} \text{ turn}].

Fixed point angular representation

By using all 16 bits for representing angle, we can do wicked fast things like sin and cos lookup tables:

const int16_t sin_lut[4096]; // a 2^12 table of precomputed sin values
const uint16_t angle; // some angle in our 16-bit turn format
const int16_t sin_of_angle = sin_lut[angle >> 4];
// rshift because there are 16x possible angles as table entries
// we can round to the nearest entry: sin_lut[(angle + 8) >> 4]

We also avoid having to wrap our calculations to stay within [0, 2\pi); instead, integer underflow and overflow does it automatically!

Derivation for atan2

To implement atan2, I whipped out Chapter 18 from Streamlining Digital Signal Processing, which had several approximations for atan, including the following formula:

{\rm arctan}(x) \approx \frac{\pi}{4}x + 0.273 x (1 - |x|)

That’s good to a worst-case error of 0.22° for -1 \le x \le 1, which I’m cool with, but it only works for octants I and VIII. Also, the output ranges from [-\frac{\pi}{4}, \frac{\pi}{4}], not [0, 1) like we want. Let’s fix the range first:

\frac{1}{2\pi}\left(\frac{\pi}{4}x + 0.273 x (1 - |x|)\right)
= \frac{1}{8}x + \frac{0.273}{2\pi} x (1 - |x|)
= x\left(\frac{1}{8} + \frac{0.273}{2\pi} - \frac{0.273}{2\pi}|x|)\right)

The last form is the one you’d actually put in your code, since it requires only two multiplies and one add. Because I’m using Q15, which represents [-1, 1) and I want [0, 1), I end up scaling the whole thing again by 2:

= x\left(\frac{1}{4} + \frac{0.273}{\pi} - \frac{0.273}{\pi}|x|)\right)

Now to handle Cartesian coordinate inputs not in octant I and VIII, I chose to manipulate the coordinates and exploit the symmetries of atan2. For example, if our input is in octants II and III, then we can flip the x and y coordinates to perform a reflection about x = y. If we do our range-restricted atan(y/x), the result will be in octants I/VIII. Now, we gotta negate the result to account for the “flip” caused by the earlier reflection, and then we need to rotate by 90° to get it back to octants II/III, which is as simple as adding 0.25 turns to the angle.

[diagram goes here]

Unfortunately, my derivations here diverged from the four-quadrant formula in the book. It turns out the book is just wrong. asdf

Never trust a Canadian

Implementation details AKA nabs rocks

So anyways, here’s the implementation. I should note that the code relies on a q15_mul function that perform rounding of the product from the 32-bit intermediate to 16-bit result for maximum accuracy. Also, s16_nabs is a function for negative absolute value. Why use nabs? Well, abs is undefined for the most negative number, and chances are if you plug the Q15 value for -1 into abs, you’ll get -1 back out. Negative absolute value, on the other hand, is pretty easy to keep fully defined.

To test my approximate arctangent, I tested all 2^{16}\times2^{16}=4294967296 possible inputs and computed their errors relative to the double-precision atan2 implementation in the GNU C library. I got a worst-case error of 0.221°, which means I’m not losing much precision or introducing quantization error by using fixed-point, and there’s an root mean square (RMS) error of about 0.0004 turns.

What I’m saying is this: relax, this code was tested by brute force. ;)

s16_nabs, q15_mul, q15_div, and of course fxpt_atan2 are all included in this MIT-licensed source file: fxpt_atan2.c

A DSP library might have faster/better versions of some of those functions that may take advantage of your microcontroller’s DSP ALU.

Codes!

/**
 * 16-bit fixed point four-quadrant arctangent. Given some Cartesian vector
 * (x, y), find the angle subtended by the vector and the positive x-axis.
 *
 * The value returned is in units of 1/65536ths of one turn. This allows the use
 * of the full 16-bit unsigned range to represent a turn. e.g. 0x0000 is 0
 * radians, 0x8000 is pi radians, and 0xFFFF is (65535 / 32768) * pi radians.
 *
 * Because the magnitude of the input vector does not change the angle it
 * represents, the inputs can be in any signed 16-bit fixed-point format.
 *
 * @param y y-coordinate in signed 16-bit
 * @param x x-coordinate in signed 16-bit
 * @return angle in (val / 32768) * pi radian increments from 0x0000 to 0xFFFF
 */
uint16_t fxpt_atan2(const int16_t y, const int16_t x) {
    if (x == y) { // x/y or y/x would return -1 since 1 isn't representable
        if (y > 0) { // 1/8
            return 8192;
        } else if (y < 0) { // 5/8
            return 40960;
        } else { // x = y = 0
            return 0;
        }
    }
    const int16_t nabs_y = s16_nabs(y), nabs_x = s16_nabs(x);
    if (nabs_x < nabs_y) { // octants 1, 4, 5, 8
        const int16_t y_over_x = q15_div(y, x);
        const int16_t correction = q15_mul(q15_from_double(0.273 * M_1_PI), s16_nabs(y_over_x));
        const int16_t unrotated = q15_mul(q15_from_double(0.25 + 0.273 * M_1_PI) + correction, y_over_x);
        if (x > 0) { // octants 1, 8
            return unrotated;
        } else { // octants 4, 5
            return 32768 + unrotated;
        }
    } else { // octants 2, 3, 6, 7
        const int16_t x_over_y = q15_div(x, y);
        const int16_t correction = q15_mul(q15_from_double(0.273 * M_1_PI), s16_nabs(x_over_y));
        const int16_t unrotated = q15_mul(q15_from_double(0.25 + 0.273 * M_1_PI) + correction, x_over_y);
        if (y > 0) { // octants 2, 3
            return 16384 - unrotated;
        } else { // octants 6, 7
            return 49152 - unrotated;
        }
    }
}
  1. Or a complex number, if you swing that way []
  2. In regular atan, you’d write atan(y/x), so atan2‘s argument order is y then x. []
  3. If that’s confusing, look them up before continuing because it’s about to get worse []
  4. Trust me, +1 really is missing from the range []

About this entry