2

デバイスの向きにジャイロとコンパスを使用する小さな屋内ナビゲーション アプリを開発しています。ジャイロを使ってコンパス データを滑らかにします。私のセンサーフュージョンは次のようになります。これは、すべてが発生する私の 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 度の間の移行時という 1 つの特定のケースを除いて、完璧に機能します。

updatedHeadingが 360 に近いが 360 より小さく、 0 のすぐ上にある場合mHeading、結果は円を描くように移動します。たとえば、updatedHeading= 355 とmHeading= 5 の場合、正しい結果は 360 から 5 の間になるはずです。しかし、私の式は 337.5 度を計算します。

この問題には一般的な回避策が必要だと思います…</p>

4

1 に答える 1

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 が得られます。

于 2012-03-12T13:08:20.217 に答える