From 694fb064cb4dfa013dfc639607843cc450edae2f Mon Sep 17 00:00:00 2001
From: Idril Geer <igeer@iri.upc.edu>
Date: Wed, 2 Feb 2022 18:32:23 +0100
Subject: [PATCH] update yamls

---
 launch/imu2d_demo.launch                      |  11 +-
 rviz/imu2d_demo.rviz                          | 164 ++++--------------
 yaml/csm_inactive.yaml                        |   2 -
 yaml/imu2d_test0.yaml                         |  31 +++-
 yaml/imu2d_test1.yaml                         |  63 ++++---
 yaml/imu2d_test5.yaml                         |  71 +++++---
 yaml/test_imu_params.yaml                     |  20 ---
 yaml/test_imu_params_bno.yaml                 |  10 ++
 yaml/test_imu_params_microstrain.yaml         |  10 ++
 yaml/test_imu_processor_bno.yaml              |  12 ++
 ...ml => test_imu_processor_microstrain.yaml} |   3 +-
 yaml/test_imu_subscriber.yaml                 |  20 ---
 yaml/test_imu_subscriber_bno.yaml             |  13 ++
 yaml/test_imu_subscriber_microstrain.yaml     |  13 ++
 yaml/test_laser_processor.yaml                |   2 +-
 yaml/test_publisher_ros_node.yaml             |   4 +-
 16 files changed, 224 insertions(+), 225 deletions(-)
 delete mode 100644 yaml/test_imu_params.yaml
 create mode 100644 yaml/test_imu_params_bno.yaml
 create mode 100644 yaml/test_imu_params_microstrain.yaml
 create mode 100644 yaml/test_imu_processor_bno.yaml
 rename yaml/{test_imu_processor.yaml => test_imu_processor_microstrain.yaml} (81%)
 delete mode 100644 yaml/test_imu_subscriber.yaml
 create mode 100644 yaml/test_imu_subscriber_bno.yaml
 create mode 100644 yaml/test_imu_subscriber_microstrain.yaml

diff --git a/launch/imu2d_demo.launch b/launch/imu2d_demo.launch
index ce4520c..505116d 100644
--- a/launch/imu2d_demo.launch
+++ b/launch/imu2d_demo.launch
@@ -12,27 +12,28 @@
     <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 /ana/sensors/bno055_imu/imu/topic_enable std_msgs/Bool "data: True"'/>
+    <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.bag -e "(.*)wolf_ros_node(.*)"'/>
 
     <node pkg="tf"
           type="static_transform_publisher"
           name="static_tf"
-      args="0 0 0 0 0 0 /helena/base_footprint /helena/base_link  100"/>
+      args="0 0 0 0 0 0 /ana/base_footprint /ana/base_link  100"/>
 
     <node pkg="tf"
           type="static_transform_publisher"
           name="static_tf2"
-      args="0 0 0 0 0 0 /helena/base_link /helena/imu_bno055  100"/>
+      args="0 0 0 0 0 0 /ana/base_link /ana/imu_bno055  100"/>
 
     <node pkg="tf"
           type="static_transform_publisher"
           name="static_tf3"
-      args="0 0 0 3.14159265 0 0 /helena/base_link /helena/velodyne  100"/>
+      args="0 0 0 3.14159265 0 0 /ana/base_link /ana/velodyne  100"/>
 
     <node pkg="tf"
           type="static_transform_publisher"
           name="static_tf4"
-      args="-0.11 0.01 0 0 0 0 /helena/base_link /helena/imu_microstrain  100"/>
+      args="-0.11 0.01 0 0 0 0 /ana/base_link /ana/imu_microstrain  100"/>
 
     <node pkg="rosbag"
     	  type="play"
diff --git a/rviz/imu2d_demo.rviz b/rviz/imu2d_demo.rviz
index 2de424e..e1e2090 100644
--- a/rviz/imu2d_demo.rviz
+++ b/rviz/imu2d_demo.rviz
@@ -6,6 +6,7 @@ Panels:
       Expanded:
         - /Global Options1
         - /TF1
