Unity componenent for smoothing out the tracking of a tracked foot

SmoothedFoot.cs

/*
 *
 *  SmoothedFoot.cs	(c) Ryan Sullivan 2017
 *  url: http://smirkingcat.software/RIPMotion/
 *
 *  Takes the real-time foot position and averages it
 *  while near the ground in order to reduce jitter.
 *  
 *  https://stackoverflow.com/a/3760851
 */

using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public class SmoothedFoot : MonoBehaviour {

    // TODO:  These publics are arbitrary and should probably be tested
    [Tooltip("The delta in position allowed before discarding current position as a glitch")]
    public float _jitterThreshold = 0.05f;

    [Tooltip("The minimum height threshold")]
    public float _minHeight = 0.01f;

    [Tooltip("The number of samples to store")]
    public int _sampleSize = 10;

    // The constant that determines the weighting of a new position
    private float _smoothingFactor;

    // Stores position samples
    private Queue<Vector3> _posQueue;

    // The transform of the foot this is attached to
    private Transform _foot;


    private void Start()
    {

        _foot = transform.parent;

        _posQueue = new Queue<Vector3>(_sampleSize);

        // With sample size of 10, the smoothingFactor is 0.9
        _smoothingFactor = 1 - (1 / _sampleSize);

    }


    private void Update()
    {

        Vector3 currPos = _foot.transform.position;

        // Unless this is overwritten, we want to just use the actual position
        Vector3 newPos = currPos;

        // If the smoothed position is under the minimum height threshold, keep smoothing
        if (transform.position.y < _minHeight)
        {

            // Get the average over the positions stored so far
            Vector3 avgPos = Average(_posQueue);

            // If it looks like we glitched, use the average instead
            if (Mathf.Abs(currPos.x - avgPos.x) > _jitterThreshold
                || Mathf.Abs(currPos.y - avgPos.y) > _jitterThreshold
                || Mathf.Abs(currPos.z - avgPos.z) > _jitterThreshold)
            {

                newPos = avgPos;

            }

            // With smoothingFactor of 0.9, the weighting of new position against old is a 9:1 ratio
            newPos = (newPos * _smoothingFactor) + (transform.position * (1.0f - _smoothingFactor));

            // Populate the queue with the smoothed position
            _posQueue.Enqueue(newPos);

        }
        else
        {

            // Empty the sample queue so we start fresh next time we are under the threshold
            _posQueue.Clear();

        }

        transform.position = newPos;

    }



    /// <summary>
    /// Gets the average position from the sum of all of the positions in the sample queue
    /// </summary>
    /// <param name="positions">A sample queue of positions</param>
    /// <returns>The average position</returns>
    private Vector3 Average(Queue<Vector3> positions)
    {

        Vector3 average = Vector3.zero;

        Vector3[] posArray = positions.ToArray();

        foreach (Vector3 pos in posArray)
        {

            average += pos;

        }
        
        average /= posArray.Length;

        return average;

    }

}