casiopeia demo

Example: Model race cars

Verschueren, Robin: Design and implementation of a time-optimal controller for model race cars, Master's thesis, KU Leuven, 2014.

Image source: Spengler, Patrick and Gammeter, Christoph: Modeling of 1:43 scale race cars, Master’s thesis, ETH Zürich, 2010.

Equation system

$$\dot{X} = v \, cos(\psi + C_{1} \delta) \\ \dot{Y} = v \, sin(\psi + C_{1} \delta) \\ \dot{\psi} = v \, \delta \, C_{2} \\ \dot{v} = C_{m_{1}} \, D - C_{m_{2}} \, D \, v - C_{r_{2}} \, v^{2} - C_{r_{0}} - (v \, \delta)^{2} \, C_{2} \, C_{1}$$

where

$$x = \begin{pmatrix} {X} \\ {Y} \\ {\psi} \\ {v} \end{pmatrix}, ~ u = \begin{pmatrix} {\delta} \\ D \end{pmatrix}, ~ p = \begin{pmatrix} {C_{1}} \\ {C_{2}} \\ {C_{m_{1}}} \\ C_{m_{2}} \\ {C_{r_{2}}} \\ {C_{r_{0}}} \end{pmatrix}$$

and

$$y = x$$



In [ ]:

    
# Define the system using CasADi symbolics

import casadi as ca
import pylab as pl

x = ca.MX.sym("x", 4)
p = ca.MX.sym("p", 6)
u = ca.MX.sym("u", 2)

f = ca.vertcat( \

    [x[3] * pl.cos(x[2] + p[0] * u[0]),

    x[3] * pl.sin(x[2] + p[0] * u[0]),

    x[3] * u[0] * p[1],

    p[2] * u[1] \
        - p[3] * u[1] * x[3] \
        - p[4] * x[3]**2 \
        - p[5] \
        - (x[3] * u[0])**2 * p[1] * p[0]])

phi = x



In [ ]:

    
# Load and define measurements, controls and weightings
# (measurement data provided by Robin Verschueren)

data = pl.array(pl.loadtxt("data_2d_vehicle.dat", delimiter = ", ", skiprows = 1))

# Let's consider some part of the race track

time_points = data[200:750:5, 1]

ydata = data[200:750:5, [2, 4, 6, 8]]
udata = data[200:750:5, [9, 10]][:-1, :]

# Plot considered measurement data

import casiopeia_demo
casiopeia_demo.plot_measurements(time_points, ydata)



In [ ]:

    
# Set up the previously defined system for casiopeia

import casiopeia as cp

system = cp.system.System(x = x, u = u, p = p, f = f, phi = phi)



In [ ]:

    
# Set up least squares parameter estimation problem

pinit = [0.5, 17.06, 11.5, 5, 0.07, 0.70]

pe = cp.pe.LSq( \
    system = system, \
    time_points = time_points, \
    udata = udata, \
    pinit = pinit, \
    ydata = ydata, \
    xinit = ydata)



In [ ]:

    
# Run parameter estimation and show results

pe.run_parameter_estimation()
pe.print_estimation_results()



In [ ]:

    
# Simulate system with estimated parameters, plot results

sim = cp.sim.Simulation(system, pe.estimated_parameters)
sim.run_system_simulation(time_points = time_points, \
    x0 = ydata[0,:], \
    udata = udata)

y_sim = sim.simulation_results

casiopeia_demo.plot_measurements_and_simulation_results( \
    time_points, ydata, y_sim.T)



In [ ]:

    
# Compute covariance matrix, show results

pe.compute_covariance_matrix()
pe.print_estimation_results()



In [ ]:

    
# Now let's consider a smaller part of the race track

time_points_sub = data[200:400:5, 1]

ydata_sub = data[200:400:5, [2, 4, 6, 8]]
udata_sub = data[200:400:5, [9, 10]][:-1, :]

# Simulate race car path, using the previously estimated parameters

sim_sub = cp.sim.Simulation(system, pe.estimated_parameters)
sim_sub.run_system_simulation(time_points = time_points_sub, \
    x0 = ydata_sub[0,:], \
    udata = udata_sub)

y_sim_sub = sim_sub.simulation_results

casiopeia_demo.plot_simulation_results_initial_controls( \
    time_points_sub, y_sim_sub.T)



In [ ]:

    
# Optimize controls for parameter identification

umin = [-0.436332, -0.3216]
umax = [0.436332, 1.0]

xmin = [-0.787, -1.531, -12.614, 0.0]
xmax = [1.2390, 0.014, 0.013, 0.7102]

doe = cp.doe.DoE(system = system, \
    time_points = time_points_sub, \
    uinit = udata_sub, \
    pdata = pe.estimated_parameters, \
    x0 = ydata[0,:], \
    umin = umin, umax = umax, \
    xmin = xmin, xmax = xmax, \
    optimality_criterion = "A")



In [ ]:

    
# We will not run the experimental design now, because it
# will take about 10 minutes to complete, but rather load
# the results of a previous optimization

# doe.run_experimental_design()

udata_opt = pl.loadtxt("optimized_controls_casiopeia_demo.txt").reshape(-1, 2)



In [ ]:

    
# Simulate the optimized race car path, plot the results

sim_opt = cp.sim.Simulation(system, pe.estimated_parameters)
sim_opt.run_system_simulation(time_points = time_points_sub, \
    x0 = ydata_sub[0,:], \
    udata = udata_opt)

y_sim_opt = sim_opt.simulation_results

casiopeia_demo.plot_simulation_results_initial_and_optimized_controls( \
    time_points_sub, y_sim_sub.T, y_sim_opt.T)



In [ ]:

    
# Plot initial and optimized controls for comparison

casiopeia_demo.plot_initial_and_optimized_controls( \
    time_points_sub, udata_sub, udata_opt, umin, umax)