Fixed the position equations. Should work as expected now.