diff --git a/bag/analysys/compare.m b/bag/analysys/compare.m
index f810641b2176661284198f9a925b58362cf56245..532e2da6c5ad47e5c00fb472e06fd9fe0502f2c1 100644
--- a/bag/analysys/compare.m
+++ b/bag/analysys/compare.m
@@ -2,7 +2,7 @@ clc; clear; clearvars; close all;
 %time at which to start counting data
 tstart=0;
 %Number of frames to cut
-N=8;
+N=0;
 
 %Folder where CSV files are saved
 CSVFolder = '../CSV/transp/';
@@ -21,6 +21,7 @@ end
 Gvec = [];
 Evec = [];
 Tvec = [];
+Svec = [];
 myFiles = dir(fullfile(CSVFolder,'*.csv')); %gets all csv files in folder
 for k = 1:length(myFiles)
     baseFileName = myFiles(k).name;
@@ -36,6 +37,9 @@ for k = 1:length(myFiles)
     elseif baseFileName(1) == 'T'
         [Tx, Ty, Tz, Tang, Tt, Tts] = extractdata(Table, tstart);
         Tvec = [Tvec, {baseFileName, Tx, Ty, Tz, Tang, Tt, Tts}'];
+    elseif baseFileName(1) == 'S'
+        [Sx, Sy, Sz, Sang, St, Sts] = extractdata(Table, tstart);
+        Svec = [Svec, {baseFileName, Sx, Sy, Sz, Sang, St, Sts}'];
     end
 end
 
@@ -46,7 +50,11 @@ min_len3d = 99999999999999999999999999999999999999999999999999999999999999999999
 for i = 1:size(Evec,2)
     %Get position and angular error vectors for exepriments comparing them
     %to GroundTruth
-    [perrors2d, angerrors2d, tstart2] = geterror(Gvec{7,i}, Evec{7,i}, N);
+    [perrors2d, angerrors2d, ~] = geterror(Gvec{7,i}, Evec{7,i}, N);
+    [perrors3d, angerrors3d, tstart2] = geterror(Gvec{7,i}, Tvec{7,i}, N);
+    if ~isempty(Svec)
+        
+    end
     [perrors3d, angerrors3d, tstart2] = geterror(Gvec{7,i}, Tvec{7,i}, N);
     tstart = max(tstart, tstart2);
 
diff --git a/bag/analysys/extractdata.m b/bag/analysys/extractdata.m
index 802975bb4667e0e6a864578ad51806a849720dc7..2db920a072d3fe81eb503fc012aca0d5f90c2742 100644
--- a/bag/analysys/extractdata.m
+++ b/bag/analysys/extractdata.m
@@ -9,8 +9,8 @@ function [gx, gy, gz, gang, gt, gts] = extractdata(GC, tstart)
     [R1, R2, R3] = quat2angle([GC{:,11} GC{:,8} GC{:,9} GC{:,10}]); %Convert quaternions to euler angles
     gang = [R1'; R2'; R3'];
     gt = GC{:,3}';
-    gt = gt-t0;
-    gt = gt*1e-9;
+    %gt = gt-t0;
+    %gt = gt*1e-9;
 
     %We cut all elements with a timestamp lower than tstart
     n = sum(gt<=tstart);
diff --git a/bag/generate_trajectory_csv_ana_lab_2.bash b/bag/generate_trajectory_csv_ana_lab_2.bash
index 947c410ab0960bb2568c214eeee3c7d0f9472088..0f3e39382bb48f39c03d78f5f31edf88e96bf230 100644
--- a/bag/generate_trajectory_csv_ana_lab_2.bash
+++ b/bag/generate_trajectory_csv_ana_lab_2.bash
@@ -12,9 +12,10 @@ fi
 #   Starting with G ==> groundtruth, we extract the trajectory
 #   Starting with E ==> experiment, we extract the poses
 #   Starting with T ==> experiment in 3D, we extract the poses
+#   Starting with S ==> experiment in 2D with slope, we extract the poses
 
 # this array lists the timespans in which the KeyFrames will be created. For each timespan the three experiments will be executed
-timespanarray=("0.1" "0.2" "0.3" "0.5" "0.7" "1.0" "2.0" "5.0" "10.0") 
+timespanarray=("1.0" "2.0" "3.0" "4.0" "5.0" "6.0" "7.0" "8.0" "9.0" "10.0") 
 for time_span in "${timespanarray[@]}"
 do
   for yamlfile in $(find ../yaml/trajectory_analysys -maxdepth 1 -type f -printf "%f\n")
@@ -31,7 +32,7 @@ do
     rm recordings/trajectory_recording$yamlname.bag
 
     #The roslaunch file takes several useful parameters. We can lower speed if we see that the processor isn't working at real time. The suffix allows us to record different bags every loop. The bag file has to be previously filtered. We disable rviz as we don't need to see anything.
-    roslaunch wolf_demo_imu2d imu2d_analysys.launch bag:=$BAG\_filtered_notf test:=$yamlfile speed:=0.05 suffix:=$yamlname rviz:=false
+    roslaunch wolf_demo_imu2d imu2d_analysys.launch bag:=$BAG\_filtered_notf test:=$yamlfile speed:=1 suffix:=$yamlname rviz:=false
     #The following ifs distinguish between the 3 types of yaml files
     if [[ ${yamlname:0:1} == 'G' ]]
     then
@@ -54,6 +55,11 @@ do
       rostopic echo -b recordings/trajectory_recording$yamlname.bag /wolf_ros_node/pose_pose_with_cov -p > CSV/$yamlname.csv
       cp CSV/$yamlname.csv CSV/transp/$yamlname.csv
     elif [[ ${yamlname:0:1} == 'T' ]]
+    then
+      rm CSV/$yamlname.csv
+      rostopic echo -b recordings/trajectory_recording$yamlname.bag /wolf_ros_node/pose_pose_with_cov -p > CSV/$yamlname.csv
+      cp CSV/$yamlname.csv CSV/transp/$yamlname.csv
+    elif [[ ${yamlname:0:1} == 'S' ]]
     then
       rm CSV/$yamlname.csv
       rostopic echo -b recordings/trajectory_recording$yamlname.bag /wolf_ros_node/pose_pose_with_cov -p > CSV/$yamlname.csv
diff --git a/launch/imu2d_analysys.launch b/launch/imu2d_analysys.launch
index 238359a961e6da030956ca87dd1dcf87bbb222d0..0e9b2ffa85d0adcb20b4329d2f6f2adc708b0bba 100644
--- a/launch/imu2d_analysys.launch
+++ b/launch/imu2d_analysys.launch
@@ -22,7 +22,7 @@
       <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"'/>
+        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" "/imu_micro_bias_accel" "/imu_micro_bias_gyro"'/>
     </group>
 
     <!-- TF -->
diff --git a/launch/imu2d_demo.launch b/launch/imu2d_demo.launch
index c58ef156f6f2f0f2ac5f25e8da77ae8ea5a2bd8c..1b53a9239cee2dbf42f70d6b00d04141c57e7b4a 100644
--- a/launch/imu2d_demo.launch
+++ b/launch/imu2d_demo.launch
@@ -22,7 +22,7 @@
       <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"'/>
+            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" "/imu_micro_bias_accel" "/imu_micro_bias_gyro"'/>
     </group>
 
     <!-- TF -->
diff --git a/rviz/imu2d_demo_ana.rviz b/rviz/imu2d_demo_ana.rviz
index 6a88cc005213a2ffbea119f1f0c0790a29d94f80..7caf7f1f9c7b57f9cf197cc8187134ebd216f020 100644
--- a/rviz/imu2d_demo_ana.rviz
+++ b/rviz/imu2d_demo_ana.rviz
@@ -17,7 +17,7 @@ Panels:
         - /PointCloud21
         - /Imu1
       Splitter Ratio: 0.708737850189209
-    Tree Height: 728
+    Tree Height: 725
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -36,7 +36,7 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: PointCloud2
+    SyncSource: LaserScan
 Preferences:
   PromptSaveOnExit: true
 Toolbars:
@@ -182,13 +182,9 @@ Visualization Manager:
       Marker Topic: /wolf_ros_node/graph_factors
       Name: Factors
       Namespaces:
-        factors_processorimu2dmicro: true
-        factors_processorloopclosureicp: true
-        factors_processorodom2d: true
+        factors_processorimumicro: true
         factors_processorodomicp: true
-        factors_text_processorimu2dmicro: false
-        factors_text_processorloopclosureicp: false
-        factors_text_processorodom2d: false
+        factors_text_processorimumicro: false
         factors_text_processorodomicp: false
         factors_text_unnamed_processor: false
         factors_unnamed_processor: true
@@ -352,10 +348,10 @@ Visualization Manager:
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1025
+  Height: 1022
   Hide Left Dock: false
   Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd00000004000000000000020500000363fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000363000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005320000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000020500000360fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000360000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006900000003efc0100000002fb0000000800540069006d0065010000000000000690000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004850000036000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -364,6 +360,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: true
-  Width: 1853
-  X: 67
-  Y: 27
+  Width: 1680
+  X: 1920
+  Y: 0
diff --git a/yaml/trajectory_analysys/E.yaml b/yaml/trajectory_analysys/E.yaml
index a993e15ac1cd8ac0083c2977c735018ce25c19f3..32b5cb684349663b3a0162dc1a5af7d89caf9d30 100644
--- a/yaml/trajectory_analysys/E.yaml
+++ b/yaml/trajectory_analysys/E.yaml
@@ -1,7 +1,7 @@
 config:
   debug:
     profiling: true
-    profiling_file: "~/profiling/wolf_demo_profiling_E-1_0_outside_1.txt"
+    profiling_file: "~/profiling/wolf_demo_profiling_E-5_0.txt"
     print_problem: false
     print_period: 2
     print_depth: 4
diff --git a/yaml/trajectory_analysys/G.yaml b/yaml/trajectory_analysys/G.yaml
index 8172adfee6bb2365215957a2b575622b2aa95e9e..dcc3d60fd015a15dd66d004f1ce9c33e28640959 100644
--- a/yaml/trajectory_analysys/G.yaml
+++ b/yaml/trajectory_analysys/G.yaml
@@ -1,7 +1,7 @@
 config:
   debug:
     profiling: true
-    profiling_file: "~/profiling/wolf_demo_profiling_G-10_0.txt"
+    profiling_file: "~/profiling/wolf_demo_profiling_G-5_0.txt"
     print_problem: false
     print_period: 2
     print_depth: 4
diff --git a/yaml/trajectory_analysys/S.yaml b/yaml/trajectory_analysys/S.yaml
index 4683ca62805c3f361a80b5d5582647653ecf63df..e4d07ccf613c5af198f45d13e5be96079ee15dd5 100644
--- a/yaml/trajectory_analysys/S.yaml
+++ b/yaml/trajectory_analysys/S.yaml
@@ -1,7 +1,7 @@
 config:
   debug:
     profiling: true
-    profiling_file: "~/profiling/wolf_demo_profiling_E-10_0.txt"
+    profiling_file: "~/profiling/wolf_demo_profiling_S-5_0.txt"
     print_problem: false
     print_period: 2
     print_depth: 4
diff --git a/yaml/trajectory_analysys/T.yaml b/yaml/trajectory_analysys/T.yaml
index 629e3a9a5766a6b10016bdd4266cffa8d7b66bdd..eddf3814e247c4243ed4c12187a5e7278f9131a5 100644
--- a/yaml/trajectory_analysys/T.yaml
+++ b/yaml/trajectory_analysys/T.yaml
@@ -1,7 +1,7 @@
 config:
   debug:
     profiling: true
-    profiling_file: "~/profiling/wolf_demo_profiling_T-1_0_outside_1.txt"
+    profiling_file: "~/profiling/wolf_demo_profiling_T-5_0.txt"
     print_problem: false
     print_period: 2
     print_depth: 4
diff --git a/yaml/trajectory_analysys/parameters/test_imu_params_microstrain.yaml b/yaml/trajectory_analysys/parameters/test_imu_params_microstrain.yaml
index 51ced290ba7db0f5ed336a3c0f8156d4c339893d..433952de0d12418dbd472d435e5dd416e5a143eb 100644
--- a/yaml/trajectory_analysys/parameters/test_imu_params_microstrain.yaml
+++ b/yaml/trajectory_analysys/parameters/test_imu_params_microstrain.yaml
@@ -7,4 +7,4 @@ 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: false
+orthogonal_gravity: true
diff --git a/yaml/trajectory_analysys/parameters/test_imu_params_microstrain_sloped.yaml b/yaml/trajectory_analysys/parameters/test_imu_params_microstrain_sloped.yaml
index 433952de0d12418dbd472d435e5dd416e5a143eb..51ced290ba7db0f5ed336a3c0f8156d4c339893d 100644
--- a/yaml/trajectory_analysys/parameters/test_imu_params_microstrain_sloped.yaml
+++ b/yaml/trajectory_analysys/parameters/test_imu_params_microstrain_sloped.yaml
@@ -7,4 +7,4 @@ 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
+orthogonal_gravity: false
diff --git a/yaml/trajectory_analysys/parameters/test_laser_processor.yaml b/yaml/trajectory_analysys/parameters/test_laser_processor.yaml
index 3aa7b0bcfbe1df1c74eb80792ef8ff16525bf5fb..6c34e389112ab16a4dd7000e17f1720382e17f81 100644
--- a/yaml/trajectory_analysys/parameters/test_laser_processor.yaml
+++ b/yaml/trajectory_analysys/parameters/test_laser_processor.yaml
@@ -3,7 +3,7 @@ keyframe_vote:
   min_features_for_keyframe: 10
   min_dist: 999
   min_angle: 999
-  min_time: 1.0
+  min_time: 5.0
   min_error: 999
   max_points: 0
 max_new_features: 15
diff --git a/yaml/trajectory_analysys/parameters/test_publisher_ros_node.yaml b/yaml/trajectory_analysys/parameters/test_publisher_ros_node.yaml
index 492c369e8a7c10b19fa0af9fd4f7c6bd38263ed2..30c8768f9cd80f6cf96f0d6803f5a6f13b506c1f 100644
--- a/yaml/trajectory_analysys/parameters/test_publisher_ros_node.yaml
+++ b/yaml/trajectory_analysys/parameters/test_publisher_ros_node.yaml
@@ -1,6 +1,6 @@
 map_frame_id: "map"
-  #odom_frame_id: "ana/odom"
-  #base_frame_id: "ana/base_footprint"
-odom_frame_id: "helena/odom"
-base_frame_id: "helena/base_footprint"
-publish_odom_tf: true
+odom_frame_id: "ana/odom"
+base_frame_id: "ana/base_footprint"
+#odom_frame_id: "helena/odom"
+#base_frame_id: "helena/base_footprint"
+publish_odom_tf: false