Fixed bug where ogmbicp could use z value from 3d position tracking before it...

Fixed bug where ogmbicp could use z value from 3d position tracking before it is initialized. Added option to allow icp to use odometry to guess the next move