Localization added

+Added Localization algorithm
-Commented out broken function till a fix is made.