+        - /TF1/Frames1
         - /TF1/Tree1
         - /Odometry1/Shape1
         - /LaserScan1
@@ -68,139 +69,40 @@ Visualization Manager:
       Frame Timeout: 15
       Frames:
         All Enabled: false
-        helena/base_footprint:
-          Value: true
-        helena/base_link:
-          Value: true
-        helena/camera_bottom_screw_frame:
-          Value: true
-        helena/camera_color_frame:
-          Value: true
-        helena/camera_color_optical_frame:
-          Value: true
-        helena/camera_depth_frame:
-          Value: true
-        helena/camera_depth_optical_frame:
-          Value: true
-        helena/camera_infra1_frame:
-          Value: true
-        helena/camera_infra1_optical_frame:
-          Value: true
-        helena/camera_infra2_frame:
-          Value: true
-        helena/camera_infra2_optical_frame:
-          Value: true
-        helena/camera_link:
-          Value: true
-        helena/front_left_axle:
-          Value: true
-        helena/front_left_hub:
-          Value: true
-        helena/front_left_wheel:
-          Value: true
-        helena/front_right_axle:
-          Value: true
-        helena/front_right_hub:
-          Value: true
-        helena/front_right_wheel:
-          Value: true
-        helena/front_sonar:
-          Value: true
-        helena/helena_box:
-          Value: true
-        helena/helena_realsense_support:
-          Value: true
-        helena/imu_bno055:
-          Value: true
-        helena/imu_bno055_base:
-          Value: true
-        helena/imu_microstrain:
-          Value: true
-        helena/odom:
-          Value: true
-        helena/rear_left_axle:
-          Value: true
-        helena/rear_left_hub:
-          Value: true
-        helena/rear_left_wheel:
-          Value: true
-        helena/rear_right_axle:
-          Value: true
-        helena/rear_right_hub:
-          Value: true
-        helena/rear_right_wheel:
-          Value: true
-        helena/rear_sonar:
-          Value: true
-        helena/robosense:
-          Value: true
-        helena/robosense_base:
-          Value: true
-        helena/top_plate:
+        ana/base_footprint:
+          Value: false
+        ana/base_footprint_robot:
+          Value: false
+        ana/base_link:
           Value: true
-        helena/velodyne:
+        ana/imu_bno055:
+          Value: false
+        ana/imu_microstrain:
+          Value: false
+        ana/odom:
           Value: true
+        ana/velodyne:
+          Value: false
         map:
           Value: false
-      Marker Scale: 1
+      Marker Scale: 5
       Name: TF
       Show Arrows: false
       Show Axes: true
       Show Names: true
       Tree:
         map:
-          helena/odom:
-            helena/base_footprint:
-              helena/base_link:
-                helena/front_left_axle:
-                  helena/front_left_hub:
-                    helena/front_left_wheel:
-                      {}
-                helena/front_right_axle:
-                  helena/front_right_hub:
-                    helena/front_right_wheel:
-                      {}
-                helena/front_sonar:
-                  {}
-                helena/imu_bno055:
-                  {}
-                helena/imu_microstrain:
+          ana/odom:
+            ana/base_footprint:
+              ana/base_link:
+                ana/imu_bno055:
                   {}
-                helena/rear_left_axle:
-                  helena/rear_left_hub:
-                    helena/rear_left_wheel:
-                      {}
-                helena/rear_right_axle:
-                  helena/rear_right_hub:
-                    helena/rear_right_wheel:
-                      {}
-                helena/rear_sonar:
+                ana/imu_microstrain:
                   {}
-                helena/top_plate:
-                  helena/helena_box:
-                    {}
-                  helena/helena_realsense_support:
-                    helena/camera_bottom_screw_frame:
-                      helena/camera_link:
-                        helena/camera_color_frame:
-                          helena/camera_color_optical_frame:
-                            {}
-                        helena/camera_depth_frame:
-                          helena/camera_depth_optical_frame:
-                            {}
-                        helena/camera_infra1_frame:
-                          helena/camera_infra1_optical_frame:
-                            {}
-                        helena/camera_infra2_frame:
-                          helena/camera_infra2_optical_frame:
-                            {}
-                  helena/robosense_base:
-                    helena/robosense:
-                      {}
-                helena/velodyne:
+                ana/velodyne:
                   {}
