Fixed point trigonometry for embedded aplications

Question

I need to do rotational (and other) transformations in an embedded application, requiring the sin() cos() and tan() functions. I know you can use look-up tables, and that's the only solution I could find doing my own research, but is there a good fixed point trig library out there?

I'm thinking of using a cortex M3 for the application, so I want to stay away from floating point as much as possible to keep applications zippy.

Answer 1

A good approach for doing trigonometry in embedded applications is to use polynomial approximations to the functions you need. The code is compact, the data consists of a few coefficients, and the only operations required are multiply and add/subtract. Many embedded systems have hardware multipliers, giving good performance.

Answer 2

Are you opposed to using the fixed point Cortex libraries for this?

q31_t arm_sin_q31 (q31_t x)
Fast approximation to the trigonometric sine function for Q31 data.

from:

CMSIS-DSP: DSP Library Collection with over 60 Functions for various data types: fix-point (fractional q7, q15, q31) and single precision floating-point (32-bit). The library is available for Cortex-M0, Cortex-M3, and Cortex-M4.

It uses a lookup table with quadratic interpolation, but it's pretty fast. You could adapt it to linear interpolation for faster speed but more error.

Also note that even Cortex M4 doesn't necessarily have FPU. I've seen them called "M4F" if they do.

Answer 3

This answer is intended to augment the currently accepted answer with a concrete example in two variants, and provide some specific design advice.

Polynomial approximation is often a superior approach if the desired accuracy is fairly high and there is a hardware multiplier available. Table sizes tend to increase rapidly even when interpolation (e.g. linear, quadratic) and compression schemes (e.g. bipartite tables) are used once more than around 16 "good" bits are required.

The use of minimax approximations for polynomials is highly recommended, as they minimize the maximum error across the interval for which they are generated. This can lead to a significant reduction in the number of terms required for a particular accuracy compared to, for example, Taylor series expansions which provide the best accuracy only at the point around which they are expanded. Commonly used tools such as Mathematica, Maple, and the open-source Sollya tool offer built-in methods to generate minimax approximations.

Multiply-high operations are the fundamental computational building block of polynomial evaluation in fixed-point arithmetic. They return the more significant half of the full product of an integer multiply. Most architectures provide signed and unsigned variants, others provide multiplications with double-width results returned in two registers. Some architectures even provide multiply-high-plus-add combinations, which can be particularly useful. Optimizing compilers are typically able to translate HLL source code idioms (such as those used in the ISO-C code below) corresponding to these operations into the appropriate hardware instructions.

To maximize the accuracy of polynomial evaluation, one would want to utilize the maximum number of bits possible at all times during intermediate computation by choosing a fixed-point format with the maximum possible number of fraction bits. For efficiency, a scale factor equal to the register width avoid the need to re-scale via shifts when used in conjunction with the multiply-high operations.

While the Horner scheme is typically used in floating-point computation to evaluate polynomials which high-accuracy, this is often unnecessary in fixed-point computation and may be detrimental to performance due to the lengthy dependency chain of polynomial evaluation exposing the multiply latency. Parallel evaluation schemes that allow the best utilization of pipelined multipliers with multi-cycle latency are often preferable. In the code below I combine the terms of each polynomial pair-wise and build up the evaluation of the full polynomial from that.

The ISO-C code below demonstrates the simultaneous computation of sine and cosine according to these design principles on the interval [0, π/2], where input and outputs are in S8.23 (Q8.23) format. It achieves essentially fully accurate results, with maximum error on the order of 10^-7 and 80+% of results correctly rounded.

