コード例 #1
0
ファイル: SensorFusionRTQF.cs プロジェクト: hmrten/idun
        private void Predict()
        {
            if (!EnableGyro || !Gyro.HasValue)
            {
                return;
            }

            double qs = StateQ.Scalar;
            double qx = StateQ.X;
            double qy = StateQ.Y;
            double qz = StateQ.Z;

            double x2 = Gyro.Value.X / 2.0;
            double y2 = Gyro.Value.Y / 2.0;
            double z2 = Gyro.Value.Z / 2.0;

            // Predict new state

            double timeDeltaSeconds = TimeDelta.TotalSeconds;

            StateQ.Scalar = qs + (-x2 * qx - y2 * qy - z2 * qz) * timeDeltaSeconds;
            StateQ.X      = qx + (x2 * qs + z2 * qy - y2 * qz) * timeDeltaSeconds;
            StateQ.Y      = qy + (y2 * qs - z2 * qx + x2 * qz) * timeDeltaSeconds;
            StateQ.Z      = qz + (z2 * qs + y2 * qx - x2 * qy) * timeDeltaSeconds;

            StateQ.Normalize();
        }
コード例 #2
0
        internal override void ProcessNewImuReadings(ref SensorReadings imuReadings)
        {
            SampleNumber++;

            Gyro = EnableGyro && imuReadings.GyroValid
                ? (Vector3?)imuReadings.Gyro
                : null;

            Acceleration = EnableAcceleration && imuReadings.AccelerationValid
                ? (Vector3?)imuReadings.Acceleration
                : null;

            MagneticField = EnableMagneticField && imuReadings.MagneticFieldValid
                ? (Vector3?)imuReadings.MagneticField
                : null;

            LastFusionTime = imuReadings.Timestamp;
            CalculatePose(Acceleration, MagneticField, CompassAdjDeclination);

            //  initialize the poses
            StateQ.FromEuler(MeasuredPose);
            FusionQPose = StateQ;
            FusionPose  = MeasuredPose;

            imuReadings.FusionPoseValid  = true;
            imuReadings.FusionQPoseValid = true;
            imuReadings.FusionPose       = FusionPose;
            imuReadings.FusionQPose      = FusionQPose;
        }
コード例 #3
0
ファイル: SensorFusionRTQF.cs プロジェクト: hmrten/idun
        internal override void ProcessNewImuReadings(ref ImuSensorReadings imuReadings)
        {
            SampleNumber++;

            Gyro = EnableGyro && imuReadings.GyroValid
                ? (Vector3?)imuReadings.Gyro
                : null;

            Acceleration = EnableAcceleration && imuReadings.AccelerationValid
                ? (Vector3?)imuReadings.Acceleration
                : null;

            MagneticField = EnableMagneticField && imuReadings.MagneticFieldValid
                ? (Vector3?)imuReadings.MagneticField
                : null;

            if (FirstTime)
            {
                LastFusionTime = imuReadings.Timestamp;
                CalculatePose(Acceleration, MagneticField, CompassAdjDeclination);

                //  initialize the poses

                StateQ.FromEuler(MeasuredPose);
                FusionQPose = StateQ;
                FusionPose  = MeasuredPose;
                FirstTime   = false;
            }
            else
            {
                TimeDelta = imuReadings.Timestamp - LastFusionTime;
                if (TimeDelta > TimeSpan.Zero)
                {
                    CalculatePose(Acceleration, MagneticField, CompassAdjDeclination);
                    Predict();
                    Update();
                    StateQ.ToEuler(out FusionPose);
                    FusionQPose = StateQ;
                }

                LastFusionTime = imuReadings.Timestamp;
            }

            imuReadings.FusionPoseValid  = true;
            imuReadings.FusionQPoseValid = true;
            imuReadings.FusionPose       = FusionPose;
            imuReadings.FusionQPose      = FusionQPose;
        }
コード例 #4
0
ファイル: SensorFusionRTQF.cs プロジェクト: hmrten/idun
        private void Update()
        {
            if (!EnableMagneticField && !EnableAcceleration)
            {
                return;
            }

            // calculate rotation delta

            Quaternion rotationDelta = StateQ.Conjugate() * MeasuredQPose;

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

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

            rotationUnitVector.Normalize();

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

            rotationPower.Normalize();

            //  multiple this by predicted value to get result

            StateQ *= rotationPower;
            StateQ.Normalize();
        }