diff --git a/config/model_car.rviz b/config/model_car.rviz
index fbcc0fdc312734a32b1dcf2a41126d1dd912299c..18ce3cdb68d47301e623c3efd8ea924005b101d3 100644
--- a/config/model_car.rviz
+++ b/config/model_car.rviz
@@ -5,7 +5,10 @@ Panels:
     Property Tree Widget:
       Expanded:
         - /Global Options1
-        - /RobotModel1
+        - /CircuitModels1
+        - /Sensors1
+        - /Sensors1/GroundTruthOdometry1/Shape1
+        - /Sensors1/Odometry1/Shape1
       Splitter Ratio: 0.625
     Tree Height: 841
   - Class: rviz/Selection
@@ -43,27 +46,141 @@ Visualization Manager:
       Name: Grid
       Normal Cell Count: 0
       Offset:
-        X: 0
-        Y: -5
+        X: 0.5
+        Y: 0.5
         Z: 0.00999999978
       Plane: XY
-      Plane Cell Count: 12
+      Plane Cell Count: 20
       Reference Frame: <Fixed Frame>
       Value: true
     - Class: rviz/TF
-      Enabled: false
+      Enabled: true
       Frame Timeout: 15
       Frames:
         All Enabled: true
+        base_footprint:
+          Value: true
+        base_link:
+          Value: true
+        body:
+          Value: true
+        front_camera_uvc_camera_base:
+          Value: true
+        front_camera_uvc_camera_optical:
+          Value: true
+        front_left_wheel:
+          Value: true
+        front_lidar_rplidar_base:
+          Value: true
+        front_lidar_rplidar_scan_frame:
+          Value: true
+        front_right_wheel:
+          Value: true
+        imu_mpu9250:
+          Value: true
+        imu_mpu9250_base:
+          Value: true
+        map:
+          Value: true
+        map_body:
+          Value: true
+        odom:
+          Value: true
+        rear_camera_uvc_camera_base:
+          Value: true
+        rear_camera_uvc_camera_optical:
+          Value: true
+        rear_center_base:
+          Value: true
+        rear_center_ranger1d:
+          Value: true
+        rear_left_base:
+          Value: true
+        rear_left_ranger1d:
+          Value: true
+        rear_left_wheel:
+          Value: true
+        rear_right_base:
+          Value: true
+        rear_right_ranger1d:
+          Value: true
+        rear_right_wheel:
+          Value: true
+        road0_image:
+          Value: true
+        road0_link:
+          Value: true
+        side_left_base:
+          Value: true
+        side_left_ranger1d:
+          Value: true
+        side_right_base:
+          Value: true
+        side_right_ranger1d:
+          Value: true
+        steer_left:
+          Value: true
+        steer_right:
+          Value: true
+        world:
+          Value: true
       Marker Scale: 0.25
       Name: TF
       Show Arrows: false
       Show Axes: true
       Show Names: true
       Tree:
-        {}
+        world:
+          map:
+            map_body:
+              {}
+            odom:
+              base_footprint:
+                base_link:
+                  body:
+                    {}
+                  front_camera_uvc_camera_base:
+                    front_camera_uvc_camera_optical:
+                      {}
+                  front_lidar_rplidar_base:
+                    front_lidar_rplidar_scan_frame:
+                      {}
+                  imu_mpu9250_base:
+                    imu_mpu9250:
+                      {}
+                  rear_camera_uvc_camera_base:
+                    rear_camera_uvc_camera_optical:
+                      {}
+                  rear_center_base:
+                    rear_center_ranger1d:
+                      {}
+                  rear_left_base:
+                    rear_left_ranger1d:
+                      {}
+                  rear_left_wheel:
+                    {}
+                  rear_right_base:
+                    rear_right_ranger1d:
+                      {}
+                  rear_right_wheel:
+                    {}
+                  side_left_base:
+                    side_left_ranger1d:
+                      {}
+                  side_right_base:
+                    side_right_ranger1d:
+                      {}
+                  steer_left:
+                    front_left_wheel:
+                      {}
+                  steer_right:
+                    front_right_wheel:
+                      {}
+            road0_link:
+              road0_image:
+                {}
       Update Interval: 0
-      Value: false
+      Value: true
     - Alpha: 1
       Class: rviz/RobotModel
       Collision Enabled: false
@@ -74,6 +191,10 @@ Visualization Manager:
         Expand Link Details: false
         Expand Tree: false
         Link Tree Style: Links in Alphabetic Order
