Пример #1
0
        private void Update()
        {
            RTQuaternion rotationDelta;
            RTQuaternion rotationPower;
            RTVector3    rotationUnitVector;

            if (EnableMag || EnableAccel)
            {
                // calculate rotation delta

                rotationDelta = mStateQ.Conjugate() * mMeasuredQPose;
                rotationDelta.Normalize();

                // take it to the power (0 to 1) to give the desired amount of correction

                double theta = Math.Acos(rotationDelta.Scalar);

                double sinPowerTheta = Math.Sin(theta * SlerpPower);
                double cosPowerTheta = Math.Cos(theta * SlerpPower);

                rotationUnitVector = new RTVector3(rotationDelta.X, rotationDelta.Y, rotationDelta.Z);
                rotationUnitVector.Normalize();

                rotationPower = new RTQuaternion(cosPowerTheta,
                                                 sinPowerTheta * rotationUnitVector.X,
                                                 sinPowerTheta * rotationUnitVector.Y,
                                                 sinPowerTheta * rotationUnitVector.Z);
                rotationPower.Normalize();

                //  multiple this by predicted value to get result

                mStateQ *= rotationPower;
                mStateQ.Normalize();
            }
        }
Пример #2
0
        public void CalculatePose(RTVector3 accel, RTVector3 mag, double magDeclination)
        {
            RTQuaternion m;
            RTQuaternion q = new RTQuaternion();

            if (EnableAccel)
            {
                accel.AccelToEuler(out mMeasuredPose);
            }
            else
            {
                mMeasuredPose   = mFusionPose;
                mMeasuredPose.Z = 0;
            }

            if (EnableMag && mMagValid)
            {
                q.FromEuler(mMeasuredPose);
                m = new RTQuaternion(0, mag.X, mag.Y, mag.Z);
                m = q * m * q.Conjugate();
                mMeasuredPose.Z = -Math.Atan2(m.Y, m.X) - magDeclination;
            }
            else
            {
                mMeasuredPose.Z = mFusionPose.Z;
            }

            mMeasuredQPose.FromEuler(mMeasuredPose);

            //  check for quaternion aliasing. If the quaternion has the wrong sign
            //  the filter will be very unhappy.

            int    maxIndex = -1;
            double maxVal   = -1000;

            for (int i = 0; i < 4; i++)
            {
                if (Math.Abs(mMeasuredQPose.data(i)) > maxVal)
                {
                    maxVal   = Math.Abs(mMeasuredQPose.data(i));
                    maxIndex = i;
                }
            }

            //  if the biggest component has a different sign in the measured and kalman poses,
            //  change the sign of the measured pose to match.

            if (((mMeasuredQPose.data(maxIndex) < 0) && (mFusionQPose.data(maxIndex) > 0)) ||
                ((mMeasuredQPose.data(maxIndex) > 0) && (mFusionQPose.data(maxIndex) < 0)))
            {
                mMeasuredQPose.Scalar = -mMeasuredQPose.Scalar;
                mMeasuredQPose.X      = -mMeasuredQPose.X;
                mMeasuredQPose.Y      = -mMeasuredQPose.Y;
                mMeasuredQPose.Z      = -mMeasuredQPose.Z;
                mMeasuredQPose.ToEuler(out mMeasuredPose);
            }
        }
Пример #3
0
        public void CalculatePose(RTVector3 accel, RTVector3 mag, double magDeclination)
        {
            RTQuaternion m;
            RTQuaternion q = new RTQuaternion();

            if (EnableAccel) {
                accel.AccelToEuler(out mMeasuredPose);
            } else {
                mMeasuredPose = mFusionPose;
                mMeasuredPose.Z = 0;
            }

            if (EnableMag && mMagValid) {
                q.FromEuler(mMeasuredPose);
                m = new RTQuaternion(0, mag.X, mag.Y, mag.Z);
                m = q * m * q.Conjugate();
                mMeasuredPose.Z = -Math.Atan2(m.Y, m.X) - magDeclination;
            } else {
                mMeasuredPose.Z = mFusionPose.Z;
            }

            mMeasuredQPose.FromEuler(mMeasuredPose);

            //  check for quaternion aliasing. If the quaternion has the wrong sign
            //  the filter will be very unhappy.

            int maxIndex = -1;
            double maxVal = -1000;

            for (int i = 0; i < 4; i++) {
                if (Math.Abs(mMeasuredQPose.data(i)) > maxVal) {
                    maxVal = Math.Abs(mMeasuredQPose.data(i));
                    maxIndex = i;
                }
            }

            //  if the biggest component has a different sign in the measured and kalman poses,
            //  change the sign of the measured pose to match.

            if (((mMeasuredQPose.data(maxIndex) < 0) && (mFusionQPose.data(maxIndex) > 0)) ||
                    ((mMeasuredQPose.data(maxIndex) > 0) && (mFusionQPose.data(maxIndex) < 0))) {
                mMeasuredQPose.Scalar = -mMeasuredQPose.Scalar;
                mMeasuredQPose.X = -mMeasuredQPose.X;
                mMeasuredQPose.Y = -mMeasuredQPose.Y;
                mMeasuredQPose.Z = -mMeasuredQPose.Z;
                mMeasuredQPose.ToEuler(out mMeasuredPose);
            }
        }
Пример #4
0
        public void AccelToQuaternion(out RTQuaternion qPose)
        {
            RTVector3 normAccel = this;
            RTVector3 vec;
            RTVector3 z = new RTVector3(0, 0, 1.0);

            qPose = new RTQuaternion();

            normAccel.Normalize();

            double angle = Math.Acos(RTVector3.DotProduct(z, normAccel));

            RTVector3.CrossProduct(normAccel, z, out vec);
            vec.Normalize();

            qPose.FromAngleVector(angle, vec);
        }
Пример #5
0
        public static RTVector3 PoseFromAccelMag(RTVector3 accel, RTVector3 mag)
        {
            RTVector3    result;
            RTQuaternion m;
            RTQuaternion q;

            accel.AccelToEuler(out result);

            //  q.fromEuler(result);
            //  since result.z() is always 0, this can be optimized a little

            double cosX2 = Math.Cos(result.X / 2.0f);
            double sinX2 = Math.Sin(result.X / 2.0f);
            double cosY2 = Math.Cos(result.Y / 2.0f);
            double sinY2 = Math.Sin(result.Y / 2.0f);

            q = new RTQuaternion(cosX2 * cosY2, sinX2 * cosY2, cosX2 * sinY2, -sinX2 * sinY2);
            m = new RTQuaternion(0, mag.X, mag.Y, mag.Z);

            m        = q * m * q.Conjugate();
            result.Z = -Math.Atan2(m.Y, m.X);
            return(result);
        }
Пример #6
0
        private void Update()
        {
            RTQuaternion rotationDelta;
            RTQuaternion rotationPower;
            RTVector3 rotationUnitVector;

            if (EnableMag || EnableAccel) {

                // calculate rotation delta

                rotationDelta = mStateQ.Conjugate() * mMeasuredQPose;
                rotationDelta.Normalize();

                // take it to the power (0 to 1) to give the desired amount of correction

                double theta = Math.Acos(rotationDelta.Scalar);

                double sinPowerTheta = Math.Sin(theta * SlerpPower);
                double cosPowerTheta = Math.Cos(theta * SlerpPower);

                rotationUnitVector = new RTVector3(rotationDelta.X, rotationDelta.Y, rotationDelta.Z);
                rotationUnitVector.Normalize();

                rotationPower = new RTQuaternion(cosPowerTheta,
                            sinPowerTheta * rotationUnitVector.X,
                            sinPowerTheta * rotationUnitVector.Y,
                            sinPowerTheta * rotationUnitVector.Z);
                rotationPower.Normalize();

                //  multiple this by predicted value to get result

                mStateQ *= rotationPower;
                mStateQ.Normalize();
            }
        }
Пример #7
0
        public void AccelToQuaternion(out RTQuaternion qPose)
        {
            RTVector3 normAccel = this;
            RTVector3 vec;
            RTVector3 z = new RTVector3(0, 0, 1.0);
            qPose = new RTQuaternion();

            normAccel.Normalize();

            double angle = Math.Acos(RTVector3.DotProduct(z, normAccel));
            RTVector3.CrossProduct(normAccel, z, out vec);
            vec.Normalize();

            qPose.FromAngleVector(angle, vec);
        }
Пример #8
0
        public static RTVector3 PoseFromAccelMag(RTVector3 accel, RTVector3 mag)
        {
            RTVector3 result;
            RTQuaternion m;
            RTQuaternion q;

            accel.AccelToEuler(out result);

            //  q.fromEuler(result);
            //  since result.z() is always 0, this can be optimized a little

            double cosX2 = Math.Cos(result.X / 2.0f);
            double sinX2 = Math.Sin(result.X / 2.0f);
            double cosY2 = Math.Cos(result.Y / 2.0f);
            double sinY2 = Math.Sin(result.Y / 2.0f);

            q = new RTQuaternion(cosX2 * cosY2, sinX2 * cosY2, cosX2 * sinY2, -sinX2 * sinY2);
            m = new RTQuaternion(0, mag.X, mag.Y, mag.Z);

            m = q * m * q.Conjugate();
            result.Z = -Math.Atan2(m.Y, m.X);
            return result;
        }
Пример #9
0
 public static string Display(string label, RTQuaternion quat)
 {
     return string.Format("{0}: scalar: {1:F4}, x: {2:F4}, y: {3:F4}, z: {4:F4}", label, quat.Scalar, quat.X, quat.Y, quat.Z);
 }
Пример #10
0
 public static string Display(string label, RTQuaternion quat)
 {
     return(string.Format("{0}: scalar: {1:F4}, x: {2:F4}, y: {3:F4}, z: {4:F4}", label, quat.Scalar, quat.X, quat.Y, quat.Z));
 }