This method of averaging angles is quite elegant, and I have used it myself a few times. It is, however, quite slow on microcontrollers lacking an FPU, especially on the 8-bit AVRs, where sin() and cos() take about 1,600 CPU cycles each.

A simpler and faster method is to unwrap the angles. This is the same concept as phase unwrapping. The simple idea is that, as you take a continuous stream of angular readings, you expect consecutive angles to differ by no more than 180°. When this is not the case, you add or subtract 360° to the last reading until you get within ±180° of the previous one.

I tried to implement a version of AverageAngle based on this idea:

void AverageAngle::add(float alpha)
{
  // Unwrap.
  while (alpha < _last - _half_turn)
    alpha += _full_turn;
  while (alpha > _last + _half_turn)
    alpha -= _full_turn;
  _last = alpha;

  _sum += alpha;
  _count++;
}

float AverageAngle::getAverage()
{
  float avg = _sum/_count;
  while (avg < 0) avg += _full_turn;
  while (avg >= _full_turn) avg -= _full_turn;
  return avg;
}

I tested this with a set of angles near 0°/360°, and got a significant speedup:

• add() is about 5 times faster
• getAverage() is about 4 times faster

Obviously, the notion of “length” of an angle does not make sense anymore with this approach, although angles could be given “weights”, which are equivalent to lengths at first order.

Would you consider a pull request with this implementation? If so, what to do with the length parameter, and with the methods getTotalLength() and getAverageLength()?