+        base_footprint:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
         base_link:
           Alpha: 1
           Show Axes: false
@@ -200,7 +321,7 @@ Visualization Manager:
       Visual Enabled: true
     - Class: rviz/Group
       Displays:
-        - Alpha: 1
+        - Alpha: 0.899999976
           Class: rviz/RobotModel
           Collision Enabled: false
           Enabled: true
@@ -210,260 +331,156 @@ Visualization Manager:
             Expand Link Details: false
             Expand Tree: false
             Link Tree Style: Links in Alphabetic Order
+            map:
+              Alpha: 1
+              Show Axes: false
+              Show Trail: false
+            map_body:
+              Alpha: 1
+              Show Axes: false
+              Show Trail: false
+              Value: true
+            road0_image:
+              Alpha: 1
+              Show Axes: false
+              Show Trail: false
+              Value: true
+            road0_link:
+              Alpha: 1
+              Show Axes: false
+              Show Trail: false
           Name: RoadModel
           Robot Description: road_description
           TF Prefix: ""
           Update Interval: 0
           Value: true
           Visual Enabled: true
-        - Alpha: 0.899999976
-          Class: rviz/RobotModel
-          Collision Enabled: false
-          Enabled: true
-          Links:
-            All Links Enabled: true
-            Expand Joint Details: false
-            Expand Link Details: false
-            Expand Tree: false
-            Link Tree Style: Links in Alphabetic Order
-          Name: PerimeterModel
-          Robot Description: perimeter_description
-          TF Prefix: ""
-          Update Interval: 0
-          Value: true
-          Visual Enabled: true
-        - Alpha: 0.899999976
-          Class: rviz/RobotModel
-          Collision Enabled: false
-          Enabled: true
-          Links:
-            All Links Enabled: true
-            Expand Joint Details: false
-            Expand Link Details: false
-            Expand Tree: false
-            Link Tree Style: Links in Alphabetic Order
-          Name: CeilingModel
-          Robot Description: ceiling_description
-          TF Prefix: ""
-          Update Interval: 0
-          Value: true
-          Visual Enabled: true
-        - Alpha: 0.899999976
-          Class: rviz/RobotModel
-          Collision Enabled: false
-          Enabled: true
-          Links:
-            All Links Enabled: true
-            Expand Joint Details: false
-            Expand Link Details: false
-            Expand Tree: false
-            Link Tree Style: Links in Alphabetic Order
-          Name: TrafficlightsModel
-          Robot Description: trafficlights_fixed_description
-          TF Prefix: ""
-          Update Interval: 0
-          Value: true
-          Visual Enabled: true
-        - Alpha: 0.899999976
-          Class: rviz/RobotModel
-          Collision Enabled: false
-          Enabled: true
-          Links:
-            All Links Enabled: true
-            Expand Joint Details: false
-            Expand Link Details: false
-            Expand Tree: false
-            Link Tree Style: Links in Alphabetic Order
-          Name: Trafficlights2Model
-          Robot Description: trafficlights_extra_example_description
-          TF Prefix: ""
-          Update Interval: 0
-          Value: true
-          Visual Enabled: true
-        - Alpha: 0.899999976
-          Class: rviz/RobotModel
-          Collision Enabled: false
-          Enabled: true
-          Links:
-            All Links Enabled: true
-            Expand Joint Details: false
-            Expand Link Details: false
-            Expand Tree: false
-            Link Tree Style: Links in Alphabetic Order
-          Name: BuildingsModel
-          Robot Description: buildings_description
-          TF Prefix: ""
-          Update Interval: 0
-          Value: true
-          Visual Enabled: true
-        - Alpha: 0.899999976
-          Class: rviz/RobotModel
-          Collision Enabled: false
-          Enabled: true
-          Links:
-            All Links Enabled: true
-            Expand Joint Details: false
-            Expand Link Details: false
-            Expand Tree: false
-            Link Tree Style: Links in Alphabetic Order
-          Name: Obstacle1Model
-          Robot Description: obstacle1_description
-          TF Prefix: obstacle1
-          Update Interval: 0
-          Value: true
-          Visual Enabled: true
-        - Alpha: 1
-          Class: rviz/RobotModel
-          Collision Enabled: false
-          Enabled: true
-          Links:
-            All Links Enabled: true
-            Expand Joint Details: false
-            Expand Link Details: false
-            Expand Tree: false
-            Link Tree Style: Links in Alphabetic Order
-          Name: Obstacle2Model
-          Robot Description: obstacle2_description
-          TF Prefix: obstacle2
-          Update Interval: 0
-          Value: true
-          Visual Enabled: true
-        - Alpha: 1
-          Class: rviz/RobotModel
-          Collision Enabled: false
-          Enabled: true
-          Links:
-            All Links Enabled: true
-            Expand Joint Details: false
-            Expand Link Details: false
-            Expand Tree: false
-            Link Tree Style: Links in Alphabetic Order
-          Name: Obstacle3aModel
-          Robot Description: obstacle3a_description
-          TF Prefix: obstacle3a
-          Update Interval: 0
-          Value: true
-          Visual Enabled: true
-        - Alpha: 1
-          Class: rviz/RobotModel
-          Collision Enabled: false
-          Enabled: true
-          Links:
-            All Links Enabled: true
-            Expand Joint Details: false
-            Expand Link Details: false
-            Expand Tree: false
-            Link Tree Style: Links in Alphabetic Order
-          Name: Obstacle3bModel
-          Robot Description: obstacle3b_description
-          TF Prefix: obstacle3b
-          Update Interval: 0
-          Value: true
-          Visual Enabled: true
-        - Alpha: 1
-          Class: rviz/RobotModel
-          Collision Enabled: false
-          Enabled: true
-          Links:
-            All Links Enabled: true
-            Expand Joint Details: false
-            Expand Link Details: false
-            Expand Tree: false
-            Link Tree Style: Links in Alphabetic Order
-          Name: Obstacle3cModel
-          Robot Description: obstacle3c_description
-          TF Prefix: obstacle3c
-          Update Interval: 0
-          Value: true
-          Visual Enabled: true
       Enabled: true
       Name: CircuitModels
