Projected Newton Euler Method Matlab