There are a lot of errors in this set of code, as none of the people involved in this project were professionals in this field. I decided to leave it as is for those who eventually find their way here.

All the code has been converted from MATLAB code used in Yuxuan Xia's lab to Python, and plenty of open source implementations are available. If you want the original Matlab Version, just search for Yuxuan Xia.