first navigation implementation, sensor array plot, lots of changes.