implemented function to check diff between current and new angles
testmethod for diff function
fixed median-filter bug