public SensorFusion(RiftHeadsetDevice sensor = null)
{
Stage = 0;
RunningTime = 0;
DeltaT = 0.001f;
Gain = 0.05f;
EnableGravity = true;
EnablePrediction = true;
PredictionDT = 0.03f;
PredictionTimeIncrement = 0.001f;
FRawMag = new SensorFilter(10);
FAngV = new SensorFilter(20);
GyroOffset = new Vector3f();
TiltAngleFilter = new SensorFilterBase_float(1000);
EnableYawCorrection = false;
MagCalibrated = false;
MagNumReferences = 0;
MagRefIdx = -1;
MagRefScore = 0;
MotionTrackingEnabled = true;
if (sensor != null)
AttachToSensor(sensor);
MagCalibrationMatrix = new Matrix4f();
MagCalibrationMatrix.SetIdentity();
}
// Constructs quaternion for rotation around the axis by an angle.
public Quatf(Vector3f axis, float angle)
{
Vector3f unitAxis = axis.Normalized();
float sinHalfAngle = (float)System.Math.Sin(angle * (float)(0.5));
w = (float)System.Math.Cos(angle * (float)(0.5));
x = unitAxis.X * sinHalfAngle;
y = unitAxis.Y * sinHalfAngle;
z = unitAxis.Z * sinHalfAngle;
}
// Projects this vector onto a plane defined by a normal vector
public Vector3f ProjectToPlane(Vector3f normal)
{
return this - this.ProjectTo(normal);
}
// Projects this vector onto the argument; in other words,
// A.Project(B) returns projection of vector A onto B.
public Vector3f ProjectTo(Vector3f b)
{
float l2 = b.LengthSq();
Debug.Assert(l2 != 0);
return b * (Dot(b) / l2);
}
// Linearly interpolates from this vector to another.
// Factor should be between 0.0 and 1.0, with 0 giving full value to this.
public Vector3f Lerp(Vector3f b, float f)
{
return this * (1 - f) + b * f;
}
// Entrywise product of two vectors
public Vector3f EntrywiseMultiply(Vector3f b)
{
return new Vector3f(X * b.X, Y * b.Y, Z * b.Z);
}
// Dot product
// Used to calculate angle q between two vectors among other things,
// as (A dot B) = |a||b|cos(q).
public float Dot(Vector3f b)
{
return X * b.X + Y * b.Y + Z * b.Z;
}
public Vector3f GetCalibratedMagValue(Vector3f rawMag)
{
Debug.Assert(HasMagCalibration());
return MagCalibrationMatrix.Transform(rawMag);
}
// Compute cross product, which generates a normal vector.
// Direction vector can be determined by right-hand rule: Pointing indeX finder in
// direction a and middle finger in direction b, thumb will point in a.Cross(b).
public Vector3f Cross(Vector3f b)
{
return new Vector3f(Y * b.Z - Z * b.Y, Z * b.X - X * b.Z, X * b.Y - Y * b.X);
}
// Compute axis and angle from quaternion
public void GetAxisAngle(out Vector3f axis, out float angle)
{
if ( x*x + y*y + z*z > Vector3f.Tolerance * Vector3f.Tolerance ) {
axis = new Vector3f(x, y, z).Normalized();
angle = (float)(2 * System.Math.Acos(w));
}
else
{
axis = new Vector3f(1, 0, 0);
angle= 0;
}
}
private void OnTrackerMessage(RiftInputReport report)
{
const float timeUnit = (1.0f / 1000.0f);
if (SequenceValid) {
uint timestampDelta;
if (report.Timestamp < LastTimestamp)
timestampDelta = (uint)((((int)report.Timestamp) + 0x10000) - (int)LastTimestamp);
else
timestampDelta = (uint)(report.Timestamp - LastTimestamp);
// If we missed a small number of samples, replicate the last sample.
if ((timestampDelta > LastSampleCount) && (timestampDelta <= 254)) {
MessageBodyFrame sensors = new MessageBodyFrame(this);
sensors.TimeDelta = (timestampDelta - LastSampleCount) * timeUnit;
sensors.Acceleration = LastAcceleration;
sensors.RotationRate = LastRotationRate;
sensors.MagneticField = LastMagneticField;
sensors.Temperature = LastTemperature;
Sensor.OnMessage(sensors);
}
} else {
LastAcceleration = new Vector3f();
LastRotationRate = new Vector3f();
LastMagneticField = new Vector3f();
LastTemperature = 0;
SequenceValid = true;
}
LastSampleCount = report.SampleCount;
LastTimestamp = report.Timestamp;
bool convertHMDToSensor = (Coordinates == CoordinateFrame.Sensor) && (HWCoordinates == CoordinateFrame.HMD);
//if (HandlerRef.GetHandler())
{
MessageBodyFrame sensors = new MessageBodyFrame(this);
Byte iterations = report.SampleCount;
if (report.SampleCount > 3) {
iterations = 3;
sensors.TimeDelta = (report.SampleCount - 2) * timeUnit;
} else {
sensors.TimeDelta = timeUnit;
}
for (Byte i = 0; i < iterations; i++) {
sensors.Acceleration = AccelFromBodyFrameUpdate(report, i, convertHMDToSensor);
sensors.RotationRate = EulerFromBodyFrameUpdate(report, i, convertHMDToSensor);
sensors.MagneticField = MagFromBodyFrameUpdate(report, convertHMDToSensor);
sensors.Temperature = report.Temperature * 0.01f;
Sensor.OnMessage(sensors);
// TimeDelta for the last two sample is always fixed.
sensors.TimeDelta = timeUnit;
}
LastAcceleration = sensors.Acceleration;
LastRotationRate = sensors.RotationRate;
LastMagneticField = sensors.MagneticField;
LastTemperature = sensors.Temperature;
}
//else
//{
// UByte i = (report.SampleCount > 3) ? 2 : (report.SampleCount - 1);
// LastAcceleration = AccelFromBodyFrameUpdate(report, i, convertHMDToSensor);
// LastRotationRate = EulerFromBodyFrameUpdate(report, i, convertHMDToSensor);
// LastMagneticField = MagFromBodyFrameUpdate(report, convertHMDToSensor);
// LastTemperature = report.Temperature * 0.01f;
//}
}
//public void SetDelegateMessageHandler(MessageHandler* handler)
//{ pDelegate = handler; }
// Internal handler for messages; bypasses error checking.
private void handleMessage(MessageBodyFrame msg)
{
// Put the sensor readings into convenient local variables
Vector3f gyro = msg.RotationRate;
Vector3f accel = msg.Acceleration;
Vector3f mag = msg.MagneticField;
// Insert current sensor data into filter history
FRawMag.AddElement(mag);
FAngV.AddElement(gyro);
// Apply the calibration parameters to raw mag
Vector3f calMag = MagCalibrated ? GetCalibratedMagValue(FRawMag.Mean()) : FRawMag.Mean();
// Set variables accessible through the class API
DeltaT = msg.TimeDelta;
AngV = gyro;
A = accel;
RawMag = mag;
CalMag = calMag;
// Keep track of time
Stage++;
RunningTime += DeltaT;
// Small preprocessing
Quatf Qinv = Q.Inverted();
Vector3f up = Qinv.Rotate(new Vector3f(0, 1, 0));
Vector3f gyroCorrected = gyro;
// Apply integral term
// All the corrections are stored in the Simultaneous Orthogonal Rotations Angle representation,
// which allows to combine and scale them by just addition and multiplication
if (EnableGravity || EnableYawCorrection)
gyroCorrected -= GyroOffset;
if (EnableGravity) {
const float spikeThreshold = 0.01f;
const float gravityThreshold = 0.1f;
float proportionalGain = 5 * Gain; // Gain parameter should be removed in a future release
float integralGain = 0.0125f;
Vector3f tiltCorrection = SensorFusion_ComputeCorrection(accel, up);
if (Stage > 5) {
// Spike detection
float tiltAngle = up.Angle(accel);
TiltAngleFilter.AddElement(tiltAngle);
if (tiltAngle > TiltAngleFilter.Mean() + spikeThreshold)
proportionalGain = integralGain = 0;
// Acceleration detection
const float gravity = 9.8f;
if (System.Math.Abs(accel.Length() / gravity - 1) > gravityThreshold)
integralGain = 0;
} else // Apply full correction at the startup
{
proportionalGain = 1 / DeltaT;
integralGain = 0;
}
gyroCorrected += (tiltCorrection * proportionalGain);
GyroOffset -= (tiltCorrection * integralGain * DeltaT);
}
if (EnableYawCorrection && MagCalibrated && RunningTime > 2.0f) {
const float maxMagRefDist = 0.1f;
const float maxTiltError = 0.05f;
float proportionalGain = 0.01f;
float integralGain = 0.0005f;
// Update the reference point if needed
if (MagRefIdx < 0 || calMag.Distance(MagRefsInBodyFrame[MagRefIdx]) > maxMagRefDist) {
// Delete a bad point
if (MagRefIdx >= 0 && MagRefScore < 0) {
MagNumReferences--;
MagRefsInBodyFrame[MagRefIdx] = MagRefsInBodyFrame[MagNumReferences];
MagRefsInWorldFrame[MagRefIdx] = MagRefsInWorldFrame[MagNumReferences];
}
// Find a new one
MagRefIdx = -1;
MagRefScore = 1000;
float bestDist = maxMagRefDist;
for (int i = 0; i < MagNumReferences; i++) {
float dist = calMag.Distance(MagRefsInBodyFrame[i]);
if (bestDist > dist) {
bestDist = dist;
MagRefIdx = i;
}
}
// Create one if needed
if (MagRefIdx < 0 && MagNumReferences < MagMaxReferences) {
MagRefIdx = MagNumReferences;
MagRefsInBodyFrame[MagRefIdx] = calMag;
MagRefsInWorldFrame[MagRefIdx] = Q.Rotate(calMag).Normalized();
MagNumReferences++;
}
}
if (MagRefIdx >= 0) {
Vector3f magEstimated = Qinv.Rotate(MagRefsInWorldFrame[MagRefIdx]);
Vector3f magMeasured = calMag.Normalized();
// Correction is computed in the horizontal plane (in the world frame)
Vector3f yawCorrection = SensorFusion_ComputeCorrection(magMeasured.ProjectToPlane(up),
magEstimated.ProjectToPlane(up));
if (System.Math.Abs(up.Dot(magEstimated - magMeasured)) < maxTiltError) {
MagRefScore += 2;
} else // If the vertical angle is wrong, decrease the score
{
MagRefScore -= 1;
proportionalGain = integralGain = 0;
}
gyroCorrected += (yawCorrection * proportionalGain);
GyroOffset -= (yawCorrection * integralGain * DeltaT);
}
}
// Update the orientation quaternion based on the corrected angular velocity vector
float angle = gyroCorrected.Length() * DeltaT;
if (angle > 0.0f)
Q = Q * new Quatf(gyroCorrected, angle);
// The quaternion magnitude may slowly drift due to numerical error,
// so it is periodically normalized.
if (Stage % 500 == 0)
Q.Normalize();
}
// Compute a rotation required to transform "estimated" into "measured"
// Returns an approximation of the goal rotation in the Simultaneous Orthogonal Rotations Angle representation
// (vector direction is the axis of rotation, norm is the angle)
public Vector3f SensorFusion_ComputeCorrection(Vector3f measured, Vector3f estimated)
{
measured.Normalize();
estimated.Normalize();
Vector3f correction = measured.Cross(estimated);
float cosError = measured.Dot(estimated);
// from the def. of cross product, correction.Length() = sin(error)
// therefore sin(error) * sqrt(2 / (1 + cos(error))) = 2 * sin(error / 2) ~= error in [-pi, pi]
// Mathf::Tolerance is used to avoid div by 0 if cos(error) = -1
return correction * (float)System.Math.Sqrt(2 / (1 + cosError + Vector3f.Tolerance));
}
// Resets the current orientation.
public void Reset()
{
Q = new Quatf();
QUncorrected = new Quatf();
Stage = 0;
RunningTime = 0;
MagNumReferences = 0;
MagRefIdx = -1;
GyroOffset = new Vector3f();
}
// Returns the angle from this vector to b, in radians.
public float Angle(Vector3f b)
{
float div = LengthSq() * b.LengthSq();
Debug.Assert(div != 0);
return (float)(System.Math.Acos((this.Dot(b)) / System.Math.Sqrt(div)));
}
// Compare two vectors for equality with tolerance. Returns true if vectors match withing tolerance.
public bool Compare(Vector3f b, float tolerance = Tolerance)
{
return (System.Math.Abs(b.X - X) < tolerance) &&
(System.Math.Abs(b.Y - Y) < tolerance) &&
(System.Math.Abs(b.Z - Z) < tolerance);
}
// Rotate transforms vector in a manner that matches Matrix rotations (counter-clockwise,
// assuming negative direction of the axis). Standard formula: q(t) * V * q(t)^-1.
public Vector3f Rotate(Vector3f v)
{
return ((this * new Quatf(v.X, v.Y, v.Z, 0)) * Inverted()).Imag();
}
// Returns distance between two points represented by vectors.
public float Distance(Vector3f b)
{
return (this - b).Length();
}
public Vector3f Transform(Vector3f v)
{
return new Vector3f(M[0, 0] * v.X + M[0, 1] * v.Y + M[0, 2] * v.Z + M[0, 3],
M[1, 0] * v.X + M[1, 1] * v.Y + M[1, 2] * v.Z + M[1, 3],
M[2, 0] * v.X + M[2, 1] * v.Y + M[2, 2] * v.Z + M[2, 3]);
}
