diff --git a/config/camera_hole_detector_far_config.yaml b/config/camera_hole_detector_far_config.yaml
index 0e90a86df029fd7bc09020b710bc16cb8e38bbe7..fe972dc80f908a370e131d90445118b5e8bcb811 100755
--- a/config/camera_hole_detector_far_config.yaml
+++ b/config/camera_hole_detector_far_config.yaml
@@ -1,7 +1,7 @@
 hole_min_p: 25
 num_cells: 3
-box_y: 0.8
+box_y: 0.9
 box_z_ini: -0.1
 box_z_end: 0.1
-box_x_ini: 1.0
+box_x_ini: 0.9
 box_x_end: 1.4
diff --git a/config/camera_hole_detector_near_config.yaml b/config/camera_hole_detector_near_config.yaml
index 84ad071579b43db7c8ab6123f87944dc686a4a04..adb3fd0a019745284cc1a700f659fc855eaf7c5d 100755
--- a/config/camera_hole_detector_near_config.yaml
+++ b/config/camera_hole_detector_near_config.yaml
@@ -1,7 +1,7 @@
 hole_min_p: 25
 num_cells: 2
-box_y: 0.45
+box_y: 0.65
 box_z_ini: -0.1
 box_z_end: 0.1
-box_x_ini: 0.6
-box_x_end: 1.0
+box_x_ini: 0.5
+box_x_end: 0.9
diff --git a/config/lidar_angle_filter_config.yaml b/config/lidar_angle_filter_config.yaml
index 99147047bed3d60aca3e61333c670801f61c41fe..8207f4b3f54d3b19334292521690f63506b61826 100644
--- a/config/lidar_angle_filter_config.yaml
+++ b/config/lidar_angle_filter_config.yaml
@@ -1,3 +1,18 @@
-n_pairs: 0
+n_pairs: 4
 ranges:
   - range1
+  - range2
+  - range3
+  - range4
+range1:
+  min: 0.75
+  max: 1.0
+range2:
+  min: -1.17
+  max: -0.90
+range3:
+  min: 1.85
+  max: 2.15
+range4:
+  min: -2.35
+  max: -2.05
diff --git a/params/costmap/3d_common_params.yaml b/params/costmap/3d_common_params.yaml
index 402900c58a3d01018470d7ee2f613ee24cb94e7e..6b9295d6a7a8442e3f34dae8ee12a262ff10fe35 100755
--- a/params/costmap/3d_common_params.yaml
+++ b/params/costmap/3d_common_params.yaml
@@ -29,7 +29,7 @@ obstacle_layer_lidar:
     topic: /ana/lidar_detector/obstacles,
     observation_persistence: 0.0,
     marking: true,
-    clearing: false,
+    clearing: true,
     min_obstacle_height: -10.0,
     max_obstacle_height: 10.0,
     expected_update_rate: 1,
diff --git a/rviz/3d_ana.rviz b/rviz/3d_ana.rviz
index 40689e0035212c2c17611b7d7b5f5648dd84be16..7343f4b4b6034c96eaa37fcdd1e5723ed46de1c5 100644
--- a/rviz/3d_ana.rviz
+++ b/rviz/3d_ana.rviz
@@ -6,7 +6,7 @@ Panels:
       Expanded:
         - /Global Options1
         - /TF1/Frames1
-        - /Nav1
+        - /Sensors1
         - /3d_nav_detection1
         - /3d_nav_detection1/lidar1
         - /3d_nav_detection1/camera1
@@ -30,7 +30,7 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: far_hole_vision
+    SyncSource: Velodyne
   - Class: rviz_plugin_tutorials/Teleop
     Name: Teleop
     Topic: ""
