Configuration and Tuning Guide
==============================

This guide covers all configuration parameters for both SLAM implementations and provides recommendations for tuning the system for optimal performance.

Configuration Files
-------------------

SLAM parameters are configured in:

.. code-block:: python

   all_settings/all_settings.py

The file contains three main configuration classes:

1. ``GraphSLAMSettings``: General SLAM behavior
2. ``GraphSLAMSolverSettings``: Optimization backend parameters
3. ``MWEKFSettings``: MWEKF-specific parameters (late stage fusion only)

General Settings
----------------

GraphSLAMSettings
~~~~~~~~~~~~~~~~~

**Environment Configuration:**

.. code-block:: python

   # Simulator vs real car
   using_simulator = True  # Use EUFS simulator
   using_ground_truth_cones = False  # Use perfect cone positions
   using_ground_truth_wheelspeeds = True  # Use perfect velocity
   bypass_SLAM = False  # Use ground truth state (testing only)

**Sensor Configuration:**

.. code-block:: python

   # Velocity estimation
   using_wheelspeeds = True  # Use wheel encoders (recommended)
   # If False, uses IMU acceleration integration (less accurate)

   # IMU orientation
   forward_imu_direction = [1, 0, 0]  # Axis aligned with vehicle forward
   # Adjust if IMU is mounted in different orientation

**Update Strategy:**

.. code-block:: python

   # Time-based (recommended for consistent behavior)
   solve_by_time = True
   solve_frequency = 0.1  # seconds (10 Hz)

   # Distance-based (better for variable speed)
   solve_by_time = False
   solve_distance = 0.5  # meters

**Map Configuration:**

.. code-block:: python

   local_radius = 15.0  # meters, cones within this radius published
   local_vision_delta = 0.5  # radians, field of view for local cones

**Visualization:**

.. code-block:: python

   publish_to_rviz = True  # Enable RViz visualizations
   # Publishes: /slam/conemap, /slam/pose, /slam/guessed_positions

**Race Management:**

.. code-block:: python

   NUM_LAPS = 10  # Race ends after this many laps

Optimization Parameters
-----------------------

GraphSLAMSolverSettings
~~~~~~~~~~~~~~~~~~~~~~~

**Matrix Initialization:**

.. code-block:: python

   initial_rows = 100  # Initial constraint matrix size
   initial_cols = 100  # Initial variable vector size
   expansion_ratio = 1.7  # Growth factor when resizing

Larger initial sizes reduce early reallocations but use more memory.

**Data Association:**

.. code-block:: python

   max_landmark_distance = 1.0  # meters

Critical parameter affecting map quality:

- **Too small** (< 0.5m): Duplicate landmarks, cluttered map
- **Too large** (> 2.0m): Incorrect associations, merged cones
- **Recommended**: 0.8-1.2m depending on odometry quality

**Constraint Weights:**

.. code-block:: python

   dx_weight = 2.0  # Odometry confidence
   z_weight = 1.0   # Observation confidence

Relative weighting of constraints:

- **High dx_weight**: Trust odometry more (good wheel encoders)
- **High z_weight**: Trust observations more (good perception)
- **Ratio matters**: dx_weight/z_weight determines balance

**ICP Parameters:**

.. code-block:: python

   dclip = 2.0  # meters, distance clipping for outliers

Only used when ICP data association is enabled. Larger values include more distant cones in alignment.

MWEKF Parameters
----------------

MWEKFSettings (Late Stage Fusion Only)
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

**Data Association Thresholds:**

.. code-block:: python

   max_landmark_distance_camera = 2.0  # meters
   max_landmark_distance_lidar = 1.5   # meters

Camera threshold is higher because:

- Camera observations are noisier
- Need to allow initialization of new cones
- Color matching provides additional constraint

LiDAR threshold is tighter because:

- LiDAR is more precise
- Only refines existing cones
- Prevents false matches

**Process Noise (in mwekf_backend.py):**

.. code-block:: python

   Q = np.eye(4) * 0.3  # Vehicle state process noise

Represents uncertainty in dynamics model:

- **Larger Q**: Trust model less, adapt quickly to measurements
- **Smaller Q**: Trust model more, smoother but slower adaptation
- **Tune if**: Vehicle dynamics don't match bicycle model well

**Measurement Noise (in mwekf_backend.py):**

.. code-block:: python

   R_camera = np.eye(1) * 1.8      # Camera cone observations
   R_lidar = np.eye(1) * 1.6       # LiDAR cone observations
   R_imu = np.eye(1) * 2.0         # IMU heading
   R_wheelspeeds = np.eye(1) * 0.5 # Wheel speed velocity
   R_SLAM = np.eye(2) * 0.25       # GraphSLAM corrections

Represents sensor noise/uncertainty:

- **Larger R**: Trust sensor less, weight measurements lower
- **Smaller R**: Trust sensor more, follow measurements closely
- **Relative values matter**: Determines sensor fusion weighting