-              helena/imu_bno055_base:
-                {}
+            ana/base_footprint_robot:
+              {}
       Update Interval: 0
       Value: true
     - Angle Tolerance: 0.10000000149011612
@@ -248,7 +150,7 @@ Visualization Manager:
       Class: rviz/LaserScan
       Color: 250; 84; 255
       Color Transformer: FlatColor
-      Decay Time: 100
+      Decay Time: 10
       Enabled: false
       Invert Rainbow: false
       Max Color: 255; 255; 255
@@ -260,7 +162,7 @@ Visualization Manager:
       Size (Pixels): 3
       Size (m): 0.05999999865889549
       Style: Flat Squares
-      Topic: /helena/sensors/scan
+      Topic: /ana/sensors/scan
       Unreliable: false
       Use Fixed Frame: true
       Use rainbow: true
@@ -278,10 +180,14 @@ Visualization Manager:
       Marker Topic: /wolf_ros_node/graph_factors
       Name: Factors
       Namespaces:
+        factors_processorimu2dbno: true
         factors_processorimu2dmicro: true
+        factors_processorloopclosureicp: true
         factors_processorodom2d: true
         factors_processorodomicp: true
+        factors_text_processorimu2dbno: false
         factors_text_processorimu2dmicro: false
+        factors_text_processorloopclosureicp: false
         factors_text_processorodom2d: false
         factors_text_processorodomicp: false
         factors_text_unnamed_processor: false
@@ -302,7 +208,7 @@ Visualization Manager:
       Length: 1
       Name: Axes
       Radius: 0.10000000149011612
-      Reference Frame: helena/base_link
+      Reference Frame: ana/base_link
       Value: true
     - Alpha: 1
       Autocompute Intensity Bounds: true
@@ -342,13 +248,13 @@ Visualization Manager:
       Unreliable: false
       Use Timestamp: false
       Value: false
-    - Alpha: 1
+    - Alpha: 0.5
       Class: rviz_plugin_tutorials/Imu
       Color: 204; 51; 204
       Enabled: true
       History Length: 1
       Name: Imu
-      Topic: /helena/sensors/imu/data
+      Topic: /ana/imu/data_remapped
       Unreliable: false
       Value: true
   Enabled: true
@@ -379,16 +285,16 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 25.464996337890625
+      Distance: 27.63036346435547
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 5.9762163162231445
-        Y: 1.321905493736267
-        Z: -1.1534262895584106
+        X: 2.3574845790863037
+        Y: -1.3090972900390625
+        Z: -0.7893345952033997
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
diff --git a/yaml/csm_inactive.yaml b/yaml/csm_inactive.yaml
index b5336a9..4ea6670 100644
--- a/yaml/csm_inactive.yaml
+++ b/yaml/csm_inactive.yaml
@@ -42,6 +42,4 @@ 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/imu2d_test0.yaml b/yaml/imu2d_test0.yaml
index c34ae67..5504762 100644
--- a/yaml/imu2d_test0.yaml
+++ b/yaml/imu2d_test0.yaml
@@ -2,7 +2,7 @@ config:
   debug:
     profiling: true
     profiling_file: "~/wolf_demo_profiling_imu2d_test0.txt"
-    print_problem: true
+    print_problem: false
     print_period: 2
     print_depth: 4
     print_constr_by: false
@@ -42,12 +42,32 @@ config:
       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: "/helena/sensors/scan"
+      topic: "/ana/sensors/scan"
       sensor_name: "scanner_front_left"
       load_params_from_msg: true
 
@@ -64,6 +84,13 @@ config:
       topic: "graph"
       period: 1
       viz_overlapped_factors: true
+    -
+      package: "wolf_ros_node"
+      type: "PublisherPose"
+      topic: "pose"
+      period: 1
+      extrinsics: false
+      frame_id: "map"
     -
       package: "wolf_ros_laser"
       type: "PublisherLaserMap"
