I ended up reading these two A.K. Jain papers: 1 and 2
Then I translated and modified the parts of the matlab code here. The code is based on the second paper, but I was able to add some steps from the first paper to enhance my results.
Hope those links help someone!
