Nav v1

Considering the task, it is important for us to reach waypoints near the pick-up
and placement zones reliably. In order to achieve this, we fuse the pose estimate
from both the tracking camera and the GPS measurement unit. The tracking
camera provides a 6DOF pose estimate based on visual inertial odometry. We
have found that this works very reliably for smaller distances but is not suitable
in the presence of moving objects, or at high speeds. Therefore, we consider
providing a higher weight to the GPS estimate. This is especially useful since
we only need to be within a certain error threshold of the waypoint, such that
the blocks are within the FOV of the camera.