fixed the integrating error. Measurements were derived from state,
instead of real state