Fixed right encoder velocity reading. It gave the wrong sign of the velocity.
Added encoder velocity buffer and median filter of velocity readings.
Added getter for wheel angular velocities.