First rough implementation with IIR filter and double integration