Implemented localization algorithm