diff --git a/launch/imu2d_demo.launch b/launch/imu2d_demo.launch
index 0b074634a0211c11e57855b9d99d2315aa844eb9..bf1cbdd9eba45feccbb43295da9df03cf92681f3 100644
--- a/launch/imu2d_demo.launch
+++ b/launch/imu2d_demo.launch
@@ -1,6 +1,8 @@
 <!-- -->
 <launch>
     <param name="use_sim_time" value="true" />
+    <arg name="record" default="true" />
+    <arg name="rviz" default="true" />
     <arg name="speed" default="1" />
     <arg name="sec" default="0" />
     <arg name="profiling" default="false" />
@@ -14,7 +16,10 @@
     <!-- using "clock" option to use the simulated time. Rosbag launch as the first node  -->
 
     <node pkg="rostopic" type="rostopic" name="" args='pub /helena/sensors/bno055_imu/imu/topic_enable std_msgs/Bool "data: True"'/>
-    <node pkg="rosbag" type="record" name="recorder" args='record -O $(find wolf_demo_imu2d)/bag/trajectory_recording$(arg suffix).bag "/wolf_ros_node/trajectory" "/wolf_ros_node/pose_pose_with_cov"'/>
+    
+    <group if="$(arg record)">
+      <node pkg="rosbag" type="record" name="recorder" args='record -O $(find wolf_demo_imu2d)/bag/trajectory_recording$(arg suffix).bag "/wolf_ros_node/trajectory" "/wolf_ros_node/pose_pose_with_cov"'/>
+    </group>
 
     <node pkg="tf"
           type="static_transform_publisher"
@@ -42,10 +47,13 @@
         required="true"
     	  args="-s $(arg sec) -r $(arg speed) --clock $(find wolf_demo_imu2d)/bag/$(arg bag).bag"/>      
 
-    <node type="rviz"
-          name="rviz"
-          pkg="rviz"
-          args="-d $(find wolf_demo_imu2d)/rviz/imu2d_demo.rviz"/>
+    <group if="$(arg rviz)">
+      <node type="rviz"
+            name="rviz"
+            pkg="rviz"
+            args="-d $(find wolf_demo_imu2d)/rviz/imu2d_demo.rviz"/>
+    </group>
+
     <node type="wolf_ros_node"
     	    name="wolf_ros_node"
     	    pkg="wolf_ros_node"
