kakoc,

I haven't well undestood the alogorithem.

But I would suggest to try it in MATLAB. matrix related operations are much easy to handle there..