Hi, I've worked as a MATLAB tutor for years and have a bachelor's and master's in engineering, in which we have to use MATLAB extensively. I would be happy to write this algorithm in MATLAB for you. I am adept at working with symbolics, differential equations, Simulink, etc.
Thanks,
Caleb