diff --git a/yaml/imu2d_test1.yaml b/yaml/imu2d_test1.yaml
index 2422ded..5ae1356 100644
--- a/yaml/imu2d_test1.yaml
+++ b/yaml/imu2d_test1.yaml
@@ -2,7 +2,7 @@ config:
   debug:
     profiling: true
     profiling_file: "~/wolf_demo_profiling_imu2d_test1.txt"
-    print_problem: true
+    print_problem: false
     print_period: 2
     print_depth: 4
     print_constr_by: false
@@ -39,16 +39,16 @@ 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_bno.yaml" 
     -
       type: "SensorImu2d"
       name: "microstrain"
       plugin: "imu"
-      follow: "test_imu_params.yaml" 
+      follow: "test_imu_params_microstrain.yaml" 
 
   processors:
     -
@@ -63,39 +63,62 @@ config:
       sensor_name: "scanner_front_left"
       plugin: "laser"
       follow: "test_laser_processor.yaml"
-        #-
-        #  type: "ProcessorImu2d"
-        #  name: "processorimu2dbno"
-        #  sensor_name: "bno"
-        #  plugin: "imu"
-        #  follow: "test_imu_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"
+    -
+      type: "ProcessorImu2d"
+      name: "processorimu2dbno"
+      sensor_name: "bno"
+      plugin: "imu"
+      follow: "test_imu_processor_bno.yaml"
     -
       type: "ProcessorImu2d"
       name: "processorimu2dmicro"
       sensor_name: "microstrain"
       plugin: "imu"
-      follow: "test_imu_processor.yaml"
+      follow: "test_imu_processor_microstrain.yaml"
       
   ROS subscriber:
     -
       package: "wolf_ros_node"
       type: "SubscriberOdom2d"
-      topic: "/helena/odom"
+      topic: "/ana/odom"
       sensor_name: "odom2d"
     -
       package: "wolf_ros_laser"
       type: "SubscriberLaser2d"
-      topic: "/helena/sensors/scan"
+      topic: "/ana/sensors/scan"
       sensor_name: "scanner_front_left"
       load_params_from_msg: true
     -
       package: "wolf_ros_imu"
       type: "SubscriberImuEnableable"
-      #topic: "/helena/sensors/bno055_imu/imu"
-      #sensor_name: "bno"
-      topic: "/helena/sensors/imu/data"
+      topic: "/ana/sensors/bno055_imu/imu"
+      sensor_name: "bno"
+      follow: "test_imu_subscriber_bno.yaml"
+    -
+      package: "wolf_ros_imu"
+      type: "SubscriberImuEnableable"
+      topic: "/ana/imu/data"
       sensor_name: "microstrain"
-      follow: "test_imu_subscriber.yaml"
+      follow: "test_imu_subscriber_microstrain.yaml"
 
   ROS publisher:
     -
diff --git a/yaml/imu2d_test5.yaml b/yaml/imu2d_test5.yaml
index 2546db5..e5243d4 100644
--- a/yaml/imu2d_test5.yaml
+++ b/yaml/imu2d_test5.yaml
@@ -1,9 +1,13 @@
 config:
   debug:
     profiling: true
-    profiling_file: "~/wolf_demo_profiling_imu2d_test1.txt"
-    print_problem: true
-    print_period: 5
+    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"
@@ -16,14 +20,14 @@ 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:
     -
@@ -35,12 +39,12 @@ config:
       type: "SensorImu2d"
       name: "bno"
       plugin: "imu"
-      follow: "test_imu_params.yaml" 
+      follow: "test_imu_params_bno.yaml" 
     -
       type: "SensorImu2d"
       name: "microstrain"
       plugin: "imu"
-      follow: "test_imu_params.yaml" 
+      follow: "test_imu_params_microstrain.yaml" 
 
   processors:
     -
@@ -49,18 +53,37 @@ config:
       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"
     -
       type: "ProcessorImu2d"
       name: "processorimu2dbno"
       sensor_name: "bno"
       plugin: "imu"
