Vehicle Dynamics Model ====================== The MPC uses a **kinematic bicycle model** to predict the vehicle's motion. This model is simpler than a full dynamic model but captures the essential behavior needed for path tracking at moderate speeds. Bicycle Model ------------- The bicycle model simplifies the four-wheeled vehicle into a two-wheeled "bicycle" by combining the front wheels into a single wheel and the rear wheels into a single wheel. .. code-block:: text ψ (heading) ↑ │ ┌────┴────┐ │ Front │ │ Wheel │ ← δ (steering angle) └─────────┘ │ │ L_f (front wheelbase) │ ┌────●────┐ │ CG │ ← (x, y, ψ) │ │ └─────────┘ │ │ L_r (rear wheelbase) │ ┌─────────┐ │ Rear │ │ Wheel │ └─────────┘ ↓ v (velocity) State Vector ------------ The vehicle state consists of 5 variables: .. math:: \mathbf{x} = \begin{bmatrix} x \\ y \\ \psi \\ v \\ \theta_{prev} \end{bmatrix} Where: - :math:`x, y`: Position in the global coordinate frame (meters) - :math:`\psi`: Heading angle (radians, 0 = East, counterclockwise positive) - :math:`v`: Forward velocity (m/s) - :math:`\theta_{prev}`: Previous steering angle (radians) Note: The previous steering angle is included in the state to enable rate constraints on steering. Control Vector -------------- The control inputs are: .. math:: \mathbf{u} = \begin{bmatrix} a \\ \dot{\theta} \end{bmatrix} Where: - :math:`a`: Longitudinal acceleration (m/s²) - :math:`\dot{\theta}`: Steering angle rate of change (rad/s) By controlling steering *rate* rather than absolute angle, we can enforce smooth steering motions and avoid discontinuous control inputs. Continuous-Time Dynamics ------------------------ The kinematic bicycle model equations are: .. math:: \beta &= \arctan\left(\frac{L_r}{L_f + L_r} \tan(\theta_{prev})\right) \\ \\ \dot{x} &= v \cos(\psi + \beta) \\ \dot{y} &= v \sin(\psi + \beta) \\ \dot{\psi} &= \frac{v}{L_r} \sin(\beta) \\ \dot{v} &= a \\ \dot{\theta}_{prev} &= \dot{\theta} Where: - :math:`\beta`: Slip angle at the center of gravity - :math:`L_f`: Distance from CG to front axle (0.79 m) - :math:`L_r`: Distance from CG to rear axle (0.79 m) **Physical Interpretation:** - The slip angle :math:`\beta` accounts for the fact that the vehicle doesn't instantaneously pivot around its center of gravity - The velocity components use :math:`\psi + \beta` because the vehicle moves in a direction slightly different from its heading due to the slip angle - The heading rate :math:`\dot{\psi}` is proportional to velocity and depends on the slip angle Implementation Details ---------------------- Discrete-Time Integration ~~~~~~~~~~~~~~~~~~~~~~~~~~ Since the MPC operates in discrete time (with time step :math:`\Delta t = 0.1` seconds), we need to convert the continuous dynamics to discrete-time dynamics. This is done using numerical integration. The implementation uses **4th-order Runge-Kutta (RK4)** integration, which provides a good balance between accuracy and computational cost: .. code-block:: python # Pseudocode for RK4 integration def integrate_rk4(x0, u, dt): k1 = dt * f(x0, u) k2 = dt * f(x0 + k1/2, u) k3 = dt * f(x0 + k2/2, u) k4 = dt * f(x0 + k3, u) x_next = x0 + (k1 + 2*k2 + 2*k3 + k4) / 6 return x_next Where :math:`f(\mathbf{x}, \mathbf{u})` represents the continuous-time dynamics (right-hand side of the differential equations above). **See:** ``mpc/mpc/mpc_controller.py:228-274`` for the full implementation. Model Accuracy ~~~~~~~~~~~~~~ The kinematic bicycle model is accurate when: - Speeds are moderate (< 15 m/s) - Lateral acceleration is not extreme - Tire slip is minimal For Formula Electric racing, these assumptions generally hold, especially during early testing and development. For higher speeds and more aggressive maneuvers, a dynamic model (accounting for tire forces, weight transfer, etc.) would be more accurate but significantly more complex. Vehicle Parameters ------------------ The current vehicle parameters are configured in ``all_settings/all_settings/all_settings.py``: .. code-block:: python L_F: float = 1.58/2 # 0.79 m L_R: float = 1.58/2 # 0.79 m These represent the 2024 vehicle geometry. For different vehicles, these parameters should be measured and updated accordingly. Jacobians (Linearization) -------------------------- For some advanced uses (e.g., computing terminal cost matrices via Linear Quadratic Regulator theory), the MPC implementation can compute Jacobians of the dynamics: .. math:: A = \frac{\partial f}{\partial \mathbf{x}}, \quad B = \frac{\partial f}{\partial \mathbf{u}} These are computed symbolically using CasADi's automatic differentiation: .. code-block:: python A = ca.Function('A', [x0, u0], [ca.jacobian(x1, x0)]) B = ca.Function('B', [x0, u0], [ca.jacobian(x1, u0)]) **See:** ``mpc/mpc/mpc_controller.py:256-257`` Alternative: Midpoint Integration ---------------------------------- The implementation also supports midpoint integration as an alternative to RK4: .. code-block:: python # Midpoint method (2nd order) x_mid = x0 + f(x0, u) * (dt/2) x_next = x0 + f(x_mid, u) * dt Midpoint is less accurate than RK4 but requires fewer function evaluations. In practice, RK4 is preferred for its accuracy, and the additional computational cost is negligible. Validation ---------- The dynamics model can be validated by: 1. **Simulation comparison**: Compare MPC predictions against a high-fidelity simulator 2. **Hardware tests**: Record vehicle trajectories and compare against model predictions 3. **Stability analysis**: Verify controllability of the linearized system (rank of controllability matrix should be 4) The controllability check is implemented (currently commented out) in ``mpc/mpc/mpc_controller.py:99-102``: .. code-block:: python # A = np.array(self.A([0, 0, 0, 10], [0, 0])) # B = np.array(self.B([0, 0, 0, 10], [0, 0])) # assert 3.9 < np.linalg.matrix_rank(ct.ctrb(A, B)) < 4.1