private List<Vector3> positions = new List<Vector3>();
private bool recordPosition = false;    //Records position when true
private int valuesRecorded = 0; //Count of recorded values
private bool firstValue = true; //true if it's the first recorded value
private bool deleteLastRecorded = false;
private float cleaningDiffTolerance = 0.2f; //Default 0.2f = 20cm
private float cleaningAngleMaxDiff = 5.0f;  //Default 5.0f = 5°

void Update(){
if (recordPosition) {
        if (deleteLastRecorded) {   //Check if the last recorded has been cleaned and delete the List element
            positions.RemoveAt (valuesRecorded - 1);
            if (showDebug) {
                Destroy (debugPoint);
                debugPoints.RemoveAt (valuesRecorded);
            }
        }
        deleteLastRecorded = false;

        Vector3 tempVect = trackableObject.position;    //Record position

        positions.Add (tempVect);   //Add value to list

        if (showDebug) {    //DEBUG
            if (!showOnlyCleaned) { //show original values
                debugPointOriginal = Instantiate (originalCube, tempVect, Quaternion.Euler (0, 0, 0), originalParentPoints.transform) as GameObject;
                debugPointsOriginal.Add (debugPointOriginal.transform);
            }
            originalCount++;
            //show cleaned values
            debugPoint = Instantiate (cleanedCube, tempVect, Quaternion.Euler (0, 0, 0), cleanedParentPoints.transform) as GameObject;
            debugPoints.Add (debugPoint.transform);
        }
        if (firstValue) {   //Remove first recorded value
            if (valuesRecorded == 0) {
                positions.RemoveAt (valuesRecorded);
                if (showDebug) {
                    Destroy (debugPoint);
                    debugPoints.RemoveAt (valuesRecorded);
                }
                valuesRecorded--;
                firstValue = false;
            }
        }
        if (valuesRecorded > 1) {   //CLEANING
            Vector3 valueLast = positions [valuesRecorded - 2];
            Vector3 valueCurrent = positions [valuesRecorded - 1];
            Vector3 valueNext = positions [valuesRecorded];

            float diffCurrLast = Vector3.Distance (valueCurrent, valueLast);    //Distance between CURRENT and LAST value
            float diffCurrNext = Vector3.Distance (valueCurrent, valueNext);    //Distance between CURRENT and NEXT value
            Vector3 lastDirection = valueLast - valueCurrent;   //Direction from LAST to CURRENT position
            Vector3 nextDirection = valueCurrent - valueNext;   //Direction from CURRENT to NEXT position
            float diffAngle = 0;

            if (diffCurrLast < cleaningDiffTolerance) { //Check Distance between CURRENT and LAST value
                diffAngle = Vector3.Angle (lastDirection, nextDirection);   //Get Diff Angle
                if (diffAngle < cleaningAngleMaxDiff) {
                    deleteLastRecorded = true;
                    valuesRecorded--;   //Reset i for next item
                }
            } else {
                if (diffCurrNext < cleaningDiffTolerance) { //Check Distance between CURRENT and NEXT value
                    diffAngle = Vector3.Angle (lastDirection, nextDirection);   //Get Diff Angle
                    if (diffAngle < cleaningAngleMaxDiff) {
                        deleteLastRecorded = true;
                        valuesRecorded--;   //Reset i for next item
                    }
                }
            }
        }
        valuesRecorded++;
    }
}