Added code for self-localization improvement