Draft: Added RANSAC estimation of ground plane for compensation of non-planar robot movement