[wip]Working on full slam... need to reverse the order of Omega/Xi update