Reversible Kalman Filter for state estimation with Manifold