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