Re-enable low-pass filtering of velocity/rate outputs

[tblaha]

Velocity/rotation rate outputs are currently computed using backward differences between 2 successive mocap samples (often at 180Hz or faster). This is very sensitive to noise. These values are then sent to the agents, usually at much lower frequencies, which makes filtering on the agents' incur much higher phase delay.

We used to have an option to enable an IIR low-pass filter that runs in UMR at mocap update frequency. This would make the outputs much smoother without high phase delay.

The only thing we have to do to enable this again is somehow observe the sample rate of the incoming mocap data, and provide commandline options to configure the filter cutoff.

[ntouev]

Tried with this->poseDiff = FilteredPoseDifferentiator(10, 10, 180); instead of this->poseDiff = PoseDifferentiator(); in rigid_body.cpp. I also changed _kVel to _KRate in pose_calculations.cpp. Cutoff frequency = 10 Hz and Motive frequency = 180 Hz.

Velocity feedback is smooth and has negligible delay. In the figure below I compare the reported filtered velocity with the unfiltered one (computed numerically from position feedback).