diff --git a/launch/imu2d_demo.launch b/launch/imu2d_demo.launch
index 5f0396bef90ed8ffe93b057e7ef0ce5f6270b1a3..22392b14c01d96474ea7160f5d24ffb6ed870e3e 100644
--- a/launch/imu2d_demo.launch
+++ b/launch/imu2d_demo.launch
@@ -6,7 +6,7 @@
<arg name="profiling" default="false" />
<arg name="gdb" default="false" />
<arg name="test" default="1" />
- <arg name="bag" default="test_filtered_no_map" />
+ <arg name="bag" default="test_no_map" />
<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)" />
diff --git a/rviz/imu2d_demo.rviz b/rviz/imu2d_demo.rviz
index 00ad9440327d16aef21d2b18053387082bf6892a..1141bab5157198a3b78cd07fb9f274e7874cf246 100644
--- a/rviz/imu2d_demo.rviz
+++ b/rviz/imu2d_demo.rviz
@@ -18,8 +18,8 @@ Panels:
- /Axes1
- /PointCloud21
- /Imu1
- Splitter Ratio: 0.603864729
- Tree Height: 775
+ Splitter Ratio: 0.6038647294044495
+ Tree Height: 728
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
@@ -28,7 +28,7 @@ Panels:
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
- Splitter Ratio: 0.588679016
+ Splitter Ratio: 0.5886790156364441
- Class: rviz/Views
Expanded:
- /Current View1
@@ -38,7 +38,9 @@ Panels:
Experimental: false
Name: Time
SyncMode: 0
- SyncSource: ""
+ SyncSource: PointCloud2
+Preferences:
+ PromptSaveOnExit: true
Toolbars:
toolButtonStyle: 2
Visualization Manager:
@@ -50,7 +52,7 @@ Visualization Manager:
Color: 160; 160; 164
Enabled: true
Line Style:
- Line Width: 0.0299999993
+ Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
@@ -67,102 +69,28 @@ Visualization Manager:
Frame Timeout: 15
Frames:
All Enabled: false
- ana/ana_box:
- Value: false
- ana/ana_realsense_support:
- Value: false
ana/base_footprint:
Value: true
ana/base_footprint_robot:
Value: false
ana/base_link:
Value: false
- ana/camera_bottom_screw_frame:
- Value: false
- ana/camera_color_frame:
- Value: false
- ana/camera_color_optical_frame:
- Value: false
- ana/camera_depth_frame:
- Value: false
- ana/camera_depth_optical_frame:
- Value: false
- ana/camera_infra1_frame:
- Value: false
- ana/camera_infra1_optical_frame:
- Value: false
- ana/camera_infra2_frame:
- Value: false
- ana/camera_infra2_optical_frame:
- Value: false
- ana/camera_link:
- Value: false
- ana/camera_pan_tilt_bottom_screw_frame:
- Value: false
- ana/camera_pan_tilt_color_frame:
- Value: false
- ana/camera_pan_tilt_color_optical_frame:
- Value: false
- ana/camera_pan_tilt_depth_frame:
- Value: false
- ana/camera_pan_tilt_depth_optical_frame:
- Value: false
- ana/camera_pan_tilt_infra1_frame:
- Value: false
- ana/camera_pan_tilt_infra1_optical_frame:
- Value: false
- ana/camera_pan_tilt_infra2_frame:
- Value: false
- ana/camera_pan_tilt_infra2_optical_frame:
- Value: false
- ana/camera_pan_tilt_link:
- Value: false
ana/front_left_axle:
Value: false
- ana/front_left_hub:
- Value: false
- ana/front_left_wheel:
- Value: false
ana/front_right_axle:
Value: false
- ana/front_right_hub:
- Value: false
- ana/front_right_wheel:
- Value: false
- ana/front_sonar:
- Value: false
ana/imu_bno055:
Value: false
- ana/imu_bno055_base:
- Value: false
ana/imu_microstrain:
Value: false
ana/odom:
Value: true
- ana/pan_frame:
- Value: false
- ana/realsense_support_pan_tilt:
- Value: false
ana/rear_left_axle:
Value: false
- ana/rear_left_hub:
- Value: false
- ana/rear_left_wheel:
- Value: false
ana/rear_right_axle:
Value: false
- ana/rear_right_hub:
- Value: false
- ana/rear_right_wheel:
- Value: false
- ana/rear_sonar:
- Value: false
- ana/top_plate:
- Value: false
ana/velodyne:
Value: false
- ana/velodyne_base:
- Value: false
map:
Value: false
Marker Scale: 1
@@ -176,59 +104,24 @@ Visualization Manager:
ana/base_footprint:
ana/base_link:
ana/front_left_axle:
- ana/front_left_hub:
- ana/front_left_wheel:
- {}
+ {}
ana/front_right_axle:
- ana/front_right_hub:
- ana/front_right_wheel:
- {}
- ana/front_sonar:
{}
ana/imu_bno055:
{}
ana/imu_microstrain:
{}
ana/rear_left_axle:
- ana/rear_left_hub:
- ana/rear_left_wheel:
- {}
+ {}
ana/rear_right_axle:
- ana/rear_right_hub:
- ana/rear_right_wheel:
- {}
- ana/rear_sonar:
{}
- ana/top_plate:
- ana/ana_box:
- ana/pan_frame:
- {}
- ana/ana_realsense_support:
- ana/camera_bottom_screw_frame:
- ana/camera_link:
- ana/camera_color_frame:
- ana/camera_color_optical_frame:
- {}
- ana/camera_depth_frame:
- ana/camera_depth_optical_frame:
- {}
- ana/camera_infra1_frame:
- ana/camera_infra1_optical_frame:
- {}
- ana/camera_infra2_frame:
- ana/camera_infra2_optical_frame:
- {}
- ana/velodyne_base:
- {}
ana/velodyne:
{}
- ana/imu_bno055_base:
- {}
ana/base_footprint_robot:
{}
Update Interval: 0
Value: true
- - Angle Tolerance: 0.100000001
+ - Angle Tolerance: 0.10000000149011612
Class: rviz/Odometry
Covariance:
Orientation:
@@ -240,7 +133,7 @@ Visualization Manager:
Scale: 1
Value: true
Position:
- Alpha: 0.300000012
+ Alpha: 0.30000001192092896
Color: 204; 51; 204
Scale: 1
Value: true
@@ -248,16 +141,16 @@ Visualization Manager:
Enabled: false
Keep: 300
Name: Odometry
- Position Tolerance: 0.100000001
+ Position Tolerance: 0.10000000149011612
Shape:
Alpha: 1
Axes Length: 1
- Axes Radius: 0.100000001
+ Axes Radius: 0.10000000149011612
Color: 255; 25; 0
- Head Length: 0.100000001
- Head Radius: 0.100000001
- Shaft Length: 0.300000012
- Shaft Radius: 0.0500000007
+ Head Length: 0.10000000149011612
+ Head Radius: 0.10000000149011612
+ Shaft Length: 0.30000001192092896
+ Shaft Radius: 0.05000000074505806
Value: Arrow
Topic: /ana/odom
Unreliable: false
@@ -277,15 +170,13 @@ Visualization Manager:
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
- Max Intensity: 47
Min Color: 0; 0; 0
- Min Intensity: 47
Name: LaserScan
Position Transformer: XYZ
Queue Size: 1
Selectable: true
Size (Pixels): 3
- Size (m): 0.0599999987
+ Size (m): 0.05999999865889549
Style: Flat Squares
Topic: /ana/sensors/scan
Unreliable: false
@@ -305,23 +196,29 @@ Visualization Manager:
Marker Topic: /wolf_ros_node/graph_factors
Name: Factors
Namespaces:
- {}
+ factors_processorimu2dbno: true
+ factors_processorodom2d: true
+ factors_processorodomicp: true
+ factors_text_processorimu2dbno: false
+ factors_text_processorodom2d: false
+ factors_text_processorodomicp: false
+ factors_text_unnamed_processor: false
+ factors_unnamed_processor: true
Queue Size: 100
Value: true
- Class: rviz/MarkerArray
- Enabled: true
+ Enabled: false
Marker Topic: /wolf_ros_node/graph_trajectory
Name: Trajectory
Namespaces:
- frames: true
- frames_text: true
+ {}
Queue Size: 100
- Value: true
+ Value: false
- Class: rviz/Axes
Enabled: true
Length: 1
Name: Axes
- Radius: 0.100000001
+ Radius: 0.10000000149011612
Reference Frame: ana/base_link
Value: true
- Alpha: 1
@@ -339,22 +236,20 @@ Visualization Manager:
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
- Max Intensity: 4096
Min Color: 0; 173; 14
- Min Intensity: 0
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 1
Selectable: false
Size (Pixels): 3
- Size (m): 0.0399999991
+ Size (m): 0.03999999910593033
Style: Squares
Topic: /wolf_ros_node/map_pointcloud
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
- - Alpha: 0.699999988
+ - Alpha: 0.699999988079071
Class: rviz/Map
Color Scheme: map
Draw Behind: false
@@ -388,7 +283,10 @@ Visualization Manager:
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
+ Theta std deviation: 0.2617993950843811
Topic: /initialpose
+ X std deviation: 0.5
+ Y std deviation: 0.5
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
@@ -398,78 +296,78 @@ Visualization Manager:
Views:
Current:
Class: rviz/Orbit
- Distance: 8.34140396
+ Distance: 14.700403213500977
Enable Stereo Rendering:
- Stereo Eye Separation: 0.0599999987
+ Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
- X: 2.42303967
- Y: -1.90124702
- Z: 1.00247037
+ X: 2.423039674758911
+ Y: -1.9012470245361328
+ Z: 1.0024703741073608
Focal Shape Fixed Size: true
- Focal Shape Size: 0.0500000007
+ Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
- Near Clip Distance: 0.00999999978
- Pitch: 0.594797194
+ Near Clip Distance: 0.009999999776482582
+ Pitch: 0.5947971940040588
Target Frame: map
Value: Orbit (rviz)
- Yaw: 4.00817299
+ Yaw: 4.008172988891602
Saved:
- Angle: 0
Class: rviz/TopDownOrtho
Enable Stereo Rendering:
- Stereo Eye Separation: 0.0599999987
+ Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Invert Z Axis: false
Name: TopDownOrtho
- Near Clip Distance: 0.00999999978
- Scale: 64.6596146
+ Near Clip Distance: 0.009999999776482582
+ Scale: 64.65961456298828
Target Frame: map
Value: TopDownOrtho (rviz)
- X: 8.82501411
- Y: -0.175666362
+ X: 8.825014114379883
+ Y: -0.17566636204719543
- Angle: 0
Class: rviz/TopDownOrtho
Enable Stereo Rendering:
- Stereo Eye Separation: 0.0599999987
+ Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Invert Z Axis: false
Name: TopDownOrtho
- Near Clip Distance: 0.00999999978
- Scale: 50.072403
+ Near Clip Distance: 0.009999999776482582
+ Scale: 50.07240295410156
Target Frame: map
Value: TopDownOrtho (rviz)
- X: 1.94783008
- Y: 3.96870375
- - Angle: -1.57000005
+ X: 1.947830080986023
+ Y: 3.9687037467956543
+ - Angle: -1.5700000524520874
Class: rviz/TopDownOrtho
Enable Stereo Rendering:
- Stereo Eye Separation: 0.0599999987
+ Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Invert Z Axis: false
Name: TopDownOrtho
- Near Clip Distance: 0.00999999978
- Scale: 42.1874428
+ Near Clip Distance: 0.009999999776482582
+ Scale: 42.187442779541016
Target Frame: map
Value: TopDownOrtho (rviz)
- X: 1.97467852
- Y: 4.4872756
+ X: 1.9746785163879395
+ Y: 4.48727560043335
Window Geometry:
Displays:
collapsed: false
- Height: 1056
+ Height: 1025
Hide Left Dock: false
Hide Right Dock: true
- QMainWindow State: 000000ff00000000fd00000004000000000000027f00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004ba0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ QMainWindow State: 000000ff00000000fd00000004000000000000027f00000363fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000363000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004b80000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
@@ -478,6 +376,6 @@ Window Geometry:
collapsed: false
Views:
collapsed: true
- Width: 1855
- X: 65
- Y: 24
+ Width: 1853
+ X: 67
+ Y: 27
diff --git a/yaml/imu2d_test1.yaml b/yaml/imu2d_test1.yaml
index 93147ca12c1511d92930e4b413e24a03cb859283..b1c8aa72f7f7c4f1a709eb1505fbecf25562d2ab 100644
--- a/yaml/imu2d_test1.yaml
+++ b/yaml/imu2d_test1.yaml
@@ -20,6 +20,7 @@ config:
O: [0]
V: [0,0]
time_tolerance: 0.1
+ node_rate: 100
map:
type: "MapBase"
plugin: "core"
@@ -31,6 +32,11 @@ config:
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
sensors:
-
diff --git a/yaml/test_imu_params.yaml b/yaml/test_imu_params.yaml
index fbbfab088d2f4c36d6fb49d9a9d3f56ece7c8868..def81eeb85c4b81bfbcc4b8df44e1362961ff193 100644
--- a/yaml/test_imu_params.yaml
+++ b/yaml/test_imu_params.yaml
@@ -5,8 +5,8 @@ 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.0000001 # m/s2/sqrt(s)
-wb_rate_stdev: 0.0000001 # rad/s/sqrt(s)
+ab_rate_stdev: 0.0001 # m/s2/sqrt(s)
+wb_rate_stdev: 0.0001 # rad/s/sqrt(s)
orthogonal_gravity: true
#microstrain
#extrinsic: