Algorithms design for localization and vehicle-to-grid control