Пример #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 AccelToEuler(out RTVector3 rollPitchYaw)
        {
            RTVector3 normAccel = this;

            normAccel.Normalize();

            rollPitchYaw = new RTVector3(Math.Atan2(normAccel.mY, normAccel.mZ),
                                         -Math.Atan2(normAccel.mX, Math.Sqrt(normAccel.mY * normAccel.mY + normAccel.mZ * normAccel.mZ)), 0);
        }
Пример #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);
            }
        }
        public void FromAngleVector(double angle, RTVector3 vec)
        {
            double sinHalfTheta = Math.Sin(angle / 2.0);

            mScalar = Math.Cos(angle / 2.0);
            mX      = vec.X * sinHalfTheta;
            mY      = vec.Y * sinHalfTheta;
            mZ      = vec.Z * sinHalfTheta;
        }
Пример #5
0
 public static void ConvertToVector(byte[] rawData, out RTVector3 vec, double scale, bool bigEndian)
 {
     if (bigEndian) {
         vec = new RTVector3((double)((UInt16)(((UInt16)rawData[0] << 8) | (UInt16)rawData[1])) * scale,
         (double)((Int16)(((UInt16)rawData[2] << 8) | (UInt16)rawData[3])) * scale,
         (double)((Int16)(((UInt16)rawData[4] << 8) | (UInt16)rawData[5])) * scale);
     } else {
         vec = new RTVector3((double)((Int16)(((UInt16)rawData[1] << 8) | (UInt16)rawData[0])) * scale,
         (double)((Int16)(((UInt16)rawData[3] << 8) | (UInt16)rawData[2])) * scale,
         (double)((Int16)(((UInt16)rawData[5] << 8) | (UInt16)rawData[4])) * scale);
     }
 }
Пример #6
0
 public void Reset()
 {
     mFirstTime  = true;
     mFusionPose = new RTVector3();
     mFusionQPose.FromEuler(mFusionPose);
     mGyro         = new RTVector3();
     mAccel        = new RTVector3();
     mMag          = new RTVector3();
     mMeasuredPose = new RTVector3();
     mMeasuredQPose.FromEuler(mMeasuredPose);
     mSampleNumber = 0;
 }
        public void FromEuler(RTVector3 vec)
        {
            double cosX2 = Math.Cos(vec.X / 2.0f);
            double sinX2 = Math.Sin(vec.X / 2.0f);
            double cosY2 = Math.Cos(vec.Y / 2.0f);
            double sinY2 = Math.Sin(vec.Y / 2.0f);
            double cosZ2 = Math.Cos(vec.Z / 2.0f);
            double sinZ2 = Math.Sin(vec.Z / 2.0f);

            mScalar = cosX2 * cosY2 * cosZ2 + sinX2 * sinY2 * sinZ2;
            mX      = sinX2 * cosY2 * cosZ2 - cosX2 * sinY2 * sinZ2;
            mY      = cosX2 * sinY2 * cosZ2 + sinX2 * cosY2 * sinZ2;
            mZ      = cosX2 * cosY2 * sinZ2 - sinX2 * sinY2 * cosZ2;
            Normalize();
        }
Пример #8
0
 public static void ConvertToVector(byte[] rawData, out RTVector3 vec, double scale, bool bigEndian)
 {
     if (bigEndian)
     {
         vec = new RTVector3((double)((UInt16)(((UInt16)rawData[0] << 8) | (UInt16)rawData[1])) * scale,
                             (double)((Int16)(((UInt16)rawData[2] << 8) | (UInt16)rawData[3])) * scale,
                             (double)((Int16)(((UInt16)rawData[4] << 8) | (UInt16)rawData[5])) * scale);
     }
     else
     {
         vec = new RTVector3((double)((Int16)(((UInt16)rawData[1] << 8) | (UInt16)rawData[0])) * scale,
                             (double)((Int16)(((UInt16)rawData[3] << 8) | (UInt16)rawData[2])) * scale,
                             (double)((Int16)(((UInt16)rawData[5] << 8) | (UInt16)rawData[4])) * scale);
     }
 }
