Fix typo & remember to filter measurements before sending to PID controller