Towards Robust Data Association and Feature Modeling for Concurrent Mapping and Localization