Adding 3D ellipse drawing for 6 DOF belief