-    - Angle Tolerance: 0.0500000007
-      Class: rviz/Odometry
-      Covariance:
-        Orientation:
-          Alpha: 0.5
-          Color: 255; 255; 127
-          Color Style: Unique
-          Frame: Local
-          Offset: 1
-          Scale: 1
-          Value: true
-        Position:
-          Alpha: 0.300000012
-          Color: 204; 51; 204
-          Scale: 1
-          Value: true
-        Value: true
-      Enabled: true
-      Keep: 100
-      Name: Odometry
-      Position Tolerance: 0.200000003
-      Shape:
-        Alpha: 1
-        Axes Length: 1
-        Axes Radius: 0.100000001
-        Color: 255; 25; 0
-        Head Length: 0.200000003
-        Head Radius: 0.100000001
-        Shaft Length: 0.5
-        Shaft Radius: 0.0500000007
-        Value: Arrow
-      Topic: /odom
-      Unreliable: false
-      Value: true
     - Class: rviz/Group
       Displays:
-        - Alpha: 0.5
-          Buffer Length: 1
-          Class: rviz/Range
-          Color: 255; 255; 255
-          Enabled: true
-          Name: side_right_range
-          Queue Size: 100
-          Topic: /sensors/range/side_right_sonar
-          Unreliable: false
-          Value: true
-        - Alpha: 0.5
-          Buffer Length: 1
-          Class: rviz/Range
-          Color: 255; 255; 255
-          Enabled: true
-          Name: side_left_range
-          Queue Size: 100
-          Topic: /sensors/range/side_left_sonar
-          Unreliable: false
-          Value: true
-        - Alpha: 0.5
-          Buffer Length: 1
-          Class: rviz/Range
-          Color: 255; 255; 255
-          Enabled: true
-          Name: rear_right_range
-          Queue Size: 100
-          Topic: /sensors/range/rear_right_sonar
-          Unreliable: false
-          Value: true
-        - Alpha: 0.5
-          Buffer Length: 1
-          Class: rviz/Range
-          Color: 255; 255; 255
+        - Angle Tolerance: 0.5
+          Class: rviz/Odometry
+          Covariance:
+            Orientation:
+              Alpha: 0.5
+              Color: 255; 255; 127
+              Color Style: Unique
+              Frame: Local
+              Offset: 1
+              Scale: 1
+              Value: true
+            Position:
+              Alpha: 0.300000012
+              Color: 204; 51; 204
+              Scale: 1
+              Value: true
+            Value: true
           Enabled: true
