diff --git a/deploy/ros2_nodes/go2_description/xacro/robot.xacro b/deploy/ros2_nodes/go2_description/xacro/robot.xacro
index 7df040a..58e8c5e 100755
--- a/deploy/ros2_nodes/go2_description/xacro/robot.xacro
+++ b/deploy/ros2_nodes/go2_description/xacro/robot.xacro
@@ -68,7 +68,7 @@
     <joint name="imu_joint" type="fixed">
         <parent link="trunk"/>
         <child link="imu_link"/>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <origin rpy="0.001697 -0.020900 -0.000007" xyz="-0.027353 -0.005499 0.039913"/>
     </joint>
 
     <link name="imu_link">
@@ -91,6 +91,72 @@
             </geometry>
         </collision>
     </link>
+
+    <joint name="vicon_joint" type="fixed">
+        <parent link="trunk"/>
+        <child link="vicon_link"/>
+        <origin rpy="-0.037935 -0.066371 -2.668897" xyz="0.022944 -0.000007 0.058458"/>
+    </joint>
+
+    <link name="vicon_link">
+        <visual>
+            <origin rpy="0 0 0" xyz="0 0 0"/>
+            <geometry>
+                <box size="0.001 0.001 0.001"/>
+            </geometry>
+            <!-- <material name="red"/> -->
+        </visual>
+    </link>
+
+
+    <joint name="infra1_joint" type="fixed">
+        <parent link="trunk"/>
+        <child link="infra1_link"/>
+        <origin rpy="-1.641669 -0.014332 -1.617155" xyz="0.321618 0.033305 0.081622"/>
+    </joint>
+
+    <link name="infra1_link">
+        <visual>
+            <origin rpy="0 0 0" xyz="0 0 0"/>
+            <geometry>
+                <box size="0.001 0.001 0.001"/>
+            </geometry>
+            <!-- <material name="red"/> -->
+        </visual>
+    </link>
+
+
+    <joint name="infra2_joint" type="fixed">
+        <parent link="trunk"/>
+        <child link="infra1_link"/>
+        <origin rpy="-1.640637 -0.014093 -1.611177" xyz="0.317969 -0.061919 0.082773"/>
+    </joint>
+
+    <link name="infra2_link">
+        <visual>
+            <origin rpy="0 0 0" xyz="0 0 0"/>
+            <geometry>
+                <box size="0.001 0.001 0.001"/>
+            </geometry>
+            <!-- <material name="red"/> -->
+        </visual>
+    </link>
+
+    <joint name="color_joint" type="fixed">
+        <parent link="trunk"/>
+        <child link="color_link"/>
+        <origin rpy="-1.640491 -0.011125 -1.610364" xyz="0.319415 -0.025790 0.082311"/>
+    </joint>
+
+    <link name="color_link">
+        <visual>
+            <origin rpy="0 0 0" xyz="0 0 0"/>
+            <geometry>
+                <box size="0.001 0.001 0.001"/>
+            </geometry>
+            <!-- <material name="red"/> -->
+        </visual>
+    </link>
     
     <xacro:leg name="FR" mirror="-1" mirror_dae= "False" front_hind="1"  front_hind_dae="True"  />
     <xacro:leg name="FL" mirror="1"  mirror_dae="True"   front_hind="1"  front_hind_dae="True"  />
diff --git a/examples/calibration_post_processing.ipynb b/examples/calibration_post_processing.ipynb
index 2e71488..230fd0c 100644
--- a/examples/calibration_post_processing.ipynb
+++ b/examples/calibration_post_processing.ipynb
@@ -16,13 +16,13 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 10,
+   "execution_count": 35,
    "metadata": {},
    "outputs": [],
    "source": [
     "from Go2Py.calibration import *\n",
     "import numpy as np\n",
-    "np.set_printoptions(formatter={'float': lambda x: \"{0:0.3f}\".format(x)})"
+    "np.set_printoptions(formatter={'float': lambda x: \"{0:0.6f}\".format(x)})"
    ]
   },
   {
@@ -537,7 +537,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 46,
+   "execution_count": 1,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -548,7 +548,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 47,
+   "execution_count": 4,
    "metadata": {},
    "outputs": [
     {
@@ -580,7 +580,7 @@
        "        [0.000, 0.000, 0.000, 1.000]])}"
       ]
      },
-     "execution_count": 47,
+     "execution_count": 4,
      "metadata": {},
      "output_type": "execute_result"
     }
@@ -632,6 +632,30 @@
     "result['ext_wrt_imu']"
    ]
   },
+  {
+   "cell_type": "code",
+   "execution_count": 41,
+   "metadata": {},
+   "outputs": [
+    {
+     "data": {
+      "text/plain": [
+       "(array([0.319415, -0.025790, 0.082311]),\n",
+       " array([-1.640491, -0.011125, -1.610364]))"
+      ]
+     },
+     "execution_count": 41,
+     "metadata": {},
+     "output_type": "execute_result"
+    }
+   ],
+   "source": [
+    "import pinocchio as pin\n",
+    "T = result['ext_wrt_imu']['color_wrt_base']\n",
+    "from scipy.spatial.transform import Rotation \n",
+    "T[0:3,-1], Rotation.from_matrix(T[0:3,0:3]).as_euler(seq='xyz')"
+   ]
+  },
   {
    "cell_type": "markdown",
    "metadata": {},