Update localizer implementation

src/subsystems/navigation/localizer/config/PARAMETERS.md

@@ -0,0 +1,141 @@
+# State Estimator Configuration
+
+Configuration reference for the GTSAM-based state estimator.
+
+## Topics
+
+| Parameter | Type | Default | Description |
+|-----------|------|---------|-------------|
+| `imu_topic` | string | `/imu` | IMU measurements input |
+| `gnss_topic` | string | `/gps/fix` | GNSS position fixes |
+| `odom_topic` | string | `/odom/ground_truth` | Wheel odometry input |
+| `output_odom_topic` | string | `/localization/odom` | Fused state output |
+
+## Frame IDs
+
+| Parameter | Type | Default | Description |
+|-----------|------|---------|-------------|
+| `tf_prefix` | string | `""` | Namespace prefix for all frames |
+| `map_frame` | string | `map` | Global reference frame |
+| `odom_frame` | string | `odom` | Odometry frame (drift-free subset) |
+| `base_frame` | string | `base_footprint` | Robot body frame |
+| `imu_frame` | string | `imu_link` | IMU sensor frame |
+| `gnss_frame` | string | `gnss_link` | GNSS antenna frame |
+
+## Rates
+
+| Parameter | Type | Default | Description |
+|-----------|------|---------|-------------|
+| `publish_rate` | double | 50.0 | Odometry message publish rate (Hz) |
+| `tf_publish_rate` | double | 50.0 | Transform broadcast rate (Hz) |
+| `state_creation_rate` | double | 50.0 | Graph state creation rate (Hz). Higher values = denser graph, more compute |
+
+## IMU Noise Model
+
+These are continuous-time noise densities for the IMU preintegration.
+
+| Parameter | Type | Default | Description |
+|-----------|------|---------|-------------|
+| `accel_noise_sigma` | double | 0.1 | Accelerometer white noise (m/s²/√Hz) |
+| `gyro_noise_sigma` | double | 0.01 | Gyroscope white noise (rad/s/√Hz) |
+| `accel_bias_rw_sigma` | double | 1e-4 | Accel bias random walk (m/s³/√Hz) |
+| `gyro_bias_rw_sigma` | double | 1e-5 | Gyro bias random walk (rad/s²/√Hz) |
+
+**Note:** Z-axis accelerometer uncertainty is automatically scaled 10x higher due to gravity dominance and poor vertical observability.
+
+### IMU Filtering (Optional)
+
+| Parameter | Type | Default | Description |
+|-----------|------|---------|-------------|
+| `enable_imu_filter` | bool | false | Apply exponential moving average filter to raw IMU |
+| `imu_filter_alpha` | double | 0.3 | EMA smoothing factor (0=no update, 1=no smoothing) |
+
+## GNSS Parameters
+
+| Parameter | Type | Default | Description |
+|-----------|------|---------|-------------|
+| `gnss_position_sigma` | double | 1.0 | Base horizontal position uncertainty (m) |
+| `gnss_altitude_sigma` | double | 0.15 | Vertical position uncertainty (m) |
+| `gnss_max_age` | double | 1.0 | Maximum message age before rejection (s) |
+| `gnss_fix_quality_multiplier` | double | 2.0 | Uncertainty multiplier for standard fix vs RTK |
+
+### Robust Estimation
+
+| Parameter | Type | Default | Description |
+|-----------|------|---------|-------------|
+| `use_robust_gnss_noise` | bool | true | Enable Huber M-estimator for outlier rejection |
+| `gnss_huber_k` | double | 1.345 | Huber threshold (lower = more aggressive rejection) |
+
+## Odometry Parameters
+
+Noise model for relative odometry measurements.
+
+| Parameter | Type | Default | Description |
+|-----------|------|---------|-------------|
+| `odom_position_sigma` | double | 0.05 | Translation uncertainty (m) |
+| `odom_rotation_sigma` | double | 0.05 | Rotation uncertainty (rad) |
+
+## ISAM2 Optimizer
+
+| Parameter | Type | Default | Description |
+|-----------|------|---------|-------------|
+| `isam2_relinearize_threshold` | double | 0.1 | Relinearization threshold (lower = more accurate but slower) |
+| `isam2_relinearize_skip` | int | 1 | Relinearize every N updates (1 = every time) |
+
+## Graph Management
+
+| Parameter | Type | Default | Description |
+|-----------|------|---------|-------------|
+| `max_graph_states` | size_t | 100 | Maximum states in graph before marginalization |
+| `imu_buffer_max_size` | size_t | 1000 | Maximum IMU measurements to buffer |
+
+**Duration:** At 50 Hz state creation, 100 states = 2 seconds of history.
+
+## Integration Parameters
+
+Internal preintegration tuning - usually don't touch these.
+
+| Parameter | Type | Default | Description |
+|-----------|------|---------|-------------|
+| `integration_covariance` | double | 1e-8 | IMU integration uncertainty scalar |
+| `bias_acc_omega_int` | double | 1e-5 | Bias coupling during integration |
+
+## Initialization
+
+| Parameter | Type | Default | Description |
+|-----------|------|---------|-------------|
+| `min_imu_samples_for_init` | size_t | 100 | IMU samples needed for gravity alignment |
+| `initial_velocity_sigma` | double | 0.1 | Initial velocity prior uncertainty (m/s) |
+| `initial_rotation_sigma` | double | 0.1 | Initial orientation prior (rad) |
+| `initial_accel_bias_sigma` | double | 0.1 | Initial accelerometer bias prior (m/s²) |
+| `initial_gyro_bias_sigma` | double | 0.01 | Initial gyroscope bias prior (rad/s) |
+
+## ENU Origin
+
+Set the local tangent plane origin. If not provided, uses first GNSS fix.
+
+| Parameter | Type | Default | Description |
+|-----------|------|---------|-------------|
+| `origin_lat` | double | 0.0 | Latitude (degrees) |
+| `origin_lon` | double | 0.0 | Longitude (degrees) |
+| `origin_alt` | double | 0.0 | Altitude (meters) |
+
+## Common Tuning Scenarios
+
+### High-Rate IMU with Drift
+- Increase `accel_bias_rw_sigma` and `gyro_bias_rw_sigma` to allow bias tracking
+- Increase `max_graph_states` to maintain longer optimization window
+
+### Poor GNSS Quality
+- Increase `gnss_position_sigma`
+- Enable `use_robust_gnss_noise`
+- Lower `gnss_huber_k` for aggressive outlier rejection
+
+### Computational Constraints
+- Reduce `state_creation_rate`
+- Reduce `max_graph_states`
+- Increase `isam2_relinearize_skip`
+
+### Initialization Issues
+- Increase `min_imu_samples_for_init` for better gravity estimate
+- Check IMU is in base_link frame or transform is available

