Innovation is now saturated at max value because quaternion control is unstable for large steps.
Observation jacobian for landmarks that are too close are ignored.