Trigonometry can be slow

[edgar-bonet]

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()?

[RobTillaart]

mmm, interesting thoughts, questions pop up

1. I understand the slowness of sin cos and the speedup,

2. assume last is initialized on zero.

3. your code don't care about degrees or radians.
   It only must now it to initialize the full & half turn

4. would the order of adding values change the outcome?
   this needs investigation, outcome should be the same % 360. (TWO_PI)
   my expectations (assumptions) are that they are the same.

5. the weight / length should not affect the outcome.
   If you think of them as vectors, adding full turn does not change it. (rotate invariant)

6. investigated a faster sin & cos long ago - https://forum.arduino.cc/index.php?topic=69723.0 - might be option ?
   takes a "lot" of RAM so I did not prefer this originally.
   note to myself: make a library of this old stuf.

7. is it possible to recognize hardware supported trig functions?

• there is no (compiler) flag afaik
• a big include file #ifdef __AVR NO #idfed ARM YES #ifdef TEENSY MAYBE (too work intensive)
• do a test at startup (e.g. a sin(45) is as fast as a x/y ) expensive,(?), would work on all platforms.

---

• is there a way to remove the while loops (although max one will iterate) - something like

float f = alpha - last
int s = sign(f);
int d = abs(f) / full_turn;
alpha = alpha -  s * d * fullturn;

this would give a constant time performance.
in practice the while loops will often iterated 0 or 1 time 99% of the times, so not that much gain,

---

So yes, please make a PR, that allows me to test it more.

[RobTillaart]

Could also be interesting if the lib could switch between two modi operandi. (just to keep the thought)

[edgar-bonet]

2. Yes, _last is initialized to zero. This initial value can have an effect on the speed of the code (number of iterations of the while loops), but not on the results.
3. My code uses _type only in the type() method, although that one could be implemented as return _half_turn==180 ? DEGREES : RADIANS;.
4. Yes, the order can affect the outcome. See below.
5. I don't quite understand. In a weighted average, the weights do affect the outcome. Giving lengths to the vectors is similar to giving weights to the angles (it's the same at first order).
6. There is a speed/accuracy trade-off in those fast sin/cos methods.
7. I do not think there is a simple way to know at compile time whether we have an FPU. Some ARM chips do have one, others don't.
8. The division is very expensive on AVR, so I expect the while loop to be faster than this fmod() implementation on all realistic scenarios, although I didn't check.

About the effect of the order of the samples on the result. One normally averages angles that are all close to one another like, e.g., compass readings affected by noise. If this is the case, and more specifically, if the set of angles being averaged spans less than 180°, then the result is not affected by the order of the samples. Conversely, if they span more than 180°, the order can affect the outcome. On the other hand, if the angles are scattered all around the compass rose, then the whole concept of “average angle” makes little sense to start with.

Example: averaging in the given order:

• average(0°, 90°, 180°, 270°) = 135°
• average(180°, 270°, 0°, 90°) = 315°

The first result is just a plain average of the numbers. The second result can be understood by noticing that the sequence unwraps to (180°, 270°, 360°, 450°). In both cases the result is sound if the angles are provided in the order in which they are measured. The assumption is that there are no large jumps (larger than 180°) between two consecutive readings.

I will submit a PR, to be considered a “draft”, just for the sake of testing.

[RobTillaart]

I just was calling it a day then I saw your remarks. (popped the following thought)
From usage perspective, does the lib implements a "statistical/mathematical average" or more a "running average".

Think the scenario that angles do not change a lot makes very much sense in Arduino world.
Think small robots that use a compass sensor or so and just want to know where they are.

to be continued,
Thanks!

[edgar-bonet]

It's a regular, or “batch” average (average of a batch of numbers). A running average could be an interesting option, although I generally prefer the exponentially weighted variant (i.e. 1^st order IIR low-pass filter), as it doesn't require as much memory.

[RobTillaart]

That would be a class on its own, I propose the name "RunningAngle" or "MovingAngle" as I expect users would understand. In the documentation it could be explained in more detail. (OK the link should be sufficient)

core code could look like

void add(float angle)
{
  // unwrap angle first 
  if (count == 0) EWV = angle;
  else EWV += (angle - EWV) * alpha;
}

float getRunningAverage()
{
  return EWV;
}

[edgar-bonet]

Or rather

EWV = wrap(EWV + wrap(angle - EWV) * alpha);

I just found a post where I discuss the different strategies for low-pass filtering an angle.

[RobTillaart]

Started on a new class implementing runningAngle class with the wrapping ideas above.

https://github.com/RobTillaart/runningAngle

update
queued for library manager - arduino/Arduino#10900

[RobTillaart]

https://github.com/RobTillaart/runningAngle is alive so I close this issue (keep this class as is for now)

Thanks!