src/subsystems/navigation/localizer/config/localizer.yaml

@@ -1,37 +1,88 @@
-localizer_node:
+state_estimator:
   ros__parameters:
-    # Topic configuration
+    # ==========================================================================
+    # Topic Configuration
+    # ==========================================================================
     imu_topic: "/imu"
     gnss_topic: "/gps/fix"
-    odom_topic: "/odom"
+    odom_topic: "/odom/ground_truth"
     output_odom_topic: "/localization/odom"
 
-    # Publish rate (Hz)
-    publish_rate: 50.0
-
-    # Frame names
+    # ==========================================================================
+    # Frame Names
+    # ==========================================================================
     tf_prefix: ""
     map_frame: "map"
     odom_frame: "odom"
-    base_frame: "base_link"
+    base_frame: "base_footprint"
     imu_frame: "imu_link"
     gnss_frame: "gnss_link"
 
-    # IMU noise parameters
-    imu_accel_noise: 0.2        # m/s^2
-    imu_gyro_noise: 0.01        # rad/s
-    imu_bias_noise: 0.0002      # bias random walk
+    # ==========================================================================
+    # Publish Rates
+    # ==========================================================================
+    publish_rate: 100.0          # Hz - high rate for smooth output
+    tf_publish_rate: 100.0       # Hz - increased from 50 for smoother transforms
+
+    # ==========================================================================
+    # IMU Noise Parameters (continuous-time spectral densities)
+    # ==========================================================================
+    accel_noise_sigma: 3.0           # m/s^2/sqrt(Hz) - realistic for raw IMU data
+    gyro_noise_sigma: 10.0           # rad/s/sqrt(Hz) - realistic for raw gyro data
+    accel_bias_rw_sigma: 5.0e-2      # m/s^3/sqrt(Hz) - allow bias adaptation for raw data
+    gyro_bias_rw_sigma: 5.0e-2       # rad/s^2/sqrt(Hz) - allow gyro bias changes
+
+    # ==========================================================================
+    # GNSS Parameters
+    # ==========================================================================
+    gnss_position_sigma: 0.0000001         # meters - realistic consumer GPS uncertainty
+    gnss_altitude_sigma: 0.0000001
+    gnss_max_age: 0.5                # seconds - moderate timing constraint
+    gnss_fix_quality_multiplier: 1.0 # neutral GPS weight
+
+    # GNSS outlier rejection (Huber robust noise model)
+    use_robust_gnss_noise: false      # reject GPS jumps/outliers
+    gnss_huber_k: 1.0                # aggressive outlier rejection
+
+    # ==========================================================================
+    # Odometry Parameters
+    # ==========================================================================
+    odom_position_sigma: 0.00001        # meters - realistic wheel odometry with slip
+    odom_rotation_sigma: 0.00001        # radians - account for steering uncertainty
+
+    # ==========================================================================
+    # Graph Management - MAXIMUM STABILITY FOR RAW DATA
+    # ==========================================================================
+    state_creation_rate: 50.0        # Hz - high rate for dense optimization
+    max_graph_states: 500            # massive window: 10 seconds @ 50Hz
+
+    # ISAM2 optimizer parameters - AGGRESSIVE OPTIMIZATION
+    isam2_relinearize_threshold: 0.01   # tight but realistic convergence
+    isam2_relinearize_skip: 1           # relinearize every update for stability
 
