Worked on the iterative algorithm + numerical debugging. Also worked on incorporating the code into the mean shifting framework. Warning: not done yet.