diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..85c6b68e9eaccb40a1f8ba91b3a77074f1734fe8
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1 @@
+bag/*
diff --git a/launch/imu2d_demo.launch b/launch/imu2d_demo.launch
index 7d5a13654a7adbe5cc9118419e018df7f1f44b17..ce4520c3193fd942f72ca6b47be0c90e98f2668f 100644
--- a/launch/imu2d_demo.launch
+++ b/launch/imu2d_demo.launch
@@ -12,7 +12,7 @@
     <arg name="launch_pref" value="gdb -ex run --args" if="$(arg gdb)" />
     <!-- 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="rostopic" type="rostopic" name="" args='pub /ana/sensors/bno055_imu/imu/topic_enable std_msgs/Bool "data: True"'/>
 
     <node pkg="tf"
           type="static_transform_publisher"
diff --git a/rviz/imu2d_demo.rviz b/rviz/imu2d_demo.rviz
index 947506c5c96f62226c02d7cbea8512bb08725351..921254e521956da2cbd5c00ed0a0425943f34de3 100644
--- a/rviz/imu2d_demo.rviz
+++ b/rviz/imu2d_demo.rviz
@@ -11,7 +11,6 @@ Panels:
         - /LaserScan1
         - /Landmarks1
         - /Factors1
-        - /Factors1/Status1
         - /Factors1/Namespaces1
         - /Trajectory1
         - /Trajectory1/Namespaces1
@@ -345,7 +344,7 @@ Visualization Manager:
       Enabled: true
       History Length: 1
       Name: Imu
-      Topic: /helena/sensors/bno055_imu/imu_remapped
+      Topic: /helena/sensors/imu/data
       Unreliable: false
       Value: true
   Enabled: true
diff --git a/yaml/csm.yaml b/yaml/csm.yaml
index 59f62f628e15cb484f40d3b0ce50b404dfdd317d..2dd8f8680123cbf977c7bd7defcb0c65dd1d33f5 100644
--- a/yaml/csm.yaml
+++ b/yaml/csm.yaml
@@ -1,35 +1,45 @@
-max_iterations:                      50
-max_correspondence_dist:             10
-use_corr_tricks:                     1
-outliers_maxPerc:                    0.9
-use_point_to_line_distance:          1
-outliers_adaptive_order:             0.7
-outliers_adaptive_mult:              1.5
-do_compute_covariance:               1
-
-max_angular_correction_deg:          10
-max_linear_correction:               1
-epsilon_xy:                          0
-epsilon_theta:                       0
-sigma:                               0.2
-restart:                             0
-restart_threshold_mean_error:        0
-restart_dt:                          0
-restart_dtheta:                      0
-clustering_threshold:                0
-orientation_neighbourhood:           0
-do_alpha_test:                       0
-do_alpha_test_thresholdDeg:          0
-do_visibility_test:                  0
-outliers_remove_doubles:             0
-debug_verify_tricks:                 0
-gpm_theta_bin_size_deg:              0
-gpm_extend_range_deg:                0
-gpm_interval:                        0
-min_reading:                         0.023
-max_reading:                         60
-use_ml_weights:                      0
-use_sigma_weights:                   0
-
-
-cov_factor:                          1
+verbose:                      false # prints debug messages
+
+# ALGORITHM OPTIONS
+use_point_to_line_distance:   true  # use PlICP (true) or use vanilla ICP (false).
+max_angular_correction_deg:   180   # Maximum angular displacement between scans (deg)
+max_linear_correction:        10    # Maximum translation between scans (m)
+
+max_correspondence_dist:      0.5   # Maximum distance for a correspondence to be valid
+use_corr_tricks:              true  # Use smart tricks for finding correspondences. Only influences speed; not convergence.
+debug_verify_tricks:          false # Checks that find_correspondences_tricks give the right answer
+
+# STOP CRITERIA
+max_iterations:               50    # maximum iterations
+epsilon_xy:                   1e-4  # distance change
+epsilon_theta:                1e-3  # angle change
+
+# RESTART
+restart:                      false # Restart algorithm
+restart_threshold_mean_error: 0     # Threshold for restarting
+restart_dt:                   0     # Displacement for restarting
+restart_dtheta:               0     # Displacement for restarting
+
+# DISCARDING POINTS/CORRESPONDENCES
+min_reading:                  0.023 # discard rays outside of this interval
+max_reading:                  60    # discard rays outside of this interval
+outliers_maxPerc:             0.9   # Percentage of correspondences to consider: if 0.9, always discard the top 10% of correspondences with more error
+outliers_adaptive_order:      0.7   # Parameters describing a simple adaptive algorithm for discarding
+outliers_adaptive_mult:       1.5   # Parameters describing a simple adaptive algorithm for discarding
+outliers_remove_doubles:      false # Do not allow two different correspondences to share a point
+do_visibility_test:           false # If initial guess, visibility test can discard points that are not visible
+do_alpha_test:                false # Discard correspondences based on the angles
+do_alpha_test_thresholdDeg:   10    # 
+
+# POINT ORIENTATION
+clustering_threshold:         0.5   # Max-distance clustering for point orientation
+orientation_neighbourhood:    4     # Number of neighbour rays used to estimate the orientation
+
+# WEIGHTS
+use_ml_weights:               false # weight the impact of each correspondence. This works fabolously if the first scan has no noise.
+use_sigma_weights:            false # If the field "readings_sigma" is used to weight the correspondence by 1/sigma^2
+sigma:                        0.2   # Noise of the scan
+
+# COVARIANCE
+do_compute_covariance:        true
+cov_factor:                   5     # Factor multiplying the cov output of csm
diff --git a/yaml/csm_inactive.yaml b/yaml/csm_inactive.yaml
index b99171d815291adbd3d320cc2e67bd53a128fd85..b5336a90b5dad444b2bfd9d946582ab8d987cfc1 100644
--- a/yaml/csm_inactive.yaml
+++ b/yaml/csm_inactive.yaml
@@ -1,35 +1,47 @@
-max_iterations:                      50
-max_correspondence_dist:             10
-use_corr_tricks:                     1
-outliers_maxPerc:                    0.9
-use_point_to_line_distance:          1
-outliers_adaptive_order:             0.7
-outliers_adaptive_mult:              1.5
-do_compute_covariance:               1
-
-max_angular_correction_deg:          10
-max_linear_correction:               1
-epsilon_xy:                          0
-epsilon_theta:                       0
-sigma:                               0.2
-restart:                             0
-restart_threshold_mean_error:        0
-restart_dt:                          0
-restart_dtheta:                      0
-clustering_threshold:                0
-orientation_neighbourhood:           0
-do_alpha_test:                       0
-do_alpha_test_thresholdDeg:          0
-do_visibility_test:                  0
-outliers_remove_doubles:             0
-debug_verify_tricks:                 0
-gpm_theta_bin_size_deg:              0
-gpm_extend_range_deg:                0
-gpm_interval:                        0
-min_reading:                         0.023
-max_reading:                         60
-use_ml_weights:                      0
-use_sigma_weights:                   0
+verbose:                      false # prints debug messages
 
+# ALGORITHM OPTIONS
+use_point_to_line_distance:   true  # use PlICP (true) or use vanilla ICP (false).
+max_angular_correction_deg:   180   # Maximum angular displacement between scans (deg)
+max_linear_correction:        10    # Maximum translation between scans (m)
+
+max_correspondence_dist:      0.5   # Maximum distance for a correspondence to be valid
+use_corr_tricks:              true  # Use smart tricks for finding correspondences. Only influences speed; not convergence.
+debug_verify_tricks:          false # Checks that find_correspondences_tricks give the right answer
+
+# STOP CRITERIA
+max_iterations:               50    # maximum iterations
+epsilon_xy:                   1e-4  # distance change
+epsilon_theta:                1e-3  # angle change
+
+# RESTART
+restart:                      false # Restart algorithm
+restart_threshold_mean_error: 0     # Threshold for restarting
+restart_dt:                   0     # Displacement for restarting
+restart_dtheta:               0     # Displacement for restarting
+
+# DISCARDING POINTS/CORRESPONDENCES
+min_reading:                  0.023 # discard rays outside of this interval
+max_reading:                  60    # discard rays outside of this interval
+outliers_maxPerc:             0.9   # Percentage of correspondences to consider: if 0.9, always discard the top 10% of correspondences with more error
+outliers_adaptive_order:      0.7   # Parameters describing a simple adaptive algorithm for discarding
+outliers_adaptive_mult:       1.5   # Parameters describing a simple adaptive algorithm for discarding
+outliers_remove_doubles:      false # Do not allow two different correspondences to share a point
+do_visibility_test:           false # If initial guess, visibility test can discard points that are not visible
+do_alpha_test:                false # Discard correspondences based on the angles
+do_alpha_test_thresholdDeg:   10    # 
+
+# POINT ORIENTATION
+clustering_threshold:         0.5   # Max-distance clustering for point orientation
+orientation_neighbourhood:    4     # Number of neighbour rays used to estimate the orientation
+
+# WEIGHTS
+use_ml_weights:               false # weight the impact of each correspondence. This works fabolously if the first scan has no noise.
+use_sigma_weights:            false # If the field "readings_sigma" is used to weight the correspondence by 1/sigma^2
+sigma:                        0.2   # Noise of the scan
+
+# COVARIANCE
+do_compute_covariance:        true
+cov_factor:                   5     # Factor multiplying the cov output of csm
 
 cov_factor:                          9999999999999999999999
diff --git a/yaml/csm_old.yaml b/yaml/csm_old.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..59f62f628e15cb484f40d3b0ce50b404dfdd317d
--- /dev/null
+++ b/yaml/csm_old.yaml
@@ -0,0 +1,35 @@
+max_iterations:                      50
+max_correspondence_dist:             10
+use_corr_tricks:                     1
+outliers_maxPerc:                    0.9
+use_point_to_line_distance:          1
+outliers_adaptive_order:             0.7
+outliers_adaptive_mult:              1.5
+do_compute_covariance:               1
+
+max_angular_correction_deg:          10
+max_linear_correction:               1
+epsilon_xy:                          0
+epsilon_theta:                       0
+sigma:                               0.2
+restart:                             0
+restart_threshold_mean_error:        0
+restart_dt:                          0
+restart_dtheta:                      0
+clustering_threshold:                0
+orientation_neighbourhood:           0
+do_alpha_test:                       0
+do_alpha_test_thresholdDeg:          0
+do_visibility_test:                  0
+outliers_remove_doubles:             0
+debug_verify_tricks:                 0
+gpm_theta_bin_size_deg:              0
+gpm_extend_range_deg:                0
+gpm_interval:                        0
+min_reading:                         0.023
+max_reading:                         60
+use_ml_weights:                      0
+use_sigma_weights:                   0
+
+
+cov_factor:                          1
diff --git a/yaml/imu2d_test1.yaml b/yaml/imu2d_test1.yaml
index b674e10644b8d88ce3176dfb00c4efa2f711f664..549158408686f6e793fa8505e7fbffe4aa1da6d9 100644
--- a/yaml/imu2d_test1.yaml
+++ b/yaml/imu2d_test1.yaml
@@ -26,18 +26,8 @@ config:
     plugin: "core"
       
   solver:
-    max_num_iterations: 10
-    verbose: 0
-    period: 0
-    update_immediately: false
-    n_threads: 2
-    compute_cov: false
-    function_tolerance: 1e-6
-    gradient_tolerance: 1e-6
-    minimizer: "LEVENBERG_MARQUARDT"
-    use_nonmonotonic_steps: False
-    max_consecutive_nonmonotonic_steps: 10
-    
+    follow: "solver.yaml"
+
   sensors:
     -
       type: "SensorOdom2d"
@@ -135,4 +125,3 @@ config:
       p_free_th: 0.2
       p_obst_th: 0.9
       discard_max_range: true
-
diff --git a/yaml/imu2d_test4.yaml b/yaml/imu2d_test4.yaml
index e9dd597b83e9c1cc4bfe09d68018bc5ce9f2e9cb..a338b354e4b6a3b66f4b64752f5f4bc2f5891900 100644
--- a/yaml/imu2d_test4.yaml
+++ b/yaml/imu2d_test4.yaml
@@ -2,8 +2,12 @@ config:
   debug:
     profiling: true
     profiling_file: "~/wolf_demo_profiling_imu2d_test4.txt"
-    print_problem: false
+    print_problem: true
     print_period: 5
+    print_depth: 4
+    print_constr_by: false
+    print_metric: true
+    print_state_blocks: true
   problem:
     tree_manager:
       type: "none"
@@ -16,14 +20,13 @@ config:
         O: [0]
         V: [0,0]
       time_tolerance: 0.1
+    node_rate: 100 
+  map:
+    type: "MapBase"
+    plugin: "core"
       
   solver:
-    max_num_iterations: 100
-    verbose: 0
-    period: 0
-    update_immediately: false
-    n_threads: 2
-    compute_cov: false
+    follow: "solver.yaml"
     
   sensors:
     -
@@ -31,11 +34,11 @@ config:
       name: "scanner_front_left"
       plugin: "laser"
       follow: "test_laser_params.yaml"
-    -
-      type: "SensorImu2d"
-      name: "bno"
-      plugin: "imu"
-      follow: "test_imu_params.yaml" 
+        # -
+        #   type: "SensorImu2d"
+        #   name: "bno"
+        #   plugin: "imu"
+        #   follow: "test_imu_params.yaml" 
     -
       type: "SensorImu2d"
       name: "microstrain"
@@ -66,7 +69,7 @@ config:
     -
       package: "wolf_ros_laser"
       type: "SubscriberLaser2d"
-      topic: "/ana/sensors/scan"
+      topic: "/helena/sensors/scan"
       sensor_name: "scanner_front_left"
       load_params_from_msg: true
     -
@@ -76,7 +79,7 @@ config:
       #topic: "/ana/sensors/bno055_imu/imu"
       #sensor_name: "bno"
       #microstrain
-      topic: "/ana/imu/data"
+      topic: "/helena/sensors/imu/data"
       sensor_name: "microstrain"
       follow: "test_imu_subscriber.yaml"
 
diff --git a/yaml/solver.yaml b/yaml/solver.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..ef04fd43fffb6b5b6ede533e948e125d365f212d
--- /dev/null
+++ b/yaml/solver.yaml
@@ -0,0 +1,13 @@
+minimizer: LEVENBERG_MARQUARDT
+max_num_iterations: 20
+verbose: 0
+period: 0.0
+n_threads: 2
+interrupt_on_problem_change: false
+min_num_iterations: 5 #if update immediately
+compute_cov: false
+cov_enum: 3 # if compute_cov
+cov_period: 1 # if compute_cov
+function_tolerance: 1e-8
+gradient_tolerance: 1e-9
+use_nonmonotonic_steps: false
diff --git a/yaml/test_imu_subscriber.yaml b/yaml/test_imu_subscriber.yaml
index cd15bf7b922f91f46823d93ae914defd97487bdc..8aa2403f6c288b9256abe8cd33a86f22b090d6f1 100644
--- a/yaml/test_imu_subscriber.yaml
+++ b/yaml/test_imu_subscriber.yaml
@@ -1,20 +1,20 @@
-#bno:
-dt: 0.016
-imu_x_axis: 3
-imu_y_axis: -2
-imu_z_axis: 1
-cov_source: "sensor"
-in_degrees: true
-##microstrain:
-#dt: 0.01
-#imu_x_axis: 1
+##bno:
+#dt: 0.016
+#imu_x_axis: 3
 #imu_y_axis: -2
-#imu_z_axis: -3
+#imu_z_axis: 1
 #cov_source: "sensor"
-#in_degrees: false
+#in_degrees: true
+#microstrain:
+dt: 0.01
+imu_x_axis: 1
+imu_y_axis: -2
+imu_z_axis: -3
+cov_source: "sensor"
+in_degrees: false
 
 #all
 topic_enable: "/ana/sensors/bno055_imu/imu/topic_enable"
-static_init_duration: 7
+static_init_duration: 1
 lowpass_filter: true
 lowpass_cutoff_freq: 5
diff --git a/yaml/test_laser_processor.yaml b/yaml/test_laser_processor.yaml
index d2aaceeee6da6c882b2965f3a7ef20989d812cf8..ae63e5c449a9c38b02edfae415b5aab12d67e98b 100644
--- a/yaml/test_laser_processor.yaml
+++ b/yaml/test_laser_processor.yaml
@@ -7,7 +7,8 @@ keyframe_vote:
   min_error: 999
   max_points: 0
 max_new_features: 15
-follow: "csm.yaml"
+icp:
+  follow: "csm.yaml"
 time_tolerance: 0.05 #Half the sample time
 state_getter: true
 state_priority: 10
diff --git a/yaml/test_laser_processor_inactive.yaml b/yaml/test_laser_processor_inactive.yaml
index a584d2cde4e2f8852b231a822d6160c2a08add4b..38d70089dc29746f0347874fac97ebf5f0bccde7 100644
--- a/yaml/test_laser_processor_inactive.yaml
+++ b/yaml/test_laser_processor_inactive.yaml
@@ -7,7 +7,8 @@ keyframe_vote:
   min_error: 999
   max_points: 0
 max_new_features: 15
-follow: "csm_inactive.yaml"
+icp:
+  follow: "csm_inactive.yaml"
 time_tolerance: 0.05 #Half the sample time
 state_getter: true
 state_priority: 10