Finished GPS localization + trajectory planning

Need to add magnetometer + collision avoidance.