-      follow: "test_imu_processor.yaml"
-    #-
-    #  type: "ProcessorImu2d"
-    #  name: "processorimu2dmicro"
-    #  sensor_name: "microstrain"
-    #  plugin: "imu"
-    #  follow: "test_imu_processor.yaml"
+      follow: "test_imu_processor_bno.yaml"
+    -
+      type: "ProcessorImu2d"
+      name: "processorimu2dmicro"
+      sensor_name: "microstrain"
+      plugin: "imu"
+      follow: "test_imu_processor_microstrain.yaml"
       
   ROS subscriber:
     -
@@ -72,11 +95,15 @@ config:
     -
       package: "wolf_ros_imu"
       type: "SubscriberImuEnableable"
-      topic: "/ana/sensors/bno055_imu/imu"
+      topic: "/ana/sensors/bno055_imu/imumal"
       sensor_name: "bno"
-      #topic: "/ana/imu/data"
-      #sensor_name: "microstrain"
-      follow: "test_imu_subscriber.yaml"
+      follow: "test_imu_subscriber_bno.yaml"
+    -
+      package: "wolf_ros_imu"
+      type: "SubscriberImuEnableable"
+      topic: "/ana/imu/data"
+      sensor_name: "microstrain"
+      follow: "test_imu_subscriber_microstrain.yaml"
 
   ROS publisher:
     -
diff --git a/yaml/test_imu_params.yaml b/yaml/test_imu_params.yaml
deleted file mode 100644
index 9a08c32..0000000
--- a/yaml/test_imu_params.yaml
+++ /dev/null
@@ -1,20 +0,0 @@
-##bno
-#extrinsic:
-#  pose: [0,0,0]    
-#a_noise: 0.096 #best: 0.96
-#w_noise: 0.02
-#ab_initial_stdev: 5     # m/s2    - initial bias
-#wb_initial_stdev: 1     # rad/sec - initial bias
-#ab_rate_stdev: 0.0001    # m/s2/sqrt(s)
-#wb_rate_stdev: 0.0001    # rad/s/sqrt(s)
-#orthogonal_gravity: true
-#microstrain
-extrinsic:
-  pose: [0,0,0]    
-a_noise: 0.900
-w_noise: 0.01
-ab_initial_stdev: 0.5     # m/s2    - initial bias
-wb_initial_stdev: 0.1     # rad/sec - initial bias
-ab_rate_stdev: 0.0001    # m/s2/sqrt(s)
-wb_rate_stdev: 0.0001    # rad/s/sqrt(s)
-orthogonal_gravity: false
diff --git a/yaml/test_imu_params_bno.yaml b/yaml/test_imu_params_bno.yaml
new file mode 100644
index 0000000..a958a69
--- /dev/null
+++ b/yaml/test_imu_params_bno.yaml
@@ -0,0 +1,10 @@
+#bno
+extrinsic:
+  pose: [0,0,0]    
+a_noise: 0.096 #best: 0.96
+w_noise: 0.02
+ab_initial_stdev: 5     # m/s2    - initial bias
+wb_initial_stdev: 1     # rad/sec - initial bias
+ab_rate_stdev: 0.0001    # m/s2/sqrt(s)
+wb_rate_stdev: 0.0001    # rad/s/sqrt(s)
+orthogonal_gravity: true
diff --git a/yaml/test_imu_params_microstrain.yaml b/yaml/test_imu_params_microstrain.yaml
new file mode 100644
index 0000000..05b747b
--- /dev/null
+++ b/yaml/test_imu_params_microstrain.yaml
@@ -0,0 +1,10 @@
+#microstrain
+extrinsic:
+  pose: [0,0,0]    
+a_noise: 0.9 #0.9
+w_noise: 0.01 #0.01
+ab_initial_stdev: 0.05     # m/s2    - initial bias
+wb_initial_stdev: 0.01     # rad/sec - initial bias
+ab_rate_stdev: 0.00001    # m/s2/sqrt(s)
+wb_rate_stdev: 0.00001    # rad/s/sqrt(s)
+orthogonal_gravity: true
diff --git a/yaml/test_imu_processor_bno.yaml b/yaml/test_imu_processor_bno.yaml
new file mode 100644
index 0000000..79f6f50
--- /dev/null
+++ b/yaml/test_imu_processor_bno.yaml
@@ -0,0 +1,12 @@
+time_tolerance: 0.008   #Half the sample time, bno
+unmeasured_perturbation_std: 0.1
+keyframe_vote:                         
+  voting_active: false
+  max_time_span: 1
+  max_buff_length: 1000000
+  dist_traveled: 10
+  angle_turned: 3.14
+state_getter: true
+state_priority: 1
+apply_loss_function: false
+use_gravity_grid: false
diff --git a/yaml/test_imu_processor.yaml b/yaml/test_imu_processor_microstrain.yaml
similarity index 81%
rename from yaml/test_imu_processor.yaml
rename to yaml/test_imu_processor_microstrain.yaml
index 139d891..7ab3a8e 100644
--- a/yaml/test_imu_processor.yaml
+++ b/yaml/test_imu_processor_microstrain.yaml
@@ -1,4 +1,3 @@
-#time_tolerance: 0.008   #Half the sample time, bno
 time_tolerance: 0.005   #Half the sample time, microstrain
 unmeasured_perturbation_std: 0.1
 keyframe_vote:                         
