tuned covariances ... drift is noticeable

Author: suchkristenwow

src/hangar_sim/config/fuse/fuse.yaml

@@ -5,7 +5,7 @@ state_estimator:
     # Fixed-lag smoother configuration
     optimization_frequency: 20.0
     transaction_timeout: 0.01
-    lag_duration: 0.5
+    lag_duration: 1.0
 
     # Motion model for mobile base (3D omnidirectional)
     motion_models:
@@ -15,7 +15,8 @@ state_estimator:
     mobile_base_motion:
       # Process noise for state variables
       # Order: x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az
-      process_noise_diagonal: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 1.0, 1.0, 1.0]
+      #        Ground robot: z, roll, pitch tightly constrained
+      process_noise_diagonal: [0.005, 0.005, 0.0001, 0.0001, 0.0001, 0.005, 0.005, 0.005, 0.0001, 0.0001, 0.0001, 0.005, 0.05, 0.05, 0.001]
 
     sensor_models:
       initial_localization_sensor:
@@ -33,41 +34,45 @@ state_estimator:
       publish_on_startup: true
       # Order: x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az
       initial_state: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
-      initial_sigma: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]
+      # Order: x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az
+      #        Moderate x,y,yaw sigma: some initial position uncertainty
+      #        Tight z,roll,pitch: ground robot constraints
+      initial_sigma: [1.0, 1.0, 0.01, 0.01, 0.01, 0.5, 0.1, 0.1, 0.01, 0.01, 0.01, 0.1, 0.1, 0.1, 0.01]
 
     wheel_odom_sensor:
       # Using /odom_reliable from QoS relay (bridges BEST_EFFORT to RELIABLE)
       topic: /odom_reliable
       queue_size: 10
-      # 2D position from wheels (no z)
-      position_dimensions: ['x', 'y']
+      # Position from wheels (z=0 from odom_zero_z anchors the ground plane)
+      position_dimensions: ['x', 'y', 'z']
       # Heading only
       orientation_dimensions: ['yaw']
       # Forward velocity
       linear_velocity_dimensions: ['x']
       # Turn rate
       angular_velocity_dimensions: ['yaw']
-      # Use relative constraints (pose changes rather than absolute poses)
+      # Use relative constraints (physically correct - wheel odom errors are correlated)
       differential: true
       # Covariances - use large values for unused dimensions
       # Order: x, y, z, roll, pitch, yaw
-      pose_covariance_diagonal: [0.05, 0.05, 1e9, 1e9, 1e9, 0.05]
+      pose_covariance_diagonal: [0.01, 0.01, 0.0001, 1e9, 1e9, 0.01]
       # Order: vx, vy, vz, vroll, vpitch, vyaw
-      twist_covariance_diagonal: [0.05, 1e9, 1e9, 1e9, 1e9, 0.05]
+      twist_covariance_diagonal: [0.01, 1e9, 1e9, 1e9, 1e9, 0.01]
 
     imu_sensor:
       topic: /imu_sensor_broadcaster/imu
-      # Orientation from IMU
+      # Orientation: all axes, but in differential mode to avoid
+      # frame mismatch (MuJoCo framequat is in world frame, not odom frame).
+      # Differential uses orientation *changes* between readings, so no
+      # absolute reference frame is needed.
       orientation_dimensions: ['roll', 'pitch', 'yaw']
+      differential: true
       # Angular velocity from gyroscope
       angular_velocity_dimensions: ['roll', 'pitch', 'yaw']
-      # Linear acceleration from accelerometer
-      linear_acceleration_dimensions: ['x', 'y', 'z']
-      # Transform accelerations into base frame
-      acceleration_target_frame: 'ridgeback_base_link'
+      # No linear acceleration - accelerometer biases double-integrate
+      # into position drift. For a ground robot with wheel odom providing
+      # position, the accelerometer is a net negative for accuracy.
       twist_target_frame: 'ridgeback_base_link'
-      # Remove gravity from acceleration measurements
-      remove_gravitational_acceleration: true
       queue_size: 10
 
     # Publish filtered odometry

src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py

@@ -41,11 +41,13 @@
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import (
     LaunchConfiguration,
+    PathJoinSubstitution,
     PythonExpression,
 )
 from launch_ros.actions import Node
 from launch_ros.actions import PushRosNamespace
 from launch_ros.descriptions import ParameterFile
+from launch_ros.substitutions import FindPackageShare
 from nav2_common.launch import RewrittenYaml, ReplaceString
 
 
@@ -282,10 +284,6 @@ def generate_launch_description():
         output="log",
     )
 
-<<<<<<< HEAD
-
-=======
->>>>>>> 5c3330bb (adding IMU fuse plugin to hangar_sim)
     # Create the launch description and populate
     ld = LaunchDescription()
 
@@ -317,4 +315,17 @@ def generate_launch_description():
     ld.add_action(odom_to_joint_state_repub)
     ld.add_action(odom_qos_relay)
 
+    # Fuse state estimator for mobile base localization
+    hangar_sim_pkg = FindPackageShare("hangar_sim")
+    fuse_state_estimator = Node(
+        package="fuse_optimizers",
+        executable="fixed_lag_smoother_node",
+        name="state_estimator",
+        parameters=[
+            PathJoinSubstitution([hangar_sim_pkg, "config", "fuse", "fuse.yaml"])
+        ],
+        output="screen",
+    )
+    ld.add_action(fuse_state_estimator)
+
     return ld