@@ -78,150 +78,149 @@ Visualization Manager:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        base_footprint:
+        ana_realsense_support_top:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        base_link:
+        base_footprint:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-          Value: true
-        camera_bottom_screw_frame:
+        base_link:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        camera_color_frame:
+          Value: true
+        front_left_axle:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        camera_color_optical_frame:
+          Value: true
+        front_left_hub:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        camera_depth_frame:
+          Value: true
+        front_left_wheel:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        camera_depth_optical_frame:
+          Value: true
+        front_right_axle:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        camera_infra1_frame:
+          Value: true
+        front_right_hub:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        camera_infra1_optical_frame:
+          Value: true
+        front_right_wheel:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        camera_infra2_frame:
+          Value: true
+        front_sonar:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        camera_infra2_optical_frame:
+          Value: true
+        imu_bno055:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        camera_link:
+        imu_sim_bno055:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        camera_pan_tilt_bottom_screw_frame:
+        imu_sim_bno055_base:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        camera_pan_tilt_color_frame:
+        nav_cam_bottom_screw_frame:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        camera_pan_tilt_color_optical_frame:
+        nav_cam_color_frame:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        camera_pan_tilt_depth_frame:
+        nav_cam_color_optical_frame:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        camera_pan_tilt_depth_optical_frame:
+        nav_cam_depth_frame:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        camera_pan_tilt_infra1_frame:
+        nav_cam_depth_optical_frame:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        camera_pan_tilt_infra1_optical_frame:
+        nav_cam_infra1_frame:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        camera_pan_tilt_infra2_frame:
+        nav_cam_infra1_optical_frame:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        camera_pan_tilt_infra2_optical_frame:
+        nav_cam_infra2_frame:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        camera_pan_tilt_link:
+        nav_cam_infra2_optical_frame:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-          Value: true
-        front_left_axle:
+        nav_cam_link:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        front_left_hub:
+        nav_cam_top_bottom_screw_frame:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-          Value: true
-        front_left_wheel:
+        nav_cam_top_color_frame:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-          Value: true
-        front_right_axle:
+        nav_cam_top_color_optical_frame:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-          Value: true
-        front_right_hub:
+        nav_cam_top_depth_frame:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-          Value: true
-        front_right_wheel:
+        nav_cam_top_depth_optical_frame:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-          Value: true
-        front_sonar:
+        nav_cam_top_infra1_frame:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-          Value: true
-        imu_bno055:
+        nav_cam_top_infra1_optical_frame:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-          Value: true
-        imu_bno055_base:
+        nav_cam_top_infra2_frame:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        pan_frame:
+        nav_cam_top_infra2_optical_frame:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-          Value: true
-        realsense_support_pan_tilt:
+        nav_cam_top_link:
           Alpha: 1
           Show Axes: false
           Show Trail: false
+          Value: true
         rear_left_axle:
           Alpha: 1
           Show Axes: false
@@ -257,11 +256,6 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
-        tilt_frame:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
         top_plate:
           Alpha: 1
           Show Axes: false
@@ -292,77 +286,79 @@ Visualization Manager:
           Value: true
         ana/ana_realsense_support:
           Value: true
+        ana/ana_realsense_support_top:
+          Value: true
         ana/base_footprint:
           Value: true
         ana/base_footprint_robot:
           Value: true
         ana/base_link:
           Value: true
-        ana/camera_aligned_depth_to_color_frame:
+        ana/front_left_axle:
           Value: true
-        ana/camera_bottom_screw_frame:
+        ana/front_left_hub:
           Value: true
-        ana/camera_color_frame:
+        ana/front_left_wheel:
           Value: true
-        ana/camera_color_optical_frame:
+        ana/front_right_axle:
           Value: true
-        ana/camera_depth_frame:
+        ana/front_right_hub:
           Value: true
-        ana/camera_depth_optical_frame:
+        ana/front_right_wheel:
           Value: true
-        ana/camera_infra1_frame:
+        ana/front_sonar:
           Value: true
-        ana/camera_infra1_optical_frame:
+        ana/imu_bno055:
           Value: true
-        ana/camera_infra2_frame:
+        ana/imu_sim_bno055:
           Value: true
