Kinect.JointPositionsFilter.FilterJoint C# (CSharp) Method

        private Vector3 FilterJoint(Vector3 rawPosition, int bodyIndex, int jointIndex, ref KinectInterop.SmoothParameters smoothingParameters)
        {
            Vector3 filteredPosition;
            Vector3 diffVec;
            Vector3 trend;
            float diffVal;

            Vector3 prevFilteredPosition = history[bodyIndex, jointIndex].filteredPosition;
            Vector3 prevTrend = this.history[bodyIndex, jointIndex].trend;
            Vector3 prevRawPosition = this.history[bodyIndex, jointIndex].rawPosition;
            bool jointIsValid = (rawPosition != Vector3.zero);

            // If joint is invalid, reset the filter
            if (!jointIsValid)
            {
                history[bodyIndex, jointIndex].frameCount = 0;
            }

            // Initial start values
            if (this.history[bodyIndex, jointIndex].frameCount == 0)
            {
                filteredPosition = rawPosition;
                trend = Vector3.zero;
            }
            else if (this.history[bodyIndex, jointIndex].frameCount == 1)
            {
                filteredPosition = (rawPosition + prevRawPosition) * 0.5f;
                diffVec = filteredPosition - prevFilteredPosition;
                trend = (diffVec * smoothingParameters.correction) + (prevTrend * (1.0f - smoothingParameters.correction));
            }
            else
            {
                // First apply jitter filter
                diffVec = rawPosition - prevFilteredPosition;
                diffVal = Math.Abs(diffVec.magnitude);

                if (diffVal <= smoothingParameters.jitterRadius)
                {
                    filteredPosition = (rawPosition * (diffVal / smoothingParameters.jitterRadius)) + (prevFilteredPosition * (1.0f - (diffVal / smoothingParameters.jitterRadius)));
                }
                else
                {
                    filteredPosition = rawPosition;
                }

                // Now the double exponential smoothing filter
                filteredPosition = (filteredPosition * (1.0f - smoothingParameters.smoothing)) + ((prevFilteredPosition + prevTrend) * smoothingParameters.smoothing);

                diffVec = filteredPosition - prevFilteredPosition;
                trend = (diffVec * smoothingParameters.correction) + (prevTrend * (1.0f - smoothingParameters.correction));
            }

            // Predict into the future to reduce latency
            Vector3 predictedPosition = filteredPosition + (trend * smoothingParameters.prediction);

            // Check that we are not too far away from raw data
            diffVec = predictedPosition - rawPosition;
            diffVal = Mathf.Abs(diffVec.magnitude);

            if (diffVal > smoothingParameters.maxDeviationRadius)
            {
                predictedPosition = (predictedPosition * (smoothingParameters.maxDeviationRadius / diffVal)) + (rawPosition * (1.0f - (smoothingParameters.maxDeviationRadius / diffVal)));
            }

            // Save the data from this frame
            history[bodyIndex, jointIndex].rawPosition = rawPosition;
            history[bodyIndex, jointIndex].filteredPosition = filteredPosition;
            history[bodyIndex, jointIndex].trend = trend;
            history[bodyIndex, jointIndex].frameCount++;

            return predictedPosition;
        }