I think you took a wrong turn in your approach. > I know at all the time the position (a) of the end tool by the T_base^tool - matrix, and from the image analysis i know the position (b) of the object relative to the camera frame for which i compute the difference as such c = b - a. You don't care about position (a). You care about the position in the center of the camera frame, which you can think of as an *imaginary* end tool. Let's call it point (i). Just like your real end tool, this point will always occupy the same spot in the camera frame (which happens to be the center, in this case). So, given position (b) and position (i), you can compute the distance between the two points. Your state vector will be 6 points -- replace the data for the "real" tool with the data for the "imaginary" tool in your inverse kinematics equation.