Initial pose creation algorithm