Add Cross Track Error, parameter optimization and visualization
Add SLAM code