diff --git a/smb_msf_graph/config/elevation_mapping/default.yaml b/smb_msf_graph/config/elevation_mapping/default.yaml
new file mode 100644
index 0000000..3807ff4
--- /dev/null
+++ b/smb_msf_graph/config/elevation_mapping/default.yaml
@@ -0,0 +1,22 @@
+robot_pose_with_covariance_topic: ""
+track_point_x: 0
+track_point_y: 0
+track_point_z: 0
+min_update_rate: 0.5
+length_in_x: 25
+length_in_y: 25
+position_x: 0
+position_y: 0
+resolution: 0.1
+min_variance: 0.000009  #0.00005 --> changes fast
+max_variance: 0.002
+mahalanobis_distance_threshold: 2.5
+multi_height_noise: 0.0001
+visibility_cleanup_rate: 1.0
+scanning_duration: 0.3
+enable_visibility_cleanup: true
+sensor_processor:
+  beam_angle: 0.0006
+  beam_constant: 0.0015
+  min_radius: 0.018
+  type: laser
\ No newline at end of file
diff --git a/smb_msf_graph/config/elevation_mapping/vlp16.yaml b/smb_msf_graph/config/elevation_mapping/vlp16.yaml
new file mode 100644
index 0000000..aba2d97
--- /dev/null
+++ b/smb_msf_graph/config/elevation_mapping/vlp16.yaml
@@ -0,0 +1,12 @@
+input_sources:
+  elevation_mapping_sensor:
+    type: pointcloud
+    topic: /rslidar/points
+    queue_size: 1
+    publish_on_update: true
+    sensor_processor:
+      type: laser
+robot_base_frame_id: base_link
+map_frame_id: odom_graph_msf
+track_point_frame_id: base_link
+sensor_frame_id: rslidar
\ No newline at end of file
diff --git a/smb_msf_graph/launch/smb_msf_graph.launch b/smb_msf_graph/launch/smb_msf_graph.launch
index 8677771..4f30724 100644
--- a/smb_msf_graph/launch/smb_msf_graph.launch
+++ b/smb_msf_graph/launch/smb_msf_graph.launch
@@ -20,6 +20,7 @@
         <arg name="lidar_odometry_topic_name" value="$(arg lidar_odometry_topic_name)"/>
     </include>
 
+    <!-- Check whether Open3D-SLAM should be launched -->
     <group if="$(arg launch_o3d_slam)">
         <!-- Launch online_slam -->
         <include file="$(find open3d_slam_ros)/launch/development.launch" if="$(arg launch_o3d_slam)">
diff --git a/smb_msf_graph/launch/smb_msf_graph_o3d_replay.launch b/smb_msf_graph/launch/smb_msf_graph_o3d_replay.launch
index 3814af6..1cf4eb3 100644
--- a/smb_msf_graph/launch/smb_msf_graph_o3d_replay.launch
+++ b/smb_msf_graph/launch/smb_msf_graph_o3d_replay.launch
@@ -1,14 +1,14 @@
-<?xml version="1.0" encoding="UTF-8"?> 
+<?xml version="1.0" encoding="UTF-8"?>
 <launch>
 
     <!-- Parameters -->
-    <arg name="use_sim_time"              value="true" />
+    <arg name="use_sim_time" value="true"/>
 
     <!-- Parameters -->
-    <arg name="imu_topic_name"            default="/imu" />
-    <arg name="lidar_odometry_topic_name" default="/mapping/scan2map_odometry"/>
-    <arg name="launch_o3d_slam" default="true"/>
+    <arg name="imu_topic_name" default="/imu"/>
     <arg name="cloud_topic" default="/rslidar/points"/>
+    <arg name="lidar_odometry_topic_name" default="/mapping/scan2map_odometry"/>
+    <arg name="launch_o3d_slam" default="false"/>
 
     <!-- Config Files -->
     <arg name="core_graph_param_file" default="$(find smb_estimator_graph)/config/core/core_graph_params.yaml"/>
@@ -16,13 +16,13 @@
 
     <!-- Alias to smb_estimator_graph replay script -->
     <include file="$(find smb_estimator_graph)/launch/smb_estimator_graph_replay.launch">
-        <arg name="use_sim_time" value="$(arg use_sim_time)" />
+        <arg name="use_sim_time" value="$(arg use_sim_time)"/>
         <!-- Topics -->
-        <arg name="imu_topic_name" value="$(arg imu_topic_name)" />
-        <arg name="lidar_odometry_topic_name" value="$(arg lidar_odometry_topic_name)" />
+        <arg name="imu_topic_name" value="$(arg imu_topic_name)"/>
+        <arg name="lidar_odometry_topic_name" value="$(arg lidar_odometry_topic_name)"/>
         <!-- Config Files -->
-        <arg name="core_graph_param_file" value="$(arg core_graph_param_file)" />
-        <arg name="smb_graph_param_file"  value="$(arg smb_graph_param_file)" />
+        <arg name="core_graph_param_file" value="$(arg core_graph_param_file)"/>
+        <arg name="smb_graph_param_file" value="$(arg smb_graph_param_file)"/>
     </include>
 
     <!-- Load Description -->
@@ -30,8 +30,21 @@
 
     <!-- Launch Open3D SLAM -->
     <include file="$(find smb_slam)/launch/online_slam.launch" if="$(arg launch_o3d_slam)">
-        <arg name="use_sim_time" value="$(arg use_sim_time)" />
-        <arg name="cloud_topic" value="$(arg cloud_topic)" />
+        <arg name="use_sim_time" value="$(arg use_sim_time)"/>
+        <arg name="cloud_topic" value="$(arg cloud_topic)"/>
     </include>
 
-</launch>
\ No newline at end of file
+    <!-- Elevation mapping node -->
+    <arg name="default_elevation_mapping_parameter_file" default="$(dirname)/../config/elevation_mapping/default.yaml"/>
+    <arg name="elevation_mapping_parameter_file" default="$(dirname)/../config/elevation_mapping/vlp16.yaml"/>
+
+    <!-- Elevation Mapping -->
+    <node pkg="elevation_mapping" type="elevation_mapping" name="elevation_mapping" output="log">
+        <rosparam command="load" file="$(arg default_elevation_mapping_parameter_file)"/>
+        <rosparam command="load" file="$(arg elevation_mapping_parameter_file)"/>
+    </node>
+
+    <!-- Static TF Publisher between world_graph_msf and odom -->
+    <!--    <node pkg="tf" type="static_transform_publisher" name="world_graph_msf_to_odom" args="0 0 0 0 0 0 world_graph_msf odom 100" />-->
+
+</launch>
diff --git a/smb_msf_graph/package.xml b/smb_msf_graph/package.xml
index 0692566..ddae527 100644
--- a/smb_msf_graph/package.xml
+++ b/smb_msf_graph/package.xml
@@ -12,5 +12,6 @@
 	<exec_depend>compslam_lio</exec_depend>
 	<exec_depend>open3d_slam_ros</exec_depend>
 	<exec_depend>smb_slam</exec_depend>
+	<exec_depend>elevation_mapping</exec_depend>
 
 </package>
diff --git a/smb_msf_graph/tutorial/config/1_smb_graph_params.yaml b/smb_msf_graph/tutorial/config/1_smb_graph_params.yaml
index a252ad0..71d92f5 100644
--- a/smb_msf_graph/tutorial/config/1_smb_graph_params.yaml
+++ b/smb_msf_graph/tutorial/config/1_smb_graph_params.yaml
@@ -1,14 +1,14 @@
 # Sensor Params
 sensor_params:
   ## Config
-  # TODO: Change here
-  useWheelOdometry: false
   useLioOdometry: false
-  # TODO: End of change
+  useWheelOdometrybetween: false
+  useWheelLinearVelocities: false
   useVioOdometry: false
   ## Rates
   lioOdometryRate: 10
-  wheelOdometryRate: 50
+  wheelOdometryBetweenRate: 50
+  wheelLinearVelocitiesRate: 50
   vioOdometryRate: 50
 
 # Alignment Parameters
@@ -20,7 +20,8 @@ noise_params:
   ## LiDAR
   lioPoseUnaryNoiseDensity: [ 0.05, 0.05, 0.05, 0.01, 0.01, 0.01 ] # StdDev, ORDER RPY(rad) - XYZ(meters)
   ## Wheel
-  wheelPoseBetweenNoiseDensity: [ 0.1, 0.1, 0.1, 0.06, 0.06, 0.06 ] # StdDev, ORDER RPY(rad) - XYZ(meters)
+  wheelPoseBetweenNoiseDensity: [ 0.2, 0.2, 0.2, 0.005, 0.005, 0.005 ] # StdDev, [ 10.0, 10.0, 10.0, 2.0, 2.0, 2.0 ]
+  wheelLinearVelocitiesNoiseDensity: [ 0.1, 0.1, 0.1 ] # StdDev, ORDER VxVyVz(meters/sec)
   ## VIO
   vioPoseBetweenNoiseDensity: [ 100.0, 100.0, 100.0, 0.1, 0.1, 0.1 ] # StdDev, ORDER RPY(rad) - XYZ(meters)
   vioPoseUnaryNoiseDensity: [ 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 ] # StdDev, ORDER RPY(rad) - XYZ(meters)