diff --git a/robot_misc_tools/scripts/tf_configs_dynamic/full_system.json b/robot_misc_tools/scripts/tf_configs_dynamic/full_system.json
index 133eb3e..41b7ff5 100644
--- a/robot_misc_tools/scripts/tf_configs_dynamic/full_system.json
+++ b/robot_misc_tools/scripts/tf_configs_dynamic/full_system.json
@@ -3,7 +3,7 @@
         {
             "frame_id" : "aruco_board_origin",
             "child_id" : "aruco_camera",
-            "pose_topic" : "cur_pose"
+            "pose_topic" : "ekf/pose_filtered"
         }
     ]
 }
\ No newline at end of file
diff --git a/robot_misc_tools/scripts/tf_configs_static/elevation_mapping_test.json b/robot_misc_tools/scripts/tf_configs_static/elevation_mapping_test.json
new file mode 100644
index 0000000..7a17d17
--- /dev/null
+++ b/robot_misc_tools/scripts/tf_configs_static/elevation_mapping_test.json
@@ -0,0 +1,26 @@
+{
+    "transforms" : [
+        {
+            "frame_id" : "world",
+            "child_id" : "aruco_board_origin",
+            "trans_x" : 0.0,
+            "trans_y" : 0.0,
+            "trans_z" : 0.515,
+            "rot_x" : 0,
+            "rot_y" : 0,
+            "rot_z" : 0,
+            "rot_w" : 1
+        },
+        {
+            "frame_id" : "aruco_camera",
+            "child_id" : "camera_link",
+            "trans_x" : 0,
+            "trans_y" : 0,
+            "trans_z" : 0,
+            "rot_x" : 0,
+            "rot_y" : 0,
+            "rot_z" : 0,
+            "rot_w" : 1
+        }
+    ]
+}
\ No newline at end of file
diff --git a/robot_misc_tools/scripts/transforms_static.py b/robot_misc_tools/scripts/transforms_static.py
index c25b400..02de950 100644
--- a/robot_misc_tools/scripts/transforms_static.py
+++ b/robot_misc_tools/scripts/transforms_static.py
@@ -54,7 +54,7 @@ def get_transform_msgs(transforms):
     rospy.init_node("transforms_static")
 
     # Read in the name of transform file from parameter server
-    file_name = rospy.get_param("tf_file_static", default="full_system.json")
+    file_name = rospy.get_param("tf_file_static", default="elevation_mapping_test.json")
 
     # Load file contents into a dictionary
     with open(PATH_TO_DIR + file_name) as f:
diff --git a/robot_sensors/cameras/image_publisher.py b/robot_sensors/cameras/image_publisher.py
index 28f2515..32fb187 100755
--- a/robot_sensors/cameras/image_publisher.py
+++ b/robot_sensors/cameras/image_publisher.py
@@ -12,9 +12,9 @@
 import sys
 
 # Configuration params
-DEFAULT_DEVICE_ID = 0
+DEFAULT_DEVICE_ID = 1
 DEFAULT_CALIB_FILE = 'camera_a.yaml'
-DEFAULT_PUB_RATE = 20
+DEFAULT_PUB_RATE = 3
 DEFAULT_IMG_WIDTH = 640
 DEFAULT_IMG_HEIGHT = 480
 DEFAULT_ALPHA = 1
diff --git a/robot_slam/ekf/ArucoEKF.py b/robot_slam/ekf/ArucoEKF.py
index 1411c77..1ecd027 100755
--- a/robot_slam/ekf/ArucoEKF.py
+++ b/robot_slam/ekf/ArucoEKF.py
@@ -16,6 +16,7 @@
 # Import other required packages
 from filterpy.kalman import EKF
 import numpy as np
+from math import sqrt
 
 
 class ArucoExtendedKalmanFilter:
