Add smoother for derivation by finite differences

Also improve interface.
Add the feedforward of the IMU position velocity and acceleration.