传感器与指南针和陀螺仪融合：0 到 360 度之间答案

【问题标题】：Sensor fusion with compass and gyroscope: between 0 and 360 degrees传感器与指南针和陀螺仪融合：0 到 360 度之间
【发布时间】：2012-01-29 03:07:32
【问题描述】：

我正在开发一个小型室内导航应用程序，我在其中使用陀螺仪和指南针进行设备定位。我使用陀螺仪来平滑罗盘数据。我的传感器融合如下所示。这是我的motionHandler，一切都发生在这里。

// 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度，这显然是完全错误的！

我认为这个问题必须有任何常见的解决方法......

【问题讨论】：

没有人可以帮忙吗？ :-/
我只是想知道，您的程序是否也跟踪您的运动。我的意思不仅是旋转，还有分界线的平移，当你只是走路时。陀螺仪和磁力计的组合是否能够跟踪设备在 3D 空间中的复杂运动？
你有什么理由直接对四元数进行操作吗？似乎 -[CMAttitude multiplyByInverseOfAttitude:] 可能会简化您的代码。

标签： ios sensors gyroscope fusion magnetometer

【解决方案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;
}

在此之后，您可能希望在 0-360 范围内得到更新的标题：

updatedHeading = fmod( updatedHeading + 360, 360 );

您的示例通过此计算得出 355.5。

【讨论】：