-        ana/camera_infra2_optical_frame:
+        ana/imu_sim_bno055_base:
           Value: true
-        ana/camera_link:
+        ana/nav_cam_aligned_depth_to_color_frame:
           Value: true
-        ana/camera_pan_tilt_bottom_screw_frame:
+        ana/nav_cam_bottom_screw_frame:
           Value: true
-        ana/camera_pan_tilt_color_frame:
+        ana/nav_cam_color_frame:
           Value: true
-        ana/camera_pan_tilt_color_optical_frame:
+        ana/nav_cam_color_optical_frame:
           Value: true
-        ana/camera_pan_tilt_depth_frame:
+        ana/nav_cam_depth_frame:
           Value: true
-        ana/camera_pan_tilt_depth_optical_frame:
+        ana/nav_cam_depth_optical_frame:
           Value: true
-        ana/camera_pan_tilt_infra1_frame:
+        ana/nav_cam_infra1_frame:
           Value: true
-        ana/camera_pan_tilt_infra1_optical_frame:
+        ana/nav_cam_infra1_optical_frame:
           Value: true
-        ana/camera_pan_tilt_infra2_frame:
+        ana/nav_cam_infra2_frame:
           Value: true
-        ana/camera_pan_tilt_infra2_optical_frame:
+        ana/nav_cam_infra2_optical_frame:
           Value: true
-        ana/camera_pan_tilt_link:
+        ana/nav_cam_link:
           Value: true
-        ana/front_left_axle:
+        ana/nav_cam_top_aligned_depth_to_color_frame:
           Value: true
-        ana/front_left_hub:
+        ana/nav_cam_top_bottom_screw_frame:
           Value: true
-        ana/front_left_wheel:
+        ana/nav_cam_top_color_frame:
           Value: true
-        ana/front_right_axle:
+        ana/nav_cam_top_color_optical_frame:
           Value: true
-        ana/front_right_hub:
+        ana/nav_cam_top_depth_frame:
           Value: true
-        ana/front_right_wheel:
+        ana/nav_cam_top_depth_optical_frame:
           Value: true
-        ana/front_sonar:
+        ana/nav_cam_top_infra1_frame:
           Value: true
-        ana/imu_bno055:
+        ana/nav_cam_top_infra1_optical_frame:
           Value: true
-        ana/imu_bno055_base:
+        ana/nav_cam_top_infra2_frame:
           Value: true
-        ana/odom:
+        ana/nav_cam_top_infra2_optical_frame:
           Value: true
-        ana/pan_frame:
+        ana/nav_cam_top_link:
           Value: true
-        ana/realsense_support_pan_tilt:
+        ana/odom:
           Value: true
         ana/rear_left_axle:
           Value: true
@@ -384,6 +380,14 @@ Visualization Manager:
           Value: true
         ana/velodyne_base:
           Value: true
+        camera_accel_frame:
+          Value: true
+        camera_accel_optical_frame:
+          Value: true
+        camera_gyro_frame:
+          Value: true
+        camera_gyro_optical_frame:
+          Value: true
         map:
           Value: true
       Marker Scale: 0.10000000149011612
@@ -418,30 +422,54 @@ Visualization Manager:
                   {}
                 ana/top_plate:
                   ana/ana_box:
-                    ana/pan_frame:
-                      {}
+                    {}
                   ana/ana_realsense_support:
-                    ana/camera_bottom_screw_frame:
-                      ana/camera_link:
-                        ana/camera_aligned_depth_to_color_frame:
-                          ana/camera_color_optical_frame:
+                    ana/nav_cam_bottom_screw_frame:
+                      ana/nav_cam_link:
+                        ana/nav_cam_aligned_depth_to_color_frame:
+                          ana/nav_cam_color_optical_frame:
+                            {}
+                        ana/nav_cam_color_frame:
+                          {}
+                        ana/nav_cam_depth_frame:
+                          ana/nav_cam_depth_optical_frame:
                             {}
