傳感器與羅盤和陀螺儀融合：0至360度之間

Question

我正在開發一個小型室內導航應用程序，其中使用陀螺儀和指南針進行設備定位。 我使用陀螺儀對指南針數據進行平滑處理。 我的傳感器融合如下所示。 這是我的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-function，它接收最新的標題信息：

- (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之間。但是我的公式計算出3375度，這顯然是完全錯誤的！

我認為這個問題必須有任何常見的解決方法。

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

您可能要通過以下方法使更新標題回到0-360范圍內：

updatedHeading = fmod( updatedHeading + 360, 360 );

您的示例使用此計算得出355.5。