The first variant, in sincos_fixed_nj(), uses a classical approach of argument reduction into [0, π/4], and polynomial approximation to sine and cosine on that interval. The reconstruction phase then maps the polynomial values to the sine and cosine based on quadrant. The second variant, in sincos_fixed_ollyw, is based on a blog post by OllyW. They propose to apply the transform a = (2/π)x-1/2 into the interval [-1/2, 1/2], on which one then needs to approximate sin ((2πa + π)/4 and cos ((2πa + π)/4. The series expansions of these (sin, cos) are identical except that the sign is inverted for odd-power terms. This means one can sum the odd-power and even-power terms separately and then compute sine and cosine as the sum and difference of the accumulated sums.

Using Compiler Explorer I compiled with Clang 11.0 for an armv7-a 32-bit ARM target with full optimization (-O3). Both variants compiled into 41-instruction subroutines, with each subroutine using nine stored 32-bit constants. sincos_fixed_ollyw() uses one more multiply instruction than sincos_fixed_nj but has slightly lower register pressure. The situation seems to be similar when building with Clang for other architecture targets, so one would want to try both variants to see which performs better on a given platform. The tangent could be computed by dividing the sine result by the cosine result.

#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <math.h>

#define SINCOS_NJ    (1)
#define SINCOS_OLLYW (2)
#define VARIANT      (SINCOS_NJ)

/* a single instruction in many 32-bit architectures */
uint32_t umul32hi (uint32_t a, uint32_t b)
{
    return (uint32_t)(((uint64_t)a * b) >> 32);
}

/* a single instruction in many 32-bit architectures */
int32_t mul32hi (int32_t a, int32_t b)
{
    return (int32_t)(uint32_t)((uint64_t)((int64_t)a * b) >> 32);
}

/*
  compute sine and cosine of argument in [0, PI/2]
  input and output in S8.23 format
  max err sine = 9.86237533e-8  max err cosine = 1.02729891e-7
  rms err sine = 4.11141973e-8  rms err cosine = 4.11752018e-8
  sin correctly rounded: 10961278 (83.19%)  
  cos correctly rounded: 11070113 (84.01%)
*/
void sincos_fixed_nj (int32_t x, int32_t *sine, int32_t *cosine)
{
    // minimax polynomial approximation for sine on [0, PI/4]
    const uint32_t s0 = (uint32_t)(1.9510998390614986e-4 * (1LL << 32) + 0.5);
    const uint32_t s1 = (uint32_t)(8.3322080317884684e-3 * (1LL << 32) + 0.5);
    const uint32_t s2 = (uint32_t)(1.6666648373939097e-1 * (1LL << 32) + 0.5);
    const uint32_t s3 = (uint32_t)(9.9999991734512150e-1 * (1LL << 32) + 0.5);
    // minimax polynomial approximation for cosine on [0, PI/4]
    const uint32_t c0 = (uint32_t)(1.3578890357166529e-3 * (1LL << 32) + 0.5);
    const uint32_t c1 = (uint32_t)(4.1654359549283981e-2 * (1LL << 32) + 0.5);
    const uint32_t c2 = (uint32_t)(4.9999838648363948e-1 * (1LL << 32) + 0.5);
    const uint32_t c3 = (uint32_t)(9.9999997159466147e-1 * (1LL << 32) + 0.5);
    // auxilliary constants
    const int32_t hpi_p23 = (int32_t)(3.141592653590 / 2 * (1LL << 23) + 0.5);
    const int32_t qpi_p23 = (int32_t)(3.141592653590 / 4 * (1LL << 23) + 0.5);
    const int32_t one_p23 = (int32_t)(1.0000000000000e+0 * (1LL << 23) + 0.5);
    uint32_t a, s, q, h, l, t, sn, cs;

    /* reduce range from [0, PI/2] to [0, PI/4] */
    t = (x > qpi_p23) ? (hpi_p23 - x) : x; // S8.23

    /* scale up argument for maximum precision in intermediate computation */
    a = t << 9; // U0.32

    /* pre-compute a**2 and a**4 */
    s = umul32hi (a, a); // U0.32
    q = umul32hi (s, s); // U0.32

    /* approximate sine on [0, PI/4] */
    h = s3 - umul32hi (s2, s); // U0.32
    l = umul32hi (s1 - umul32hi (s0, s), q); // U0.32
    sn = umul32hi (h + l, a); // U0.32

    /* approximate cosine on [0, PI/4] */
    h = c3 - umul32hi (c2, s); // U0.32
    l = umul32hi (c1 - umul32hi (c0, s), q); // U0.32
    cs = h + l; // U0.32

    /* round results to target precision */
    sn = ((sn + 256) >> 9); // S8.23
    cs = ((cs + 256) >> 9); // S8.23

    /* cosine result overflows U0.32 format for small arguments */
    cs = (t < 0xb50) ? one_p23 : cs; // S8.23

    /* map sine/cosine approximations based on quadrant */
    *sine   = (t != x) ? cs : sn; // S8.23
    *cosine = (t != x) ? sn : cs; // S8.23
}   

/*
  compute sine and cosine of argument in [0, PI/2]
  input and output in S8.23 format
  max err sine = 1.13173883e-7  max err cosine = 1.13158773e-7
  rms err sine = 4.30955921e-8  rms err cosine = 4.31472191e-8
  sin correctly rounded: 10844170 (82.30%)  
  cos correctly rounded: 10855609 (82.38%)

  Based on an approach by OllyW (http://www.olliw.eu/2014/fast-functions/, 
  retrieved 10/23/2020). We transform a = 2/PI*x-1/2, then we approximate
  sin ((2*PI*a + PI)/4 and cos ((2*PI*a + PI)/4. Except for sign flipping
  in the odd-power terms of the expansions the two series expansions match:

https://www.wolframalpha.com/input/?i=series++sin+%28%282*pi*a+%2B+pi%29%2F4%29
https://www.wolframalpha.com/input/?i=series++cos+%28%282*pi*a+%2B+pi%29%2F4%29

  This means we can sum the odd-power and the even-power terms seperately,
  then compute the sum and difference of those sums giving sine and cosine.
*/
void sincos_fixed_ollyw (int32_t x, int32_t *sine, int32_t *cosine)
{
    // minimax polynomial approximation for sin ((2*PI*a + PI)/4 on [-0.5, 0.5]
    const uint32_t c0 = (uint32_t)(7.0710676768794656e-1 * (1LL << 32) + 0.5);
    const uint32_t c1 = (uint32_t)((1.110721191857 -.25) * (1LL << 32) + 0.5);
    const uint32_t c2 = (uint32_t)(8.7235601339489222e-1 * (1LL << 32) + 0.5);
    const uint32_t c3 = (uint32_t)(4.5677902549505234e-1 * (1LL << 32) + 0.5);
    const uint32_t c4 = (uint32_t)(1.7932640877552330e-1 * (1LL << 32) + 0.5);
    const uint32_t c5 = (uint32_t)(5.6449491763487458e-2 * (1LL << 32) + 0.5);
    const uint32_t c6 = (uint32_t)(1.4444266213104129e-2 * (1LL << 32) + 0.5);
    const uint32_t c7 = (uint32_t)(3.4931597765535116e-3 * (1LL << 32) + 0.5);
    // auxiliary constants
    const uint32_t twoopi = (uint32_t)(2/3.1415926535898 * (1LL << 32) + 0.5);
    const uint32_t half_p31 = (uint32_t)(0.5000000000000 * (1LL << 31) + 0.5);
    const uint32_t quarter_p30 = (uint32_t)(0.2500000000 * (1LL << 30) + 0.5);
    uint32_t s, t, q, h, l;
    int32_t a, o, e, sn, cs;

    /* scale up argument for maximum precision in intermediate computation */
    t = (uint32_t)x << 8; // U1.31

    /* a = 2*PI*x - 0.5 */
    a = umul32hi (twoopi, t) - half_p31; // S0.31

    /* precompute a**2 and a**4 */
    s = (uint32_t)mul32hi (a, a) << 2; // U0.32
    q = umul32hi (s, s); // U0.32

    /* sum odd power terms; add in second portion of c1 (= 0.25) at the end */
    h = c1 - umul32hi (c3, s); // U0.32
    l = umul32hi ((c5 - umul32hi (c7, s)), q); // U0.32
    o = ((h + l) >> 2) + quarter_p30; // S1.30
    o = mul32hi (o, a); // S2.29

    /* sum even power terms */
    h = c0 - umul32hi (c2, s); // U0.32
    l = umul32hi ((c4 - umul32hi (c6, s)), q); // U0.32
    e = (h + l) >> 3; // S2.29 

    /* compute sine and cosine as sum and difference of odd / even terms */
    sn = e + o; // S2.29 sum -> sine 
    cs = e - o; // S2.29 difference -> cosine

    /* round results to target precision */
    sn = (sn + 32) >> 6; // S8.23
    cs = (cs + 32) >> 6; // S8.23

    *sine = sn;
    *cosine = cs;
}

double s8p23_to_double (int32_t a)
{
    return (double)a / (1LL << 23);
}

int32_t double_to_s8p23 (double a)
{
    return (int32_t)(a * (1LL << 23) + 0.5);
}

/* exhaustive test of S8.23 fixed-point sincos on [0,PI/2] */
int main (void)
{
    double errc, errs, maxerrs, maxerrc, errsqs, errsqc;
    int32_t arg, sin_correctly_rounded, cos_correctly_rounded;

#if VARIANT == SINCOS_OLLYW
    printf ("S8.23 fixed-point sincos OllyW variant\n");
#elif VARIANT == SINCOS_NJ
    printf ("S8.23 fixed-point sincos NJ variant\n");
#else // VARIANT
#error unsupported VARIANT
#endif // VARIANT

    maxerrs = 0; 
    maxerrc = 0;
    errsqs = 0;
    errsqc = 0;
    sin_correctly_rounded = 0;
    cos_correctly_rounded = 0;

    for (arg = 0; arg <= double_to_s8p23 (3.14159265358979 / 2); arg++) {
        double argf, refs, refc;
        int32_t sine, cosine, refsi, refci;
#if VARIANT == SINCOS_OLLYW
        sincos_fixed_ollyw (arg, &sine, &cosine);
#elif VARIANT == SINCOS_NJ
        sincos_fixed_nj (arg, &sine, &cosine);
#endif // VARIANT
        argf = s8p23_to_double (arg);
        refs = sin (argf);
        refc = cos (argf);
        refsi = double_to_s8p23 (refs);
        refci = double_to_s8p23 (refc);
        /* print function values near endpoints of interval */
        if ((arg < 5) || (arg > 0xc90fd5)) {
            printf ("arg=%08x  sin=%08x  cos=%08x\n", arg, sine, cosine);
        }
        if (sine == refsi) sin_correctly_rounded++;
        if (cosine == refci) cos_correctly_rounded++;
        errs = fabs (s8p23_to_double (sine) - refs);
        errc = fabs (s8p23_to_double (cosine) - refc);
        errsqs += errs * errs;
        errsqc += errc * errc;
        if (errs > maxerrs) maxerrs = errs;
        if (errc > maxerrc) maxerrc = errc;
    }
    printf ("max err sine = %15.8e  max err cosine = %15.8e\n", 
            maxerrs, maxerrc);
    printf ("rms err sine = %15.8e  rms err cosine = %15.8e\n", 
            sqrt (errsqs / arg), sqrt (errsqc / arg));
    printf ("sin correctly rounded: %d (%.2f%%)  cos correctly rounded: %d (%.2f%%)\n", 
            sin_correctly_rounded, 1.0 * sin_correctly_rounded / arg * 100,
            cos_correctly_rounded, 1.0 * cos_correctly_rounded / arg * 100);
    return EXIT_SUCCESS;
}

The output of the enclosed test framework should look essentially like this:

S8.23 fixed-point sincos NJ variant
arg=00000000  sin=00000000  cos=00800000
arg=00000001  sin=00000001  cos=00800000
arg=00000002  sin=00000002  cos=00800000
arg=00000003  sin=00000003  cos=00800000
arg=00000004  sin=00000004  cos=00800000
arg=00c90fd6  sin=00800000  cos=00000005
arg=00c90fd7  sin=00800000  cos=00000004
arg=00c90fd8  sin=00800000  cos=00000003
arg=00c90fd9  sin=00800000  cos=00000002
arg=00c90fda  sin=00800000  cos=00000001
arg=00c90fdb  sin=00800000  cos=00000000
max err sine = 9.86237533e-008  max err cosine = 1.02729891e-007
rms err sine = 4.11141973e-008  rms err cosine = 4.11752018e-008
sin correctly rounded: 10961278 (83.19%)  cos correctly rounded: 11070113 (84.01%)

fixed-point sincos OllyW variant
arg=00000000  sin=00000000  cos=00800000
arg=00000001  sin=00000001  cos=00800000
arg=00000002  sin=00000002  cos=00800000
arg=00000003  sin=00000003  cos=00800000
arg=00000004  sin=00000004  cos=00800000
arg=00c90fd6  sin=00800000  cos=00000005
arg=00c90fd7  sin=00800000  cos=00000004
arg=00c90fd8  sin=00800000  cos=00000003
arg=00c90fd9  sin=00800000  cos=00000002
arg=00c90fda  sin=00800000  cos=00000001
arg=00c90fdb  sin=00800000  cos=00000000
max err sine = 1.13173883e-007  max err cosine = 1.13158773e-007
rms err sine = 4.30955921e-008  rms err cosine = 4.31472191e-008
sin correctly rounded: 10844170 (82.30%)  cos correctly rounded: 10855609 (82.38%)