Changed system structure so everything is in the same node
Did that because things started getting messy with map manager
Correspondence Estimation is working
Next step: Correspondence Rejection and figuring out how to give that info to state_estimator