Smith Predictor

[nerdCopter]

• Cherry-Picked @Quick-Flash code from #756

Smith predictors feed inverse delay into a system in order to reduce delay of a system. Here it is used to help remove delay from filtering with a simple time delay model. Seems to work fairly well. The settings are the strength, delay, and filtering. Filtering adds a pt1 filter over the prediction to help reduce noise. Strength decides how much of the prediction we use, AKA a strength of 50 uses half the prediction. Delay decides how many ms/10 of delay the predictor will try to compensate for, AKA a value of 40 tries to account for 4.0 ms of delay.

For logging set the debug mode to smith_predictor. Debug 0 is the input, debug 1 is the output, debug 2 is the gyro sample it is using for the prediction, and debug 3 is the amount that the predictor changes the input.