internal static FilteredTransform Scale(FilteredTransform joint, float scale)
{
FilteredTransform op = new FilteredTransform();
op.Position = joint.Position * scale;
op.Rotation = Quaternion.Slerp(Quaternion.identity, joint.Rotation, scale);
return(op);
}
internal static FilteredTransform Subtract(FilteredTransform joint, FilteredTransform prevJoint)
{
FilteredTransform op = new FilteredTransform();
op.Position = joint.Position - prevJoint.Position;
op.Rotation = joint.Rotation * Quaternion.Inverse(prevJoint.Rotation);
return(op);
}
internal static FilteredTransform Add(FilteredTransform joint, FilteredTransform prevJoint)
{
FilteredTransform op = new FilteredTransform();
op.Position = joint.Position + prevJoint.Position;
op.Rotation = joint.Rotation * prevJoint.Rotation;
return(op);
}
//--------------------------------------------------------------------------------------
// Implementation of a Holt Double Exponential Smoothing filter. The double exponential
// smooths the curve and predicts. There is also noise jitter removal. And maximum
// prediction bounds. The paramaters are commented in the init function.
//--------------------------------------------------------------------------------------
public void UpdateValue(Vector3 position, Quaternion rotation)
{
// Check for divide by zero. Use an epsilon of a 10th of a millimeter
smoothParams.fJitterRadius = Mathf.Max(0.0001f, smoothParams.fJitterRadius);
TRANSFORM_SMOOTH_PARAMETERS smoothingParams;
smoothingParams.fSmoothing = smoothParams.fSmoothing;
smoothingParams.fCorrection = smoothParams.fCorrection;
smoothingParams.fPrediction = smoothParams.fPrediction;
smoothingParams.fJitterRadius = smoothParams.fJitterRadius;
smoothingParams.fMaxDeviationRadius = smoothParams.fMaxDeviationRadius;
FilteredTransform fj = new FilteredTransform(position, rotation);
Update(fj, smoothingParams);
}
public MyDoubleExponentialFilter()
{
filteredTransform = new FilteredTransform();
history = new DoubleExponentialFilterData();
Init();
}
private void Update(FilteredTransform rawJoint, TRANSFORM_SMOOTH_PARAMETERS smoothingParams)
{
FilteredTransform prevFilteredJoint;
FilteredTransform prevRawJoint;
FilteredTransform prevTrend;
FilteredTransform predictedJoint;
FilteredTransform diff;
FilteredTransform trend;
float fDiff;
prevFilteredJoint = history.FilteredJoint;
prevTrend = history.Trend;
prevRawJoint = history.RawJoint;
// initial start values
if (history.FrameCount == 0)
{
filteredTransform = rawJoint;
trend = new FilteredTransform();
history.FrameCount++;
}
else if (history.FrameCount == 1)
{
filteredTransform = FilteredTransform.Scale(FilteredTransform.Add(rawJoint, prevRawJoint), 0.5f);
diff = FilteredTransform.Subtract(filteredTransform, prevFilteredJoint);
trend = FilteredTransform.Add(FilteredTransform.Scale(diff, smoothingParams.fCorrection), FilteredTransform.Scale(prevTrend, 1.0f - smoothingParams.fCorrection));
history.FrameCount++;
}
else
{
// First apply jitter filter
diff = FilteredTransform.Subtract(rawJoint, prevFilteredJoint);
fDiff = diff.Position.magnitude;
if (fDiff <= smoothingParams.fJitterRadius)
{
filteredTransform = FilteredTransform.Add(FilteredTransform.Scale(rawJoint, fDiff / smoothingParams.fJitterRadius),
FilteredTransform.Scale(prevFilteredJoint, 1.0f - fDiff / smoothingParams.fJitterRadius));
}
else
{
filteredTransform = rawJoint;
}
// Now the double exponential smoothing filter
filteredTransform = FilteredTransform.Add(FilteredTransform.Scale(filteredTransform, 1.0f - smoothingParams.fSmoothing),
FilteredTransform.Scale(FilteredTransform.Add(prevFilteredJoint, prevTrend), smoothingParams.fSmoothing));
diff = FilteredTransform.Subtract(filteredTransform, prevFilteredJoint);
trend = FilteredTransform.Add(FilteredTransform.Scale(diff, smoothingParams.fCorrection), FilteredTransform.Scale(prevTrend, 1.0f - smoothingParams.fCorrection));
}
// Predict into the future to reduce latency
predictedJoint = FilteredTransform.Add(filteredTransform, FilteredTransform.Scale(trend, smoothingParams.fPrediction));
// Check that we are not too far away from raw data
diff = FilteredTransform.Subtract(predictedJoint, rawJoint);
fDiff = diff.Position.magnitude;
if (fDiff > smoothingParams.fMaxDeviationRadius)
{
predictedJoint = FilteredTransform.Add(FilteredTransform.Scale(predictedJoint, smoothingParams.fMaxDeviationRadius / fDiff),
FilteredTransform.Scale(rawJoint, 1.0f - smoothingParams.fMaxDeviationRadius / fDiff));
}
// Save the data from this frame
history.RawJoint = rawJoint;
history.FilteredJoint = filteredTransform;
history.Trend = trend;
// Output the data
filteredTransform = predictedJoint;
}
