// Initialize the filter with a set of TransformSmoothParameters.
public void Init(SmoothParameters smoothingParameters)
{
this.smoothParameters = smoothingParameters;
this.Reset();
this.init = true;
}
/// <summary>
/// Initialize the filter with a set of manually specified TransformSmoothParameters.
/// </summary>
/// <param name="smoothingValue">Smoothing = [0..1], lower values is closer to the raw data and more noisy.</param>
/// <param name="correctionValue">Correction = [0..1], higher values correct faster and feel more responsive.</param>
/// <param name="predictionValue">Prediction = [0..n], how many frames into the future we want to predict.</param>
/// <param name="jitterRadiusValue">JitterRadius = The deviation distance in m that defines jitter.</param>
/// <param name="maxDeviationRadiusValue">MaxDeviation = The maximum distance in m that filtered positions are allowed to deviate from raw data.</param>
public void Init(float smoothingValue, float correctionValue, float predictionValue, float jitterRadiusValue, float maxDeviationRadiusValue)
{
this.smoothParameters = new SmoothParameters();
this.smoothParameters.fSmoothing = smoothingValue; // How much soothing will occur. Will lag when too high
this.smoothParameters.fCorrection = correctionValue; // How much to correct back from prediction. Can make things springy
this.smoothParameters.fPrediction = predictionValue; // Amount of prediction into the future to use. Can over shoot when too high
this.smoothParameters.fJitterRadius = jitterRadiusValue; // Size of the radius where jitter is removed. Can do too much smoothing when too high
this.smoothParameters.fMaxDeviationRadius = maxDeviationRadiusValue; // Size of the max prediction radius Can snap back to noisy data when too high
this.Reset();
this.init = true;
}
// Update the filter for one set of values.
protected void FilterValues(ref float fValue, ref SmoothParameters smoothingParameters)
{
float filteredState;
float trend;
float diffVal;
float rawState = fValue;
float prevFilteredState = history[0].FilteredState;
float prevTrend = history[0].Trend;
float prevRawState = history[0].RawState;
// If value is invalid, reset the filter
if (rawState < 0f)
{
history[0].FrameCount = 0;
}
// Initial start values
if (history[0].FrameCount == 0)
{
filteredState = rawState;
trend = 0f;
}
else if (this.history[0].FrameCount == 1)
{
filteredState = (rawState + prevRawState) * 0.5f;
diffVal = filteredState - prevFilteredState;
trend = (diffVal * smoothingParameters.fCorrection) + (prevTrend * (1.0f - smoothingParameters.fCorrection));
}
else
{
// // First apply jitter filter
// diffVal = rawState - prevFilteredState;
//
// if (diffVal <= smoothingParameters.fJitterRadius)
// {
// filteredState = (rawState * (diffVal / smoothingParameters.fJitterRadius)) + (prevFilteredState * (1.0f - (diffVal / smoothingParameters.fJitterRadius)));
// }
// else
// {
// filteredState = rawState;
// }
filteredState = rawState;
// Now the double exponential smoothing filter
filteredState = (filteredState * (1.0f - smoothingParameters.fSmoothing)) + ((prevFilteredState + prevTrend) * smoothingParameters.fSmoothing);
diffVal = filteredState - prevFilteredState;
trend = (diffVal * smoothingParameters.fCorrection) + (prevTrend * (1.0f - smoothingParameters.fCorrection));
}
// Predict into the future to reduce latency
float predictedState = filteredState + (trend * smoothingParameters.fPrediction);
// Check that we are not too far away from raw data
diffVal = predictedState - rawState;
if (diffVal > smoothingParameters.fMaxDeviationRadius)
{
predictedState = (predictedState * (smoothingParameters.fMaxDeviationRadius / diffVal)) + (rawState * (1.0f - (smoothingParameters.fMaxDeviationRadius / diffVal)));
}
// Save the data from this frame
history[0].RawState = rawState;
history[0].FilteredState = filteredState;
history[0].Trend = trend;
history[0].FrameCount++;
// Set the filtered data back into the value
fValue = predictedState;
}
