First and second task almost done.

TODO: Likelihood function (right now it's just my own invention, but it should use Gaussian). Plus debugging and adding navigateToWaypoint.