modified from https://github.com/fkanehiro/hrpsys-base/blob/master/rtc/TorqueFilter/IIRFilter.h
modified from https://github.com/ishiguroJSK/hrpsys-base/blob/wbms-dev/rtc/TorqueFilter/IIRFilter.h
modified from https://github.com/fkanehiro/hrpsys-base/blob/master/rtc/SequencePlayer/interpolator.h