-    # GNSS noise parameters
-    gnss_noise: 0.25            # meters
+    # ==========================================================================
+    # IMU Integration Parameters - REALISTIC FOR RAW DATA
+    # ==========================================================================
+    integration_covariance: 1.0e-6   # account for discretization errors in raw data
+    bias_acc_omega_int: 1.0e-5       # realistic bias integration uncertainty
 
-    # Odometry noise parameters
-    odom_noise: 0.5             # meters
+    # ==========================================================================
+    # Initialization Parameters
+    # ==========================================================================
+    min_imu_samples_for_init: 100    # more samples for noisy raw data (1 second @ 100Hz)
+    initial_velocity_sigma: 0.5      # m/s - less confident with raw data
+    initial_rotation_sigma: 0.1      # rad - ~6° uncertainty for raw IMU
+    initial_accel_bias_sigma: 0.1    # m/s^2 - significant initial bias uncertainty
+    initial_gyro_bias_sigma: 0.01    # rad/s - significant gyro bias uncertainty
 
-    # State estimation window parameters
-    max_time_window: 10.0       # seconds
-    max_states: 100
+    # ==========================================================================
+    # Buffer Configuration
+    # ==========================================================================
+    imu_buffer_max_size: 5000        # large buffer for dense processing and delays
 
+    # ==========================================================================
+    # ENU Origin (optional - if not set, uses first GNSS fix)
+    # ==========================================================================
     origin_lat: 38.42391162772634
     origin_lon: -110.78490558433397
-    origin_alt: 0.0
+    origin_alt: 0.0