Tuning Guidelines
-----------------

Scenario: Odometry Drift
~~~~~~~~~~~~~~~~~~~~~~~~

**Symptoms:**

- State estimate drifts over time
- Doesn't return to same position on loop closure
- Landmarks appear shifted

**Solutions:**

1. **Check sensor calibration:**

   - Verify IMU ``initial_imu`` offset is correct
   - Confirm wheel radius/tick conversion is accurate
   - Ensure IMU forward direction is set correctly

2. **Adjust weights:**

   .. code-block:: python

      # Trust observations more than odometry
      dx_weight = 1.5  # Decrease from 2.0
      z_weight = 1.5   # Increase from 1.0

3. **Increase update frequency:**

   .. code-block:: python

      solve_frequency = 0.05  # 20 Hz instead of 10 Hz

4. **Enable ICP** (requires code modification):

   .. code-block:: python

      # In GraphSLAMSolve.update_graph()
      zprime = self.data_association(z+self.xhat[-1, :], color)
      # Instead of:
      # zprime = z+self.xhat[-1, :]

Scenario: Too Many Landmarks
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

**Symptoms:**

- Map has duplicate cones
- Many closely-spaced landmarks
- Increasing computational cost

**Solutions:**

1. **Decrease association threshold:**

   .. code-block:: python

      max_landmark_distance = 0.7  # From 1.0

2. **Improve perception quality:**

   - Filter spurious detections
   - Reduce false positives
   - Ensure consistent cone colors

3. **For MWEKF, tighten camera threshold:**

   .. code-block:: python

      max_landmark_distance_camera = 1.5  # From 2.0

Scenario: Missing Landmarks
~~~~~~~~~~~~~~~~~~~~~~~~~~~~

**Symptoms:**

- Cones visible in perception not in map
- Sparse map with gaps
- Path planner fails due to insufficient cones

**Solutions:**

1. **Increase association threshold:**

   .. code-block:: python

      max_landmark_distance = 1.5  # From 1.0

2. **Trust observations more:**

   .. code-block:: python

      z_weight = 1.5  # From 1.0

3. **Check solve frequency:**

   .. code-block:: python

      # Solve more frequently
      solve_frequency = 0.05  # 20 Hz

      # Or use distance-based
      solve_by_time = False
      solve_distance = 0.3  # meters

Scenario: Poor Loop Closure
~~~~~~~~~~~~~~~~~~~~~~~~~~~~

**Symptoms:**

- First and second lap maps don't align
- Duplicate cones at lap start
- Orange gate detection fails

**Solutions:**

1. **Enable ICP data association**

2. **Reduce dclip for tighter matching:**

   .. code-block:: python

      dclip = 1.5  # From 2.0

3. **For MWEKF, trust SLAM corrections more:**

   .. code-block:: python

      R_SLAM = np.eye(2) * 0.15  # From 0.25

4. **Increase GraphSLAM sync frequency** (late stage only):

   .. code-block:: python

      # In graphslam_frontend_mwekf.py:52
      self.timer = self.create_timer(0.1, self.load_mwekf_to_slam)
      # From: 0.25

Scenario: High Computational Cost
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

**Symptoms:**

- Solve time > 2-5 ms
- Real-time factor < 1.0
- Dropped sensor messages

**Solutions:**

1. **Reduce solve frequency:**

   .. code-block:: python

      solve_frequency = 0.2  # 5 Hz from 10 Hz

2. **Use distance-based solving:**

   .. code-block:: python

      solve_by_time = False
      solve_distance = 1.0  # Larger distance

3. **Reduce local map size:**

   .. code-block:: python

      local_radius = 10.0  # From 15.0

4. **Increase initial matrix sizes** (avoid reallocations):

   .. code-block:: python

      initial_rows = 500  # From 100
      initial_cols = 500  # From 100

Scenario: MWEKF Divergence (Late Stage Only)
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

**Symptoms:**

- State estimate becomes unrealistic
- Covariance grows unbounded
- Cones positions drift wildly

**Solutions:**

1. **Increase process noise:**

   .. code-block:: python

      # In mwekf_backend.py
      self.Q = np.eye(self.n) * 0.5  # From 0.3

2. **Check control inputs:**

   - Verify MPC commands arriving on ``/cmd``
   - Ensure vehicle parameters ``l_f``, ``l_r`` are correct
   - Confirm dynamics model matches vehicle

3. **Trust sensors more:**

   .. code-block:: python

      R_camera = np.eye(1) * 1.0   # From 1.8
      R_lidar = np.eye(1) * 0.8    # From 1.6

4. **Increase SLAM correction weight:**

   .. code-block:: python

      R_SLAM = np.eye(2) * 0.1  # From 0.25

Coordinate Frame Configuration
-------------------------------

