Fixed output of estimate to be 1D array from 2D.
Tested and fixed bugs.
Added depth conversion onboard arduino. Added motor timeout safety.