Ported the biquad filter from firmware.
Not in use yet, but at some point I intend to switch out the filter that is currently used in the pid controller classes.
What issues does this fix?