Model Predictive Control
=========================

The Model Predictive Control (MPC) module computes optimal control commands (acceleration and steering) to follow a reference trajectory while respecting vehicle constraints. The MPC predicts future vehicle behavior over a receding horizon and solves a constrained optimization problem at each time step to minimize tracking error.

Overview
--------

MPC is an advanced control technique that:

1. **Predicts** future vehicle states over a finite horizon using a dynamics model
2. **Optimizes** control inputs to minimize trajectory tracking error
3. **Enforces** physical constraints (speed limits, steering limits, acceleration limits)
4. **Applies** the first control input and repeats the process at the next time step

This approach is particularly well-suited for autonomous racing because it can explicitly handle vehicle limits while maximizing performance.

Key Features
~~~~~~~~~~~~

- **Real-time capable**: Achieves 1-5ms solve times with C code generation
- **Nonlinear dynamics**: Uses bicycle kinematic model with numerical integration
- **Hard constraints**: Enforces velocity, acceleration, and steering rate limits
- **Warm-starting**: Leverages previous solutions for faster convergence
- **Structure-exploiting solver**: Uses FATROP for efficient optimization
- **Flexible tuning**: Adjustable cost weights for different driving styles

System Integration
------------------

The MPC controller integrates with the autonomy stack as follows:

**Inputs:**

- Vehicle state from SLAM module (position, heading, velocity)
- Reference trajectory from global or local path planner
- Previous control inputs for warm-starting

**Outputs:**

- Acceleration commands (m/s²)
- Steering angle rate commands (rad/s)
- Predicted trajectory for visualization

**Control Loop:**

- Runs at 100 Hz (10ms period)
- MPC horizon: 10 time steps × 100ms = 1 second lookahead
- Typical lookahead distance: ~10m at 10 m/s

.. toctree::
   :maxdepth: 2
   :caption: MPC Documentation

   mpc/overview
   mpc/vehicle_model
   mpc/optimization
   mpc/configuration

Published Topics
----------------

.. list-table::
   :widths: 40 30 30
   :header-rows: 1

   * - Topic
     - Type
     - Description
   * - ``/cmd``
     - ackermann_msgs/AckermannDriveStamped
     - Control commands (acceleration, steering)
   * - ``/control/throttle``
     - std_msgs/Float64
     - Acceleration command only
   * - ``/control/steer``
     - std_msgs/Float64
     - Steering angle command only
   * - ``/mpc/viz``
     - geometry_msgs/PoseArray
     - Predicted and reference trajectories for visualization

Subscribed Topics
-----------------

.. list-table::
   :widths: 40 30 30
   :header-rows: 1

   * - Topic
     - Type
     - Description
   * - ``/slam/state``
     - feb_msgs/State
     - Current vehicle state (x, y, heading, velocity)
   * - ``/path/global``
     - feb_msgs/FebPath
     - Reference trajectory from global planner (after first lap)
   * - ``/path/local``
     - feb_msgs/FebPath
     - Reference trajectory from local planner (exploration lap)
   * - ``/ground_truth/wheel_speeds``
     - eufs_msgs/WheelSpeedsStamped
     - Current steering angle feedback
   * - ``/end_race``
     - std_msgs/Bool
     - Emergency stop signal (triggers maximum braking)

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

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

- **Prediction horizon**: 1 second (10 steps × 100ms)
- **Control frequency**: 100 Hz
- **Solve time**: 1-5 ms with compiled solver, 10-20 ms without
- **Tracking accuracy**: <0.1m RMS lateral error on smooth paths
- **Maximum speed**: 10 m/s (configurable up to 15+ m/s)

Computational Requirements
~~~~~~~~~~~~~~~~~~~~~~~~~~

- **CPU**: ~5-10% of one core at 100 Hz (with compiled solver)
- **Memory**: ~50 MB (including Python runtime and dependencies)
- **Dependencies**: CasADi, NumPy, SciPy, FATROP or IPOPT

API Reference
-------------

.. automodule:: mpc.mpc_controller
   :members:
   :undoc-members:
   :show-inheritance:

.. automodule:: mpc.main
   :members:
   :undoc-members:
   :show-inheritance: