Chapter 26: Applications in Aerospace and Automotive Systems
Lesson 3: Adaptive Cruise Control and Powertrain Control (Parameter Adaptation)
This lesson develops an integrated adaptive longitudinal-control architecture for road vehicles. A constant-time-headway cruise controller generates a desired acceleration, while a direct adaptive inner loop converts that demand into a generalized propulsion or braking command despite uncertain vehicle mass, driveline effectiveness, aerodynamic drag, rolling resistance, and road grade. A parallel recursive least-squares estimator provides physically interpretable powertrain diagnostics. The emphasis is on parameterization, Lyapunov stability, excitation, projection, saturation handling, and implementation rather than production-level perception or certification.
1. Learning Objectives and Control Architecture
After completing this lesson, students should be able to:
- derive a linearly parameterized longitudinal vehicle model;
- design a constant-time-headway adaptive cruise-control outer loop;
- prove stability of a direct adaptive torque-domain inner loop;
- derive and implement normalized RLS for propulsion and road-load parameters;
- distinguish tracking convergence from parameter convergence;
- apply projection, dead zones, leakage, excitation gating, and saturation logic;
- map the equations to Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica.
flowchart TD
Lead["Lead vehicle: position and speed"] --> Outer["ACC outer loop: gap and speed control"]
Ego["Ego position and speed"] --> Outer
Outer --> Aref["Desired acceleration a_ref"]
Aref --> Inner["Direct adaptive torque controller"]
Ego --> Inner
Grade["Road grade estimate"] --> Inner
Inner --> Act["Propulsion and brake command"]
Act --> Plant["Powertrain plus longitudinal vehicle"]
Plant --> Ego
Plant --> RLS["Normalized RLS diagnostics"]
RLS --> Monitor["Mass, gain, and road-load monitoring"]
The architecture is deliberately separated into an outer motion objective and an inner uncertain actuation model. The outer loop decides how the ego vehicle should accelerate to maintain speed and spacing. The inner loop adapts the command-to-acceleration map. This separation is useful because payload, gear ratio, tire radius, driveline efficiency, and road load affect the inner dynamics, while the traffic objective remains expressed in meters, seconds, and meters per second.
2. Longitudinal Vehicle and Powertrain Model
Let \(x\) and \(v\) denote the ego vehicle position and speed. A standard longitudinal force balance is
\[ m\dot v = F_x - m g\sin\alpha - m g C_r\cos\alpha - \frac{1}{2}\rho C_D A v^2 - F_d, \qquad \dot x=v. \]
Here \(m\) is vehicle mass, \(\alpha\) is road grade, \(C_r\) is rolling-resistance coefficient, \(\rho C_D A/2\) is the aerodynamic coefficient, and \(F_d\) represents unmodeled disturbances. When a generalized engine/brake torque command \(T\) is mapped through gear ratio \(i_g\), effective driveline efficiency \(\eta_g\), and tire radius \(R_w\),
\[ F_x = \frac{\eta_g i_g}{R_w}T. \]
Dividing the force balance by mass and collecting uncertain coefficients gives the control-oriented model
\[ \dot v = bT-a_0-a_1v-a_2v^2-g\sin\alpha+d(t), \]
\[ b=\frac{\eta_g i_g}{mR_w}, \qquad a_0\approx gC_r, \qquad a_2=\frac{\rho C_D A}{2m}. \]
The coefficient \(a_1\) is included to absorb speed-proportional losses and reduced-order powertrain effects. The high-frequency gain has a known positive sign: \(b>0\). Its magnitude can change when payload, gear, clutch state, tire radius, or efficiency changes.
2.1 Lumped versus physical parameters
The direct adaptive controller will estimate lumped controller parameters, not necessarily the individual physical quantities. Physical mass is recoverable from an estimate of \(b\) only when \(\eta_g\), \(i_g\), and \(R_w\) are known with adequate accuracy:
\[ \widehat m = \frac{\eta_g i_g}{R_w\widehat b}. \]
This distinction prevents a common interpretation error: a parameter vector can yield excellent tracking while not converging to unique physical values.
3. Constant-Time-Headway Adaptive Cruise-Control Outer Loop
Let \(x_L,v_L\) denote the lead-vehicle position and speed, and let \(L\) be the effective lead-vehicle length. The measured bumper-to-bumper gap is
\[ s=x_L-x-L. \]
A constant-time-headway policy specifies
\[ s_d=s_0+h v, \qquad e_s=s-s_d, \qquad \Delta v=v_L-v, \]
where \(s_0>0\) is the standstill gap and \(h>0\) is the time headway. In gap-control mode, choose
\[ a_g=k_p e_s+k_d\Delta v, \qquad k_p>0,\quad k_d>0. \]
A separate speed controller is \(a_v=k_v^s(v_{set}-v)\). A practical ACC command is
\[ a_{ref}= \operatorname{sat}_{[a_{min},a_{max}]} \left(\min\{a_g,a_v\}\right). \]
Thus speed control dominates on an open road, while gap control dominates when the lead vehicle limits motion.
3.1 Gap-mode stability
Assume perfect inner-loop acceleration tracking and locally constant lead speed, so \(\dot v=a_g\) and \(\dot v_L=0\). Then
\[ \dot e_s=\Delta v-h a_g, \qquad \dot{\Delta v}=-a_g. \]
With state \(z=[e_s\;\Delta v]^T\),
\[ \dot z= \begin{bmatrix} -hk_p & 1-hk_d\\ -k_p & -k_d \end{bmatrix}z. \]
The characteristic polynomial is
\[ p(\lambda)= \lambda^2+(hk_p+k_d)\lambda+k_p. \]
Since all coefficients are positive for \(h,k_p,k_d>0\), the second-order Routh criterion proves local exponential stability of the gap-mode equilibrium.
3.2 String-stability condition
Under the same ideal acceleration assumption, the transfer function from lead speed to ego speed is
\[ G(s)=\frac{V(s)}{V_L(s)}= \frac{k_d s+k_p} {s^2+(k_d+hk_p)s+k_p}. \]
A sufficient frequency-domain condition for non-amplification is \(|G(j\omega)|\leq 1\). Direct calculation gives
\[ |D(j\omega)|^2-|N(j\omega)|^2 = \omega^2\left[ \omega^2+h^2k_p^2+2hk_pk_d-2k_p \right]. \]
Therefore a convenient sufficient condition is
\[ h^2k_p+2hk_d\geq 2. \]
This condition concerns propagation through a homogeneous idealized string. Sensor delay, actuator lag, saturation, switching, heterogeneous vehicles, and communication loss require a more complete analysis.
4. Direct Adaptive Torque-Domain Inner Loop
Generate a differentiable command speed \(v_c\) by integrating the saturated outer-loop acceleration:
\[ \dot v_c=a_{ref}, \qquad e=v-v_c. \]
Define the regressor
\[ \phi= \begin{bmatrix} a_{ref}-k_e e\\ 1\\ v\\ v^2\\ g\sin\alpha \end{bmatrix}, \qquad k_e>0. \]
If the plant parameters are constant and \(d(t)=0\), an ideal torque command is generated by
\[ T^*=\theta^{*T}\phi, \qquad \theta^*= \begin{bmatrix} 1/b& a_0/b& a_1/b& a_2/b& 1/b \end{bmatrix}^T. \]
The implementable direct adaptive controller is
\[ T=\widehat\theta^T\phi, \qquad \dot{\widehat\theta}=-\Gamma\phi e, \qquad \Gamma=\Gamma^T>0. \]
4.1 Lyapunov proof
Let \(\widetilde\theta=\widehat\theta-\theta^*\). Substitution into the plant gives
\[ \dot e=-k_e e+b\widetilde\theta^T\phi. \]
Choose
\[ V= \frac{1}{2}e^2+ \frac{b}{2} \widetilde\theta^T\Gamma^{-1} \widetilde\theta. \]
Because \(b>0\), this function is positive definite. Its derivative is
\[ \begin{aligned} \dot V &= e\left(-k_e e+b\widetilde\theta^T\phi\right) +b\widetilde\theta^T\Gamma^{-1} \left(-\Gamma\phi e\right)\\ &=-k_e e^2\leq 0. \end{aligned} \]
Consequently \(e\) and \(\widetilde\theta\) are bounded and \(e\in L_2\). If the command, grade, speed, and their required derivatives remain bounded, then \(\dot e\) is bounded. Barbalat's lemma yields
\[ \lim_{t\to\infty}e(t)=0. \]
The proof establishes tracking, not convergence of every parameter. Parameter convergence requires sufficient excitation of \(\phi\). Steady cruising on a flat road is typically not persistently exciting.
5. Interpretable Powertrain Estimation with Normalized RLS
Direct controller parameters are optimized for tracking. For diagnostics, scheduling, mass estimation, or supervisory monitoring, use a parallel regression based on measured acceleration:
\[ y= \dot v+g\sin\alpha = \varphi^T\vartheta, \]
\[ \varphi= \begin{bmatrix} T&-1&-v&-v^2 \end{bmatrix}^T, \qquad \vartheta= \begin{bmatrix} b&a_0&a_1&a_2 \end{bmatrix}^T. \]
At sample \(k\), exponentially weighted RLS is
\[ K_k= \frac{P_{k-1}\varphi_k} {\lambda+\varphi_k^TP_{k-1}\varphi_k}, \]
\[ \widehat\vartheta_k= \widehat\vartheta_{k-1}+ K_k\left(y_k-\varphi_k^T\widehat\vartheta_{k-1}\right), \]
\[ P_k= \frac{1}{\lambda} \left( P_{k-1}- K_k\varphi_k^TP_{k-1} \right), \qquad 0<\lambda\leq 1. \]
A smaller forgetting factor tracks changes faster but increases sensitivity to noise. Since torque and speed-squared have very different magnitudes, numerical scaling is essential. The supplied implementations use
\[ \bar\varphi= \begin{bmatrix} T/1000&-1&-v/30&-(v/30)^2 \end{bmatrix}^T \]
and estimate \(\bar\vartheta=[1000b,\;a_0,\;30a_1,\;900a_2]^T\). This improves covariance conditioning.
5.1 Identifiability and excitation
When torque, speed, and grade vary little, columns of the data matrix become nearly dependent. In that case, the estimate may fit acceleration well without uniquely separating mass, drag, rolling resistance, and grade. Useful excitation can arise naturally from acceleration/deceleration, gear changes, moderate road-grade variation, or deliberately designed small test signals. Any deliberate excitation must remain subordinate to safety, comfort, and actuator constraints.
6. Robust Adaptation, Saturation, and Gear Changes
The ideal proof assumes constant parameters, exact grade, no unmodeled actuator dynamics, and no saturation. Automotive implementations violate all of these assumptions. A robust update law can combine projection, a dead zone, saturation gating, and leakage:
\[ \dot{\widehat\theta}= \operatorname{Proj}_{\Omega} \left[ -\chi_{sat}\Gamma\phi e -\sigma\left(\widehat\theta-\theta_0\right) \right], \]
where:
- \(\Omega\) contains physically plausible controller parameters;
- \(\chi_{sat}\) is zero when actuator saturation invalidates the nominal error model;
- \(\sigma>0\) prevents drift under disturbances and weak excitation;
- adaptation may be disabled when \(|e|\leq\varepsilon\).
6.1 Piecewise-constant powertrain modes
The propulsion gain contains the gear ratio. A gear shift creates a parameter jump rather than slow drift:
\[ b_g=\frac{\eta_g i_g}{mR_w}. \]
Three practical strategies are: maintain one estimator per gear; schedule nominal parameters by gear and adapt only residual uncertainty; or use a forgetting factor/reset when a validated mode change occurs. Per-gear parameter banks generally preserve interpretability better than forcing one estimate to average incompatible modes.
6.2 Safety constraints
Adaptive tracking alone is not a collision-avoidance certificate. Production architectures require independent minimum-distance logic, sensor-validity checks, emergency braking, acceleration and jerk limits, watchdogs, and fallback control. A simple educational safety margin is
\[ s_{safe}= s_0+h v+ \frac{\max(0,v^2-v_L^2)}{2a_{brake}}, \]
followed by a supervisory override when \(s\leq s_{safe}\). This formula is only a simplified kinematic layer; delay, tire-road friction, grade, uncertainty, and actuator dynamics must be included for safety-critical design.
7. Discrete-Time Implementation Sequence
flowchart TD
S["Read radar, wheel speed, \ngrade, gear, torque"] --> V["Validate and filter signals"]
V --> E["Compute gap, desired gap, relative speed"]
E --> O["Compute gap-mode and speed-mode accelerations"]
O --> C["Apply acceleration, speed, and jerk limits"]
C --> P["Build adaptive regressor"]
P --> T["Compute raw torque command"]
T --> Q["Apply propulsion and braking saturation"]
Q --> U["Update vehicle or send actuator command"]
U --> A["Update projected \nadaptive parameters"]
U --> R["Update normalized RLS diagnostics"]
A --> M["Log residuals and safety metrics"]
R --> M
M --> S
For sampling period \(T_s\), the core recursions are
\[ v_{c,k+1}= \operatorname{sat}_{[0,v_{set}]} \left(v_{c,k}+T_s a_{ref,k}\right), \]
\[ e_k=v_k-v_{c,k}, \qquad T_k= \operatorname{sat} \left(\widehat\theta_k^T\phi_k\right), \]
\[ \widehat\theta_{k+1}= \operatorname{Proj}_{\Omega} \left( \widehat\theta_k- T_s\Gamma\phi_k e_k \right). \]
The Euler parameter update is appropriate for demonstration. Embedded deployment should check numerical rate limits, fixed-point range, task overruns, asynchronous sensor timestamps, derivative filtering, and bumpless transfer between propulsion and braking.
8. Software Libraries and Engineering Mapping
| Environment | Useful libraries or products | Role in this lesson |
|---|---|---|
| Python | NumPy, SciPy, Matplotlib, python-control, CVXPY | Simulation, linear algebra, plotting, transfer-function analysis, optional constrained safety filters |
| C++ | Eigen, Boost.Odeint, OSQP or qpOASES | Embedded-oriented matrix operations, numerical integration, optional real-time quadratic programs |
| Java | EJML, Apache Commons Math, XChart | Matrix computations, estimation, desktop or server-side validation tools |
| MATLAB/Simulink | Control System Toolbox, Simulink, Simulink Control Design | Linear analysis, block-diagram implementation, code-generation-oriented prototyping |
| Wolfram Mathematica | NDSolve, StateSpaceModel, LyapunovSolve, SystemModeler interface | Symbolic derivation, numerical simulation, stability calculations, parameter studies |
The supplied C++ and Java files use only their standard libraries so they compile without package installation. External matrix and optimization libraries become valuable when the model, parameter vector, safety layer, or vehicle platoon is enlarged.
9. Python Implementation
Chapter26_Lesson3.py
"""
Chapter26_Lesson3.py
Adaptive Cruise Control and Powertrain Control (Parameter Adaptation)
Dependencies:
pip install numpy matplotlib
The simulation combines:
1. A constant-time-headway ACC outer loop.
2. A direct adaptive torque-domain inner loop.
3. A normalized recursive least-squares estimator for diagnostic
estimates of propulsion gain and road-load coefficients.
"""
from __future__ import annotations
import csv
import math
from dataclasses import dataclass
import matplotlib.pyplot as plt
import numpy as np
def clamp(value: float, lower: float, upper: float) -> float:
return max(lower, min(upper, value))
@dataclass
class SimulationConfig:
dt: float = 0.01
duration: float = 80.0
vehicle_length: float = 4.8
standstill_gap: float = 8.0
time_headway: float = 1.4
speed_setpoint: float = 30.0
kp_gap: float = 0.28
kd_relative_speed: float = 0.75
kp_speed: float = 0.60
kv_inner: float = 1.50
acceleration_min: float = -3.5
acceleration_max: float = 2.0
torque_min: float = -2500.0
torque_max: float = 2500.0
forgetting_factor: float = 0.997
gravity: float = 9.81
def lead_acceleration(time_s: float) -> float:
"""Piecewise lead-vehicle acceleration profile in m/s^2."""
if 20.0 <= time_s < 24.0:
return -1.5
if 40.0 <= time_s < 46.0:
return 1.0
if 60.0 <= time_s < 64.0:
return -1.0
return 0.0
def road_grade_angle(time_s: float) -> float:
"""Smooth grade-angle profile in radians."""
return 0.03 * math.sin(0.08 * time_s)
def true_propulsion_gain(time_s: float) -> float:
"""
Generalized acceleration-per-torque gain.
The step at 45 s emulates a payload, gear, efficiency, or driveline
change that reduces effective propulsion authority.
"""
return 9.0e-4 if time_s < 45.0 else 7.2e-4
def run_simulation(config: SimulationConfig) -> dict[str, np.ndarray]:
dt = config.dt
sample_count = int(config.duration / dt) + 1
# Lead and ego states.
x_lead, v_lead = 45.0, 24.0
x_ego, v_ego = 0.0, 20.0
v_command = v_ego
# Direct adaptive controller parameters. The ideal vector is
# [1/b, a0/b, a1/b, a2/b, 1/b], but only tracking is guaranteed
# without persistent excitation.
theta_hat = np.array([900.0, 80.0, 4.0, 0.30, 900.0])
gamma = np.array([50.0, 5.0, 0.20, 0.002, 50.0])
theta_lower = np.array([300.0, 0.0, 0.0, 0.0, 300.0])
theta_upper = np.array([2500.0, 500.0, 50.0, 5.0, 2500.0])
# Normalized RLS estimate:
# beta = [1000*b, a0, 30*a1, 900*a2].
beta_hat = np.array([0.80, 0.10, 0.10, 0.10])
covariance = 100.0 * np.eye(4)
# True normalized road-load parameters.
a0_true = 0.08
a1_true = 0.005
a2_true = 0.0004
names = [
"time",
"gap",
"desired_gap",
"gap_error",
"ego_speed",
"lead_speed",
"command_speed",
"desired_acceleration",
"velocity_tracking_error",
"torque_command",
"true_propulsion_gain",
"estimated_propulsion_gain",
"estimated_a0",
"estimated_a1",
"estimated_a2",
"theta_0",
"theta_1",
"theta_2",
"theta_3",
"theta_4",
]
log = {name: np.zeros(sample_count) for name in names}
for index in range(sample_count):
time_s = index * dt
# Lead vehicle.
v_lead = max(5.0, v_lead + dt * lead_acceleration(time_s))
x_lead += dt * v_lead
# Outer-loop ACC.
gap = x_lead - x_ego - config.vehicle_length
desired_gap = config.standstill_gap + config.time_headway * v_ego
gap_error = gap - desired_gap
relative_speed = v_lead - v_ego
acceleration_gap = (
config.kp_gap * gap_error
+ config.kd_relative_speed * relative_speed
)
acceleration_speed = config.kp_speed * (
config.speed_setpoint - v_ego
)
desired_acceleration = clamp(
min(acceleration_gap, acceleration_speed),
config.acceleration_min,
config.acceleration_max,
)
old_v_command = v_command
v_command = clamp(
v_command + dt * desired_acceleration,
0.0,
config.speed_setpoint,
)
desired_acceleration = (v_command - old_v_command) / dt
# Direct adaptive powertrain controller.
grade = road_grade_angle(time_s)
tracking_error = v_ego - v_command
regressor = np.array(
[
desired_acceleration
- config.kv_inner * tracking_error,
1.0,
v_ego,
v_ego * v_ego,
config.gravity * math.sin(grade),
]
)
torque_command = clamp(
float(theta_hat @ regressor),
config.torque_min,
config.torque_max,
)
# Plant dynamics.
b_true = true_propulsion_gain(time_s)
acceleration = (
b_true * torque_command
- a0_true
- a1_true * v_ego
- a2_true * v_ego * v_ego
- config.gravity * math.sin(grade)
)
v_ego = max(0.0, v_ego + dt * acceleration)
x_ego += dt * v_ego
# Lyapunov gradient update with box projection.
theta_hat = np.clip(
theta_hat - dt * gamma * regressor * tracking_error,
theta_lower,
theta_upper,
)
# Diagnostic RLS. Regressors are scaled to avoid poor conditioning.
measured_output = acceleration + config.gravity * math.sin(grade)
normalized_regressor = np.array(
[
torque_command / 1000.0,
-1.0,
-v_ego / 30.0,
-(v_ego / 30.0) ** 2,
]
)
denominator = (
config.forgetting_factor
+ normalized_regressor @ covariance @ normalized_regressor
)
gain = covariance @ normalized_regressor / denominator
prediction_error = (
measured_output - normalized_regressor @ beta_hat
)
beta_hat = beta_hat + gain * prediction_error
covariance = (
covariance
- np.outer(gain, normalized_regressor) @ covariance
) / config.forgetting_factor
b_est = beta_hat[0] / 1000.0
a0_est = beta_hat[1]
a1_est = beta_hat[2] / 30.0
a2_est = beta_hat[3] / 900.0
values = [
time_s,
gap,
desired_gap,
gap_error,
v_ego,
v_lead,
v_command,
desired_acceleration,
tracking_error,
torque_command,
b_true,
b_est,
a0_est,
a1_est,
a2_est,
*theta_hat,
]
for name, value in zip(names, values):
log[name][index] = value
return log
def save_csv(log: dict[str, np.ndarray], path: str) -> None:
names = list(log.keys())
with open(path, "w", newline="", encoding="utf-8") as handle:
writer = csv.writer(handle)
writer.writerow(names)
for row in zip(*(log[name] for name in names)):
writer.writerow(row)
def plot_results(log: dict[str, np.ndarray]) -> None:
time_s = log["time"]
plt.figure()
plt.plot(time_s, log["ego_speed"], label="ego speed")
plt.plot(time_s, log["lead_speed"], label="lead speed")
plt.plot(time_s, log["command_speed"], "--", label="command speed")
plt.xlabel("Time (s)")
plt.ylabel("Speed (m/s)")
plt.grid(True)
plt.legend()
plt.tight_layout()
plt.figure()
plt.plot(time_s, log["gap"], label="actual gap")
plt.plot(time_s, log["desired_gap"], "--", label="desired gap")
plt.xlabel("Time (s)")
plt.ylabel("Gap (m)")
plt.grid(True)
plt.legend()
plt.tight_layout()
plt.figure()
plt.plot(
time_s,
1000.0 * log["true_propulsion_gain"],
label="1000 b true",
)
plt.plot(
time_s,
1000.0 * log["estimated_propulsion_gain"],
"--",
label="1000 b RLS",
)
plt.xlabel("Time (s)")
plt.ylabel("Normalized propulsion gain")
plt.grid(True)
plt.legend()
plt.tight_layout()
plt.figure()
plt.plot(time_s, log["torque_command"])
plt.xlabel("Time (s)")
plt.ylabel("Generalized torque/brake command")
plt.grid(True)
plt.tight_layout()
plt.show()
def main() -> None:
config = SimulationConfig()
log = run_simulation(config)
save_csv(log, "Chapter26_Lesson3_results.csv")
minimum_gap = float(np.min(log["gap"]))
maximum_speed_error = float(
np.max(np.abs(log["velocity_tracking_error"]))
)
print(f"Minimum simulated gap: {minimum_gap:.3f} m")
print(
"Maximum inner-loop speed tracking error: "
f"{maximum_speed_error:.3f} m/s"
)
print(
"Final RLS propulsion-gain estimate: "
f"{log['estimated_propulsion_gain'][-1]:.6e}"
)
plot_results(log)
if __name__ == "__main__":
main()
10. C++ Implementation
Chapter26_Lesson3.cpp
/*
* Chapter26_Lesson3.cpp
* Adaptive Cruise Control and Powertrain Control (Parameter Adaptation)
*
* Build:
* g++ -std=c++17 -O2 Chapter26_Lesson3.cpp -o Chapter26_Lesson3
*
* The program implements:
* 1. Constant-time-headway ACC.
* 2. Direct adaptive torque-domain control.
* 3. Normalized recursive least squares for powertrain diagnostics.
*/
#include <algorithm>
#include <array>
#include <cmath>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <stdexcept>
#include <string>
namespace {
constexpr double kPi = 3.14159265358979323846;
double clamp(double value, double lower, double upper) {
return std::max(lower, std::min(upper, value));
}
double leadAcceleration(double timeSeconds) {
if (timeSeconds >= 20.0 && timeSeconds < 24.0) {
return -1.5;
}
if (timeSeconds >= 40.0 && timeSeconds < 46.0) {
return 1.0;
}
if (timeSeconds >= 60.0 && timeSeconds < 64.0) {
return -1.0;
}
return 0.0;
}
double roadGradeAngle(double timeSeconds) {
return 0.03 * std::sin(0.08 * timeSeconds);
}
double truePropulsionGain(double timeSeconds) {
return timeSeconds < 45.0 ? 9.0e-4 : 7.2e-4;
}
using Vector4 = std::array<double, 4>;
using Vector5 = std::array<double, 5>;
using Matrix4 = std::array<std::array<double, 4>, 4>;
double dot4(const Vector4& left, const Vector4& right) {
double result = 0.0;
for (std::size_t i = 0; i < left.size(); ++i) {
result += left[i] * right[i];
}
return result;
}
double dot5(const Vector5& left, const Vector5& right) {
double result = 0.0;
for (std::size_t i = 0; i < left.size(); ++i) {
result += left[i] * right[i];
}
return result;
}
Vector4 multiply(const Matrix4& matrix, const Vector4& vector) {
Vector4 result{};
for (std::size_t row = 0; row < 4; ++row) {
for (std::size_t column = 0; column < 4; ++column) {
result[row] += matrix[row][column] * vector[column];
}
}
return result;
}
} // namespace
int main() {
try {
const double dt = 0.01;
const double duration = 80.0;
const int sampleCount =
static_cast<int>(std::round(duration / dt)) + 1;
const double vehicleLength = 4.8;
const double standstillGap = 8.0;
const double timeHeadway = 1.4;
const double speedSetpoint = 30.0;
const double kpGap = 0.28;
const double kdRelativeSpeed = 0.75;
const double kpSpeed = 0.60;
const double kvInner = 1.50;
const double accelerationMin = -3.5;
const double accelerationMax = 2.0;
const double torqueMin = -2500.0;
const double torqueMax = 2500.0;
const double forgettingFactor = 0.997;
const double gravity = 9.81;
double xLead = 45.0;
double vLead = 24.0;
double xEgo = 0.0;
double vEgo = 20.0;
double vCommand = vEgo;
Vector5 thetaHat{900.0, 80.0, 4.0, 0.30, 900.0};
const Vector5 gamma{50.0, 5.0, 0.20, 0.002, 50.0};
const Vector5 thetaLower{300.0, 0.0, 0.0, 0.0, 300.0};
const Vector5 thetaUpper{2500.0, 500.0, 50.0, 5.0, 2500.0};
Vector4 betaHat{0.80, 0.10, 0.10, 0.10};
Matrix4 covariance{};
for (std::size_t i = 0; i < 4; ++i) {
covariance[i][i] = 100.0;
}
const double a0True = 0.08;
const double a1True = 0.005;
const double a2True = 0.0004;
std::ofstream output("Chapter26_Lesson3_results.csv");
if (!output) {
throw std::runtime_error("Cannot open output CSV file.");
}
output << std::setprecision(12);
output
<< "time,gap,desired_gap,gap_error,ego_speed,lead_speed,"
<< "command_speed,desired_acceleration,tracking_error,"
<< "torque_command,true_b,estimated_b,estimated_a0,"
<< "estimated_a1,estimated_a2,theta_0,theta_1,theta_2,"
<< "theta_3,theta_4\n";
double minimumGap = 1.0e9;
double maximumTrackingError = 0.0;
double finalEstimatedB = betaHat[0] / 1000.0;
for (int index = 0; index < sampleCount; ++index) {
const double timeSeconds = index * dt;
vLead = std::max(
5.0,
vLead + dt * leadAcceleration(timeSeconds)
);
xLead += dt * vLead;
const double gap = xLead - xEgo - vehicleLength;
const double desiredGap =
standstillGap + timeHeadway * vEgo;
const double gapError = gap - desiredGap;
const double relativeSpeed = vLead - vEgo;
const double accelerationGap =
kpGap * gapError + kdRelativeSpeed * relativeSpeed;
const double accelerationSpeed =
kpSpeed * (speedSetpoint - vEgo);
double desiredAcceleration = clamp(
std::min(accelerationGap, accelerationSpeed),
accelerationMin,
accelerationMax
);
const double oldVCommand = vCommand;
vCommand = clamp(
vCommand + dt * desiredAcceleration,
0.0,
speedSetpoint
);
desiredAcceleration = (vCommand - oldVCommand) / dt;
const double grade = roadGradeAngle(timeSeconds);
const double trackingError = vEgo - vCommand;
Vector5 regressor{
desiredAcceleration - kvInner * trackingError,
1.0,
vEgo,
vEgo * vEgo,
gravity * std::sin(grade),
};
const double torqueCommand = clamp(
dot5(thetaHat, regressor),
torqueMin,
torqueMax
);
const double bTrue = truePropulsionGain(timeSeconds);
const double acceleration =
bTrue * torqueCommand
- a0True
- a1True * vEgo
- a2True * vEgo * vEgo
- gravity * std::sin(grade);
vEgo = std::max(0.0, vEgo + dt * acceleration);
xEgo += dt * vEgo;
for (std::size_t i = 0; i < thetaHat.size(); ++i) {
thetaHat[i] = clamp(
thetaHat[i]
- dt * gamma[i] * regressor[i]
* trackingError,
thetaLower[i],
thetaUpper[i]
);
}
const double measuredOutput =
acceleration + gravity * std::sin(grade);
const Vector4 normalizedRegressor{
torqueCommand / 1000.0,
-1.0,
-vEgo / 30.0,
-std::pow(vEgo / 30.0, 2.0),
};
const Vector4 pTimesRegressor =
multiply(covariance, normalizedRegressor);
const double denominator =
forgettingFactor
+ dot4(normalizedRegressor, pTimesRegressor);
Vector4 rlsGain{};
for (std::size_t i = 0; i < 4; ++i) {
rlsGain[i] = pTimesRegressor[i] / denominator;
}
const double predictionError =
measuredOutput
- dot4(normalizedRegressor, betaHat);
for (std::size_t i = 0; i < 4; ++i) {
betaHat[i] += rlsGain[i] * predictionError;
}
Matrix4 updatedCovariance{};
for (std::size_t row = 0; row < 4; ++row) {
for (std::size_t column = 0; column < 4; ++column) {
double correction = 0.0;
for (std::size_t inner = 0; inner < 4; ++inner) {
correction +=
normalizedRegressor[inner]
* covariance[inner][column];
}
updatedCovariance[row][column] =
(
covariance[row][column]
- rlsGain[row] * correction
) / forgettingFactor;
}
}
covariance = updatedCovariance;
const double estimatedB = betaHat[0] / 1000.0;
const double estimatedA0 = betaHat[1];
const double estimatedA1 = betaHat[2] / 30.0;
const double estimatedA2 = betaHat[3] / 900.0;
minimumGap = std::min(minimumGap, gap);
maximumTrackingError = std::max(
maximumTrackingError,
std::abs(trackingError)
);
finalEstimatedB = estimatedB;
output
<< timeSeconds << ','
<< gap << ','
<< desiredGap << ','
<< gapError << ','
<< vEgo << ','
<< vLead << ','
<< vCommand << ','
<< desiredAcceleration << ','
<< trackingError << ','
<< torqueCommand << ','
<< bTrue << ','
<< estimatedB << ','
<< estimatedA0 << ','
<< estimatedA1 << ','
<< estimatedA2 << ','
<< thetaHat[0] << ','
<< thetaHat[1] << ','
<< thetaHat[2] << ','
<< thetaHat[3] << ','
<< thetaHat[4] << '\n';
}
std::cout << std::fixed << std::setprecision(6)
<< "Minimum simulated gap: "
<< minimumGap << " m\n"
<< "Maximum inner-loop speed tracking error: "
<< maximumTrackingError << " m/s\n"
<< "Final RLS propulsion-gain estimate: "
<< finalEstimatedB << '\n';
return 0;
} catch (const std::exception& exception) {
std::cerr << "Error: " << exception.what() << '\n';
return 1;
}
}
11. Java Implementation
Chapter26_Lesson3.java
/*
* Chapter26_Lesson3.java
* Adaptive Cruise Control and Powertrain Control (Parameter Adaptation)
*
* Build and run:
* javac Chapter26_Lesson3.java
* java Chapter26_Lesson3
*
* The program writes Chapter26_Lesson3_results.csv.
*/
import java.io.BufferedWriter;
import java.io.IOException;
import java.nio.charset.StandardCharsets;
import java.nio.file.Files;
import java.nio.file.Path;
import java.util.Locale;
public final class Chapter26_Lesson3 {
private Chapter26_Lesson3() {
}
private static double clamp(
double value,
double lower,
double upper) {
return Math.max(lower, Math.min(upper, value));
}
private static double leadAcceleration(double timeSeconds) {
if (timeSeconds >= 20.0 && timeSeconds < 24.0) {
return -1.5;
}
if (timeSeconds >= 40.0 && timeSeconds < 46.0) {
return 1.0;
}
if (timeSeconds >= 60.0 && timeSeconds < 64.0) {
return -1.0;
}
return 0.0;
}
private static double roadGradeAngle(double timeSeconds) {
return 0.03 * Math.sin(0.08 * timeSeconds);
}
private static double truePropulsionGain(double timeSeconds) {
return timeSeconds < 45.0 ? 9.0e-4 : 7.2e-4;
}
private static double dot(double[] left, double[] right) {
if (left.length != right.length) {
throw new IllegalArgumentException(
"Vector dimensions do not match."
);
}
double result = 0.0;
for (int i = 0; i < left.length; i++) {
result += left[i] * right[i];
}
return result;
}
private static double[] multiply(
double[][] matrix,
double[] vector) {
double[] result = new double[matrix.length];
for (int row = 0; row < matrix.length; row++) {
for (int column = 0; column < vector.length; column++) {
result[row] += matrix[row][column] * vector[column];
}
}
return result;
}
public static void main(String[] args) {
Locale.setDefault(Locale.US);
final double dt = 0.01;
final double duration = 80.0;
final int sampleCount =
(int) Math.round(duration / dt) + 1;
final double vehicleLength = 4.8;
final double standstillGap = 8.0;
final double timeHeadway = 1.4;
final double speedSetpoint = 30.0;
final double kpGap = 0.28;
final double kdRelativeSpeed = 0.75;
final double kpSpeed = 0.60;
final double kvInner = 1.50;
final double accelerationMin = -3.5;
final double accelerationMax = 2.0;
final double torqueMin = -2500.0;
final double torqueMax = 2500.0;
final double forgettingFactor = 0.997;
final double gravity = 9.81;
double xLead = 45.0;
double vLead = 24.0;
double xEgo = 0.0;
double vEgo = 20.0;
double vCommand = vEgo;
double[] thetaHat =
{900.0, 80.0, 4.0, 0.30, 900.0};
double[] gamma =
{50.0, 5.0, 0.20, 0.002, 50.0};
double[] thetaLower =
{300.0, 0.0, 0.0, 0.0, 300.0};
double[] thetaUpper =
{2500.0, 500.0, 50.0, 5.0, 2500.0};
// beta = [1000*b, a0, 30*a1, 900*a2]
double[] betaHat = {0.80, 0.10, 0.10, 0.10};
double[][] covariance = new double[4][4];
for (int i = 0; i < 4; i++) {
covariance[i][i] = 100.0;
}
final double a0True = 0.08;
final double a1True = 0.005;
final double a2True = 0.0004;
double minimumGap = Double.POSITIVE_INFINITY;
double maximumTrackingError = 0.0;
double finalEstimatedB = betaHat[0] / 1000.0;
Path outputPath =
Path.of("Chapter26_Lesson3_results.csv");
try (BufferedWriter output = Files.newBufferedWriter(
outputPath,
StandardCharsets.UTF_8)) {
output.write(
"time,gap,desired_gap,gap_error,ego_speed,"
+ "lead_speed,command_speed,desired_acceleration,"
+ "tracking_error,torque_command,true_b,"
+ "estimated_b,estimated_a0,estimated_a1,"
+ "estimated_a2,theta_0,theta_1,theta_2,"
+ "theta_3,theta_4"
);
output.newLine();
for (int index = 0; index < sampleCount; index++) {
double timeSeconds = index * dt;
vLead = Math.max(
5.0,
vLead
+ dt * leadAcceleration(timeSeconds)
);
xLead += dt * vLead;
double gap =
xLead - xEgo - vehicleLength;
double desiredGap =
standstillGap + timeHeadway * vEgo;
double gapError = gap - desiredGap;
double relativeSpeed = vLead - vEgo;
double accelerationGap =
kpGap * gapError
+ kdRelativeSpeed * relativeSpeed;
double accelerationSpeed =
kpSpeed * (speedSetpoint - vEgo);
double desiredAcceleration = clamp(
Math.min(
accelerationGap,
accelerationSpeed
),
accelerationMin,
accelerationMax
);
double oldVCommand = vCommand;
vCommand = clamp(
vCommand + dt * desiredAcceleration,
0.0,
speedSetpoint
);
desiredAcceleration =
(vCommand - oldVCommand) / dt;
double grade =
roadGradeAngle(timeSeconds);
double trackingError =
vEgo - vCommand;
double[] regressor = {
desiredAcceleration
- kvInner * trackingError,
1.0,
vEgo,
vEgo * vEgo,
gravity * Math.sin(grade)
};
double torqueCommand = clamp(
dot(thetaHat, regressor),
torqueMin,
torqueMax
);
double bTrue =
truePropulsionGain(timeSeconds);
double acceleration =
bTrue * torqueCommand
- a0True
- a1True * vEgo
- a2True * vEgo * vEgo
- gravity * Math.sin(grade);
vEgo = Math.max(
0.0,
vEgo + dt * acceleration
);
xEgo += dt * vEgo;
for (int i = 0;
i < thetaHat.length;
i++) {
thetaHat[i] = clamp(
thetaHat[i]
- dt * gamma[i]
* regressor[i]
* trackingError,
thetaLower[i],
thetaUpper[i]
);
}
double measuredOutput =
acceleration
+ gravity * Math.sin(grade);
double normalizedSpeed =
vEgo / 30.0;
double[] normalizedRegressor = {
torqueCommand / 1000.0,
-1.0,
-normalizedSpeed,
-normalizedSpeed * normalizedSpeed
};
double[] pTimesRegressor =
multiply(
covariance,
normalizedRegressor
);
double denominator =
forgettingFactor
+ dot(
normalizedRegressor,
pTimesRegressor
);
double[] rlsGain = new double[4];
for (int i = 0; i < 4; i++) {
rlsGain[i] =
pTimesRegressor[i] / denominator;
}
double predictionError =
measuredOutput
- dot(
normalizedRegressor,
betaHat
);
for (int i = 0; i < 4; i++) {
betaHat[i] +=
rlsGain[i] * predictionError;
}
double[][] updatedCovariance =
new double[4][4];
for (int row = 0; row < 4; row++) {
for (int column = 0;
column < 4;
column++) {
double correction = 0.0;
for (int inner = 0;
inner < 4;
inner++) {
correction +=
normalizedRegressor[inner]
* covariance[inner][column];
}
updatedCovariance[row][column] =
(
covariance[row][column]
- rlsGain[row]
* correction
) / forgettingFactor;
}
}
covariance = updatedCovariance;
double estimatedB =
betaHat[0] / 1000.0;
double estimatedA0 = betaHat[1];
double estimatedA1 =
betaHat[2] / 30.0;
double estimatedA2 =
betaHat[3] / 900.0;
minimumGap =
Math.min(minimumGap, gap);
maximumTrackingError = Math.max(
maximumTrackingError,
Math.abs(trackingError)
);
finalEstimatedB = estimatedB;
output.write(String.format(
Locale.US,
"%.12g,%.12g,%.12g,%.12g,%.12g,"
+ "%.12g,%.12g,%.12g,%.12g,%.12g,"
+ "%.12g,%.12g,%.12g,%.12g,%.12g,"
+ "%.12g,%.12g,%.12g,%.12g,%.12g",
timeSeconds,
gap,
desiredGap,
gapError,
vEgo,
vLead,
vCommand,
desiredAcceleration,
trackingError,
torqueCommand,
bTrue,
estimatedB,
estimatedA0,
estimatedA1,
estimatedA2,
thetaHat[0],
thetaHat[1],
thetaHat[2],
thetaHat[3],
thetaHat[4]
));
output.newLine();
}
} catch (IOException exception) {
System.err.println(
"Failed to write results: "
+ exception.getMessage()
);
System.exit(1);
}
System.out.printf(
"Minimum simulated gap: %.6f m%n",
minimumGap
);
System.out.printf(
"Maximum inner-loop speed tracking error: "
+ "%.6f m/s%n",
maximumTrackingError
);
System.out.printf(
"Final RLS propulsion-gain estimate: "
+ "%.6e%n",
finalEstimatedB
);
}
}
12. MATLAB/Simulink Implementation
Chapter26_Lesson3.m
% Chapter26_Lesson3.m
% Adaptive Cruise Control and Powertrain Control (Parameter Adaptation)
%
% Required for this script: MATLAB only.
% Useful optional products for extension:
% Control System Toolbox, Simulink, Simulink Control Design,
% Model Predictive Control Toolbox.
%
% The script implements:
% 1. Constant-time-headway ACC.
% 2. Direct adaptive torque-domain control.
% 3. Normalized RLS for powertrain diagnostics.
% 4. Optional construction of a Simulink architecture shell.
clear; clc; close all;
dt = 0.01;
duration = 80.0;
time = (0:dt:duration)';
sampleCount = numel(time);
vehicleLength = 4.8;
standstillGap = 8.0;
timeHeadway = 1.4;
speedSetpoint = 30.0;
kpGap = 0.28;
kdRelativeSpeed = 0.75;
kpSpeed = 0.60;
kvInner = 1.50;
accelerationMin = -3.5;
accelerationMax = 2.0;
torqueMin = -2500.0;
torqueMax = 2500.0;
forgettingFactor = 0.997;
gravity = 9.81;
xLead = 45.0;
vLead = 24.0;
xEgo = 0.0;
vEgo = 20.0;
vCommand = vEgo;
thetaHat = [900.0; 80.0; 4.0; 0.30; 900.0];
gamma = [50.0; 5.0; 0.20; 0.002; 50.0];
thetaLower = [300.0; 0.0; 0.0; 0.0; 300.0];
thetaUpper = [2500.0; 500.0; 50.0; 5.0; 2500.0];
% betaHat = [1000*b; a0; 30*a1; 900*a2].
betaHat = [0.80; 0.10; 0.10; 0.10];
covariance = 100.0 * eye(4);
a0True = 0.08;
a1True = 0.005;
a2True = 0.0004;
gapLog = zeros(sampleCount, 1);
desiredGapLog = zeros(sampleCount, 1);
gapErrorLog = zeros(sampleCount, 1);
egoSpeedLog = zeros(sampleCount, 1);
leadSpeedLog = zeros(sampleCount, 1);
commandSpeedLog = zeros(sampleCount, 1);
desiredAccelerationLog = zeros(sampleCount, 1);
trackingErrorLog = zeros(sampleCount, 1);
torqueCommandLog = zeros(sampleCount, 1);
trueBLog = zeros(sampleCount, 1);
estimatedBLog = zeros(sampleCount, 1);
estimatedA0Log = zeros(sampleCount, 1);
estimatedA1Log = zeros(sampleCount, 1);
estimatedA2Log = zeros(sampleCount, 1);
thetaLog = zeros(sampleCount, 5);
for k = 1:sampleCount
t = time(k);
leadAcceleration = 0.0;
if t >= 20.0 && t < 24.0
leadAcceleration = -1.5;
elseif t >= 40.0 && t < 46.0
leadAcceleration = 1.0;
elseif t >= 60.0 && t < 64.0
leadAcceleration = -1.0;
end
vLead = max(5.0, vLead + dt * leadAcceleration);
xLead = xLead + dt * vLead;
gap = xLead - xEgo - vehicleLength;
desiredGap = standstillGap + timeHeadway * vEgo;
gapError = gap - desiredGap;
relativeSpeed = vLead - vEgo;
accelerationGap = ...
kpGap * gapError + kdRelativeSpeed * relativeSpeed;
accelerationSpeed = kpSpeed * (speedSetpoint - vEgo);
desiredAcceleration = min(accelerationGap, accelerationSpeed);
desiredAcceleration = min( ...
accelerationMax, ...
max(accelerationMin, desiredAcceleration));
oldVCommand = vCommand;
vCommand = min( ...
speedSetpoint, ...
max(0.0, vCommand + dt * desiredAcceleration));
desiredAcceleration = (vCommand - oldVCommand) / dt;
grade = 0.03 * sin(0.08 * t);
trackingError = vEgo - vCommand;
regressor = [ ...
desiredAcceleration - kvInner * trackingError; ...
1.0; ...
vEgo; ...
vEgo^2; ...
gravity * sin(grade)];
torqueCommand = thetaHat' * regressor;
torqueCommand = min( ...
torqueMax, ...
max(torqueMin, torqueCommand));
if t < 45.0
bTrue = 9.0e-4;
else
bTrue = 7.2e-4;
end
acceleration = ...
bTrue * torqueCommand ...
- a0True ...
- a1True * vEgo ...
- a2True * vEgo^2 ...
- gravity * sin(grade);
vEgo = max(0.0, vEgo + dt * acceleration);
xEgo = xEgo + dt * vEgo;
thetaHat = thetaHat ...
- dt * gamma .* regressor * trackingError;
thetaHat = min(thetaUpper, max(thetaLower, thetaHat));
measuredOutput = acceleration + gravity * sin(grade);
normalizedSpeed = vEgo / 30.0;
normalizedRegressor = [ ...
torqueCommand / 1000.0; ...
-1.0; ...
-normalizedSpeed; ...
-normalizedSpeed^2];
denominator = forgettingFactor ...
+ normalizedRegressor' * covariance ...
* normalizedRegressor;
rlsGain = covariance * normalizedRegressor / denominator;
predictionError = measuredOutput ...
- normalizedRegressor' * betaHat;
betaHat = betaHat + rlsGain * predictionError;
covariance = ( ...
covariance ...
- rlsGain * normalizedRegressor' * covariance ...
) / forgettingFactor;
estimatedB = betaHat(1) / 1000.0;
estimatedA0 = betaHat(2);
estimatedA1 = betaHat(3) / 30.0;
estimatedA2 = betaHat(4) / 900.0;
gapLog(k) = gap;
desiredGapLog(k) = desiredGap;
gapErrorLog(k) = gapError;
egoSpeedLog(k) = vEgo;
leadSpeedLog(k) = vLead;
commandSpeedLog(k) = vCommand;
desiredAccelerationLog(k) = desiredAcceleration;
trackingErrorLog(k) = trackingError;
torqueCommandLog(k) = torqueCommand;
trueBLog(k) = bTrue;
estimatedBLog(k) = estimatedB;
estimatedA0Log(k) = estimatedA0;
estimatedA1Log(k) = estimatedA1;
estimatedA2Log(k) = estimatedA2;
thetaLog(k, :) = thetaHat';
end
results = table( ...
time, ...
gapLog, ...
desiredGapLog, ...
gapErrorLog, ...
egoSpeedLog, ...
leadSpeedLog, ...
commandSpeedLog, ...
desiredAccelerationLog, ...
trackingErrorLog, ...
torqueCommandLog, ...
trueBLog, ...
estimatedBLog, ...
estimatedA0Log, ...
estimatedA1Log, ...
estimatedA2Log, ...
thetaLog(:, 1), ...
thetaLog(:, 2), ...
thetaLog(:, 3), ...
thetaLog(:, 4), ...
thetaLog(:, 5), ...
'VariableNames', { ...
'time', ...
'gap', ...
'desired_gap', ...
'gap_error', ...
'ego_speed', ...
'lead_speed', ...
'command_speed', ...
'desired_acceleration', ...
'tracking_error', ...
'torque_command', ...
'true_b', ...
'estimated_b', ...
'estimated_a0', ...
'estimated_a1', ...
'estimated_a2', ...
'theta_0', ...
'theta_1', ...
'theta_2', ...
'theta_3', ...
'theta_4'});
writetable(results, 'Chapter26_Lesson3_results.csv');
fprintf('Minimum simulated gap: %.3f m\n', min(gapLog));
fprintf( ...
'Maximum inner-loop speed tracking error: %.3f m/s\n', ...
max(abs(trackingErrorLog)));
fprintf( ...
'Final RLS propulsion-gain estimate: %.6e\n', ...
estimatedBLog(end));
figure;
plot(time, egoSpeedLog, 'LineWidth', 1.2); hold on;
plot(time, leadSpeedLog, 'LineWidth', 1.2);
plot(time, commandSpeedLog, '--', 'LineWidth', 1.2);
grid on;
xlabel('Time (s)');
ylabel('Speed (m/s)');
legend('Ego', 'Lead', 'Command', 'Location', 'best');
title('Adaptive Cruise-Control Speeds');
figure;
plot(time, gapLog, 'LineWidth', 1.2); hold on;
plot(time, desiredGapLog, '--', 'LineWidth', 1.2);
grid on;
xlabel('Time (s)');
ylabel('Gap (m)');
legend('Actual gap', 'Desired gap', 'Location', 'best');
title('Constant-Time-Headway Spacing');
figure;
plot(time, 1000.0 * trueBLog, 'LineWidth', 1.2); hold on;
plot(time, 1000.0 * estimatedBLog, '--', 'LineWidth', 1.2);
grid on;
xlabel('Time (s)');
ylabel('Normalized propulsion gain');
legend('1000 b true', '1000 b RLS', 'Location', 'best');
title('Powertrain-Gain Adaptation');
figure;
plot(time, torqueCommandLog, 'LineWidth', 1.2);
grid on;
xlabel('Time (s)');
ylabel('Generalized torque/brake command');
title('Adaptive Inner-Loop Command');
buildSimulinkArchitecture = false;
if buildSimulinkArchitecture
createAdaptiveCruiseSimulinkShell();
end
function createAdaptiveCruiseSimulinkShell()
% Create a reusable architecture shell. The numerical equations in
% this script map to the corresponding subsystems.
modelName = 'Chapter26_Lesson3_Simulink';
if bdIsLoaded(modelName)
close_system(modelName, 0);
end
new_system(modelName);
open_system(modelName);
add_block( ...
'simulink/Ports & Subsystems/In1', ...
[modelName '/Lead state']);
add_block( ...
'simulink/Ports & Subsystems/Subsystem', ...
[modelName '/ACC outer loop']);
add_block( ...
'simulink/Ports & Subsystems/Subsystem', ...
[modelName '/Adaptive torque controller']);
add_block( ...
'simulink/Ports & Subsystems/Subsystem', ...
[modelName '/Powertrain and vehicle']);
add_block( ...
'simulink/Ports & Subsystems/Subsystem', ...
[modelName '/Normalized RLS']);
add_block( ...
'simulink/Ports & Subsystems/Out1', ...
[modelName '/Ego state']);
set_param( ...
[modelName '/Lead state'], ...
'Position', [30 70 60 90]);
set_param( ...
[modelName '/ACC outer loop'], ...
'Position', [120 45 260 115]);
set_param( ...
[modelName '/Adaptive torque controller'], ...
'Position', [320 45 490 115]);
set_param( ...
[modelName '/Powertrain and vehicle'], ...
'Position', [550 45 710 115]);
set_param( ...
[modelName '/Normalized RLS'], ...
'Position', [550 165 710 230]);
set_param( ...
[modelName '/Ego state'], ...
'Position', [770 70 800 90]);
add_line( ...
modelName, ...
'Lead state/1', ...
'ACC outer loop/1', ...
'autorouting', 'on');
add_line( ...
modelName, ...
'ACC outer loop/1', ...
'Adaptive torque controller/1', ...
'autorouting', 'on');
add_line( ...
modelName, ...
'Adaptive torque controller/1', ...
'Powertrain and vehicle/1', ...
'autorouting', 'on');
add_line( ...
modelName, ...
'Powertrain and vehicle/1', ...
'Ego state/1', ...
'autorouting', 'on');
add_line( ...
modelName, ...
'Powertrain and vehicle/1', ...
'Normalized RLS/1', ...
'autorouting', 'on');
save_system(modelName);
fprintf('Created %s.slx architecture shell.\n', modelName);
end
The script performs the complete numerical experiment and can optionally create a Simulink architecture shell. Populate the four subsystems with the equations from Sections 3–7, use discrete integrators at the controller sample time, and expose saturation and reset signals explicitly.
13. Wolfram Mathematica Implementation
Chapter26_Lesson3.nb
Notebook[{
Cell["Chapter 26, Lesson 3: Adaptive Cruise Control and Powertrain Control", "Title"],
Cell["Direct adaptive torque control, constant-time-headway ACC, and normalized RLS diagnostics.", "Text"],
Cell[
BoxData["ClearAll[\"Global`*\"];\n\ndt = 0.01;\nduration = 80.0;\ntime = Range[0.0, duration, dt];\nsampleCount = Length[time];\n
vehicleLength = 4.8;\nstandstillGap = 8.0;\ntimeHeadway = 1.4;\nspeedSetpoint = 30.0;\nkpGap = 0.28;\nkdRelativeSpeed = 0.75;
kpSpeed = 0.60;\nkvInner = 1.50;\naccelerationMin = -3.5;\naccelerationMax = 2.0;\ntorqueMin = -2500.0;\ntorqueMax = 2500.0;
forgettingFactor = 0.997;\ngravity = 9.81;\n\nClampValue[value_, lower_, upper_] := Max[lower, Min[upper, value]];\n
LeadAcceleration[t_] := Piecewise[\n {\n {-1.5, 20.0 <= t < 24.0},\n {1.0, 40.0 <= t < 46.0},\n {-1.0, 60.0 <= t < 64.0}\n },
0.0\n];\n\nRoadGradeAngle[t_] := 0.03 Sin[0.08 t];\n\nTruePropulsionGain[t_] := If[t < 45.0, 9.0*10^-4, 7.2*10^-4];\n\nxLead = 45.0;
vLead = 24.0;\nxEgo = 0.0;\nvEgo = 20.0;\nvCommand = vEgo;\n\nthetaHat = {900.0, 80.0, 4.0, 0.30, 900.0};
gamma = {50.0, 5.0, 0.20, 0.002, 50.0};\nthetaLower = {300.0, 0.0, 0.0, 0.0, 300.0};\nthetaUpper = {2500.0, 500.0, 50.0, 5.0, 2500.0};\n
(* betaHat = {1000 b, a0, 30 a1, 900 a2}. *)\nbetaHat = {0.80, 0.10, 0.10, 0.10};\ncovariance = 100.0 IdentityMatrix[4];\n\na0True = 0.08;
a1True = 0.005;\na2True = 0.0004;\n\nlog = Reap[\n Do[\n t = time[[k]];\n\n vLead = Max[\n 5.0,
vLead + dt LeadAcceleration[t]\n ];\n xLead = xLead + dt vLead;\n\n gap = xLead - xEgo - vehicleLength;
desiredGap = standstillGap + timeHeadway vEgo;\n gapError = gap - desiredGap;\n relativeSpeed = vLead - vEgo;\n
accelerationGap =\n kpGap gapError + kdRelativeSpeed relativeSpeed;\n accelerationSpeed =\n kpSpeed (speedSetpoint - vEgo);
desiredAcceleration = ClampValue[\n Min[accelerationGap, accelerationSpeed],\n accelerationMin,\n accelerationMax\n ];
\n oldVCommand = vCommand;\n vCommand = ClampValue[\n vCommand + dt desiredAcceleration,\n 0.0,\n speedSetpoint\n ];
desiredAcceleration = (vCommand - oldVCommand)/dt;\n\n grade = RoadGradeAngle[t];\n trackingError = vEgo - vCommand;
regressor = {\n desiredAcceleration - kvInner trackingError,\n 1.0,\n vEgo,\n vEgo^2,\n gravity Sin[grade]
};\n\n torqueCommand = ClampValue[\n thetaHat . regressor,\n torqueMin,\n torqueMax\n ];\n
bTrue = TruePropulsionGain[t];\n acceleration =\n bTrue torqueCommand\n - a0True\n - a1True vEgo
- a2True vEgo^2\n - gravity Sin[grade];\n\n vEgo = Max[0.0, vEgo + dt acceleration];\n xEgo = xEgo + dt vEgo;\n
thetaHat = MapThread[\n ClampValue,\n {\n thetaHat\n - dt gamma regressor trackingError,\n thetaLower,
thetaUpper\n }\n ];\n\n measuredOutput = acceleration + gravity Sin[grade];\n normalizedSpeed = vEgo/30.0;
normalizedRegressor = {\n torqueCommand/1000.0,\n -1.0,\n -normalizedSpeed,\n -normalizedSpeed^2\n };\n
denominator =\n forgettingFactor\n + normalizedRegressor . covariance .\n normalizedRegressor;\n rlsGain =
covariance . normalizedRegressor/denominator;\n predictionError =\n measuredOutput - normalizedRegressor . betaHat;
betaHat = betaHat + rlsGain predictionError;\n covariance = (\n covariance\n - Outer[Times, rlsGain, normalizedRegressor] .
covariance\n )/forgettingFactor;\n\n estimatedB = betaHat[[1]]/1000.0;\n estimatedA0 = betaHat[[2]];
estimatedA1 = betaHat[[3]]/30.0;\n estimatedA2 = betaHat[[4]]/900.0;\n\n Sow[\n {\n t,\n gap,
desiredGap,\n gapError,\n vEgo,\n vLead,\n vCommand,\n desiredAcceleration,
trackingError,\n torqueCommand,\n bTrue,\n estimatedB,\n estimatedA0,\n estimatedA1,
estimatedA2,\n Sequence @@ thetaHat\n }\n ],\n {k, 1, sampleCount}\n ]\n][[2, 1]];\n\nheadings = {\n \"time\",
\"gap\",\n \"desired_gap\",\n \"gap_error\",\n \"ego_speed\",\n \"lead_speed\",\n \"command_speed\",\n \"desired_acceleration\",
\"tracking_error\",\n \"torque_command\",\n \"true_b\",\n \"estimated_b\",\n \"estimated_a0\",\n \"estimated_a1\",
\"estimated_a2\",\n \"theta_0\",\n \"theta_1\",\n \"theta_2\",\n \"theta_3\",\n \"theta_4\"\n};\n\nExport[
\"Chapter26_Lesson3_results.csv\",\n Prepend[log, headings]\n];\n\nminimumGap = Min[log[[All, 2]]];
maximumTrackingError = Max[Abs[log[[All, 9]]]];\nfinalEstimatedB = Last[log][[12]];\n\nPrint[\n \"Minimum simulated gap: \",
NumberForm[minimumGap, {8, 3}],\n \" m\"\n];\nPrint[\n \"Maximum inner-loop speed tracking error: \",
NumberForm[maximumTrackingError, {8, 3}],\n \" m/s\"\n];\nPrint[\n \"Final RLS propulsion-gain estimate: \",
ScientificForm[finalEstimatedB, 6]\n];\n\nspeedPlot = ListLinePlot[\n {\n log[[All, {1, 5}]],\n log[[All, {1, 6}]],
log[[All, {1, 7}]]\n },\n PlotLegends -> {\"Ego\", \"Lead\", \"Command\"},\n Frame -> True,
FrameLabel -> {\"Time (s)\", \"Speed (m/s)\"},\n PlotLabel -> \"Adaptive Cruise-Control Speeds\",\n ImageSize -> Large\n];\n
gapPlot = ListLinePlot[\n {\n log[[All, {1, 2}]],\n log[[All, {1, 3}]]\n },\n PlotLegends -> {\"Actual gap\", \"Desired gap\"},
Frame -> True,\n FrameLabel -> {\"Time (s)\", \"Gap (m)\"},\n PlotLabel -> \"Constant-Time-Headway Spacing\",\n ImageSize -> Large\n];
\ngainPlot = ListLinePlot[\n {\n Transpose[\n {\n log[[All, 1]],\n 1000.0 log[[All, 11]]\n }\n ],
Transpose[\n {\n log[[All, 1]],\n 1000.0 log[[All, 12]]\n }\n ]\n },
PlotLegends -> {\"1000 b true\", \"1000 b RLS\"},\n Frame -> True,\n FrameLabel -> {\n \"Time (s)\",
\"Normalized propulsion gain\"\n },\n PlotLabel -> \"Powertrain-Gain Adaptation\",\n ImageSize -> Large\n];\n
Column[{speedPlot, gapPlot, gainPlot}]"],
"Input"
]
},
WindowSize -> {1200, 900},
StyleDefinitions -> "Default.nb"
]
14. Expected Simulation Behavior and Validation Metrics
The example starts with a faster lead vehicle and a positive spacing margin. The lead vehicle later brakes, accelerates, and brakes again. At 45 s, the true propulsion gain decreases to emulate a payload, gear, or efficiency change. Validation should include:
- minimum gap and minimum time-to-collision estimate;
- peak and RMS spacing error;
- peak acceleration and jerk;
- inner-loop command-speed tracking error;
- fraction of time in propulsion or braking saturation;
- parameter-bound contact and covariance conditioning;
- RLS one-step prediction residual before and after the gain change.
The direct adaptive parameters need not equal the physical values. Their primary test is boundedness and tracking performance. The RLS estimates should be assessed through prediction residuals and confidence/conditioning, not by visual convergence alone.
14.1 Verification hierarchy
- software unit tests for signs, units, saturation, and projection;
- nominal simulation with constant parameters;
- parameter-step and grade scenarios;
- noise, delay, bias, dropout, and actuator-lag tests;
- Monte Carlo uncertainty sweeps;
- processor-in-the-loop and hardware-in-the-loop tests;
- closed-course experiments with independent emergency supervision.
15. Problems and Solutions
Problem 1 — Outer-loop eigenvalues. For \(h=1.4\), \(k_p=0.28\), and \(k_d=0.75\), determine the gap-mode characteristic polynomial and verify stability.
Solution:
\[ p(\lambda)= \lambda^2+ (1.4\times0.28+0.75)\lambda+0.28 = \lambda^2+1.142\lambda+0.28. \]
Both coefficients are positive, so the second-order Routh criterion proves asymptotic stability. The roots are real or complex depending on the discriminant:
\[ \Delta=1.142^2-4(0.28)=0.184164>0. \]
The two stable roots are approximately \(-0.357\) and \(-0.785\).
Problem 2 — String-stability test. Check the sufficient condition for the same gains.
Solution:
\[ h^2k_p+2hk_d = 1.4^2(0.28)+2(1.4)(0.75) = 2.6488\geq2. \]
Hence the idealized homogeneous linear model satisfies the derived non-amplification condition. This does not include delay or saturation.
Problem 3 — Lyapunov cancellation. Starting from \(\dot e=-k_e e+b\widetilde\theta^T\phi\), find an update law that removes the cross term in \(\dot V\) for \(V=e^2/2+b\widetilde\theta^T\Gamma^{-1}\widetilde\theta/2\).
Solution:
The derivative contains \(be\widetilde\theta^T\phi+ b\widetilde\theta^T\Gamma^{-1}\dot{\widehat\theta}\). Selecting \(\dot{\widehat\theta}=-\Gamma\phi e\) makes the terms equal and opposite, leaving \(\dot V=-k_e e^2\).
Problem 4 — Physical mass from propulsion gain. Suppose \(\widehat b=8.0\times10^{-4}\), \(\eta_g=0.90\), \(i_g=4.0\), and \(R_w=0.32\,\text{m}\). Estimate mass.
Solution:
\[ \widehat m= \frac{0.90(4.0)}{0.32(8.0\times10^{-4})} = 14062.5\;\text{kg}. \]
The large value is plausible for a heavy vehicle. Its validity depends on whether the torque variable and gear/efficiency model use consistent units.
Problem 5 — RLS scaling. Show the parameter transformation produced by \(\bar\varphi=[T/1000,-1,-v/30,-(v/30)^2]^T\).
Solution:
\[ y= \left(\frac{T}{1000}\right)(1000b) -a_0 -\left(\frac{v}{30}\right)(30a_1) -\left(\frac{v}{30}\right)^2(900a_2). \]
Thus the scaled parameter vector is \([1000b,a_0,30a_1,900a_2]^T\). Scaling changes numerical conditioning but not the represented model.
Problem 6 — Lack of persistent excitation. Explain why flat-road constant-speed cruising cannot uniquely estimate all four RLS parameters.
Solution:
At constant speed and torque, every regressor sample is approximately the same vector. The accumulated information matrix is therefore close to rank one rather than rank four. Only one linear combination of parameters is strongly constrained. Acceleration, deceleration, speed variation, and grade variation are needed to increase regressor rank.
Problem 7 — Saturation-induced drift. The requested torque exceeds the maximum for several seconds while the tracking error remains nonzero. Why can the nominal gradient law drift?
Solution:
The derivation assumes that the plant receives \(T=\widehat\theta^T\phi\). During saturation, the actual input differs from this value, so the error equation used for Lyapunov cancellation is invalid. The update can keep increasing parameters in an attempt to obtain unavailable torque. Projection, saturation gating, and leakage prevent this mechanism.
16. Summary
Adaptive cruise control can be organized as an outer spacing/speed loop and an inner adaptive powertrain loop. The constant-time-headway controller has a transparent second-order stability analysis and admits a simple string-stability condition. A torque-domain linear parameterization leads to a direct adaptive law whose Lyapunov derivative is negative semidefinite. Parallel normalized RLS provides interpretable estimates of propulsion gain and road-load coefficients. Tracking convergence does not imply physical parameter convergence; persistent excitation, numerical scaling, mode handling, projection, robust modification, and independent safety supervision are essential.
17. References
- Ioannou, P.A., & Chien, C.C. (1993). Autonomous intelligent cruise control. IEEE Transactions on Vehicular Technology, 42(4), 657–672. https://doi.org/10.1109/25.260745
- Swaroop, D., & Hedrick, J.K. (1996). String stability of interconnected systems. IEEE Transactions on Automatic Control, 41(3), 349–357. https://doi.org/10.1109/9.486636
- Vahidi, A., Stefanopoulou, A., & Peng, H. (2005). Recursive least squares with forgetting for online estimation of vehicle mass and road grade: Theory and experiments. Vehicle System Dynamics, 43(1), 31–55. https://doi.org/10.1080/00423110412331290446
- Naus, G.J.L., Vugts, R.P.A., Ploeg, J., van de Molengraft, M.J.G., & Steinbuch, M. (2010). String-stable CACC design and experimental validation: A frequency-domain approach. IEEE Transactions on Vehicular Technology, 59(9), 4268–4279. https://doi.org/10.1109/TVT.2010.2076320
- Narendra, K.S., & Annaswamy, A.M. (1987). A new adaptive law for robust adaptation without persistent excitation. IEEE Transactions on Automatic Control, 32(2), 134–145. https://doi.org/10.1109/TAC.1987.1104543
- Ioannou, P.A., & Kokotovic, P.V. (1984). Instability analysis and improvement of robustness of adaptive control. Automatica, 20(5), 583–594. https://doi.org/10.1016/0005-1098(84)90009-8
- Ioannou, P.A., & Kokotovic, P.V. (1984). Robust redesign of adaptive control. IEEE Transactions on Automatic Control, 29(3), 202–211. https://doi.org/10.1109/TAC.1984.1103490
- Berger, T., & Rauert, A.-L. (2020). Funnel cruise control. Automatica, 119, 109061. https://doi.org/10.1016/j.automatica.2020.109061
- Xu, X., Grizzle, J.W., Tabuada, P., & Ames, A.D. (2018). Correctness guarantees for the composition of lane keeping and adaptive cruise control. IEEE Transactions on Automation Science and Engineering, 15(3), 1216–1229.
- Lan, J. (2024). Data-driven cooperative adaptive cruise control for unknown nonlinear vehicle platoons. IET Intelligent Transport Systems, 18(11), 2114–2123. https://doi.org/10.1049/itr2.12556
Help keep these engineering tutorials free and growing
If these lessons, examples, and project pages help you, a small donation supports the continued creation and improvement of free control, robotics, software, and engineering education resources.
Created and maintained by Abolfazl Mohammadijoo.