@@ -38,7 +39,7 @@ def __init__(self):
         '''
 
         # Define the measurement noise covariance matrix
-        self.arucoEKF.R = np.eye(7) * 1
+        self.arucoEKF.R = np.eye(7) * 0.01
 
         # Define the process noise covariance matrix
         #arucoEKF.Q = 
@@ -59,17 +60,29 @@ def update(self,pose):
         z[5][0] = pose.orientation.z
         z[6][0] = pose.orientation.w
 
-        self.arucoEKF.predict_update(z, self.HJacobian, self.hx)
+        if pose.orientation.w != 0:
+            self.arucoEKF.predict_update(z, self.HJacobian, self.hx)
 
     def getPose(self):
         pose = Pose()
-        pose.position.x = 0.0 #self.arucoEKF.x[0][0]
-        pose.position.y = 0.0 #self.arucoEKF.x[1][0]
+        pose.position.x = self.arucoEKF.x[0][0]
+        pose.position.y = self.arucoEKF.x[1][0]
         pose.position.z = self.arucoEKF.x[2][0]
-        pose.orientation.x = self.arucoEKF.x[3][0]
-        pose.orientation.y = self.arucoEKF.x[4][0]
-        pose.orientation.z = self.arucoEKF.x[5][0]
-        pose.orientation.w = 1.0 #self.arucoEKF.x[6][0]
+        new_x = self.arucoEKF.x[3][0]
+        new_y = self.arucoEKF.x[4][0]
+        new_z = self.arucoEKF.x[5][0]
+        new_w = self.arucoEKF.x[6][0]
+
+        # Normalize quaternion
+        mag = sqrt(new_x**2 + new_y**2 + new_z**2 + new_w**2)
+
+        if mag == 0:
+            mag = 1.0
+
+        pose.orientation.x = new_x / mag
+        pose.orientation.y = new_y / mag
+        pose.orientation.z = new_z / mag
+        pose.orientation.w = new_w / mag
 
         return pose
 
diff --git a/robot_slam/launch/test_ekf.launch b/robot_slam/launch/test_ekf.launch
index c5db48f..bc744ab 100644
--- a/robot_slam/launch/test_ekf.launch
+++ b/robot_slam/launch/test_ekf.launch
@@ -1,25 +1,25 @@
 <launch>
 
-    <!-- Parameters to define true pose for reference in error calculations-->
-    <param name="true_x" type="double" value="0"/>
-    <param name="true_y" type="double" value="0"/>
-    <param name="true_z" type="double" value="1"/>
-    <param name="true_orientation_x" type="double" value="0"/>
-    <param name="true_orientation_y" type="double" value="0"/>
-    <param name="true_orientation_z" type="double" value="0"/>
-    <param name="true_orientation_w" type="double" value="1"/>
-    <param name="total_data_points" type="int" value="500"/>
+    <!--&lt;!&ndash; Parameters to define true pose for reference in error calculations&ndash;&gt;-->
+    <!--<param name="true_x" type="double" value="0"/>-->
+    <!--<param name="true_y" type="double" value="0"/>-->
+    <!--<param name="true_z" type="double" value="1"/>-->
+    <!--<param name="true_orientation_x" type="double" value="0"/>-->
+    <!--<param name="true_orientation_y" type="double" value="0"/>-->
+    <!--<param name="true_orientation_z" type="double" value="0"/>-->
+    <!--<param name="true_orientation_w" type="double" value="1"/>-->
+    <!--<param name="total_data_points" type="int" value="500"/>-->
 
     <!-- Parameters to define Aruco Board length & separation values -->
     <param name="board_x_markers" type="int" value="5"/>
     <param name="board_y_markers" type="int" value="7"/>
-    <param name="board_marker_length" type="double" value=".029"/>
-    <param name="board_marker_separation" type="double" value=".006"/>
+    <param name="board_marker_length" type="double" value=".025"/>
+    <param name="board_marker_separation" type="double" value=".005"/>
 
     <!-- parameters for image_publisher node -->
-    <param name="device_id" value="0"/>
+    <param name="device_id" value="1"/>
     <param name="calibration_file" value="camera_a.yaml"/>
-    <param name="publish_rate" value="20"/>
+    <param name="publish_rate" value="3"/>
     <param name="image_width" value="640"/>
     <param name="image_height" value="480"/>
 
@@ -27,7 +27,7 @@
     <node name="image_publisher" pkg="robot_sensors" type="image_publisher.py"/> 
     <node name="aruco" pkg="robot_slam" type="aruco_localization.py"/>
     <node name="aruco_ekf" pkg="robot_slam" type="ArucoEKF.py"/>
-    <node name="test_ekf" pkg="robot_slam" type="testEKF.py"/>
+    <!--<node name="test_ekf" pkg="robot_slam" type="testEKF.py"/>-->
 
     <!-- Launch the rqt_console to view the log info printed at the end of the test -->
     <node name="rqt_console" pkg="rqt_console" type="rqt_console"/>
diff --git a/robot_slam/localization/aruco_localization.py b/robot_slam/localization/aruco_localization.py
index 633ddcc..f6f73d3 100755
--- a/robot_slam/localization/aruco_localization.py
+++ b/robot_slam/localization/aruco_localization.py
@@ -56,7 +56,7 @@ def image_callback(self, data):
         # Define variables for messages to publish
         pose_msg = Pose()
         bool_msg = Bool()
-        avg_corners = 0;
+        avg_corners = 0
 
         # Convert from ROS message to OpenCV image
         cv_image = self.bridge.imgmsg_to_cv2(data, desired_encoding="passthrough")
diff --git a/robot_slam/obstacle_avoidance/elevation_mapping.rviz b/robot_slam/obstacle_avoidance/elevation_mapping.rviz
new file mode 100644
index 0000000..b308157
--- /dev/null
+++ b/robot_slam/obstacle_avoidance/elevation_mapping.rviz
@@ -0,0 +1,198 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /TF1/Frames1
+        - /Map1
+      Splitter Ratio: 0.551246524
+    Tree Height: 719
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679016
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: PointCloud2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 0.25
+      Class: rviz/Grid
+      Color: 114; 114; 117
+      Enabled: true
+      Line Style:
+        Line Width: 0.0299999993
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 18
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: rviz/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+        camera_depth_frame:
+          Value: false
+        camera_depth_optical_frame:
+          Value: false
+        camera_link:
+          Value: true
+        camera_rgb_frame:
+          Value: false
+        camera_rgb_optical_frame:
+          Value: false
+      Marker Scale: 0.25
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        camera_link:
+          camera_depth_frame:
+            camera_depth_optical_frame:
+              {}
+          camera_rgb_frame:
+            camera_rgb_optical_frame:
+              {}
+      Update Interval: 0
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 0; 0; 0
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 0; 85; 0
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 1
+      Size (m): 0.00499999989
+      Style: Points
+      Topic: /camera/depth/points
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: false
+      Value: true
+    - Alpha: 1
+      Axes Length: 1
+      Axes Radius: 0.100000001
+      Class: rviz/Pose
+      Color: 255; 25; 0
+      Enabled: true
+      Head Length: 0.100000001
+      Head Radius: 0.0250000004
+      Name: Pose
+      Shaft Length: 0.100000001
+      Shaft Radius: 0.00999999978
+      Shape: Arrow
+      Topic: /cur_pose
+      Unreliable: false
+      Value: true
+    - Alpha: 0.699999988
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: /elevation_map_fused_visualization/elevation_grid
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 230; 230; 230
+    Default Light: true
+    Fixed Frame: camera_link
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 3.18881035
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.0599999987
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 1.63787627
+        Y: 0.0910898745
+        Z: -0.757259429
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.0500000007
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.00999999978
+      Pitch: 0.560201287
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 3.30861092
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1007
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000016b00000362fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006300fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000362000000db00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000345fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000345000000b300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003ffc0100000002fb0000000800540069006d00650100000000000007800000031700fffffffb0000000800540069006d006501000000000000045000000000000000000000060f0000036200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1920
+  X: 0
+  Y: 24
diff --git a/robot_slam/obstacle_avoidance/elevation_mapping_test.launch b/robot_slam/obstacle_avoidance/elevation_mapping_test.launch
new file mode 100644
index 0000000..3acb36f
--- /dev/null
+++ b/robot_slam/obstacle_avoidance/elevation_mapping_test.launch
@@ -0,0 +1,34 @@
+
+<launch>
+
+	<!-- Launch elevation mapping node and set corresponding parameters-->
+	<node pkg="elevation_mapping" type="elevation_mapping" name="elevation_mapping">
+		<param name="point_cloud_topic" value="/camera/depth/points"/>
+		<param name="robot_pose_with_covariance_topic" value="/cur_pose_wcs"/>
+		<param name="map_frame_id" value="camera_link"/>
+	    <param name="robot_base_frame_id" value="camera_link"/>
+	    <!--<param name="track_point_frame_id" value="camera_link"/>-->
+		<param name="sensor_frame_id" value="camera_link"/>
+		<param name="resolution" value="0.01"/>
+		<param name="length_in_x" value="3.0"/>
+		<param name="length_in_y" value="1.5"/>
+		<!--<param name="position_x" value="-1.5"/>-->
+		<!--<param name="position_y" value="0.0"/>-->
+		<!--<param name="scanning_duration" value="0.033"/>-->
+		<!--<param name="mahalanobis_distance_threshold" value="0.3"/>-->
+		<!--<param name="multi_height_noise" value="0.0001"/>-->
+
+		<!-- kinect-specific parameters -->
+		<!--<rosparam command="load" file="$(find elevation_mapping)/config/sensor_processors/kinect_nguyen_et_al.yaml" />-->
+	</node>
+
+	<!-- Launch nodes for elevation_map visualization -->
+	<node pkg="grid_map_visualization" type="grid_map_visualization" name="elevation_map_fused_visualization" output="screen">
+		<param name="grid_map_topic" value="/elevation_mapping/elevation_map" />
+		<rosparam command="load" file="$(find elevation_mapping_demos)/config/visualization/fused.yaml" />
+	</node>
+
+	<!-- Start RViz -->
+	<node pkg="rviz" type="rviz" name="rviz" args="-d $(find test_launch_files)/rviz_configs/elevation_mapping.rviz"/>
+
+</launch>