Fixed kinematic update code