Little change to advance branch.
Added missing orientation to the pose output.
Merge branch 'develop'