Bug fix for current position update in the Z axis.

Current position wasn't updated properly in Z.