void calculatePosition(SpatialEventData data, double timeChangeSeconds) { Vector3 accelForcesBody = new Vector3(data.Acceleration[0], data.Acceleration[2], -data.Acceleration[1]); Vector3 accelForcesRef = Matrix3x3.Multiply(accelForcesBody, p.rotMatrix); Vector3 accelForcesRefWithoutGravity = Vector3.Subtract(accelForcesRef, gravityRef); //convert from g's to m/s^2 - also, X is backwards accelForcesRefWithoutGravity.X = accelForcesRefWithoutGravity.X * g; accelForcesRefWithoutGravity.Y = -accelForcesRefWithoutGravity.Y * g; accelForcesRefWithoutGravity.Z = accelForcesRefWithoutGravity.Z * g; //Integrate accelerations into velocities in m/s: v2 = v1 + at velocities.X += timeChangeSeconds * accelForcesRefWithoutGravity.X; velocities.Y += timeChangeSeconds * accelForcesRefWithoutGravity.Y; velocities.Z += timeChangeSeconds * accelForcesRefWithoutGravity.Z; //Integrate velocities into positions in m: s2 = s1 + v2t positions.X += timeChangeSeconds * velocities.X; positions.Y += timeChangeSeconds * velocities.Y; positions.Z += timeChangeSeconds * velocities.Z; TimeSpan passed = DateTime.Now.Subtract(timer); //print every 50 ms if (passed.TotalMilliseconds > milliseconds + 50) { milliseconds = passed.TotalMilliseconds; xVelTxt.Text = velocities.X.ToString("F4"); yVelTxt.Text = velocities.Y.ToString("F4"); zVelTxt.Text = velocities.Z.ToString("F4"); xPosnTxt.Text = positions.X.ToString("F4"); yPosnTxt.Text = positions.Y.ToString("F4"); zPosnTxt.Text = positions.Z.ToString("F4"); xAccelTxt.Text = accelForcesRefWithoutGravity.X.ToString("F4"); yAccelTxt.Text = accelForcesRefWithoutGravity.Y.ToString("F4"); zAccelTxt.Text = accelForcesRefWithoutGravity.Z.ToString("F4"); totVelTxt.Text = Vector3.Length(velocities).ToString("F4"); totPosnTxt.Text = Vector3.Length(positions).ToString("F4"); totAccelTxt.Text = Vector3.Length(accelForcesRefWithoutGravity).ToString("F4"); timeTxt.Text = "" + passed.Minutes.ToString().PadLeft(2) + ":" + passed.Seconds.ToString().PadLeft(2) + "." + passed.Milliseconds.ToString().PadLeft(2); } }
void calculateAttitude(SpatialEventData data, double timeChangeSeconds) { Vector3 rots = new Vector3(0, 0, 0); foreach(double angRate in data.AngularRate) if ((angRate >= 400) || (angRate <= -400)) { Pipeline.fillpen = Color.Red; overRotCount++; overRotsTxt.Text = overRotCount.ToString(); } rots.X = -(timeChangeSeconds * data.AngularRate[0] * Math.PI / 180); rots.Y = -(timeChangeSeconds * data.AngularRate[2] * Math.PI / 180); rots.Z = (timeChangeSeconds * data.AngularRate[1] * Math.PI / 180); Matrix3x3 nextRotMatrix = Math3D.nextRotMatrix2(p.rotMatrix, rots); TimeSpan passed = DateTime.Now.Subtract(timer); //accumulate magnetic data if (data.MagneticField.Length > 0) { magTemp.X += data.MagneticField[0]; magTemp.Y += data.MagneticField[2]; magTemp.Z -= data.MagneticField[1]; magSamplesTaken++; //factor into rotations at some interval if (passed.TotalMilliseconds > milliseconds2 + 100) { milliseconds2 = passed.TotalMilliseconds; //convert vector in reference frame to body frame Vector3 expectedMag = Matrix3x3.Multiply(magRef, Matrix3x3.Transpose(nextRotMatrix)); //actual magnetic vector magTemp.X /= magSamplesTaken; magTemp.Y /= magSamplesTaken; magTemp.Z /= magSamplesTaken; magSamplesTaken = 0; if (doMag) { //find the angles between the two magnetic vectors. This gives a rotation matrix Matrix3x3 magRot = Math3D.getRotationMatrix(magTemp, expectedMag); //If these are off, it's because of slight errors - these are no longer Rotation matrices, strictly speaking //The determinant should be 1 double det = Matrix3x3.Determinant(magRot); //This should give an Identity matrix Matrix3x3 I = Matrix3x3.Multiply(Matrix3x3.Transpose(magRot), magRot); Vector3 magTempRot = Matrix3x3.Multiply(magTemp, magRot); Vector3 magDiff = Vector3.Subtract(Vector3.Normalize(magTemp), Vector3.Normalize(expectedMag)); // These should be close to 0 xMagdiffTxt.Text = (magDiff.X * 180.0 / Math.PI).ToString("F4"); yMagdiffTxt.Text = (magDiff.Y * 180.0 / Math.PI).ToString("F4"); zMagdiffTxt.Text = (magDiff.Z * 180.0 / Math.PI).ToString("F4"); totMagDivTxt.Text = (Math.Acos(Vector3.DotProduct(Vector3.Normalize(magTemp), Vector3.Normalize(expectedMag))) * 180.0 / Math.PI).ToString("F4"); //correct the rotation matrix if (compassCorrections.Checked) { nextRotMatrix = Matrix3x3.Normalize(Matrix3x3.Multiply(Matrix3x3.Normalize(nextRotMatrix), Matrix3x3.Normalize(magRot))); } } magTemp.X = 0; magTemp.Y = 0; magTemp.Z = 0; doMag = true; } } p.rotMatrix = nextRotMatrix; }
//This integrates gyro angular rate into heading over time void calculateGyroHeading(SpatialEventData[] data, int index) { double gyro = 0; for (int i = 0; i < data.Length; i++) { gyro = data[i].AngularRate[index]; if (lastMsCountGood[index]) { //calculate heading double timechange = data[i].Timestamp.TotalMilliseconds - lastMsCount[index]; // in ms double timeChangeSeconds = (double)timechange / 1000.0; //if (index == 0) // Console.WriteLine("X Gyro rate: " + gyro.ToString() + " Time: " + timeChangeSeconds.ToString() + ", " + data[i].Timestamp.TotalMilliseconds.ToString()); gyroHeading[index] += timeChangeSeconds * gyro; } lastMsCount[index] = data[i].Timestamp.TotalMilliseconds; lastMsCountGood[index] = true; } }
void calculateAttitude(SpatialEventData data, double timeChangeSeconds) { Vector3 rots = new Vector3(0, 0, 0); foreach (double angRate in data.AngularRate) { if ((angRate >= phid.gyroAxes[0].AngularRateMax) || (angRate <= phid.gyroAxes[0].AngularRateMin)) { Pipeline.fillpen = Color.Red; overRotCount++; overRotsTxt.Text = overRotCount.ToString(); } } rots.X = -(timeChangeSeconds * data.AngularRate[0] * Math.PI / 180); rots.Y = -(timeChangeSeconds * data.AngularRate[2] * Math.PI / 180); rots.Z = (timeChangeSeconds * data.AngularRate[1] * Math.PI / 180); Matrix3x3 nextRotMatrix = Math3D.nextRotMatrix2(p.rotMatrix, rots); TimeSpan passed = DateTime.Now.Subtract(timer); //accumulate magnetic data if (data.MagneticField.Length > 0) { magTemp.X += data.MagneticField[0]; magTemp.Y += data.MagneticField[2]; magTemp.Z -= data.MagneticField[1]; magSamplesTaken++; //factor into rotations at some interval if (passed.TotalMilliseconds > milliseconds2 + 100) { milliseconds2 = passed.TotalMilliseconds; //convert vector in reference frame to body frame Vector3 expectedMag = Matrix3x3.Multiply(magRef, Matrix3x3.Transpose(nextRotMatrix)); //actual magnetic vector magTemp.X /= magSamplesTaken; magTemp.Y /= magSamplesTaken; magTemp.Z /= magSamplesTaken; magSamplesTaken = 0; if (doMag) { //find the angles between the two magnetic vectors. This gives a rotation matrix Matrix3x3 magRot = Math3D.getRotationMatrix(magTemp, expectedMag); //If these are off, it's because of slight errors - these are no longer Rotation matrices, strictly speaking //The determinant should be 1 double det = Matrix3x3.Determinant(magRot); //This should give an Identity matrix Matrix3x3 I = Matrix3x3.Multiply(Matrix3x3.Transpose(magRot), magRot); Vector3 magTempRot = Matrix3x3.Multiply(magTemp, magRot); Vector3 magDiff = Vector3.Subtract(Vector3.Normalize(magTemp), Vector3.Normalize(expectedMag)); // These should be close to 0 xMagdiffTxt.Text = (magDiff.X * 180.0 / Math.PI).ToString("F4"); yMagdiffTxt.Text = (magDiff.Y * 180.0 / Math.PI).ToString("F4"); zMagdiffTxt.Text = (magDiff.Z * 180.0 / Math.PI).ToString("F4"); totMagDivTxt.Text = (Math.Acos(Vector3.DotProduct(Vector3.Normalize(magTemp), Vector3.Normalize(expectedMag))) * 180.0 / Math.PI).ToString("F4"); //correct the rotation matrix if (compassCorrections.Checked) { nextRotMatrix = Matrix3x3.Normalize(Matrix3x3.Multiply(Matrix3x3.Normalize(nextRotMatrix), Matrix3x3.Normalize(magRot))); } } magTemp.X = 0; magTemp.Y = 0; magTemp.Z = 0; doMag = true; } } p.rotMatrix = nextRotMatrix; }