-          Name: rear_left_range
-          Queue Size: 100
-          Topic: /sensors/range/rear_left_sonar
+          Keep: 1000
+          Name: GroundTruthOdometry
+          Position Tolerance: 0.5
+          Shape:
+            Alpha: 1
+            Axes Length: 1
+            Axes Radius: 0.100000001
+            Color: 0; 255; 255
+            Head Length: 0.100000001
+            Head Radius: 0.0500000007
+            Shaft Length: 0.400000006
+            Shaft Radius: 0.0250000004
+            Value: Arrow
+          Topic: /odom_ground_truth
           Unreliable: false
           Value: true
-        - Alpha: 0.5
-          Buffer Length: 1
-          Class: rviz/Range
-          Color: 255; 255; 255
+        - Angle Tolerance: 0.5
+          Class: rviz/Odometry
+          Covariance:
+            Orientation:
+              Alpha: 0.5
+              Color: 255; 255; 127
+              Color Style: Unique
+              Frame: Local
+              Offset: 1
+              Scale: 1
+              Value: true
+            Position:
+              Alpha: 0.300000012
+              Color: 204; 51; 204
+              Scale: 1
+              Value: true
+            Value: true
           Enabled: true
-          Name: rear_center_range
-          Queue Size: 100
-          Topic: /sensors/range/rear_center_sonar
+          Keep: 100
+          Name: Odometry
+          Position Tolerance: 0.5
+          Shape:
+            Alpha: 1
+            Axes Length: 1
+            Axes Radius: 0.100000001
+            Color: 255; 25; 0
+            Head Length: 0.100000001
+            Head Radius: 0.0500000007
+            Shaft Length: 0.400000006
+            Shaft Radius: 0.0250000004
+            Value: Arrow
+          Topic: /odom
           Unreliable: false
           Value: true
+        - Class: rviz/Group
+          Displays:
+            - Alpha: 0.5
+              Buffer Length: 1
+              Class: rviz/Range
+              Color: 255; 255; 255
+              Enabled: true
+              Name: side_right_range
+              Queue Size: 100
+              Topic: /sensors/range/side_right_sonar
+              Unreliable: false
+              Value: true
+            - Alpha: 0.5
+              Buffer Length: 1
+              Class: rviz/Range
+              Color: 255; 255; 255
+              Enabled: true
+              Name: side_left_range
+              Queue Size: 100
+              Topic: /sensors/range/side_left_sonar
+              Unreliable: false
+              Value: true
+            - Alpha: 0.5
+              Buffer Length: 1
+              Class: rviz/Range
+              Color: 255; 255; 255
+              Enabled: true
+              Name: rear_right_range
+              Queue Size: 100
+              Topic: /sensors/range/rear_right_sonar
+              Unreliable: false
+              Value: true
+            - Alpha: 0.5
+              Buffer Length: 1
+              Class: rviz/Range
+              Color: 255; 255; 255
+              Enabled: true
+              Name: rear_left_range
+              Queue Size: 100
+              Topic: /sensors/range/rear_left_sonar
+              Unreliable: false
+              Value: true
+            - Alpha: 0.5
+              Buffer Length: 1
+              Class: rviz/Range
+              Color: 255; 255; 255
+              Enabled: true
+              Name: rear_center_range
+              Queue Size: 100
+              Topic: /sensors/range/rear_center_sonar
+              Unreliable: false
+              Value: true
+          Enabled: false
+          Name: Sonars
         - Alpha: 1
           Class: rviz_plugin_tutorials/Imu
           Color: 204; 51; 204
@@ -512,7 +529,7 @@ Visualization Manager:
           Enabled: true
           Invert Rainbow: false
           Max Color: 255; 255; 255
-          Max Intensity: 999999
+          Max Intensity: 5.53419047e-11
           Min Color: 0; 0; 0
           Min Intensity: 8.40779079e-45
           Name: RP_Lidar
@@ -533,7 +550,7 @@ Visualization Manager:
   Global Options:
     Background Color: 26; 26; 26
     Default Light: true
-    Fixed Frame: base_link
+    Fixed Frame: map
     Frame Rate: 30
   Name: root
   Tools:
@@ -554,25 +571,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 1.55903006
+      Distance: 15.7351933
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.0599999987
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 0.744714677
-        Y: -0.03747968
-        Z: 0.0418403521
+        X: 0
+        Y: 0
+        Z: 0
       Focal Shape Fixed Size: false
       Focal Shape Size: 0.0500000007
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.00999999978
-      Pitch: 0.224798039
+      Pitch: 0.785398185
       Target Frame: base_link
       Value: Orbit (rviz)
