Back to Repositories

Validating Vehicle Dynamics Model Testing in OpenPilot Control System

This test suite validates the VehicleModel class implementation in OpenPilot’s control system, focusing on vehicle dynamics calculations and state space modeling. The tests verify yaw rate calculations, state space solutions, and simulation accuracy for various driving conditions.

Test Coverage Overview

The test suite provides comprehensive coverage of the VehicleModel class functionality, focusing on three critical areas:

Round-trip validation of yaw rate calculations across different speeds, roll angles, and steering angles
Verification of dynamic state space solutions against direct yaw rate computations
Validation of steady-state solutions against discrete time simulations

Edge cases include zero-speed conditions and extreme steering angles, with integration points to Honda vehicle interfaces.

Implementation Analysis

The testing approach employs systematic parameter space exploration using numpy’s linspace for comprehensive validation. The implementation leverages pytest’s fixtures and approximation assertions for numerical comparisons.

Parametric testing across speed, roll, and steering angle ranges
Matrix operations for state space modeling
Discrete time system simulation with factorial series approximation

Technical Details

Testing tools and libraries:

pytest for test framework and assertions
numpy for numerical computations and matrix operations
math module for trigonometric calculations
Custom VehicleModel class from OpenPilot controls library
Honda car interface integration for parameter initialization

Best Practices Demonstrated

The test suite exemplifies high-quality testing practices through methodical validation of mathematical models. Notable practices include:

Systematic boundary testing with linspace parameter ranges
Numerical approximation handling with pytest.approx
Clear test method organization with setup fixtures
Comprehensive documentation of test purposes and methods
Separation of concerns between different validation approaches

commaai/openpilot

selfdrive/controls/lib/tests/test_vehicle_model.py

            
import pytest
import math

import numpy as np

from opendbc.car.honda.interface import CarInterface
from opendbc.car.honda.values import CAR
from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel, dyn_ss_sol, create_dyn_state_matrices


class TestVehicleModel:
  def setup_method(self):
    CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
    self.VM = VehicleModel(CP)

  def test_round_trip_yaw_rate(self):
    # TODO: fix VM to work at zero speed
    for u in np.linspace(1, 30, num=10):
      for roll in np.linspace(math.radians(-20), math.radians(20), num=11):
        for sa in np.linspace(math.radians(-20), math.radians(20), num=11):
          yr = self.VM.yaw_rate(sa, u, roll)
          new_sa = self.VM.get_steer_from_yaw_rate(yr, u, roll)

          assert sa == pytest.approx(new_sa)

  def test_dyn_ss_sol_against_yaw_rate(self):
    """Verify that the yaw_rate helper function matches the results
    from the state space model."""

    for roll in np.linspace(math.radians(-20), math.radians(20), num=11):
      for u in np.linspace(1, 30, num=10):
        for sa in np.linspace(math.radians(-20), math.radians(20), num=11):

          # Compute yaw rate based on state space model
          _, yr1 = dyn_ss_sol(sa, u, roll, self.VM)

          # Compute yaw rate using direct computations
          yr2 = self.VM.yaw_rate(sa, u, roll)
          assert float(yr1[0]) == pytest.approx(yr2)

  def test_syn_ss_sol_simulate(self):
    """Verifies that dyn_ss_sol matches a simulation"""

    for roll in np.linspace(math.radians(-20), math.radians(20), num=11):
      for u in np.linspace(1, 30, num=10):
        A, B = create_dyn_state_matrices(u, self.VM)

        # Convert to discrete time system
        dt = 0.01
        top = np.hstack((A, B))
        full = np.vstack((top, np.zeros_like(top))) * dt
        Md = sum([np.linalg.matrix_power(full, k) / math.factorial(k) for k in range(25)])
        Ad = Md[:A.shape[0], :A.shape[1]]
        Bd = Md[:A.shape[0], A.shape[1]:]

        for sa in np.linspace(math.radians(-20), math.radians(20), num=11):
          inp = np.array([[sa], [roll]])

          # Simulate for 1 second
          x1 = np.zeros((2, 1))
          for _ in range(100):
            x1 = Ad @ x1 + Bd @ inp

          # Compute steady state solution directly
          x2 = dyn_ss_sol(sa, u, roll, self.VM)

          np.testing.assert_almost_equal(x1, x2, decimal=3)