Optimization Problem
====================

The MPC solves a constrained optimization problem at every control cycle to compute the best control inputs. This page explains the mathematical formulation and implementation details.

Problem Formulation
-------------------

At each time step :math:`k`, given:

- Current state :math:`\mathbf{x}_0`
- Reference trajectory :math:`\{\mathbf{\bar{x}}_1, \ldots, \mathbf{\bar{x}}_N\}` and :math:`\{\mathbf{\bar{u}}_0, \ldots, \mathbf{\bar{u}}_{N-1}\}`

Find the optimal control sequence :math:`\{\mathbf{u}_0, \ldots, \mathbf{u}_{N-1}\}` that minimizes:

.. math::

    J = \sum_{i=0}^{N-1} \left[ \|\mathbf{x}_i - \mathbf{\bar{x}}_i\|_Q^2 + \|\mathbf{u}_i - \mathbf{\bar{u}}_i\|_R^2 \right] + \|\mathbf{x}_N - \mathbf{\bar{x}}_N\|_P^2

Subject to:

.. math::

    \mathbf{x}_{i+1} &= F(\mathbf{x}_i, \mathbf{u}_i) \quad \text{(dynamics constraint)} \\
    \mathbf{x}_0 &= \text{current state} \quad \text{(initial condition)} \\
    v_{min} \leq v_i &\leq v_{max} \quad \text{(velocity bounds)} \\
    \theta_{min} \leq \theta_i &\leq \theta_{max} \quad \text{(steering bounds)} \\
    a_{min} \leq a_i &\leq a_{max} \quad \text{(acceleration bounds)} \\
    \dot{\theta}_{min} \leq \dot{\theta}_i &\leq \dot{\theta}_{max} \quad \text{(steering rate bounds)}

Where :math:`\|\mathbf{x}\|_Q^2 = \mathbf{x}^T Q \mathbf{x}` is the weighted squared norm.

Cost Function Components
------------------------

Stage Cost
~~~~~~~~~~

The **stage cost** penalizes deviations from the reference trajectory at each time step:

.. math::

    L_i = \|\mathbf{x}_i - \mathbf{\bar{x}}_i\|_Q^2 + \|\mathbf{u}_i - \mathbf{\bar{u}}_i\|_R^2

- **Q matrix** (state weights): Diagonal matrix with weights for :math:`[x, y, \psi, v, \theta]` deviations
- **R matrix** (control weights): Diagonal matrix with weights for :math:`[a, \dot{\theta}]` deviations

Default weights (from ``all_settings.py``):

.. code-block:: python

    Q = [5.0, 5.0, 3.0, 0.0, 0.0]  # Emphasize position and heading tracking
    R = [0.0, 0.0]                  # No control effort penalty

**Design rationale:**

- High weights on :math:`x, y`: Prioritize staying on the reference path
- Moderate weight on :math:`\psi`: Track the reference heading
- Zero weight on :math:`v, \theta`: Allow flexibility in speed and steering to optimize tracking
- Zero control weights: Maximize performance without penalizing aggressive control

Terminal Cost
~~~~~~~~~~~~~

The **terminal cost** penalizes deviation at the end of the prediction horizon:

.. math::

    L_N = \|\mathbf{x}_N - \mathbf{\bar{x}}_N\|_P^2

The terminal cost matrix **P** is typically larger than Q to encourage the vehicle to be "headed in the right direction" at the end of the horizon, which improves stability.

Default terminal weights:

.. code-block:: python

    P = [5.0, 5.0, 100.0, 0.3, 0.1]  # Much higher heading weight

**Theoretical Note:** In infinite-horizon LQR theory, P should be the solution to the Continuous Algebraic Riccati Equation (CARE). The implementation includes commented-out code to compute this (``mpc_controller.py:98-109``). However, in practice, hand-tuned values work well and avoid numerical issues.

Angle Wrapping
~~~~~~~~~~~~~~

