diff --git a/CMakeLists.txt b/CMakeLists.txt
index b659c65..a4a3964 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -161,6 +161,14 @@ install(DIRECTORY config/
   DESTINATION share/${PROJECT_NAME}/config
   PATTERN ".git" EXCLUDE)
 
+# Install URDF and meshes
+install(DIRECTORY urdf/
+  DESTINATION share/${PROJECT_NAME}/urdf
+)
+
+install(DIRECTORY meshes/
+  DESTINATION share/${PROJECT_NAME}/meshes
+)
 
 ###################################
 ## ament specific configuration  ##
diff --git a/meshes/ellipse.stl b/meshes/ellipse.stl
new file mode 100644
index 0000000..27b6b34
Binary files /dev/null and b/meshes/ellipse.stl differ
diff --git a/package.xml b/package.xml
index 704991f..d36643a 100644
--- a/package.xml
+++ b/package.xml
@@ -26,6 +26,10 @@
   <depend>tf2_geometry_msgs</depend>
   <depend>nmea_msgs</depend>
 
+  <build_depend>urdf</build_depend>
+
+  <exec_depend>urdf</exec_depend>
+  <exec_depend>xacro</exec_depend>
   <exec_depend>rosidl_default_runtime</exec_depend>
 
   <member_of_group>rosidl_interface_packages</member_of_group>
diff --git a/src/message_wrapper.cpp b/src/message_wrapper.cpp
index d0c0b6d..ec91cc2 100644
--- a/src/message_wrapper.cpp
+++ b/src/message_wrapper.cpp
@@ -1036,7 +1036,7 @@ const nav_msgs::msg::Odometry MessageWrapper::createRosOdoMessage(const sbg_driv
       pose.position.y = first_valid_northing_;
       pose.position.z = first_valid_altitude_;
 
-      fillTransform(odom_init_frame_id_, odom_frame_id_, pose, transform);
+      fillTransform(odom_init_frame_id_, odom_base_frame_id_, pose, transform);
       tf_broadcaster_->sendTransform(transform);
       static_tf_broadcaster_->sendTransform(transform);
     }
@@ -1084,7 +1084,7 @@ const nav_msgs::msg::Odometry MessageWrapper::createRosOdoMessage(const sbg_driv
   if (odom_publish_tf_)
   {
     // Publish odom transformation.
-    fillTransform(odo_ros_msg.header.frame_id, odom_base_frame_id_, odo_ros_msg.pose.pose, transform);
+    fillTransform(odom_base_frame_id_, odo_ros_msg.header.frame_id, odo_ros_msg.pose.pose, transform);
     tf_broadcaster_->sendTransform(transform);
   }
 
diff --git a/urdf/ellipse.urdf b/urdf/ellipse.urdf
new file mode 100644
index 0000000..7b7e057
--- /dev/null
+++ b/urdf/ellipse.urdf
@@ -0,0 +1,10 @@
+<robot name="ellipse">
+  <link name="odom">
+    <visual>
+      <origin xyz="0 0 0" rpy="1.5708 0 -1.5708"/> <!-- Add rotation to align the 3D model with odometry -->
+      <geometry>
+        <mesh filename="package://sbg_driver/meshes/ellipse.stl" scale="0.01 0.01 0.01"/>
+      </geometry>
+    </visual>
+  </link>
+</robot>
\ No newline at end of file