Filtering compass readings

Question

I'm using compass heading to rotate an MKMapView. The rotation was a bit jerky so I'm trying to filter it like Google Maps on the iphone does (or appears to do some trickery).

I'm trying to filter the reading from the iphone compass using a moving average formula but it fails on the crossover between 359 adn 0 becuase it starts to average backwards from 35x to 0 and causes the map to rotate backwards as it approaches north from the west.

Any ideas what the best way is to filter this data so that it crosses from 359 back to zero and maintain the rolling average.

Code is here:

- (void)locationManager:(CLLocationManager *)manager didUpdateHeading:(CLHeading *)newHeading {
static float xd=0;
static float k = 0.22;

// Moving average formula
xd = k * xd + (1.0 - k) * newHeading.magneticHeading;

NSLog(@"%0.2f : %0.2f", newHeading.magneticHeading, xd);    
[map setTransform:CGAffineTransformMakeRotation((-1 * xd * M_PI) /180)];}

Thanks for any help

Aeschylus

See this recent question on averaging angles: https://mcmap.net/q/261291/-averaging-angles/5987 – Citronellal

Answer 1

If the previous moving average and the new heading are different by more than 180 degrees, add 360 to whichever is smaller. Then mod by 360 when storing the new moving average. So (without precise math):

HDG   MA
350   350
355   353
  0   356  (because 353 - 0 > 180 so adjusted HDG is 360)
  5   359  (likewise)
 10     2  (likewise, then 362 is new MA, mod 360 to normalize)
350   356  (because 2 - 350 < -180 so adjusted MA is 362)

My hope is that this works and is more efficient than the trigonometric method described in Averaging angles (credit to Mark Ransom for referring to that).

Wolverine

I think it's just if newHeading - xd > 180 then xd += 360; if newHeading - xd < -180 then newHeading += 360 before you compute the new xd, then right after do xd %= 360 to clean it up. – Wolverine

That still glitches at the crossing point between 359 and 0 in both directions. I did solve it and I'll post the code to github and post a link here. It wasn't easy and I'd like to make it a more elegant solution. – Aeschylus

Why do you think it glitches? When I thought about it and wrote the same input/output code block in my answer, it seemed like it could work. But apparently you see different results? I don't doubt it really, but would like to know why. – Wolverine

Hi John, it still exhibits the same problem I was having myself when the values go from 0 to 359 or 359 to 0; the average comes in less than that at around say 180 and turns the map upside down for a split second. I needs to cross the 0-359 boundary without hitting 180. I can't seem to figure out how to do it mathematically so had to resort to massaging the values in the queue when it was approaching and leaving the boundary. – Aeschylus

Answer 2

- (void)locationManager:(CLLocationManager *)manager didUpdateHeading:(CLHeading *)heading
{
    int newHeading;
    int queuecount;

    newHeading = heading.trueHeading;
    [queue addObject:[NSNumber numberWithInt:newHeading]];

    if([queue count] > 10) [queue removeObjectAtIndex:0];
    queuecount = [queue count];

    NSEnumerator *e = [queue objectEnumerator];
    NSNumber *sum;
    int oldd = 0 , newd, average =0;
    BOOL firstLoop = YES;
    while ((sum = [e nextObject])) 
    {
        newd = [sum intValue];
        if(firstLoop) {oldd = newd;firstLoop=NO;}

        if((newd +180) < oldd)
        {
            newd +=360; oldd = newd;
            average = average + newd;
            continue;
        }
        if((newd - 180) > oldd) 
        {
            newd -=360;oldd = newd;
            average = average + newd;
            continue;
        }

        average = average + newd;
        oldd = newd;

    }
    average = (average / queuecount) % 360;

    [map setTransform:CGAffineTransformMakeRotation((-1 * average * M_PI) /180)];

}

Aeschylus

As you can see it is a nasty problem but I'm sure somebody can do better than me. While I am happy enough with the result I'm kinda disappointed there wasn't a slicker solution that I could come up with. – Aeschylus

This looks about how I expected when I wrote my answer. Thanks for posting the new code. – Wolverine

I've added a project on github so that you can play with setup. If you would like to improve my code and especially the compass section please feel free. github.com/d0n13/MapTest – Aeschylus

Answer 3

I just wanted to add my version of the code that d0n posted with a few tweaks.

My code is C code for an Arduino so I have used no libraries (for example I get the absolute value without abs function)...

This thread was very helpful to me. Thank you D0n and John...

int Robot::getHeadingAverage() {

  // setup
  int lastReading = 0;
  int newReading = 0;
  int totalHeadings = 0;
  int avgHeading = 0;

  // loop through all the readings
  for(int i=0; i<totalHeadingReadings; i++) {
    // get the reading
    newReading = headings[i];

    // make sure we have an old reading
    if(i==0) {
      lastReading = newReading;
    }

    if((newReading + 180) < lastReading) {
      newReading = newReading + 360;
    }
    if((newReading - 180) > lastReading) {
      newReading = newReading - 360;
    }

    lastReading = newReading;
    totalHeadings = totalHeadings + newReading;

  }

  // get the average and make sure we do not end up over 360
  avgHeading = (totalHeadings / totalHeadingReadings) % 360;

  // check for negative value
  if(avgHeading < 0) {
    avgHeading = -1 * avgHeading; 
  }

  return avgHeading; 
}

Veiling

Answer 4

In the end, this is what worked for me.

• Code in C#.
• Measurements in Radians, though the code samples in the link will use degrees. -
• Adapted from https://rosettacode.org/wiki/Averages/Mean_angle.
• Omitted is the queue management similar to @d0n13's.

---

static double MeanAngle(List<double> angles)
{
    double x = 0, y = 0;
    foreach (double angle in angles)
    {
        x += Math.Cos(angle);
        y += Math.Sin(angle);
    }

    return Math.Atan2(y / angles.Count, x / angles.Count);
}

Bharal