Filtering compass readings

问题

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

回答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).

回答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)];

}

回答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; 
}

回答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);
}

来源：https://stackoverflow.com/questions/5347653/filtering-compass-readings