哪位大神有下面这个算法的代码

MATLAB的，类似的也可以，目的一样就可以