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