与罗盘和陀螺仪的传感器融合：在0和360度之间

我正在开发一个小型的室内导航应用程序，其中我使用陀螺仪和指南针作为设备方向。我使用陀螺仪来平滑指南针数据。我的传感器融合看起来如下所示。这是我的运动处理者，一切都在发生。与罗盘和陀螺仪的传感器融合：在0和360度之间

// 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是最后的四元数。除了一个特殊的情况，这个工作是完美的：在0到360度之间的过渡。

如果updatedHeading接近但小于360且mHeading刚好大于0，则结果以圆圈形式移动。例如，如果updatedHeading = 355和mHeading = 5，则正确的结果应该在360和5之间。但是，我的公式计算337,5度，这显然是完全错误的！

有必须要对这个问题的任何常见的解决方法，我想......

来源

2012-01-29 Juuro

有没有人可以帮忙？： -/ – Juuro 2012-03-04 12:40:46

我只是想知道，你的程序是否也跟踪你的动作。我的意思不仅仅是轮换，而且还意味着当你走路时的鸿沟翻译。陀螺仪和磁力计的组合能够追踪3D设备中复杂的运动吗？ – BartoNaz 2012-04-03 11:10:10

你有什么理由直接在这个季度工作？看起来 - [CMAttitude multiplyByInverseOfAttitude：]可能会简化你的代码。 – jamesmoschou 2012-07-17 04:33:21

我平时做这样的事情，在这些类型的角度计算的以下内容：

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。

来源

2012-03-12 13:08:20 fishinear

与罗盘和陀螺仪的传感器融合：在0和360度之间

回答

相关问题