@@ -8,6 +7,6 @@ keyframe_vote:
   dist_traveled: 10
   angle_turned: 3.14
 state_getter: true
-state_priority: 1
+state_priority: 0
 apply_loss_function: false
 use_gravity_grid: false
diff --git a/yaml/test_imu_subscriber.yaml b/yaml/test_imu_subscriber.yaml
deleted file mode 100644
index 8aa2403..0000000
--- a/yaml/test_imu_subscriber.yaml
+++ /dev/null
@@ -1,20 +0,0 @@
-##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
-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: 1
-lowpass_filter: true
-lowpass_cutoff_freq: 5
diff --git a/yaml/test_imu_subscriber_bno.yaml b/yaml/test_imu_subscriber_bno.yaml
new file mode 100644
index 0000000..34e3cf7
--- /dev/null
+++ b/yaml/test_imu_subscriber_bno.yaml
@@ -0,0 +1,13 @@
+#bno:
+dt: 0.016
+imu_x_axis: 3
+imu_y_axis: -2
+imu_z_axis: 1
+cov_source: "sensor"
+in_degrees: true
+
+#all
+topic_enable: "/helena/sensors/bno055_imu/imu/topic_enable"
+static_init_duration: 5
+lowpass_filter: true
+lowpass_cutoff_freq: 5
diff --git a/yaml/test_imu_subscriber_microstrain.yaml b/yaml/test_imu_subscriber_microstrain.yaml
new file mode 100644
index 0000000..7c747b9
--- /dev/null
+++ b/yaml/test_imu_subscriber_microstrain.yaml
@@ -0,0 +1,13 @@
+#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: "/helena/sensors/bno055_imu/imu/topic_enable"
+static_init_duration: 3
+lowpass_filter: true
+lowpass_cutoff_freq: 5
diff --git a/yaml/test_laser_processor.yaml b/yaml/test_laser_processor.yaml
index ae63e5c..c10e2a5 100644
--- a/yaml/test_laser_processor.yaml
+++ b/yaml/test_laser_processor.yaml
@@ -13,4 +13,4 @@ time_tolerance: 0.05 #Half the sample time
 state_getter: true
 state_priority: 10
 apply_loss_function: false
-initial_guess: "state" 
+initial_guess: "zero" 
diff --git a/yaml/test_publisher_ros_node.yaml b/yaml/test_publisher_ros_node.yaml
index b4abe64..a3e3745 100644
--- a/yaml/test_publisher_ros_node.yaml
+++ b/yaml/test_publisher_ros_node.yaml
@@ -1,4 +1,4 @@
 map_frame_id: "map"
-odom_frame_id: "helena/odom"
-base_frame_id: "helena/base_footprint"
+odom_frame_id: "ana/odom"
+base_frame_id: "ana/base_footprint"
 publish_odom_tf: false
-- 
GitLab