I changed scales for plotting and went on with polishing the code for automatic navigation