Refactor + state estimation update