-      Yaw: 3.58039188
+      Yaw: 0.785398185
     Saved: ~
 Window Geometry:
   Displays:
@@ -580,7 +597,7 @@ Window Geometry:
   Height: 1056
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 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
+  QMainWindow State: 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
   Selection:
     collapsed: false
   Time:
diff --git a/package.xml b/package.xml
index b6bec3261a13df1e0606695691c3271657186d50..ae225071d0787c8f384d960cadbd8bde8dbdfecf 100755
--- a/package.xml
+++ b/package.xml
@@ -42,7 +42,6 @@
   <run_depend>gazebo_ros</run_depend>
   <run_depend>gazebo_plugins</run_depend>
   <run_depend>hector_gazebo_plugins</run_depend>
-  <run_depend>odometry</run_depend> <!-- from https://github.com/AutoModelCar/model_car/tree/version-3 -->
   <run_depend>imu_filter_madgwick</run_depend>
 
   <!-- Use test_depend for packages you need only for testing: -->
diff --git a/urdf/include/platform.gazebo b/urdf/include/platform.gazebo
index 4e92815fabf4628efebb49b86e32547fee55854f..fc6931baa4d9c328f64e62876dcda5007fac085b 100644
--- a/urdf/include/platform.gazebo
+++ b/urdf/include/platform.gazebo
@@ -51,7 +51,7 @@
   <gazebo>
     <plugin name="model_car_ground_truth" filename="libgazebo_ros_p3d.so">
       <frameName>map</frameName>
-      <bodyName>base_link</bodyName>
+      <bodyName>base_footprint</bodyName>
       <topicName>odom_ground_truth</topicName>
       <updateRate>30.0</updateRate>
     </plugin>
diff --git a/urdf/include/platform.xacro b/urdf/include/platform.xacro
index 36baac492fd868adab2c43528316d1288b558866..704b5e29d5ae06a90e7f3dec5651260ea14e417e 100644
--- a/urdf/include/platform.xacro
+++ b/urdf/include/platform.xacro
@@ -7,7 +7,18 @@
 
   <xacro:property name="PI" value="3.1415926535897931" />
   <xacro:property name="angle_limit" value="${PI/3.0}" />
+  <xacro:property name="wheel_radius" value="0.05" />
+  
 
+  <link name="base_footprint">
+  </link>
+  
+  <joint name="base_footprint_to_base_link" type="fixed">
+    <origin xyz="0.0 0.0 ${wheel_radius}" rpy="0 0 0" />
+    <parent link="base_footprint" />
+    <child link="base_link" />
+  </joint>
+  
   <link name="base_link">
     <inertial>
       <mass value="0.001" />
@@ -50,12 +61,12 @@
   <!-- ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ WHEELS ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ -->
   <!-- ======= REAR LEFT ======= -->
   <xacro:wheel name="rear_left" parent="base_link" direction="1.0">
-    <origin xyz="0 0.108 0" rpy="1.5707 0 3.14159" />
+    <origin xyz="0 0.108 0" rpy="${PI/2.0} 0 ${PI}" />
   </xacro:wheel>
 
   <!-- ======= REAR RIGHT ======= -->
   <xacro:wheel name="rear_right" parent="base_link" direction="-1.0">
-    <origin xyz="0 -0.108 0" rpy="1.5707 0 0" />
+    <origin xyz="0 -0.108 0" rpy="${PI/2.0} 0 0" />
   </xacro:wheel>
 
   <link name="steer_left">
@@ -91,7 +102,7 @@
 
   <!-- ======= FRONT LEFT ======= -->
   <xacro:wheel name="front_left" parent="steer_left" direction="1.0">
-    <origin xyz="0.0 0.0 0" rpy="1.5707 0 3.14159" />
+    <origin xyz="0.0 0.0 0" rpy="${PI/2.0} 0 ${PI}" />
   </xacro:wheel>
 
   <link name="steer_right">
@@ -127,7 +138,7 @@
 
   <!-- ======= FRONT RIGHT ======= -->
   <xacro:wheel name="front_right" parent="steer_right" direction="-1.0">
-    <origin xyz="0.0 0.0 0" rpy="1.5707 0 0" />
+    <origin xyz="0.0 0.0 0" rpy="${PI/2.0} 0 0" />
   </xacro:wheel>
 
 </robot>