Fix objective constraint

Fix algorithm based on MATLAB demo
(http://www.mathworks.com/matlabcentral/fileexchange/downloads/498137/download)

Optimizing some code
Add accurate test

Add gamma test

Add p test

Add benchmark