Geometric motion estimation and control for robotic-assisted beating-heart surgery