Пример #9
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);
            }
        }
Пример #10
0
        public void NewIMUData(ref RTIMUData data)
        {
            mSampleNumber++;

            if (EnableGyro)
            {
                mGyro = data.gyro;
            }
            else
            {
                mGyro = new RTVector3();
            }
            mAccel    = data.accel;
            mMag      = data.mag;
            mMagValid = data.magValid;

            if (mFirstTime)
            {
                mLastFusionTime = data.timestamp;
                CalculatePose(mAccel, mMag, CompassAdjDeclination);

                //  initialize the poses

                mStateQ.FromEuler(mMeasuredPose);
                mFusionQPose = mStateQ;
                mFusionPose  = mMeasuredPose;
                mFirstTime   = false;
            }
            else
            {
                mTimeDelta = (double)(data.timestamp - mLastFusionTime) / (double)1000000;
                if (mTimeDelta > 0)
                {
                    CalculatePose(mAccel, mMag, CompassAdjDeclination);
                    Predict();
                    Update();
                    mStateQ.ToEuler(out mFusionPose);
                    mFusionQPose = mStateQ;
                }
                mLastFusionTime = data.timestamp;
            }
            data.fusionPoseValid  = true;
            data.fusionQPoseValid = true;
            data.fusionPose       = mFusionPose;
            data.fusionQPose      = mFusionQPose;
        }
Пример #11
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);
        }
        public void ToAngleVector(out double angle, out RTVector3 vec)
        {
            double halfTheta;
            double sinHalfTheta;

            halfTheta    = Math.Acos(mScalar);
            sinHalfTheta = Math.Sin(halfTheta);

            if (sinHalfTheta == 0)
            {
                vec = new RTVector3(1.0, 0, 0);
            }
            else
            {
                vec = new RTVector3(mX / sinHalfTheta, mY / sinHalfTheta, mZ / sinHalfTheta);
            }
            angle = 2.0 * halfTheta;
        }
Пример #13
0
        public void NewIMUData(ref RTIMUData data)
        {
            mSampleNumber++;

            if (EnableGyro)
                mGyro = data.gyro;
            else
                mGyro = new RTVector3();
            mAccel = data.accel;
            mMag = data.mag;
            mMagValid = data.magValid;

            if (mFirstTime) {
                mLastFusionTime = data.timestamp;
                CalculatePose(mAccel, mMag, CompassAdjDeclination);

                //  initialize the poses

                mStateQ.FromEuler(mMeasuredPose);
                mFusionQPose = mStateQ;
                mFusionPose = mMeasuredPose;
                mFirstTime = false;
            } else {
                mTimeDelta = (double)(data.timestamp - mLastFusionTime) / (double)1000000;
                if (mTimeDelta > 0) {
                    CalculatePose(mAccel, mMag, CompassAdjDeclination);
                    Predict();
                    Update();
                    mStateQ.ToEuler(out mFusionPose);
                    mFusionQPose = mStateQ;
                }
                mLastFusionTime = data.timestamp;
            }
            data.fusionPoseValid = true;
            data.fusionQPoseValid = true;
            data.fusionPose = mFusionPose;
            data.fusionQPose = mFusionQPose;
        }
Пример #14
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);
        }
Пример #15
0
        public RTVector3 GetAccelResiduals()
        {
            RTQuaternion rotatedGravity;
            RTQuaternion fusedConjugate;
            RTQuaternion qTemp;
            RTVector3    residuals;

            //  do gravity rotation and subtraction

            // create the conjugate of the pose

            fusedConjugate = mFusionQPose.Conjugate();

            // now do the rotation - takes two steps with qTemp as the intermediate variable

            qTemp          = mGravity * mFusionQPose;
            rotatedGravity = fusedConjugate * qTemp;

            // now adjust the measured accel and change the signs to make sense

            residuals = new RTVector3(-(mAccel.X - rotatedGravity.X), -(mAccel.Y - rotatedGravity.Y), -(mAccel.Z - rotatedGravity.Z));
            return(residuals);
        }
