Слияние датчиков с компасом и гироскопом: от 0 до 360 градусов

Question

Я разрабатываю небольшое приложение для внутренней навигации, в котором я использую гироскоп и компас для ориентации устройства. Я использую гироскоп, чтобы сгладить данные компаса. Мой слияние датчиков выглядит следующим образом. Это мое движениеHandler, где все происходит.

// Listen to events from the motionManager 
motionHandler =^(CMDeviceMotion *motion, NSError *error) { 

     __block float heading; 
     heading = mHeading; 

     CMAttitude *currentAttitude = motion.attitude; 

     //Initial heading setting 
     if (lastHeading == 0 && heading != 0) { 
      updatedHeading = heading; 
     } 
     lastHeading = heading; 

     if (oldQuaternion.w != 0 || oldQuaternion.x != 0 || oldQuaternion.y != 0 || oldQuaternion.z != 0){ 
      diffQuaternion = [self multiplyQuaternions:[self inverseQuaternion:oldQuaternion] :currentAttitude.quaternion]; 
      diffQuaternion = [self normalizeQuaternion:diffQuaternion]; 
     }    
     oldQuaternion = currentAttitude.quaternion; 

     diffYaw = RADIANS_TO_DEGREES([self yawFromQuaternion:diffQuaternion]);   

     quaternion = currentAttitude.quaternion; 

     //Get Pitch 
     rpy.pitch = -[self pitchFromQuaternion:quaternion]; 
     rpy.pitch += M_PI/2;       

     //Use Yaw-Difference for Heading 
     updatedHeading = updatedHeading - diffYaw; 

     //Heading has to be between 0 and 360 degrees 
     if (updatedHeading < 0) { 
      updatedHeading = 360 + updatedHeading; 
     } 
     else if (updatedHeading > 360) { 
      updatedHeading -= 360; 
     } 

     //fusionate gyro estimated heading with new magneticHeading 
     updatedHeading = (19.0*updatedHeading + 1.0*heading)/20.0; 

     //generate queternion 
     rotation = [self createFromAxisAngle:0 :rpy.pitch :DEGREES_TO_RADIANS(updatedHeading)]; 
    }; 

Фактическое слияние датчика формула эта линия: updatedHeading = (19.0*updatedHeading + 1.0*heading)/20.0;. И это моя didUpdateHeading-функция, которая получает новейшую заголовок-информацию:

- (void)locationManager:(CLLocationManager *)manager didUpdateHeading:(CLHeading *)newHeading 
{ 
    // Get new heading 
    mHeading = newHeading.magneticHeading;  

    mHeading += 90; 

    if (mHeading > 360) { 
     mHeading -= 360; 
    } 
} 

diffYaw является изменение заголовка вычисленного гироскопом. rotation ist окончательный кватернион. Это работает отлично, за исключением одного конкретного случая: при переходе от 0 до 360 градусов.

Если updatedHeading находится рядом с, но меньше 360 и mHeading находится чуть выше 0, результат перемещается по кругу. Например, если updatedHeading = 355 и mHeading = 5, правильный результат должен быть между 360 и 5. Но моя формула вычисляет 337,5 градусов, что явно совершенно неправильно!

Там должны быть какие-то общие методы обхода этой проблемы, я думаю ...

Answer 1

я обычно делаю что-то вроде следующего в этих типах угловых вычислений:

updatedHeading -= angleDiff(updatedHeading, mHeading) * 0.05; 

где angleDiff() является:

double angleDiff(double angle1, double angle2) 
{ 
    double angle = angle1 - angle2; 
    if(angle > 180) { 
     angle -= 360; 
    } else if(angle <= -180) { 
     angle += 360; 
    } 
    return angle; 
} 

Вы можете получить updatedHeading обратно в 0-360 диапазоне после этого по:

updatedHeading = fmod(updatedHeading + 360, 360); 

Ваш пример дает 355,5 с этим вычислением.