Fixed reading junk data, also cleaned up groundToImage (#202)

* Fixed reading junk data when interpolating postion and velocity

* Fixed ground to image not using height in checks

* Removed old bisection method check in ground to image