Towards Better Navigation: Optimizing Algorithms For Mapping, Localization And Planning