Add filter time scale
Correct ki and v

ki is necessary otherwise, using derivative on measurement, the
setpoint change is never registered and nothing happens.

Also, I had buggily not synchronized v and u on entry to the loop,
and consequently the difference was causint madness.