diff --git a/yaml/imu2d_test10.yaml b/yaml/imu2d_test10.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..00406b56b52c2c3f178dc880b61e24c83dabc2ab
--- /dev/null
+++ b/yaml/imu2d_test10.yaml
@@ -0,0 +1,115 @@
+config:
+  debug:
+    profiling: true
+    profiling_file: "~/wolf_demo_profiling_imu2d_test0.txt"
+    print_problem: false
+    print_period: 2
+    print_depth: 4
+    print_constr_by: false
+    print_metric: true
+    print_state_blocks: true
+  problem:
+    tree_manager:
+      type: "none"
+    frame_structure: "POV"
+    # dimension: 2
+    # prior:
+    #   mode: "fix"
+    #   $state:
+    #     P: [0,0]
+    #     O: [0]
+    #     V: [0,0]
+    #   time_tolerance: 0.1
+    dimension: 3
+    prior:
+      mode: "fix"
+      $state:
+        P: [0,0,0]
+        O: [0,0,0,1]
+        V: [0,0,0]
+      time_tolerance: 0.1
+    node_rate: 100
+  map:
+    type: "MapBase"
+    plugin: "core"
+      
+  solver:
+    follow: "solver.yaml"
+
+  sensors:
+    -
+      type: "SensorLaser2d"
+      name: "scanner_front_left"
+      plugin: "laser"
+      follow: "test_laser_params.yaml"
+
+  processors:
+    -
+      type: "ProcessorOdomIcp"
+      name: "processorodomicp"
+      sensor_name: "scanner_front_left"
+      plugin: "laser"
+      follow: "test_laser_processor.yaml"
+    # -
+    #   type: "ProcessorLoopClosureIcp"
+    #   name: "processorloopclosureicp"
+    #   sensor_name: "scanner_front_left"
+    #   plugin: "laser"
+    #   time_tolerance: 0.1
+    #   apply_loss_function: true
+    #   keyframe_vote:
+    #     voting_active: false
+    #   recent_frames_ignored: 3
+    #   frames_ignored_after_loop: 0
+    #   max_error_threshold: 0.02
+    #   min_points_percent: 40
+    #   max_loops: 3
+    #   max_candidates: 5
+    #   max_attempts: 15
+    #   candidate_generation: "random" # 'random' or 'tree'
+    #   icp:
+    #     follow: "csm.yaml"
+
+      
+  ROS subscriber:
+    -
+      package: "wolf_ros_laser"
+      type: "SubscriberLaser2d"
+      topic: "/ana/sensors/scan"
+      sensor_name: "scanner_front_left"
+      load_params_from_msg: true
+
+  ROS publisher:
+    -
+      package: "wolf_ros_node"
+      type: "PublisherTf"
+      topic: " "
+      period: 0.1
+      follow: "test_publisher_ros_node.yaml"
+    -
+      package: "wolf_ros_node"
+      type: "PublisherGraph"
+      topic: "graph"
+      period: 1
+      viz_overlapped_factors: true
+    # -
+    #   package: "wolf_ros_node"
+    #   type: "PublisherTrajectory"
+    #   topic: "trajectory"
+    #   period: 1
+    #   frame_id: "map"
+    # -
+    #   package: "wolf_ros_laser"
+    #   type: "PublisherLaserMap"
+    #   topic: "map"
+    #   period: 1
+    #   map_frame_id: "map"
+    #   update_dist_th: 0.05
+    #   update_angle_th: 0.05
+    #   max_n_cells: 1000000
+    #   grid_size: 0.1
+    #   p_free: 0.3
+    #   p_obst: 0.8
+    #   p_free_th: 0.2
+    #   p_obst_th: 0.9
+    #   discard_max_range: true
diff --git a/yaml/imu2d_test15.yaml b/yaml/imu2d_test15.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..853c457a44352782cdb64cf11e7b9a27f6b954ca
--- /dev/null
+++ b/yaml/imu2d_test15.yaml
@@ -0,0 +1,168 @@
+config:
+  debug:
+    profiling: true
+    profiling_file: "~/wolf_demo_profiling_imu2d_test5.txt"
+    print_problem: false
+    print_period: 2
+    print_depth: 4
+    print_constr_by: false
+    print_metric: true
+    print_state_blocks: true
+  problem:
+    tree_manager:
+      type: "none"
+    frame_structure: "POV"
+    # dimension: 2
+    # prior:
+    #   mode: "fix"
+    #   $state:
+    #     P: [0,0]
+    #     O: [0]
+    #     V: [0,0]
+    #   time_tolerance: 0.1
+    dimension: 3
+    prior:
+      mode: "factor"
+      $state:
+        P: [0,0,0]
+        O: [0,0,0,1]
+        V: [0,0,0]
+      $sigma:
+        P: [0.01, 0.01, 0.01]
+        O: [1, 1, 1]
+        V: [0.01, 0.01, 0.01]
+      time_tolerance: 0.1
+    node_rate: 100
+  map:
+    type: "MapBase"
+    plugin: "core"
+      
+  solver:
+    follow: "solver.yaml"
+
+    
+  sensors:
+    -
+      type: "SensorLaser2d"
+      name: "scanner_front_left"
+      plugin: "laser"
+      follow: "test_laser_params.yaml"
+    -
+      type: "SensorImu"
+      name: "bno"
+      plugin: "imu"
+      extrinsic:
+        pose: [0,0,0,0,0,0,1]  
+      follow: "test_imu_params_bno.yaml" 
+    -
+      type: "SensorImu"
+      name: "microstrain"
+      plugin: "imu"
+      extrinsic:
+        pose: [0,0,0,0,0,0,1]  
+      follow: "test_imu_params_microstrain.yaml" 
+
+  processors:
+    -
+      type: "ProcessorOdomIcp"
+      name: "processorodomicp"
+      sensor_name: "scanner_front_left"
+      plugin: "laser"
+      follow: "test_laser_processor.yaml"
+    # -
+    #   type: "ProcessorLoopClosureIcp"
+    #   name: "processorloopclosureicp"
+    #   sensor_name: "scanner_front_left"
+    #   plugin: "laser"
+    #   time_tolerance: 0.1
+    #   apply_loss_function: true
+    #   keyframe_vote:
+    #     voting_active: false
+    #   recent_frames_ignored: 10
+    #   frames_ignored_after_loop: 0
+    #   max_error_threshold: 0.02
+    #   min_points_percent: 40
+    #   max_loops: 1
+    #   max_candidates: 5
+    #   max_attempts: 5
+    #   candidate_generation: "random" # 'random' or 'tree'
+    #   icp:
+    #     follow: "csm.yaml"
+    -
+      type: "ProcessorImu"
+      name: "processorimubno"
+      sensor_name: "bno"
+      plugin: "imu"
+      follow: "test_imu_processor_bno.yaml"
+    -
+      type: "ProcessorImu"
+      name: "processorimumicro"
+      sensor_name: "microstrain"
+      plugin: "imu"
+      follow: "test_imu_processor_microstrain.yaml"
+      
+  ROS subscriber:
+    -
+      package: "wolf_ros_laser"
+      type: "SubscriberLaser2d"
+      # topic: "/ana/sensors/scan"
+      topic: "/helena/sensors/scan"
+      sensor_name: "scanner_front_left"
+      load_params_from_msg: true
+    -
+      package: "wolf_ros_imu"
+      type: "SubscriberImuEnableable"
+      # topic: "/ana/sensors/bno055_imu/imumal"
+      topic: "/helena/sensors/bno055_imu/imumal"
+      sensor_name: "bno"
+      follow: "test_imu_subscriber_bno.yaml"
+    -
+      package: "wolf_ros_imu"
+      type: "SubscriberImuEnableable"
+      # topic: "/ana/imu/data"
+      topic: "/helena/imu/data"
+      sensor_name: "microstrain"
+      follow: "test_imu_subscriber_microstrain.yaml"
+
+  ROS publisher:
+    -
+      package: "wolf_ros_node"
+      type: "PublisherTf"
+      topic: " "
+      period: 0.1
+      follow: "test_publisher_ros_node.yaml"
+    -
+      package: "wolf_ros_node"
+      type: "PublisherGraph"
+      topic: "graph"
+      period: 1
+      viz_overlapped_factors: true
+    # -
+    #   package: "wolf_ros_node"
+    #   type: "PublisherTrajectory"
+    #   topic: "trajectory"
+    #   period: 1
+    #   frame_id: "map"
+    -
+      package: "wolf_ros_node"
+      type: "PublisherPose"
+      topic: "pose"
+      extrinsics: false
+      period: 1
+      frame_id: "map"
+    # -
+    #   package: "wolf_ros_laser"
+    #   type: "PublisherLaserMap"
+    #   topic: "map"
+    #   period: 1
+    #   map_frame_id: "map"
+    #   update_dist_th: 0.05
+    #   update_angle_th: 0.05
+    #   max_n_cells: 1000000
+    #   grid_size: 0.1
+    #   p_free: 0.3
+    #   p_obst: 0.8
+    #   p_free_th: 0.2
+    #   p_obst_th: 0.9
+    #   discard_max_range: true
+