Пример #16
0
        public void FromEuler(RTVector3 vec)
        {
            double cosX2 = Math.Cos(vec.X / 2.0f);
            double sinX2 = Math.Sin(vec.X / 2.0f);
            double cosY2 = Math.Cos(vec.Y / 2.0f);
            double sinY2 = Math.Sin(vec.Y / 2.0f);
            double cosZ2 = Math.Cos(vec.Z / 2.0f);
            double sinZ2 = Math.Sin(vec.Z / 2.0f);

            mScalar = cosX2 * cosY2 * cosZ2 + sinX2 * sinY2 * sinZ2;
            mX = sinX2 * cosY2 * cosZ2 - cosX2 * sinY2 * sinZ2;
            mY = cosX2 * sinY2 * cosZ2 + sinX2 * cosY2 * sinZ2;
            mZ = cosX2 * cosY2 * sinZ2 - sinX2 * sinY2 * cosZ2;
            Normalize();
        }
Пример #17
0
 public static string DisplayDegrees(string label, RTVector3 vec)
 {
     return string.Format("{0}: roll: {1:F4}, pitch: {2:F4}, yaw: {3:F4}", label, vec.X * RTMATH_RAD_TO_DEGREE,
         vec.Y * RTMATH_RAD_TO_DEGREE, vec.Z * RTMATH_RAD_TO_DEGREE);
 }
Пример #18
0
 public static void CrossProduct(RTVector3 a, RTVector3 b, out RTVector3 c)
 {
     c = new RTVector3(a.mY * b.mZ - a.mZ * b.mY, a.mZ * b.mX - a.mX * b.mZ, a.mX * b.mY - a.mY * b.mX);
 }
Пример #19
0
 public static double DotProduct(RTVector3 a, RTVector3 b)
 {
     return(a.mX * b.mX + a.mY * b.mY + a.mZ * b.mZ);
 }
Пример #20
0
 public void FromAngleVector(double angle, RTVector3 vec)
 {
     double sinHalfTheta = Math.Sin(angle / 2.0);
     mScalar = Math.Cos(angle / 2.0);
     mX = vec.X * sinHalfTheta;
     mY = vec.Y * sinHalfTheta;
     mZ = vec.Z * sinHalfTheta;
 }
Пример #21
0
 public void ToEuler(out RTVector3 vec)
 {
     vec = new RTVector3(Math.Atan2(2.0 * (mY * mZ + mScalar * mX), 1 - 2.0 * (mX * mX + mY * mY)),
             Math.Asin(2.0 * (mScalar * mY - mX * mZ)),
             Math.Atan2(2.0 * (mX * mY + mScalar * mZ), 1 - 2.0 * (mY * mY + mZ * mZ)));
 }
Пример #22
0
        public RTVector3 GetAccelResiduals()
        {
            RTQuaternion rotatedGravity;
            RTQuaternion fusedConjugate;
            RTQuaternion qTemp;
            RTVector3 residuals;

            //  do gravity rotation and subtraction

            // create the conjugate of the pose

            fusedConjugate = mFusionQPose.Conjugate();

            // now do the rotation - takes two steps with qTemp as the intermediate variable

            qTemp = mGravity * mFusionQPose;
            rotatedGravity = fusedConjugate * qTemp;

            // now adjust the measured accel and change the signs to make sense

            residuals = new RTVector3(-(mAccel.X - rotatedGravity.X), -(mAccel.Y - rotatedGravity.Y), -(mAccel.Z - rotatedGravity.Z));
            return residuals;
        }
Пример #23
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();
            }
        }
Пример #24
0
 public static string DisplayDegrees(string label, RTVector3 vec)
 {
     return(string.Format("{0}: roll: {1:F4}, pitch: {2:F4}, yaw: {3:F4}", label, vec.X * RTMATH_RAD_TO_DEGREE,
                          vec.Y * RTMATH_RAD_TO_DEGREE, vec.Z * RTMATH_RAD_TO_DEGREE));
 }
Пример #25
0
 public static string DisplayRadians(string label, RTVector3 vec)
 {
     return string.Format("{0}: x:{1:F4}, y: {2:F4}, z: {3:F4}", label, vec.X, vec.Y, vec.Z);
 }
Пример #26
0
 public static void CrossProduct(RTVector3 a, RTVector3 b, out RTVector3 c)
 {
     c = new RTVector3(a.mY * b.mZ - a.mZ * b.mY, a.mZ * b.mX - a.mX * b.mZ, a.mX * b.mY - a.mY * b.mX);
 }
Пример #27
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);
        }
Пример #28
0
        //  Note - code assumes that this is the first thing called after axis swapping
        //  for each specific IMU chip has occurred.

        protected void HandleGyroBias()
        {
            // do axis rotation

            if ((mAxisRotation > 0) && (mAxisRotation < RTIMU_AXIS_ROTATION_COUNT))
            {
                // need to do an axis rotation
                RTIMUData tempIMU = mImuData;

                // do new x value
                if (mAxisRotationArray[mAxisRotation, 0] != 0)
                {
                    mImuData.gyro.X  = tempIMU.gyro.X * mAxisRotationArray[mAxisRotation, 0];
                    mImuData.accel.X = tempIMU.accel.X * mAxisRotationArray[mAxisRotation, 0];
                    mImuData.mag.X   = tempIMU.mag.X * mAxisRotationArray[mAxisRotation, 0];
                }
                else if (mAxisRotationArray[mAxisRotation, 1] != 0)
                {
                    mImuData.gyro.X  = tempIMU.gyro.Y * mAxisRotationArray[mAxisRotation, 1];
                    mImuData.accel.X = tempIMU.accel.Y * mAxisRotationArray[mAxisRotation, 1];
                    mImuData.mag.X   = tempIMU.mag.Y * mAxisRotationArray[mAxisRotation, 1];
                }
                else if (mAxisRotationArray[mAxisRotation, 2] != 0)
                {
                    mImuData.gyro.X  = tempIMU.gyro.Z * mAxisRotationArray[mAxisRotation, 2];
                    mImuData.accel.X = tempIMU.accel.Z * mAxisRotationArray[mAxisRotation, 2];
                    mImuData.mag.X   = tempIMU.mag.Z * mAxisRotationArray[mAxisRotation, 2];
                }

                // do new y value
                if (mAxisRotationArray[mAxisRotation, 3] != 0)
                {
                    mImuData.gyro.Y  = tempIMU.gyro.X * mAxisRotationArray[mAxisRotation, 3];
                    mImuData.accel.Y = tempIMU.accel.X * mAxisRotationArray[mAxisRotation, 3];
                    mImuData.mag.Y   = tempIMU.mag.X * mAxisRotationArray[mAxisRotation, 3];
                }
                else if (mAxisRotationArray[mAxisRotation, 4] != 0)
                {
                    mImuData.gyro.Y  = tempIMU.gyro.Y * mAxisRotationArray[mAxisRotation, 4];
                    mImuData.accel.Y = tempIMU.accel.Y * mAxisRotationArray[mAxisRotation, 4];
                    mImuData.mag.Y   = tempIMU.mag.Y * mAxisRotationArray[mAxisRotation, 4];
                }
                else if (mAxisRotationArray[mAxisRotation, 5] != 0)
                {
                    mImuData.gyro.Y  = tempIMU.gyro.Z * mAxisRotationArray[mAxisRotation, 5];
                    mImuData.accel.Y = tempIMU.accel.Z * mAxisRotationArray[mAxisRotation, 5];
                    mImuData.mag.Y   = tempIMU.mag.Z * mAxisRotationArray[mAxisRotation, 5];
                }

                // do new z value
                if (mAxisRotationArray[mAxisRotation, 6] != 0)
                {
                    mImuData.gyro.Z  = tempIMU.gyro.X * mAxisRotationArray[mAxisRotation, 6];
                    mImuData.accel.Z = tempIMU.accel.X * mAxisRotationArray[mAxisRotation, 6];
                    mImuData.mag.Z   = tempIMU.mag.X * mAxisRotationArray[mAxisRotation, 6];
                }
                else if (mAxisRotationArray[mAxisRotation, 7] != 0)
                {
                    mImuData.gyro.Z  = tempIMU.gyro.Y * mAxisRotationArray[mAxisRotation, 7];
                    mImuData.accel.Z = tempIMU.accel.Y * mAxisRotationArray[mAxisRotation, 7];
                    mImuData.mag.Z   = tempIMU.mag.Y * mAxisRotationArray[mAxisRotation, 7];
                }
                else if (mAxisRotationArray[mAxisRotation, 8] != 0)
                {
                    mImuData.gyro.Z  = tempIMU.gyro.Z * mAxisRotationArray[mAxisRotation, 8];
                    mImuData.accel.Z = tempIMU.accel.Z * mAxisRotationArray[mAxisRotation, 8];
                    mImuData.mag.Z   = tempIMU.mag.Z * mAxisRotationArray[mAxisRotation, 8];
                }
            }

            RTVector3 deltaAccel = mPreviousAccel;

            deltaAccel    -= mImuData.accel; // compute difference
            mPreviousAccel = mImuData.accel;

            if ((deltaAccel.length() < RTIMU_FUZZY_ACCEL_ZERO) && (mImuData.gyro.length() < RTIMU_FUZZY_GYRO_ZERO))
            {
                // what we are seeing on the gyros should be bias only so learn from this

                if (mGyroSampleCount < (5 * mSampleRate))
                {
                    mGyroBias.X = (1.0 - mGyroLearningAlpha) * mGyroBias.X + mGyroLearningAlpha * mImuData.gyro.X;
                    mGyroBias.Y = (1.0 - mGyroLearningAlpha) * mGyroBias.Y + mGyroLearningAlpha * mImuData.gyro.Y;
                    mGyroBias.Z = (1.0 - mGyroLearningAlpha) * mGyroBias.Z + mGyroLearningAlpha * mImuData.gyro.Z;

                    mGyroSampleCount++;

                    if (mGyroSampleCount == (5 * mSampleRate))
                    {
                        // this could have been true already of course
                        mGyroBiasValid = true;
                    }
                }
                else
                {
                    mGyroBias.X = (1.0 - mGyroContinuousAlpha) * mGyroBias.X + mGyroContinuousAlpha * mImuData.gyro.X;
                    mGyroBias.Y = (1.0 - mGyroContinuousAlpha) * mGyroBias.Y + mGyroContinuousAlpha * mImuData.gyro.Y;
                    mGyroBias.Z = (1.0 - mGyroContinuousAlpha) * mGyroBias.Z + mGyroContinuousAlpha * mImuData.gyro.Z;
                }
            }

            mImuData.gyro -= mGyroBias;
        }
 public void ToEuler(out RTVector3 vec)
 {
     vec = new RTVector3(Math.Atan2(2.0 * (mY * mZ + mScalar * mX), 1 - 2.0 * (mX * mX + mY * mY)),
                         Math.Asin(2.0 * (mScalar * mY - mX * mZ)),
                         Math.Atan2(2.0 * (mX * mY + mScalar * mZ), 1 - 2.0 * (mY * mY + mZ * mZ)));
 }
Пример #30
0
        public void ToAngleVector(out double angle, out RTVector3 vec)
        {
            double halfTheta;
            double sinHalfTheta;

            halfTheta = Math.Acos(mScalar);
            sinHalfTheta = Math.Sin(halfTheta);

            if (sinHalfTheta == 0) {
                vec = new RTVector3(1.0, 0, 0);
            } else {
                vec = new RTVector3(mX / sinHalfTheta, mY / sinHalfTheta, mZ / sinHalfTheta);
            }
            angle = 2.0 * halfTheta;
        }
Пример #31
0
 public void Reset()
 {
     mFirstTime = true;
     mFusionPose = new RTVector3();
     mFusionQPose.FromEuler(mFusionPose);
     mGyro = new RTVector3();
     mAccel = new RTVector3();
     mMag = new RTVector3();
     mMeasuredPose = new RTVector3();
     mMeasuredQPose.FromEuler(mMeasuredPose);
     mSampleNumber = 0;
 }
Пример #32
0
        //  Note - code assumes that this is the first thing called after axis swapping
        //  for each specific IMU chip has occurred.
        protected void HandleGyroBias()
        {
            // do axis rotation

            if ((mAxisRotation > 0) && (mAxisRotation < RTIMU_AXIS_ROTATION_COUNT)) {
                // need to do an axis rotation
                RTIMUData tempIMU = mImuData;

                // do new x value
                if (mAxisRotationArray[mAxisRotation, 0] != 0) {
                    mImuData.gyro.X = tempIMU.gyro.X * mAxisRotationArray[mAxisRotation, 0];
                    mImuData.accel.X = tempIMU.accel.X * mAxisRotationArray[mAxisRotation, 0];
                    mImuData.mag.X = tempIMU.mag.X * mAxisRotationArray[mAxisRotation, 0];
                } else if (mAxisRotationArray[mAxisRotation, 1] != 0) {
                    mImuData.gyro.X = tempIMU.gyro.Y * mAxisRotationArray[mAxisRotation, 1];
                    mImuData.accel.X = tempIMU.accel.Y * mAxisRotationArray[mAxisRotation, 1];
                    mImuData.mag.X = tempIMU.mag.Y * mAxisRotationArray[mAxisRotation, 1];
                } else if (mAxisRotationArray[mAxisRotation, 2] != 0) {
                    mImuData.gyro.X = tempIMU.gyro.Z * mAxisRotationArray[mAxisRotation, 2];
                    mImuData.accel.X = tempIMU.accel.Z * mAxisRotationArray[mAxisRotation, 2];
                    mImuData.mag.X = tempIMU.mag.Z * mAxisRotationArray[mAxisRotation, 2];
                }

                // do new y value
                if (mAxisRotationArray[mAxisRotation, 3] != 0) {
                    mImuData.gyro.Y = tempIMU.gyro.X * mAxisRotationArray[mAxisRotation, 3];
                    mImuData.accel.Y = tempIMU.accel.X * mAxisRotationArray[mAxisRotation, 3];
                    mImuData.mag.Y = tempIMU.mag.X * mAxisRotationArray[mAxisRotation, 3];
                } else if (mAxisRotationArray[mAxisRotation, 4] != 0) {
                    mImuData.gyro.Y = tempIMU.gyro.Y * mAxisRotationArray[mAxisRotation, 4];
                    mImuData.accel.Y = tempIMU.accel.Y * mAxisRotationArray[mAxisRotation, 4];
                    mImuData.mag.Y = tempIMU.mag.Y * mAxisRotationArray[mAxisRotation, 4];
                } else if (mAxisRotationArray[mAxisRotation, 5] != 0) {
                    mImuData.gyro.Y = tempIMU.gyro.Z * mAxisRotationArray[mAxisRotation, 5];
                    mImuData.accel.Y = tempIMU.accel.Z * mAxisRotationArray[mAxisRotation, 5];
                    mImuData.mag.Y = tempIMU.mag.Z * mAxisRotationArray[mAxisRotation, 5];
                }

                // do new z value
                if (mAxisRotationArray[mAxisRotation, 6] != 0) {
                    mImuData.gyro.Z = tempIMU.gyro.X * mAxisRotationArray[mAxisRotation, 6];
                    mImuData.accel.Z = tempIMU.accel.X * mAxisRotationArray[mAxisRotation, 6];
                    mImuData.mag.Z = tempIMU.mag.X * mAxisRotationArray[mAxisRotation, 6];
                } else if (mAxisRotationArray[mAxisRotation, 7] != 0) {
                    mImuData.gyro.Z = tempIMU.gyro.Y * mAxisRotationArray[mAxisRotation, 7];
                    mImuData.accel.Z = tempIMU.accel.Y * mAxisRotationArray[mAxisRotation, 7];
                    mImuData.mag.Z = tempIMU.mag.Y * mAxisRotationArray[mAxisRotation, 7];
                } else if (mAxisRotationArray[mAxisRotation, 8] != 0) {
                    mImuData.gyro.Z = tempIMU.gyro.Z * mAxisRotationArray[mAxisRotation, 8];
                    mImuData.accel.Z = tempIMU.accel.Z * mAxisRotationArray[mAxisRotation, 8];
                    mImuData.mag.Z = tempIMU.mag.Z * mAxisRotationArray[mAxisRotation, 8];
                }
            }

            RTVector3 deltaAccel = mPreviousAccel;
            deltaAccel -= mImuData.accel;   // compute difference
            mPreviousAccel = mImuData.accel;

            if ((deltaAccel.length() < RTIMU_FUZZY_ACCEL_ZERO) && (mImuData.gyro.length() < RTIMU_FUZZY_GYRO_ZERO)) {
                // what we are seeing on the gyros should be bias only so learn from this

                if (mGyroSampleCount < (5 * mSampleRate)) {
                    mGyroBias.X = (1.0 - mGyroLearningAlpha) * mGyroBias.X + mGyroLearningAlpha * mImuData.gyro.X;
                    mGyroBias.Y = (1.0 - mGyroLearningAlpha) * mGyroBias.Y + mGyroLearningAlpha * mImuData.gyro.Y;
                    mGyroBias.Z = (1.0 - mGyroLearningAlpha) * mGyroBias.Z + mGyroLearningAlpha * mImuData.gyro.Z;

                    mGyroSampleCount++;

                    if (mGyroSampleCount == (5 * mSampleRate)) {
                        // this could have been true already of course
                        mGyroBiasValid = true;
                     }
                } else {
                    mGyroBias.X = (1.0 - mGyroContinuousAlpha) * mGyroBias.X + mGyroContinuousAlpha * mImuData.gyro.X;
                    mGyroBias.Y = (1.0 - mGyroContinuousAlpha) * mGyroBias.Y + mGyroContinuousAlpha * mImuData.gyro.Y;
                    mGyroBias.Z = (1.0 - mGyroContinuousAlpha) * mGyroBias.Z + mGyroContinuousAlpha * mImuData.gyro.Z;
                }
            }

            mImuData.gyro -= mGyroBias;
        }
Пример #33
0
 public static double DotProduct(RTVector3 a, RTVector3 b)
 {
     return a.mX * b.mX + a.mY * b.mY + a.mZ * b.mZ;
 }
Пример #34
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;
        }
Пример #35
0
        public void AccelToEuler(out RTVector3 rollPitchYaw)
        {
            RTVector3 normAccel = this;
            normAccel.Normalize();

            rollPitchYaw = new RTVector3(Math.Atan2(normAccel.mY, normAccel.mZ),
            -Math.Atan2(normAccel.mX, Math.Sqrt(normAccel.mY * normAccel.mY + normAccel.mZ * normAccel.mZ)), 0);
        }
Пример #36
0
 public static string DisplayRadians(string label, RTVector3 vec)
 {
     return(string.Format("{0}: x:{1:F4}, y: {2:F4}, z: {3:F4}", label, vec.X, vec.Y, vec.Z));
 }