Fixed bug in DiscreteFilteredPidController