Trigonométrie en virgule fixe

De Wiki LOGre
Révision de 19 mai 2016 à 20:17 par Ebonet (discuter | contributions) (Utilisation de la balise <math>)

(diff) ← Version précédente | Voir la version courante (diff) | Version suivante → (diff)
Aller à : navigation, rechercher


Projet réalisé par : Edgar Bonet.

Le code présenté ici implémente cos(), sin() et atan2() en virgule fixe pour Arduino et autres microcontrôleurs AVR. Ces fonctions reposent sur la multiplication décrite dans la page Calculs en virgule fixe pour évaluer des approximations polynomiales.

Au prix d'une précision limitée, on gagne en vitesse plus d'un facteur 15. Sur un Arduino Uno (16 MHz) :

  • 107,5 µs, avec une précision de 2,98 × 10−8, pour la fonction cos() standard ;
  • 6,77 µs, avec une précision de 9,53 × 10−5, pour ma fonction cos_fix().

Dans cette page j'essaye de documenter ma démarche. Si vous voulez accéder directement au code, le voici en fin de page.


Approximation polynomiale du cosinus

Puisque les calculs se feront en virgule fixe, on commence par choisir les unités et formats de l'argument et du résultat. J'ai fait les choix suivants :

  • l'argument, qui est un angle, est exprimé en tours (2π radians), au format u16,16, ou bien, de façon équivalente, en cadrants (quarts de tour) au format u16,14 ;
  • le résultat est un nombre entre −1 et +1 au format s16,14.

On peut remarquer que le choix pour l'argument ne laisse aucun bit pour compter un nombre entier de tours, ce qui veut dire que l'arithmétique sur cet angle se fait automatiquement modulo un tour, est c'est bien pratique. Dans la suite j'utiliserai l'interprétation en cadrants au format u16,14. Le format du résultat est critiquable en ce qu'il gâche presque un bit. On pourrait utiliser s16,15, et gagner un bit de précision, si on acceptait que le cosinus ne puisse pas atteindre tout à fait +1. Mais comme je ne cherche pas une haute précision, j'ai préféré respecter le fait que cos(0) = 1.

Grâce aux symétries de la fonction cosinus, on peut se contenter de la calculer sur le premier cadrant, et les valeurs sur les autres cadrants s'en déduisent par les symétries. Je définis alors la fonction

[math]c(x) = \cos(\pi/2 \; x)[/math]

que j'essaye d'approcher dans [0, 1] par un polynôme. L'approche évidente consisterait à utiliser un polynôme de meilleure approximation. Cependant, un tel polynôme ne respecte pas certaines propriétés de la fonction cosinus qu'il serait désirable de retrouver dans l'approximation, à savoir :

  • c(0) = 1 [prop. 1]
  • c(1) = 0 [2]
  • c′(0) = 0 [3]
  • c(x) ≤ 1, pour tout x [4]

Je fais de plus le choix d'utiliser un polynôme pair, autrement dit j'approxime avec :

[math]c(x) \approx P(x^2)[/math]

En faisant le changement de variable u = x2 (et donc [math]x = \sqrt{u}[/math]), cela s'écrit :

[math]P(u) \approx c(\sqrt{u}) = \cos(\pi/2 \sqrt{u})[/math]

où P(u) doit vérifier

  • P(0) = 1 [1]
  • P(1) = 0 [2]
  • P(u) ≤ 1, pour tout u [4]

L'intérêt d'avoir un polynôme pair est d'une part que la propriété [3] est automatiquement vérifiée, et dans la pratique une bonne approximation qui respecte [3] respecte automatiquement [4]. Mais d'autre part les polynômes pairs sont peu coûteux à évaluer : n/2 + 1 multiplications pour un polynôme pair de degré n, contre n multiplications pour un polynôme générique de même degré.

Approximation de c(√u) = cos(π/2 √u) par la somme de deux polynômes.