Heading angles :math:`\psi` wrap around at :math:`2\pi`. To avoid discontinuities when the reference angle is near 0 and the actual angle is near :math:`2\pi` (or vice versa), the implementation uses a custom angle difference metric:

.. math::

    \text{angle\_error} = 2\pi \sin\left(\frac{\psi - \bar{\psi}}{2}\right)

This maps the angle difference to :math:`[-\pi, \pi]` continuously.

**See:** ``mpc/mpc/mpc_controller.py:95`` for the implementation.

Constraints
-----------

Dynamics Constraints
~~~~~~~~~~~~~~~~~~~~

The dynamics constraints enforce that the predicted states follow the vehicle model:

.. math::

    \mathbf{x}_{i+1} = F(\mathbf{x}_i, \mathbf{u}_i)

These are implemented as **equality constraints** :math:`g(\mathbf{x}, \mathbf{u}) = 0`.

In the code, these are formulated using CasADi's `.map()` operator for efficient vectorization:

.. code-block:: python

    dynamics_constr = self.x[:, 1:] - self.F.map(self.N)(self.x[:, :-1], self.u[:, :-1])

**See:** ``mpc/mpc/mpc_controller.py:112``

Box Constraints
~~~~~~~~~~~~~~~

The box constraints enforce physical limits on states and controls:

.. list-table:: Constraint Limits
   :header-rows: 1
   :widths: 20 20 20 40

   * - Variable
     - Lower Bound
     - Upper Bound
     - Description
   * - :math:`v`
     - 0 m/s
     - 10 m/s
     - Velocity (no reverse)
   * - :math:`\theta`
     - -0.6 rad
     - +0.6 rad
     - Steering angle (±34°)
   * - :math:`a`
     - -5 m/s²
     - +3 m/s²
     - Acceleration (braking/throttle)
   * - :math:`\dot{\theta}`
     - -0.5 rad/s
     - +0.5 rad/s
     - Steering rate

These are implemented as **inequality constraints** :math:`l \leq g(\mathbf{x}, \mathbf{u}) \leq u`.

**Design notes:**

- Asymmetric acceleration limits: Braking is more powerful than acceleration for safety
- Steering rate limits: Prevent violent steering motions that could destabilize the vehicle
- No lateral position constraints: The cost function handles path tracking; hard constraints would make the problem infeasible

Constraint Structure
~~~~~~~~~~~~~~~~~~~~

The constraints are carefully ordered to match the structure expected by FATROP (a structure-exploiting solver):

.. code-block:: text

    Stage 0:
        - Dynamics (x_1 = F(x_0, u_0))
        - Control bounds (a_min ≤ a_0 ≤ a_max, etc.)
        - Initial state (x_0 = x_current)

    Stage 1 to N-1:
        - Dynamics (x_{i+1} = F(x_i, u_i))
        - Control bounds
        - State bounds (v_min ≤ v_i ≤ v_max, etc.)

    Stage N:
        - State bounds

**See:** ``mpc/mpc/mpc_controller.py:129-151`` for the constraint ordering logic.

Solver Configuration
--------------------

The MPC can use two different solvers:

FATROP (Default)
~~~~~~~~~~~~~~~~

FATROP is a fast solver that exploits the structure of optimal control problems. It's typically 2-5x faster than IPOPT for MPC problems.

.. code-block:: python

    nlpsolver = FATROPSolver
    # Options:
    {
        'structure_detection': 'auto',
        'expand': False,
        'debug': False,
        'fatrop.print_level': -1,  # Suppress output
        'print_time': False
    }

**Key requirement:** FATROP needs to know which constraints are equalities. This is automatically detected by the implementation (``mpc_controller.py:174-175``).

**Performance Note:** FATROP achieves exceptional performance when the problem has a **uniform diagonal structure** in the constraint matrices. This occurs when:

- Each stage has the same constraint structure
- Cost function weights (Q, R, P) are diagonal matrices
- Constraints are ordered consistently across stages

