Change in measuring Difodo execution time

It now includes the time spent in "filterSpeedAndPoseUpdate()" (as it
should)