positionbaseddynamicsimplementedinmatlab