Localization Complete isn probably maybe lol idk

Localization complete except for filtering outliers. Not yet debugged.