changed code to use Pose instead of 3x1 matrix for positions