MatLab implementation of autobalanced rotor

I am looking for someone skilled in MatLab, that will find the mistake I made in my MatLab implementation. I have used the attached paper to implement the autobalancing of a rotor. However I can't find the correct solution as shown on page 7 and 8 in the paper. My implementation comes close but not close enough. Especially the part with constant acceleration seems to be wrong.