Fix algorithm

[pnsaevik]

There is an error in the algorithm which must be fixed. Check the documentation and make sure they agree.