Our implementation is specifically designed to take advantage of this structure. The cost matrices Q, R, and P are all diagonal (``mpc_controller.py:37-38``), and constraints are carefully ordered to maintain uniformity across stages (``mpc_controller.py:129-151``). This uniform diagonal structure allows FATROP's factorization algorithms to be highly efficient, often achieving solve times under 2ms even for 10-step horizons.

IPOPT (Alternative)
~~~~~~~~~~~~~~~~~~~

IPOPT is a general-purpose interior-point optimizer. It's more robust but slower than FATROP.

.. code-block:: python

    nlpsolver = IPOPTSolver
    # Options:
    {
        'expand': False,
        'ipopt.linear_solver': 'MA27',
        'print_time': False,
        'ipopt.print_level': 0
    }

**Linear solvers:** IPOPT requires a linear solver (MA27, MA57, MUMPS, etc.). MA27 is used by default.

Warm-Starting
-------------

To reduce computation time, the MPC uses **warm-starting**: each optimization is initialized with the solution from the previous time step.

.. code-block:: python

    self.warmstart = {
        'x0': res['x'],        # Primal variables
        'lam_x0': res['lam_x'], # Primal variable multipliers
        'lam_g0': res['lam_g'], # Constraint multipliers
    }

This dramatically reduces the number of iterations needed, often achieving convergence in 5-10 iterations instead of 20-50.

**See:** ``mpc/mpc/mpc_controller.py:218-223``

Performance Optimizations
-------------------------

C Code Generation
~~~~~~~~~~~~~~~~~

For maximum performance, the optimization problem can be pre-compiled to C code:

.. code-block:: python

    mpc.construct_solver(
        generate_c=True,  # Generate C code
        compile_c=True,   # Compile with GCC
        use_c=True,       # Load compiled library
        gcc_opt_flag='-Ofast'  # Aggressive optimization
    )

This typically provides a **10x speedup** over the Python-interpreted version.

**See:** ``mpc/mpc/compile_solver.py`` for the build script.

Expression Graph Optimization
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

CasADi uses symbolic expression graphs to represent the optimization problem. The `.map()` operator is used to vectorize the dynamics constraints, which creates a more compact expression graph and enables better optimization.

.. code-block:: python

    # Instead of a loop (slow):
    # for i in range(N):
    #     dynamics_constr[i] = x[:, i+1] - F(x[:, i], u[:, i])

    # Use map (fast):
    dynamics_constr = x[:, 1:] - F.map(N)(x[:, :-1], u[:, :-1])

**See:** ``mpc/mpc/mpc_controller.py:112``

Typical Solve Times
-------------------

On a modern CPU (e.g., Intel i7 or Apple M1):

- **Without C compilation:** 10-20 ms
- **With C compilation:** 1-5 ms
- **With warm-starting:** ~30% faster

These times are well within the 10ms budget for 100Hz control.

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

Solver Status
~~~~~~~~~~~~~

The solver returns a status code indicating success or failure:

.. code-block:: python

    res = self.solver(p=p, lbg=self.lbg, ubg=self.ubg, **self.warmstart)
    # Check: res['status'] should be 0 (success)

Infeasibility
~~~~~~~~~~~~~

If the problem becomes infeasible (no solution satisfies all constraints), possible causes:

1. **Initial state violates bounds** (e.g., v > v_max): Check state estimation
2. **Reference trajectory is unrealistic**: Path planner may be too aggressive
3. **Solver numerical issues**: Try tightening constraint tolerances or using IPOPT

Convergence Issues
~~~~~~~~~~~~~~~~~~

If the solver fails to converge:

1. **Increase iteration limit**: Modify solver options
2. **Scale the problem**: Ensure states and controls are O(1)
3. **Check warm-start quality**: Disable warm-starting temporarily to diagnose
4. **Verify dynamics implementation**: Test F() at known points