Changed stuff and fixed bugs - ICP does not work better than initial AR estimate.