コード例 #1
0
 public static Derivative evaluate(State initial, Vector3 acceleration, float dt, Derivative d)
 {
     State state;
     state.position = initial.position + d.dx * dt;
     state.velocity = initial.velocity + d.dv * dt;
     Derivative output;
     output.dx = state.velocity;
     output.dv = acceleration;
     return output;
 }
コード例 #2
0
        public static State Integrate(State oldState, Vector3 accel, float timeDT)
        {
            Derivative orig;
            orig.dx = Vector3.Zero;
            orig.dv = Vector3.Zero;
            Derivative a = evaluate(oldState, accel, 0, orig);
            Derivative b = evaluate(oldState, accel, timeDT * 0.5f, a);
            Derivative c = evaluate(oldState, accel, timeDT * 0.5f, b);
            Derivative d = evaluate(oldState, accel, timeDT, c);

            Vector3 dxdt = 1.0f / 6.0f * (a.dx + 2.0f * (b.dx + c.dx) + d.dx);
            Vector3 dvdt = 1.0f / 6.0f * (a.dv + 2.0f * (b.dv + c.dv) + d.dv);

            State newState;
            newState.position = oldState.position + dxdt * timeDT;
            newState.velocity = oldState.velocity + dvdt * timeDT;
            return newState;
        }
コード例 #3
0
        void OnMicroControllerDataReceived(object sender, EventArgs e)
        {
            string[] data = microcontroller.ReadLine().Split(',', '\n', '\r');
            if (data.Length == 8)
            {

                double elapsedTime = .03;
                if(data[0] != string.Empty)
                    elapsedTime = double.Parse(data[0])/1000.0;

                double[] rawValues = new double[sensorData.Length];
                for (int i = 0; i < sensorData.Length; i++)
                {
                    rawValues[i] = double.Parse(data[i + 1]);
                }

                rawValues[5] += rotationMatrix.Up.X;
                rawValues[3] += rotationMatrix.Up.Y;
                rawValues[4] += rotationMatrix.Up.Z;

                for (int i = 0; i < sensorData.Length; i++)
                {
                    double observedData = (rawValues[i] - sensorMean[i]) * sensorCalibScales[i];
                    sensorData[i] = KalmanFilter(observedData * elapsedTime, ref sensorPredictData[i], ref sensorPredictVar[i], sensorVar[i], 0.96);
                }

                /*
                 *  Sensor data in form of:
                 *  s0-aX   s1-aY   s2-aZ
                 *  s3-gz   s4-gx   s5-gy
                 */
                //AZ=CX AY=CZ AX=CY GZ=CRX GX=CRZ GY=-CRY
                cameraARotation.X += (float)sensorData[2];
                cameraARotation.Y += (float)sensorData[0];
                cameraARotation.Z += (float)sensorData[1];

                Vector3 acceleration = Vector3.Zero;// rotationMatrix.Right * (float)sensorData[5] + rotationMatrix.Up * (float)sensorData[3] + rotationMatrix.Forward * (float)sensorData[4];
                //Vector3 acceleration = new Vector3((float)sensorData[5], (float)sensorData[3], (float)sensorData[4]);

                physicsState = PhysicsHelper.Integrate(physicsState, acceleration, (float)elapsedTime);
                Console.WriteLine("Accelerometer (XYZ): {0} {1} {2}", data[6], data[4], data[5]);
                //Console.WriteLine("Accelerometer (XYZ): {0} {1} {2}", rawValues[5], rawValues[3], rawValues[4]);
                Console.WriteLine("Up Vector: {0}", rotationMatrix.Up);
                Console.WriteLine("Rotation Vector: {0}", cameraARotation);
                //Console.WriteLine("Kalman (XYZ): {0} {1} {2}", sensorData[5], sensorData[3], sensorData[4]);
                //Console.WriteLine("Gyro (XYZ): {0} {1} {2}", sensorData[4], sensorData[5], sensorData[3]);
            }
        }