diff --git a/bag/filter_bag.bash b/bag/filter_bag.bash
index a8971ae1ca3e51830727f875b6fdec16d294ec41..f9458ab76357603251d54cac4b9f7bbc906002eb 100644
--- a/bag/filter_bag.bash
+++ b/bag/filter_bag.bash
@@ -1,8 +1,10 @@
#!/bin/bash
-BAG="ana_lab_2"
+if [[ -z "${BAG}" ]]
+then
+ BAG="ana_lab_2"
+fi
rm $BAG\_filtered.bag
rm $BAG\_filtered_notf.bag
rosbag filter $BAG.bag $BAG\_filtered.bag "topic == '/ana/imu/data' or topic == '/ana/odom' or topic == '/ana/sensors/bno055_imu/imu' or topic == '/ana/sensors/scan' or topic == '/tf'"
python3 filter_tf.py $BAG\_filtered.bag $BAG\_filtered_notf.bag
-#python3 filter_tf2.py -i $BAG\_filtered.bag -o $BAG\_filtered_notf.bag -f ana/front_left_axle
diff --git a/bag/generate_trajectory_csv.bash b/bag/generate_trajectory_csv.bash
index 67bdbf5b774a88bfc2020b29564064fb5ce394bb..b90eef1192c71e480be9f39dc59df976e11e6a2a 100644
--- a/bag/generate_trajectory_csv.bash
+++ b/bag/generate_trajectory_csv.bash
@@ -1,17 +1,35 @@
#!/bin/bash
-BAG="ana_lab_2"
-t0=1620123952.21
-tend=1620123952.21
+echo "Press CTRL+C to proceed."
+trap "pkill -f 'sleep 1h'" INT
+trap "set +x ; sleep 1h ; set -x" DEBUG
-#roslaunch wolf_demo_imu2d imu2d_demo.launch bag:=$BAG\_filtered_notf test:=0 speed:=1 suffix:="_GT"
-#rosbag filter trajectory_recording_GT.bag trajectory_recording_GT_tmp.bag "t.to_sec >= $tend"
-#rostopic echo -b trajectory_recording_GT_tmp.bag /wolf_ros_node/trajectory -p > trajectory_$BAG\_exp_$i.csv
-rm trajectory_recording_GT_tmp.bag
-#python3 transpose_csv.py trajectory_$BAG\_exp_$i.csv trajectory_$BAG\_exp_$i\_transp.csv
-for i in 10 20 30 40 50
+if [[ -z "${BAG}" ]]
+then
+ BAG="ana_lab_2"
+fi
+
+#yamlfiles are divided into two types:
+# Starting with G ==> groundtruth, we extract the trajectory
+# Starting with E ==> experiment, we extract the poses
+for yamlfile in $(find ../yaml/trajectory_analysys -maxdepth 1 -type f -printf "%f\n")
do
- filtertime=$t0+$i+30.0-1.0
- roslaunch wolf_demo_imu2d imu2d_demo.launch bag:=$BAG\_shortened_$i test:=5 speed:=1 suffix:="_$i"
- rostopic echo -b trajectory_recording_$i.bag /wolf_ros_node/pose_pose_with_cov -p > trajectory_$BAG\_exp_$i.csv
- python3 transpose_csv.py trajectory_$BAG\_exp_$i.csv trajectory_$BAG\_exp_$i\_transp.csv
+ yamlname=${yamlfile%.*}
+ roslaunch wolf_demo_imu2d imu2d_analysys.launch bag:=$BAG\_filtered_notf test:=$yamlfile speed:=1 suffix:=$yamlname
+ if [[ ${yamlname:0:1} == 'G' ]]
+ then
+ tend=$(rosbag info -y -k end recordings/trajectory_recording$yamlname.bag)
+ tend=${tend%.*}
+ rosbag filter recordings/trajectory_recording$yamlname.bag recordings/trajectory_recording$yamlname\_tmp.bag "topic == '/wolf_ros_node/trajectory' and t.to_sec() >= $tend"
+
+ rm CSV/$yamlname.csv
+ rostopic echo -b recordings/trajectory_recording$yamlname\_tmp.bag /wolf_ros_node/trajectory -p > CSV/$yamlname.csv
+ rm recordings/trajectory_recording$yamlname\_tmp.bag
+ elif [[ ${yamlname:0:1} == 'E' ]]
+ then
+ rm CSV/$yamlname.csv
+ rostopic echo -b recordings/trajectory_recording$yamlname.bag /wolf_ros_node/pose_pose_with_cov -p > CSV/$yamlname.csv
+ fi
+ rm CSV/transp/$yamlname\_transp.csv
+ python3 transpose_csv.py CSV/$yamlname.csv CSV/transp/$yamlname\_transp.csv
+
done
diff --git a/bag/shorten_topic.bash b/bag/shorten_topic.bash
index 7657ccbee8d1abd243035241aeef2ae303f28854..3eb83234043a5a977b27078db260d91db995a3dd 100644
--- a/bag/shorten_topic.bash
+++ b/bag/shorten_topic.bash
@@ -1,12 +1,15 @@
#!/bin/bash
-BAG="ana_lab_2"
-tstartstr=$(rosbag info $BAG.bag | grep start)
-t0=$(echo $tstr | cut -d "(" -f2 | cut -d ")" -f1 | bc -l)
+if [[ -z "${BAG}" ]]
+then
+ BAG="ana_lab_2"
+fi
+t0=$(rosbag info -y -k start $BAG\_filtered_notf.bag)
for i in 10 20 30 40 50
do
- cuttime=$(($t0+$i))
+ cuttime=$( bc <<< "$t0+$i" )
echo $cuttime
- endtime=$(($cuttime+30.0))
- rosbag filter $BAG\_filtered_notf.bag $BAG\_shortened\_$i.bag "(topic != '/ana/sensors/scan' and t.to_sec() <=$endtime) or (topic == '/ana/sensors/scan' and t.to_sec() <= $cuttime)"
+ endtime=$(bc <<< "$cuttime+30")
+ rm $BAG\_shortened\_$i.bag
+ rosbag filter $BAG\_filtered_notf.bag $BAG\_shortened\_$i.bag "(topic != '/ana/sensors/scan' and t.to_sec() <= $endtime ) or (topic == '/ana/sensors/scan' and t.to_sec() <= $cuttime)"
done
diff --git a/launch/imu2d_analysys.launch b/launch/imu2d_analysys.launch
new file mode 100644
index 0000000000000000000000000000000000000000..a3a4c66ce33dce7b3d71fe0a57226e46136779fa
--- /dev/null
+++ b/launch/imu2d_analysys.launch
@@ -0,0 +1,57 @@
+<!-- -->
+<launch>
+ <param name="use_sim_time" value="true" />
+ <arg name="speed" default="1" />
+ <arg name="sec" default="0" />
+ <arg name="profiling" default="false" />
+ <arg name="gdb" default="false" />
+ <arg name="test" default="" />
+ <arg name="bag" default="" />
+ <arg name="suffix" default="" />
+ <arg name="launch_pref" default="" unless="$(eval profiling or gdb)"/>
+ <arg name="launch_pref" value="valgrind --tool=callgrind --callgrind-out-file='callgrind.wolf.%p'" if="$(arg profiling)" />
+ <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="rosbag" type="record" name="recorder" args='record -O $(find wolf_demo_imu2d)/bag/recordings/trajectory_recording$(arg suffix).bag "/wolf_ros_node/trajectory" "/wolf_ros_node/pose_pose_with_cov"'/>
+
+ <node pkg="tf"
+ type="static_transform_publisher"
+ name="static_tf"
+ 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 /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 /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 /ana/base_link /ana/imu_microstrain 100"/>
+
+ <node pkg="rosbag"
+ type="play"
+ name="player"
+ 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"/>
+ <node type="wolf_ros_node"
+ name="wolf_ros_node"
+ pkg="wolf_ros_node"
+ required="true"
+ output="screen"
+ launch-prefix="$(arg launch_pref)">
+ <param name="~yaml_file_path" value="$(find wolf_demo_imu2d)/yaml/trajectory_analysys/$(arg test)" />
+ </node>
+</launch>
diff --git a/launch/imu2d_demo.launch b/launch/imu2d_demo.launch
index bf1cbdd9eba45feccbb43295da9df03cf92681f3..ea29fa3954cbd96710f7ada8d324579bf73c4bc0 100644
--- a/launch/imu2d_demo.launch
+++ b/launch/imu2d_demo.launch
@@ -18,7 +18,7 @@
<node pkg="rostopic" type="rostopic" name="" args='pub /helena/sensors/bno055_imu/imu/topic_enable std_msgs/Bool "data: True"'/>
<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"'/>
+ <node pkg="rosbag" type="record" name="recorder" args='record -O $(find wolf_demo_imu2d)/bag/recordings/trajectory_recording$(arg suffix).bag "/wolf_ros_node/trajectory" "/wolf_ros_node/pose_pose_with_cov"'/>
</group>
<node pkg="tf"
diff --git a/yaml/test_imu_params_microstrain.yaml b/yaml/test_imu_params_microstrain.yaml
index 8ff559405ab5152c537da910a4d9abfac552dc6f..433952de0d12418dbd472d435e5dd416e5a143eb 100644
--- a/yaml/test_imu_params_microstrain.yaml
+++ b/yaml/test_imu_params_microstrain.yaml
@@ -1,10 +1,10 @@
#microstrain
extrinsic:
pose: [0,0,0]
-a_noise: 0.9 #0.9
+a_noise: 0.05 #0.9
w_noise: 0.01 #0.01
ab_initial_stdev: 0.5 # m/s2 - initial bias
wb_initial_stdev: 0.1 # rad/sec - initial bias
-ab_rate_stdev: 0.001 # m/s2/sqrt(s)
-wb_rate_stdev: 0.001 # rad/s/sqrt(s)
+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_laser_processor.yaml b/yaml/test_laser_processor.yaml
index c10e2a5142e4684d8de228893a3093a8ff2e5dcd..fa2d98e94d6d031d86ad80300781d72d09e68036 100644
--- a/yaml/test_laser_processor.yaml
+++ b/yaml/test_laser_processor.yaml
@@ -3,7 +3,7 @@ keyframe_vote:
min_features_for_keyframe: 10
min_dist: 999
min_angle: 999
- min_time: 1
+ min_time: 0.1
min_error: 999
max_points: 0
max_new_features: 15
diff --git a/yaml/test_odom2d_params.yaml b/yaml/test_odom2d_params.yaml
index 0e0922a64325ef75f6981deb6db802c65c91fbe0..f1f24a5154e114ddaa5808780866fa28f4f179de 100644
--- a/yaml/test_odom2d_params.yaml
+++ b/yaml/test_odom2d_params.yaml
@@ -1,4 +1,4 @@
extrinsic:
pose: [0,0,0]
k_disp_to_disp: 0.08
-k_rot_to_rot: 0.6
+k_rot_to_rot: 0.6 #0.6
diff --git a/yaml/trajectory_analysys/E-1_0.yaml b/yaml/trajectory_analysys/E-1_0.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..933b2502b6964339b3bfee73d5a9c4f8484b940c
--- /dev/null
+++ b/yaml/trajectory_analysys/E-1_0.yaml
@@ -0,0 +1,157 @@
+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
+ node_rate: 100
+ map:
+ type: "MapBase"
+ plugin: "core"
+
+ solver:
+ follow: "parameters/solver.yaml"
+
+
+ sensors:
+ -
+ type: "SensorLaser2d"
+ name: "scanner_front_left"
+ plugin: "laser"
+ follow: "parameters/test_laser_params.yaml"
+ -
+ type: "SensorImu2d"
+ name: "bno"
+ plugin: "imu"
+ follow: "parameters/test_imu_params_bno.yaml"
+ -
+ type: "SensorImu2d"
+ name: "microstrain"
+ plugin: "imu"
+ follow: "parameters/test_imu_params_microstrain.yaml"
+
+ processors:
+ -
+ type: "ProcessorOdomIcp"
+ name: "processorodomicp"
+ sensor_name: "scanner_front_left"
+ plugin: "laser"
+ keyframe_vote:
+ voting_active: true
+ min_features_for_keyframe: 10
+ min_dist: 999
+ min_angle: 999
+ min_time: 1
+ min_error: 999
+ max_points: 0
+ follow: "parameters/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: "parameters/csm.yaml"
+ -
+ type: "ProcessorImu2d"
+ name: "processorimu2dbno"
+ sensor_name: "bno"
+ plugin: "imu"
+ follow: "parameters/test_imu_processor_bno.yaml"
+ -
+ type: "ProcessorImu2d"
+ name: "processorimu2dmicro"
+ sensor_name: "microstrain"
+ plugin: "imu"
+ follow: "parameters/test_imu_processor_microstrain.yaml"
+
+ ROS subscriber:
+ -
+ package: "wolf_ros_laser"
+ type: "SubscriberLaser2d"
+ topic: "/ana/sensors/scan"
+ sensor_name: "scanner_front_left"
+ load_params_from_msg: true
+ -
+ package: "wolf_ros_imu"
+ type: "SubscriberImuEnableable"
+ topic: "/ana/sensors/bno055_imu/imumal"
+ sensor_name: "bno"
+ follow: "parameters/test_imu_subscriber_bno.yaml"
+ -
+ package: "wolf_ros_imu"
+ type: "SubscriberImuEnableable"
+ topic: "/ana/imu/data"
+ sensor_name: "microstrain"
+ follow: "parameters/test_imu_subscriber_microstrain.yaml"
+
+ ROS publisher:
+ -
+ package: "wolf_ros_node"
+ type: "PublisherTf"
+ topic: " "
+ period: 0.1
+ follow: "parameters/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
+
diff --git a/yaml/trajectory_analysys/G-1_0.yaml b/yaml/trajectory_analysys/G-1_0.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..a0ddcb4c8c5a5c8769cb3de2cbd53d25658183f9
--- /dev/null
+++ b/yaml/trajectory_analysys/G-1_0.yaml
@@ -0,0 +1,114 @@
+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
+ node_rate: 100
+ map:
+ type: "MapBase"
+ plugin: "core"
+
+ solver:
+ follow: "parameters/solver.yaml"
+
+ sensors:
+ -
+ type: "SensorLaser2d"
+ name: "scanner_front_left"
+ plugin: "laser"
+ follow: "parameters/test_laser_params.yaml"
+
+ processors:
+ -
+ type: "ProcessorOdomIcp"
+ name: "processorodomicp"
+ sensor_name: "scanner_front_left"
+ plugin: "laser"
+ voting_active: true
+ min_features_for_keyframe: 10
+ min_dist: 999
+ min_angle: 999
+ min_time: 1
+ min_error: 999
+ max_points: 0
+ follow: "parameters/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: "parameters/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: "parameters/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/trajectory_analysys/parameters/csm.yaml b/yaml/trajectory_analysys/parameters/csm.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..2dd8f8680123cbf977c7bd7defcb0c65dd1d33f5
--- /dev/null
+++ b/yaml/trajectory_analysys/parameters/csm.yaml
@@ -0,0 +1,45 @@
+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/trajectory_analysys/parameters/csm_inactive.yaml b/yaml/trajectory_analysys/parameters/csm_inactive.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..4ea66701ad3a5dfc836c70f988b99d7db48bba60
--- /dev/null
+++ b/yaml/trajectory_analysys/parameters/csm_inactive.yaml
@@ -0,0 +1,45 @@
+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: 9999999999999999999999
diff --git a/yaml/trajectory_analysys/parameters/solver.yaml b/yaml/trajectory_analysys/parameters/solver.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..cb52fa3d63983c71bcc06e5bc4c4f6b1daa6d28a
--- /dev/null
+++ b/yaml/trajectory_analysys/parameters/solver.yaml
@@ -0,0 +1,13 @@
+minimizer: LEVENBERG_MARQUARDT
+max_num_iterations: 100
+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-9
+gradient_tolerance: 1e-10
+use_nonmonotonic_steps: false
diff --git a/yaml/trajectory_analysys/parameters/test_imu_params_bno.yaml b/yaml/trajectory_analysys/parameters/test_imu_params_bno.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..a958a695eaa28b8cc4973f233b09f19e7b3bef33
--- /dev/null
+++ b/yaml/trajectory_analysys/parameters/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/trajectory_analysys/parameters/test_imu_params_microstrain.yaml b/yaml/trajectory_analysys/parameters/test_imu_params_microstrain.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..433952de0d12418dbd472d435e5dd416e5a143eb
--- /dev/null
+++ b/yaml/trajectory_analysys/parameters/test_imu_params_microstrain.yaml
@@ -0,0 +1,10 @@
+#microstrain
+extrinsic:
+ pose: [0,0,0]
+a_noise: 0.05 #0.9
+w_noise: 0.01 #0.01
+ab_initial_stdev: 0.5 # m/s2 - initial bias
+wb_initial_stdev: 0.1 # 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/trajectory_analysys/parameters/test_imu_processor_bno.yaml b/yaml/trajectory_analysys/parameters/test_imu_processor_bno.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..79f6f5001b813bf2d55f53b424359930cb160594
--- /dev/null
+++ b/yaml/trajectory_analysys/parameters/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/trajectory_analysys/parameters/test_imu_processor_microstrain.yaml b/yaml/trajectory_analysys/parameters/test_imu_processor_microstrain.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..7ab3a8eef1ea08955e2b999fda99120f14da1507
--- /dev/null
+++ b/yaml/trajectory_analysys/parameters/test_imu_processor_microstrain.yaml
@@ -0,0 +1,12 @@
+time_tolerance: 0.005 #Half the sample time, microstrain
+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: 0
+apply_loss_function: false
+use_gravity_grid: false
diff --git a/yaml/trajectory_analysys/parameters/test_imu_subscriber_bno.yaml b/yaml/trajectory_analysys/parameters/test_imu_subscriber_bno.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..34e3cf7f9c4c37df9bb107fb8d9b7d4763ba0c47
--- /dev/null
+++ b/yaml/trajectory_analysys/parameters/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/trajectory_analysys/parameters/test_imu_subscriber_microstrain.yaml b/yaml/trajectory_analysys/parameters/test_imu_subscriber_microstrain.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..1bf1e8c462c3917a25c3351d3749473cc930d7d2
--- /dev/null
+++ b/yaml/trajectory_analysys/parameters/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: false
+lowpass_cutoff_freq: 5
diff --git a/yaml/trajectory_analysys/parameters/test_laser_params.yaml b/yaml/trajectory_analysys/parameters/test_laser_params.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..0fb41c67ad15463c536ae63b6744979b1ea5ef90
--- /dev/null
+++ b/yaml/trajectory_analysys/parameters/test_laser_params.yaml
@@ -0,0 +1,11 @@
+extrinsic:
+ pose: [0,0,3.1415926535] #Last value was pi
+LaserScanParams:
+ angle_min: -3.14159274101
+ angle_max: 3.14159274101
+ angle_step: 0.007 #0.00700000021607
+ scan_time: 0.0
+ range_min: 0.0
+ range_max: 200
+ range_std_dev: 0.2
+ angle_std_dev: 0.01
diff --git a/yaml/trajectory_analysys/parameters/test_laser_processor.yaml b/yaml/trajectory_analysys/parameters/test_laser_processor.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..66083e377849ce867db1906ef0b00265e5e0cabb
--- /dev/null
+++ b/yaml/trajectory_analysys/parameters/test_laser_processor.yaml
@@ -0,0 +1,8 @@
+max_new_features: 15
+icp:
+ follow: "parameters/csm.yaml"
+time_tolerance: 0.05 #Half the sample time
+state_getter: true
+state_priority: 10
+apply_loss_function: false
+initial_guess: "zero"
diff --git a/yaml/trajectory_analysys/parameters/test_laser_processor_inactive.yaml b/yaml/trajectory_analysys/parameters/test_laser_processor_inactive.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..54a33e4da380ef91a3f8544ce120932de3aa8709
--- /dev/null
+++ b/yaml/trajectory_analysys/parameters/test_laser_processor_inactive.yaml
@@ -0,0 +1,16 @@
+keyframe_vote:
+ voting_active: true
+ min_features_for_keyframe: 10
+ min_dist: 999
+ min_angle: 999
+ min_time: 2
+ min_error: 999
+ max_points: 0
+max_new_features: 15
+icp:
+ follow: "parameters/csm_inactive.yaml"
+time_tolerance: 0.05 #Half the sample time
+state_getter: true
+state_priority: 10
+apply_loss_function: false
+initial_guess: "zero"
diff --git a/yaml/trajectory_analysys/parameters/test_publisher_ros_node.yaml b/yaml/trajectory_analysys/parameters/test_publisher_ros_node.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..a3e37459266db719cbea053e906bc686d902fec3
--- /dev/null
+++ b/yaml/trajectory_analysys/parameters/test_publisher_ros_node.yaml
@@ -0,0 +1,4 @@
+map_frame_id: "map"
+odom_frame_id: "ana/odom"
+base_frame_id: "ana/base_footprint"
+publish_odom_tf: false