Added filtering feature. The sampling rate of the system is so slow, that it is already filtering the sensor data in. Due to this factor, the filter we implemented doesn't show, unless we go super high, in which case we see a very small effect.
Added filter. It works, but it needs some tunning. The lower values are too fast.