Je cherche donc un polynôme P(u) vérifiant [1] et [2]. En m'inspirant de ce qu'on fait avec les équations différentielles (solution particulière plus solution générale de l'équation homogène), et comme illustré sur la figure ci-contre, je décompose la solution sous la forme :

[math]P(u) = P_1(u) + P_2(u)[/math]

où P1(u) est le plus simple des polynômes respectant [1] et [2], et P2(u) respecte les « conditions homogènes » [1, 2′], avec la condition [2′] :

  • P2(1) = 0 [2′]

On peut facilement se convaincre que P1 est une simple interpolation linéaire : P1(u) = 1−u, et que P2(u), puisqu'il s'annule en 0 et en 1, doit être multiple de u(1−u). On a donc

[math]P(u) = (1-u) + u(1-u)Q(u) = (1-u)\big(1+uQ(u)\big)[/math]

où le polynôme Q(u) reste à déterminer. L'écriture de droite est celle qui sera implémentée dans le code, car elle minimise le nombre de multiplications. Si maintenant on extrait Q(u) de l'équation ci-dessus, et qu'on note que P(u) est une approximation de cos(π/2 √u), on a

Approximations polynomiales de degré 0 et 1 de la fonction f(u) = (cos(π/2 √u) − (1−u)) / (u(1−u)).
[math]Q(u) \approx f(u) = \big(\cos(\pi/2 \sqrt{u}) - (1-u)\big) / \big(u(1-u)\big)[/math]

En dépit de son air « compliqué », la fonction f(u) ainsi définie est très régulière sur [0, 1], et facile d'approcher avec des polynômes. La figure ci-contre montre cette fonction, ainsi que des approximations polynomiales de celle-ci de degrés 0 et 1.

On voit sur cette figure que l'approximation linéaire est déjà très bonne mais pas parfaite. Il serait tentant de pousser l'approximation au degré 2, et on aurait un bien meilleur ajustement. Cela est cependant inutile, car l'erreur qui nous intéresse est |P(x2)−c(x)|, et elle est bien plus petite que l'écart |Q(u)−f(u)| visible sur cette figure. Elle est même plus petite que la précision autorisée par un format de virgule fixe su 16 bits, on ne gagnerait donc rien à améliorer le polynôme. De fait, même l'approximation de degré zéro (Q constant) permet d'obtenir une approximation raisonnable de c(x), comme on le voyait déjà sur la figure précédente.

Erreur de deux approximations polynomiales de cos(π/2 x), de degrés 4 et 6.

La dernière figure ci-contre montre l'erreur P(x2)−c(x) pour ces deux cas. À noter, dans la légende, que P a un degré plus élevé que Q (deg P = deg Q + 2), et le degré (en x) de P(x2) est le double du degré de P(u). Remarquer aussi que pour l'approximation de degré 6 (Q de degré 1) l'erreur est inférieure à 10−5, et elle a été multipliée par 100 pour la rendre visible sur le graphique. On remarque enfin que l'erreur est une fonction oscillante d'amplitude constante, c'est à dire que l'erreur maximale est atteinte à chaque oscillation (donc 2 ou 3 fois suivant le cas). En ce sens cela ressemble beaucoup au comportement d'un polynôme de meilleure approximation. On pourrait appeler cela un « polynôme de meilleure approximation sous contrainte ».

Cette propriété de l'erreur optimale révèle aussi la méthode que j'ai utilisée pour l'optimisation : j'ai ajusté manuellement les paramètres de Q jusqu'à ce que les maxima locaux de |P(x2)−c(x)| aient tous la même hauteur. Pour des degrés si faibles (de Q) l'ajustement manuel est facile.

Le tableau ci-dessous montre, pour plusieurs choix de Q(u), le degré en x du polynôme P(x2) correspondant, et l'erreur maximale obtenue max|P(x2)−c(x)|. J'ai inclus le cas trivial Q(u) = 0, qui pourrait suffire dans des applications demandant très peu de précision.

Q(u) degré de P(x2) erreur max.
0 2 5,60 × 10−2
−0.224 4 9,20 × 10−4
−0.2335216 + 0.0190963 u 6 9,20 × 10−6


Le code

Ci-dessous l'implémentation de l'approximation de degré 6. Mais d'abord les mesures de précision et de temps d'exécution avec, pour comparaison, les fonctions cos() et sin() de la bibliothèque standard :

fonction erreur max. cycles temps
cos_fix() 9.53e-5 108.25 6.77 µs
sin_fix() 110.25 6.89 µs
cos() 2.98e-8 1720.8 107.5 µs
sin() 1725.1 107.8 µs

L'erreur maximale (9,53 × 10−5), bien plus grande que l'erreur théorique du polynôme (9,20 × 10−6), correspond à 1,56 fois la résolution du format du résultat. Ceci montre que la précision de la fonction est seulement limitée par les erreurs d'arrondi. Le fait que le polynôme soit plus précis que nécessaire a d'ailleurs permis une optimisation supplémentaire : l'une des multiplications (le terme linéaire de Q(u)) est calculée de façon grossière en ne multipliant que les octets de poids fort. Pour les fonctions standard, l'erreur indiquée ici est FLT_EPSILON / 4, ce qui correspond à la limite imposée par le format float.

Les temps d'exécution ont été mesurés sur un Arduino Uno (ATmega328P à 16 MHz) et moyennés sur des milliers d'angles entre 0 et 2π.

Le fichier d'en-tête cos_fix.h

/*
 * cos_fix.h: Fixed-point cos() and sin() functions, based on a sixth
 * degree polynomial approximation.
 *
 * argument is in units of 2*M_PI/2^16.
 * result is in units of 1/2^14 (range = [-2^14 : 2^14]).
 *
 * The cosine function uses an even-polynomial approximation of
 * cos(M_PI/2*x) for x in [0:1], and symmetries when x is outside [0:1].
 * Sine is defined as sin(x) = cos(3*M_PI/2+x).
 */
 
#include <stdint.h>
 
#ifdef __cplusplus
extern "C" {
#endif
 
/*
 * Sixth degree polynomial:
 *      cos(M_PI/2*x) ~ (1 - x^2)*(1 - x^2*(0.23352 - 0.019531*x^2))
 * for x in [0:1]. Max error = 9.53e-5
 */
int16_t cos_fix(uint16_t x);
 
/*
 * Fixed point sin().
 */
static inline int16_t sin_fix(uint16_t x)
{
    return cos_fix(0xc000 + x);
}
 
#ifdef __cplusplus
}
#endif

L'implémentation cos_fix.c

/*
 * cos_fix.c: Fixed-point cos() function.
 *
 * Compile for AVR:
 *      avr-gcc -c -mmcu=avr5 -Os -Wall -Wextra cos_fix.c
 *
 * Compile for AVR with no ASM code:
 *      avr-gcc -DNO_ASM -c -mmcu=avr5 -Os -Wall -Wextra cos_fix.c
 *
 * Compile test program:
 *      gcc -DRUN_TEST -O -Wall -Wextra cos_fix.c -o cos_fix
 *
 * Usage (test program):
 *      ./cos_fix > cos_fix.tsv
 */
 
#include "cos_fix.h"
 
#define FIXED(x, n) ((uint16_t)((float)(x) * ((uint32_t)1 << (n)) + .5))
 
#if !defined(NO_ASM)
# if defined(__AVR_HAVE_MUL__) && defined(__AVR_HAVE_MOVW__)
#  define USE_AVR_ASM
# endif
#endif
 
/*
 * Fixed point multiplication.
 *
 * Multiply two fixed point numbers in u16,16 (unsigned 0.16) format.
 * Returns result in the same format.
 * Rounds to nearest, ties rounded up.
 */
static uint16_t mul_fix_u16(uint16_t x, uint16_t y)
{
    uint16_t result;
#ifdef USE_AVR_ASM
    /* Optimized ASM version. */
    asm volatile(
	    "mul  %B1, %B2\n\t"
	    "movw %A0, r0\n\t"
	    "ldi  r19, 0x80\n\t"
	    "clr  r18\n\t"
	    "mul  %A1, %A2\n\t"
	    "add  r19, r1\n\t"
	    "adc  %A0, r18\n\t"
	    "adc  %B0, r18\n\t"
	    "mul  %B1, %A2\n\t"
	    "add  r19, r0\n\t"
	    "adc  %A0, r1\n\t"
	    "adc  %B0, r18\n\t"
	    "mul  %A1, %B2\n\t"
	    "add  r19, r0\n\t"
	    "adc  %A0, r1\n\t"
	    "adc  %B0, r18\n\t"
	    "clr  r1"
        : "=&r" (result)
        : "r" (x), "r" (y)
        : "r18", "r19"
    );
#else
    /* Generic C version. Compiles to inefficient 32 bit code. */
    result = ((uint32_t) x * y + 0x8000) >> 16;
#endif
    return result;
}
 
/*
 * Cheap and rough fixed point multiplication: multiply only the high
 * bytes of the operands, return 16 bit result.
 *
 * For some reason, the equivalent macro compiles to inefficient code.
 * This compiles to 3 instructions (mul a,b; movw res,r0; clr r1).
 */
static uint16_t mul_high_bytes(uint16_t x, uint16_t y)
{
    return (uint8_t)(x >> 8) * (uint8_t)(y >> 8);
}
 
/*
 * Fixed point cos() function: sixth degree polynomial approximation.
 *
 * argument is in units of 2*M_PI/2^16.
 * result is in units of 1/2^14 (range = [-2^14 : 2^14]).
 *
 * Uses the approximation
 *      cos(M_PI/2*x) ~ P(x^2), with
 *      P(u) = (1 - u) * (1 - u * (0.23352 - 0.019531 * u))
 * for x in [0 : 1]. Max error = 9.53e-5
 */
int16_t cos_fix(uint16_t x)
{
    uint16_t y, s;
    uint8_t i = (x >> 8) & 0xc0;  // quadrant information
    x = (x & 0x3fff) << 1;        // .15
    if (i & 0x40) x = FIXED(1, 15) - x;
    x = mul_fix_u16(x, x) << 1;   // .15
    y = FIXED(1, 15) - x;         // .15
    s = FIXED(0.23361, 16) - mul_high_bytes(FIXED(0.019531, 17), x); // .16
    s = FIXED(1, 15) - mul_fix_u16(x, s);  // .15
    s = mul_fix_u16(y, s);        // .14
    return (i == 0x40 || i == 0x80) ? -s : s;
}
 
#ifdef RUN_TEST
 
#include <stdio.h>
 
/* Print out a table of values for checking accuracy. */
int main(void)
{
    uint32_t x;
    for (x = 0; x < (1UL << 16); x++)
        printf("%u\t%d\t%g\n", (uint16_t) x, cos_fix(x));
    return 0;
}
 
#endif

Fonction atan()

Travail en cours !

Le code ci-dessous calcule la fonction atan(x), uniquement pour x compris entre 0 et 1. Pour des arguments plus grands que 1, il faut utiliser avec ce code la relation atan(x) = π/2 - atan(1/x). L'erreur maximale est de 0.103 mrad, soit 0.0059°.

/*
 * Fixed point atan() function: sixth degree polynomial approximation.
 *
 * argument is in units of 1/2^15 (range = [0 : 2^15]).
 * result is in units of 2*M_PI/2^16 (range = [0 : 2^13]).
 *
 * Uses the approximation
 *      atan(x) ~ M_PI/4 * P(x), with
 *      P(x) = x * (1 + (1 - x) * Q(x))
 *      Q(x) = 0.271553 + x*(0.299045 + x*(-0.270519 + x*0.0625))
 * for x in [0 : 1]. Max error = 1.31e-4
 */
uint16_t atan_fix(uint16_t x)  // x: .15
{
    uint16_t s;
    s = FIXED(0.0625, 18);                        // .18
    s = FIXED(0.270519, 17) - mul_high_bytes(x, s);    // .17
    s = FIXED(0.299045, 16) - mul_fix_u16(x, s);     // .16
    s = FIXED(0.271553, 15) + mul_fix_u16(x, s);     // .15
    s = FIXED(1, 14)+mul_fix_u16(FIXED(1, 15)-x, s); // .14
    return mul_fix_u16(x, s);                        // .13
}

Fonction atan2()

/*
 * Fixed point atan2().
 *
 * arguments are in arbitrary units, as long as they have the *same*
 * unit, range is [-32767 : +32767], -32768 does *not* work!
 * result is in units of 2*M_PI/2^16 (range = [0 : 2^13]).
 *
 * Reduces the argument to the first octant and calculates atan(y/x).
 * The octants are numbered, CCW: 0, 1, 5, 4, 6, 7, 3, 2.
 */
uint16_t atan2_fix(int16_t y, int16_t x)
{
    static const uint8_t axis[8] = {0x00, 0x40, 0x00, 0xc0,
        0x80, 0x40, 0x80, 0xc0};
    uint8_t octant = 0;
    if (x < 0) { x = -x; octant |= 4; }
    if (y < 0) { y = -y; octant |= 2; }
    if (y > x) { int16_t tmp = x; x = y; y = tmp; octant |= 1; }
    uint16_t angle = atan_fix((((uint32_t) y) << 15) / x);
    if ((octant ^ (octant>>1) ^ (octant>>2)) & 1) angle = -angle;
    return angle + (axis[octant] << 8);
}