Yaw(angle) 值不稳定,有几度的漂移。有谁知道如何解决它?

Yaw(angle) value are not stable, it is drifting a few degrees. Does anyone know how to solve it?

我目前正在从事 iOS 项目,我在其中使用运动数据。 我在俯仰和滚动值上得到了很好的结果,但偏航值一直在漂移。我应用了卡尔曼滤波器,结果保持不变。 有谁知道如何解决它? 这是一些源代码(Objective C)

[self.motionManager startDeviceMotionUpdatesUsingReferenceFrame:CMAttitudeReferenceFrameXArbitraryCorrectedZVertical
            toQueue:[NSOperationQueue currentQueue]
            withHandler:^(CMDeviceMotion *motion, NSError *error)
        {
            //NSString *yaw = [NSString
                //stringWithFormat:@" %.3f", motion.attitude.yaw];

            NSString *pitch = [NSString
                stringWithFormat:@" %.3f", motion.attitude.pitch];

            NSString *roll = [NSString
                stringWithFormat:@" %.3f", motion.attitude.roll];

            //Converting NSSring type variable in to a double
            //double a_yaw = [yaw doubleValue];
            double a_pitch = [pitch doubleValue];
            double a_roll = [roll doubleValue];

            CMQuaternion quat = self.motionManager.deviceMotion.attitude.quaternion;
            double yaw = 180/M_PI * (asin(2*quat.x*quat.y + 2*quat.w*quat.z));

            // Kalman filtering
            static float q = 0.1;   // process noise
            static float r = 0.1;   // sensor noise
            static float p = 0.1;   // estimated error
            static float k = 0.5;   // kalman filter gain

            float x = motionLastYaw;
            p = p + q;
            k = p / (p + r);
            x = x + k*(yaw - x);
            p = (1 - k)*p;
            motionLastYaw = x;

            //Converting angles to degrees
            //yaw = yaw * 180/M_PI;
            a_pitch = a_pitch * 180/M_PI;
            a_roll = a_roll * 180/M_PI;

"yaw" 值需要引用附加坐标系(相机...)。