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:

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:

# 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:

# 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:

# 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:

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

Visualization:

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

Race Management:

NUM_LAPS = 10  # Race ends after this many laps

Optimization Parameters

GraphSLAMSolverSettings

Matrix Initialization:

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:

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:

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:

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:

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):

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):

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:

   # 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:

   solve_frequency = 0.05  # 20 Hz instead of 10 Hz
   

4. Enable ICP (requires code modification):

   # 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:

   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:

   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:

   max_landmark_distance = 1.5  # From 1.0
   

2. Trust observations more:

   z_weight = 1.5  # From 1.0
   

3. Check solve frequency:

   # 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:

   dclip = 1.5  # From 2.0
   

3. For MWEKF, trust SLAM corrections more:

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

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

   # 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:

   solve_frequency = 0.2  # 5 Hz from 10 Hz
   

2. Use distance-based solving:

   solve_by_time = False
   solve_distance = 1.0  # Larger distance
   

3. Reduce local map size:

   local_radius = 10.0  # From 15.0
   

4. Increase initial matrix sizes (avoid reallocations):

   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:

   # 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:

   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:

   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):

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:

# 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:

# 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:

# 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:

# 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) 30 Hz (late)	5 Hz 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%