The algorithm that you are searching for is feasible and clear! I did a lot of algorithms like these ones.
I am a Civil Engineer and Maths Professor, and I used Matlab for creating dozens of professional applications and sophisticated softwares. If you have a Matlab issue you have just chosen the right Freelancer.
I can develop for you any type of advanced Matlab problems, related to image, signal processing, finite elements projects, engineering projects, statistical and probability projects….or other types of data analysis problems.
Don't hesitate to hire me and consider your project done!