[fix] Major sensor integrator logic bugfix.

This adds an automated discarding of the first value in every measurement
round, as well as a numerical bugfix, refactor of the integrator code, and
some corresponding unit tests.