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 \(k\), given:

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

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

\[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:

\[\begin{split}\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)}\end{split}\]

Where \(\|\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:

\[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 \([x, y, \psi, v, \theta]\) deviations
R matrix (control weights): Diagonal matrix with weights for \([a, \dot{\theta}]\) deviations

Default weights (from all_settings.py):

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 \(x, y\): Prioritize staying on the reference path
Moderate weight on \(\psi\): Track the reference heading
Zero weight on \(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:

\[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:

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 \(\psi\) wrap around at \(2\pi\). To avoid discontinuities when the reference angle is near 0 and the actual angle is near \(2\pi\) (or vice versa), the implementation uses a custom angle difference metric:

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

This maps the angle difference to \([-\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:

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

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

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

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:

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

These are implemented as inequality constraints \(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):

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.

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.

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.

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:

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.

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

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:

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

Convergence Issues

If the solver fails to converge:

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