I am attaching a function created by Dr. Gaolang Gong some time ago for matlab which does what you are trying to accomplish. Perhaps it will work out of the box for you or it may give you some insight on how to tackle this problem.<div>

<br></div><div>Good luck.</div><div><br></div><div>Luis</div>