Overview
========

The Model Predictive Control (MPC) module is responsible for computing optimal control commands to follow a reference trajectory. This module takes the vehicle's current state and a planned path as inputs and outputs acceleration and steering commands that minimize tracking error while respecting vehicle constraints.

What is Model Predictive Control?
----------------------------------

Model Predictive Control is an advanced control technique that:

1. **Predicts future behavior**: Uses a model of the vehicle dynamics to predict how the car will behave over a time horizon
2. **Optimizes decisions**: Solves an optimization problem at each time step to find the best control actions
3. **Handles constraints**: Explicitly enforces physical limits (max speed, steering angle, acceleration, etc.)
4. **Plans ahead**: Considers the next N time steps (prediction horizon) rather than just the current moment

The key advantage of MPC over simpler controllers (like PID) is its ability to handle constraints and predict future behavior, making it ideal for autonomous racing where operating at the limits of vehicle performance is critical.

System Architecture
-------------------

The MPC module consists of three main components:

.. code-block:: text

    ┌─────────────────────────────────────────────────────────┐
    │                      MPC Module                         │
    │                                                         │
    │  ┌──────────────────────────────────────────────────┐  │
    │  │         MPCPathFollower Class                    │  │
    │  │  (Core optimization problem formulation)         │  │
    │  │  - Vehicle dynamics model                        │  │
    │  │  - Cost function setup                           │  │
    │  │  - Constraint formulation                        │  │
    │  │  - Solver interface (FATROP/IPOPT)               │  │
    │  └──────────────────────────────────────────────────┘  │
    │                       ↕                                 │
    │  ┌──────────────────────────────────────────────────┐  │
    │  │            MPC ROS2 Node                         │  │
    │  │  (Integration with vehicle system)               │  │
    │  │  - State/path subscriptions                      │  │
    │  │  - Control command publishing                    │  │
    │  │  - Trajectory indexing & management              │  │
    │  └──────────────────────────────────────────────────┘  │
    │                                                         │
    └─────────────────────────────────────────────────────────┘
              ↑                                    ↓
       State & Path                        Control Commands
       (from SLAM & Planning)              (to Vehicle)

Control Flow
------------

At each control cycle (100 Hz):

1. **Receive current state** from SLAM module: position (x, y), heading (ψ), velocity (v)
2. **Find nearest point** on the reference trajectory
3. **Extract lookahead trajectory**: N points ahead on the path (N = 10 by default)
4. **Solve optimization problem**: Find acceleration and steering commands that minimize tracking error
5. **Publish control commands**: Send to vehicle actuators via ROS2 topics
6. **Warm-start next iteration**: Use current solution to initialize the next optimization

Key Features
------------

Real-Time Performance
~~~~~~~~~~~~~~~~~~~~~

- **Fast solvers**: Uses FATROP (structure-exploiting) or IPOPT for efficient optimization
- **C code generation**: Pre-compiles the optimization problem to native C code for 10x speedup
- **Warm-starting**: Initializes each solve with the previous solution to reduce computation time
- **Achieves**: ~1-5ms solve times on modern hardware, enabling 100Hz control rates

Constraint Handling
~~~~~~~~~~~~~~~~~~~

The MPC explicitly enforces:

- Velocity limits: 0-10 m/s
- Acceleration limits: -5 to +3 m/s²
- Steering angle limits: ±0.6 rad (±34°)
- Steering rate limits: ±0.5 rad/s

These constraints ensure the vehicle operates within safe physical limits while maximizing performance.

Robustness Features
~~~~~~~~~~~~~~~~~~~

- **Angle wrapping**: Special handling of heading angles to avoid 2π discontinuities
- **Graceful path handling**: Supports both closed-loop (racing laps) and open-loop (exploration) trajectories
- **Emergency stop**: Can immediately brake to full stop when receiving end-race signal

Integration with System
------------------------

The MPC module integrates tightly with other system components:

**Inputs:**

- Vehicle state from **SLAM** module (position, heading, velocity)
- Reference trajectory from **Path Planning** modules (global or local path)
- Previous steering angle from wheel speed sensors

**Outputs:**

- Acceleration commands to throttle/brake actuators
- Steering angle commands to steering motor
- Predicted trajectory for visualization

**Timing:**

- Control loop runs at 100 Hz (every 10ms)
- MPC horizon: 10 steps × 100ms = 1 second lookahead
- Solver typically completes in 1-5ms, leaving margin for other tasks

Performance Characteristics
---------------------------

Typical Operating Conditions
~~~~~~~~~~~~~~~~~~~~~~~~~~~~

- **Lookahead distance**: ~10m at 10 m/s speed
- **Lateral tracking error**: <0.1m RMS on smooth paths
- **Computation time**: 1-5ms per solve (depends on hardware and solver)
- **Control frequency**: 100 Hz

Parameter Tuning
~~~~~~~~~~~~~~~~

The MPC behavior can be tuned via cost function weights:

- **Q matrix** (state weights): Controls how aggressively the car tracks the reference path
- **R matrix** (control weights): Penalizes large control inputs for smoother driving
- **P matrix** (terminal cost): Encourages stability at the end of the horizon

See :doc:`configuration` for detailed parameter tuning guidelines.