LiDAR to Vehicle Transform
~~~~~~~~~~~~~~~~~~~~~~~~~~~

**Early Stage Fusion** (in ``graphslam_global.py:348``):

.. code-block:: python

   T = np.array([-0.9652, 0])  # [forward, lateral] offset in meters

This transforms LiDAR observations from the LiDAR frame to the vehicle's Center of Gravity (CoG).

**To update:**

1. Measure physical offset from LiDAR to CoG
2. Update ``T`` vector (forward is negative if LiDAR is ahead of CoG)
3. Ensure rotation matrix uses vehicle heading correctly

**Late Stage Fusion:**

Transforms handled via ``rotate_and_translate_cones()`` utility function with sensor-specific configurations.

IMU Orientation
~~~~~~~~~~~~~~~

If IMU is mounted in non-standard orientation:

.. code-block:: python

   # Forward aligned with x-axis (standard)
   forward_imu_direction = [1, 0, 0]

   # Forward aligned with y-axis
   forward_imu_direction = [0, 1, 0]

   # Forward aligned with negative x-axis
   forward_imu_direction = [-1, 0, 0]

Debugging and Diagnostics
--------------------------

Monitoring Performance
~~~~~~~~~~~~~~~~~~~~~~

**Check solve times:**

.. code-block:: python

   # In GraphSLAMSolve.solve_graph()
   start = time.perf_counter()
   soln = sp.sparse.linalg.spsolve(A.T@A, A.T@b)
   print(f"Solve time: {(time.perf_counter()-start)*1000:.2f} ms")

Target: < 2 ms for real-time performance

**Monitor topic rates:**

.. code-block:: bash

   # Check SLAM output rate
   ros2 topic hz /slam/state

   # Check input rates
   ros2 topic hz /fusion/cones  # Early stage
   ros2 topic hz /camera/cones  # Late stage
   ros2 topic hz /lidar/cones   # Late stage

**Visualize in RViz:**

- Enable ``publish_to_rviz = True``
- Add PointCloud displays for ``/slam/conemap``
- Add PoseStamped display for ``/slam/pose``
- Add PointCloud display for ``/slam/guessed_positions``

Common Visualization Issues
~~~~~~~~~~~~~~~~~~~~~~~~~~~~

**Cones not appearing in RViz:**

- Check ``publish_to_rviz`` is True
- Verify frame_id is "map" in displays
- Ensure at least one lap started (for global map)

**Vehicle pose jumping:**

- Odometry drift (see tuning above)
- Data association errors (adjust thresholds)
- Insufficient constraints (need more cone observations)

**Duplicate cones in map:**

- Association threshold too small
- ICP not enabled for loop closure
- Odometry drift causing misalignment

Best Practices
--------------

Recommended Starting Configuration
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

**For most scenarios:**

.. code-block:: python

   # Early Stage Fusion
   solve_by_time = True
   solve_frequency = 0.1
   max_landmark_distance = 1.0
   dx_weight = 2.0
   z_weight = 1.0

   # Late Stage Fusion (MWEKF)
   max_landmark_distance_camera = 2.0
   max_landmark_distance_lidar = 1.5
   Q = 0.3 * I
   R_camera = 1.8 * I
   R_lidar = 1.6 * I

Tuning Process
~~~~~~~~~~~~~~

1. **Start with defaults** - Don't change everything at once
2. **Identify specific issue** - Use symptoms to guide changes
3. **Change one parameter** - Isolate effect of each change
4. **Test systematically** - Run multiple laps, check consistency
5. **Document changes** - Keep notes on what works
6. **Version control** - Commit working configurations

Testing Procedure
~~~~~~~~~~~~~~~~~

1. **Simulator testing** - Use ground truth for debugging
2. **Single lap validation** - Check map quality
3. **Multi-lap consistency** - Verify loop closure
4. **Variable speed** - Test at different velocities
5. **Full race simulation** - Endurance test (10+ laps)

Performance Targets
~~~~~~~~~~~~~~~~~~~

+---------------------------+-------------------+-------------------+
| **Metric**                | **Target**        | **Acceptable**    |
+===========================+===================+===================+
| Solve time                | < 1 ms            | < 3 ms            |
+---------------------------+-------------------+-------------------+
| State update rate         | 10 Hz (early)     | 5 Hz              |
|                           | 30 Hz (late)      | 15 Hz             |
+---------------------------+-------------------+-------------------+
| Position error (1 lap)    | < 0.5 m           | < 1.0 m           |
+---------------------------+-------------------+-------------------+
| Loop closure error        | < 0.3 m           | < 0.8 m           |
+---------------------------+-------------------+-------------------+
| Landmark duplicates       | < 5%              | < 10%             |
+---------------------------+-------------------+-------------------+
| Dropped sensor messages   | 0%                | < 2%              |
+---------------------------+-------------------+-------------------+