-                        ana/camera_color_frame:
+                        ana/nav_cam_infra1_frame:
+                          ana/nav_cam_infra1_optical_frame:
+                            {}
+                        ana/nav_cam_infra2_frame:
+                          ana/nav_cam_infra2_optical_frame:
+                            {}
+                  ana/ana_realsense_support_top:
+                    ana/nav_cam_top_bottom_screw_frame:
+                      ana/nav_cam_top_link:
+                        ana/nav_cam_top_aligned_depth_to_color_frame:
+                          ana/nav_cam_top_color_optical_frame:
+                            {}
+                        ana/nav_cam_top_color_frame:
                           {}
-                        ana/camera_depth_frame:
-                          ana/camera_depth_optical_frame:
+                        ana/nav_cam_top_depth_frame:
+                          ana/nav_cam_top_depth_optical_frame:
                             {}
-                        ana/camera_infra1_frame:
-                          ana/camera_infra1_optical_frame:
+                        ana/nav_cam_top_infra1_frame:
+                          ana/nav_cam_top_infra1_optical_frame:
                             {}
-                        ana/camera_infra2_frame:
-                          ana/camera_infra2_optical_frame:
+                        ana/nav_cam_top_infra2_frame:
+                          ana/nav_cam_top_infra2_optical_frame:
+                            {}
+                        camera_accel_frame:
+                          camera_accel_optical_frame:
+                            {}
+                        camera_gyro_frame:
+                          camera_gyro_optical_frame:
                             {}
                   ana/velodyne_base:
                     ana/velodyne:
                       {}
-              ana/imu_bno055_base:
-                ana/imu_bno055:
+              ana/imu_bno055:
+                {}
+              ana/imu_sim_bno055_base:
+                ana/imu_sim_bno055:
                   {}
             ana/base_footprint_robot:
               {}
@@ -459,7 +487,7 @@ Visualization Manager:
           Unreliable: false
           Value: false
         - Class: rviz/Image
-          Enabled: true
+          Enabled: false
           Image Topic: /ana/sensors/nav_cam/color/image_raw
           Max Value: 1
           Median window: 5
@@ -469,7 +497,7 @@ Visualization Manager:
           Queue Size: 2
           Transport Hint: raw
           Unreliable: false
-          Value: true
+          Value: false
         - Alpha: 0.5
           Autocompute Intensity Bounds: true
           Autocompute Value Bounds:
@@ -482,7 +510,7 @@ Visualization Manager:
           Color: 255; 255; 255
           Color Transformer: RGB8
           Decay Time: 0
-          Enabled: true
+          Enabled: false
           Invert Rainbow: false
           Max Color: 255; 255; 255
           Max Intensity: 4096
@@ -495,16 +523,16 @@ Visualization Manager:
           Size (Pixels): 3
           Size (m): 0.009999999776482582
           Style: Points
-          Topic: /ana/sensors/nav_cam/depth_registered/points
+          Topic: /ana/sensors/nav_cam_top/depth_registered/points
           Unreliable: false
           Use Fixed Frame: true
           Use rainbow: true
-          Value: true
+          Value: false
         - Alpha: 1
           Autocompute Intensity Bounds: true
           Autocompute Value Bounds:
-            Max Value: 16.410022735595703
-            Min Value: -0.006432712078094482
+            Max Value: 5.824672222137451
+            Min Value: -0.5254461169242859
             Value: true
           Axis: Z
           Channel Name: intensity
@@ -542,7 +570,7 @@ Visualization Manager:
           Color: 255; 0; 0
           Color Transformer: FlatColor
           Decay Time: 0
-          Enabled: true
+          Enabled: false
           Invert Rainbow: false
           Max Color: 255; 255; 255
           Max Intensity: 4096
@@ -559,8 +587,8 @@ Visualization Manager:
           Unreliable: false
           Use Fixed Frame: true
           Use rainbow: true
-          Value: true
-      Enabled: false
+          Value: false
+      Enabled: true
       Name: Sensors
     - Class: rviz/Group
       Displays:
@@ -720,7 +748,7 @@ Visualization Manager:
           Topic: /ana/move_base/current_goal
           Unreliable: false
           Value: true
-      Enabled: true
+      Enabled: false
       Name: Nav
     - Class: rviz/Group
       Displays:
@@ -738,7 +766,7 @@ Visualization Manager:
               Color: 255; 0; 0
               Color Transformer: FlatColor
               Decay Time: 0
-              Enabled: true
+              Enabled: false
               Invert Rainbow: false
               Max Color: 255; 255; 255
               Max Intensity: 4096
@@ -755,7 +783,7 @@ Visualization Manager:
               Unreliable: false
               Use Fixed Frame: true
               Use rainbow: true
-              Value: true
+              Value: false
             - Alpha: 1
               Autocompute Intensity Bounds: true
               Autocompute Value Bounds:
@@ -768,10 +796,10 @@ Visualization Manager:
               Color: 255; 255; 255
               Color Transformer: Intensity
               Decay Time: 0
-              Enabled: true
+              Enabled: false
               Invert Rainbow: false
               Max Color: 255; 255; 255
-              Max Intensity: 157
+              Max Intensity: 121
               Min Color: 0; 0; 0
               Min Intensity: 1
               Name: lidar_zone
@@ -785,7 +813,7 @@ Visualization Manager:
               Unreliable: false
               Use Fixed Frame: true
               Use rainbow: true
-              Value: true
+              Value: false
             - Class: rviz/Image
               Enabled: false
               Image Topic: /ana/lidar_detector/obstacles_img/image_raw
@@ -982,7 +1010,7 @@ Visualization Manager:
               Use Fixed Frame: true
               Use rainbow: true
               Value: true
-          Enabled: true
+          Enabled: false
           Name: camera
       Enabled: true
       Name: 3d_nav_detection
@@ -1014,25 +1042,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/XYOrbit
-      Distance: 3.5477471351623535
+      Distance: 7.981015682220459
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 0.5333200693130493
-        Y: 0.21448162198066711
-        Z: -9.5367431640625e-7
+        X: -0.01600027084350586
+        Y: 0.20595836639404297
+        Z: -1.1920928955078125e-6
       Focal Shape Fixed Size: false
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 1.3147964477539062
+      Pitch: 0.6047971844673157
       Target Frame: ana/base_footprint
       Value: XYOrbit (rviz)
-      Yaw: 1.6722356081008911
+      Yaw: 3.2740530967712402
     Saved: ~
 Window Geometry:
   Displays:
@@ -1040,7 +1068,7 @@ Window Geometry:
   Height: 1025
   Hide Left Dock: false
   Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd0000000400000000000001d3000003a7fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003a7000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001e007200650061006c00730065006e00730065005f0063006f006c006f007200000002630000011b0000001600fffffffb0000000c00540065006c0065006f00700000000315000000b30000004700fffffffb0000001e007200650061006c00730065006e00730065005f0063006f006c006f007200000002f9000001090000000000000000000000010000010f000003dafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000003da000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000007efc0100000003fb00000016006c0069006400610072005f0069006d00610067006500000000000000073d0000008400fffffffb0000000800540069006d006500000000000000073f000004f300fffffffb0000000800540069006d0065010000000000000450000000000000000000000564000003a700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000001d3000003a7fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003a7000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001e007200650061006c00730065006e00730065005f0063006f006c006f0072000000030c000000d80000001600fffffffb0000000c00540065006c0065006f00700000000315000000b30000004700fffffffb0000001e007200650061006c00730065006e00730065005f0063006f006c006f007200000002f9000001090000000000000000000000010000010f000003dafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000003da000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000007efc0100000003fb00000016006c0069006400610072005f0069006d00610067006500000000000000073d0000008400fffffffb0000000800540069006d006500000000000000073f000004f300fffffffb0000000800540069006d0065010000000000000450000000000000000000000564000003a700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Teleop: