From 9e5f9fba8391c264d04a4a946e3c87d49f02fb81 Mon Sep 17 00:00:00 2001 From: mohitks3000 Date: Thu, 8 Oct 2026 00:01:50 +0530 Subject: [PATCH] fix: fixed docs linkages --- doc/4_path_tracking/4_1_stanley_controller.md | 28 +++++----- doc/4_path_tracking/4_2_mpc_controller.md | 44 +++++++-------- doc/4_path_tracking/4_3_mppi_controller.md | 54 +++++++++---------- doc/4_path_tracking/4_path_tracking.md | 13 +++++ doc/DESIGN_DOCUMENT.md | 2 +- 5 files changed, 77 insertions(+), 64 deletions(-) create mode 100644 doc/4_path_tracking/4_path_tracking.md diff --git a/doc/4_path_tracking/4_1_stanley_controller.md b/doc/4_path_tracking/4_1_stanley_controller.md index 1fb95297..9f6b5301 100644 --- a/doc/4_path_tracking/4_1_stanley_controller.md +++ b/doc/4_path_tracking/4_1_stanley_controller.md @@ -1,4 +1,4 @@ -# 4. Stanley Controller +## 4.1 Stanley Controller In this chapter, the Stanley steering controller class is implemented for path tracking. This class implements the Stanley path tracking algorithm, which computes a steering angle to guide a vehicle's front axle back onto a reference course by correcting both heading error and lateral (cross-track) error. @@ -36,7 +36,7 @@ Ref: https://www.ri.cmu.edu/pub_files/2009/2/Automatic_Steering_Methods_for_Auto - can be unstable at low speed(divison be 0) and steady state error. -## 4.1 StanleyController Class +### 4.1.1 StanleyController Class The controller class is located at: [stanley_controller.py](/src/components/control/stanley/stanley_controller.py) @@ -70,7 +70,7 @@ This class imports `sin`, `cos`, `tan`, and `atan2` from Python's `math` module, --- -### 4.1.1 Constructor +#### 4.1.1.1 Constructor ```python def __init__(self, spec, course=None): @@ -100,9 +100,9 @@ The constructor takes a `VehicleSpecification` object and an optional `CubicSpli --- -## 4.2 Private Methods +### 4.1.2 Private Methods -### 4.2.1 `_calculate_front_axle_state` +#### 4.1.2.1 `_calculate_front_axle_state` ```python def _calculate_front_axle_state(self, state): @@ -131,7 +131,7 @@ where `L` is the wheelbase. --- -### 4.2.3 `_calculate_target_course_index` +#### 4.1.2.2 `_calculate_target_course_index` ```python def _calculate_target_course_index(self, state): @@ -143,7 +143,7 @@ This method finds the index of the nearest point on the course to the front axle --- -### 4.2.4 `_decide_target_speed_mps` +#### 4.1.2.3 `_decide_target_speed_mps` ```python def _decide_target_speed_mps(self): @@ -154,7 +154,7 @@ The target speed is read directly from the course at the nearest point index. Th --- -### 4.2.4 `_calculate_target_acceleration_mps2` +#### 4.1.2.4 `_calculate_target_acceleration_mps2` ```python def _calculate_target_acceleration_mps2(self, state): @@ -170,7 +170,7 @@ accel = Kp * (target_speed - current_speed) --- -### 4.2.5 `_calculate_tracking_error` +#### 4.1.2.5 `_calculate_tracking_error` ```python def _calculate_tracking_error(self, state): @@ -190,7 +190,7 @@ Only `error_lat_m` and `error_yaw_rad` are used for steering. --- -### 4.2.6 `_calculate_control_input` +#### 4.1.2.6 `_calculate_control_input` ```python def _calculate_control_input(self, state, error_lat_m, error_yaw_rad): @@ -231,9 +231,9 @@ tan(δ) = L / R => R = L / tan(δ) --- -## 4.3 Public Methods +### 4.1.3 Public Methods -### 4.3.1 `update` +#### 4.1.3.1 `update` ```python def update(self, state, time_s): @@ -263,7 +263,7 @@ If no course is set, the method returns early without computing anything. --- -### 4.3.2 Getter Methods +#### 4.1.3.2 Getter Methods ```python def get_target_accel_mps2(self): @@ -280,7 +280,7 @@ These three getter methods expose the computed control outputs. They are called --- -### 4.3.3 `draw` +#### 4.1.3.3 `draw` ```python def draw(self, axes, elems): diff --git a/doc/4_path_tracking/4_2_mpc_controller.md b/doc/4_path_tracking/4_2_mpc_controller.md index 5f68bca5..18ee3389 100644 --- a/doc/4_path_tracking/4_2_mpc_controller.md +++ b/doc/4_path_tracking/4_2_mpc_controller.md @@ -1,4 +1,4 @@ -# 5. MPC Controller +## 4.2 MPC Controller In this chapter, the MPC (Model Predictive Control) path tracking controller class is implemented. This class implements the MPC path tracking algorithm, which computes a steering angle and acceleration command by solving an **optimization problem** over a finite prediction horizon at every time step. @@ -45,7 +45,7 @@ Before getting into the code, let's get some basic understanding behind the algo --- -## 5.1 MpcController Class +### 4.2.1 MpcController Class The controller class is located at: [mpc_controller.py](/src/components/control/mpc/mpc_controller.py) @@ -74,7 +74,7 @@ This class uses **do-mpc** as the MPC framework and **CasADi** as the symbolic a --- -### 5.1.1 Constructor +#### 4.2.1.1 Constructor ```python def __init__(self, spec, course=None, color="r", @@ -135,9 +135,9 @@ The constructor takes a `VehicleSpecification` object and an optional `CubicSpli --- -## 5.2 Algorithm Background +### 4.2.2 Algorithm Background -### 5.2.1 State and Control Vectors +#### 4.2.2.1 State and Control Vectors MPC operates on a **state vector** $x$ and a **control vector** $u$: @@ -161,7 +161,7 @@ At each time step, MPC finds the sequence $U = {u_0, u_1, …, u_{N-1}}$ that mi --- -### 5.2.2 Vehicle Model - Discrete Bicycle Kinematics +#### 4.2.2.2 Vehicle Model - Discrete Bicycle Kinematics MPC uses an internal motion model to predict how the vehicle will move in response to each control. The **kinematic bicycle model** is used: @@ -180,7 +180,7 @@ CasADi writes these equations as symbolic expressions, meaning it can automatica --- -### 5.2.3 The Optimization Problem +#### 4.2.2.3 The Optimization Problem At every time step, MPC solves the optimization problem: @@ -221,11 +221,11 @@ The constraints are **hard constraints**, meaning the optimizer (e.g., IPOPT) en --- -### 5.2.4 Stage Cost $\ell(x_t, u_t, u_{t-1})$ +#### 4.2.2.4 Stage Cost $\ell(x_t, u_t, u_{t-1})$ The stage cost is evaluated at every step $t = 0,\ldots,N-1$ of the horizon. It has three parts: -#### 1. Tracking Error +##### 1. Tracking Error Penalises deviation from the reference trajectory: @@ -244,7 +244,7 @@ The heading error uses **$\text{atan2}$** rather than a direct subtraction. This --- -#### 2. Control Effort +##### 2. Control Effort Penalises large control inputs: @@ -254,7 +254,7 @@ $$ --- -#### 3. Smoothness +##### 3. Smoothness Penalises rapid changes between consecutive control inputs: @@ -275,7 +275,7 @@ mpc.set_rterm( --- -### 5.2.5 Terminal Cost $\phi(x_N)$ +#### 4.2.2.5 Terminal Cost $\phi(x_N)$ The terminal cost is evaluated only at the **last step** $t = N$ of the horizon: @@ -292,7 +292,7 @@ The terminal weights $W$ are set heavier than the stage weights (default: $W = 2 --- -### 5.2.6 The Receding Horizon Principle +#### 4.2.2.6 The Receding Horizon Principle MPC solves for the full sequence $\{u_0, u_1, \ldots, u_{N-1}\}$ but **only applies $u_0$** - the first control action. At the next time step, the horizon shifts forward by one step, the current state is re-measured, and the problem is solved again from scratch. This is why it is called a **receding horizon controller**. @@ -312,9 +312,9 @@ This strategy is known as the **receding horizon** (or **moving horizon**) princ --- -## 5.3 Private Methods +### 4.2.3 Private Methods -### 5.3.1 `_build_arclength_table()` +#### 4.2.3.1 `_build_arclength_table()` ```python def _build_arclength_table(self): @@ -342,7 +342,7 @@ The tiling handles closed or looping courses: when a vehicle near the end of the --- -### 5.3.2 `_build_model()` +#### 4.2.3.2 `_build_model()` ```python def _build_model(self): @@ -390,7 +390,7 @@ The `yaw_err` auxiliary expression is registered separately so it can be referen --- -### 5.3.3 `_build_mpc(model)` +#### 4.2.3.3 `_build_mpc(model)` ```python def _build_mpc(self, model): @@ -438,7 +438,7 @@ This method assembles the full MPC optimization problem using the do-mpc API: --- -### 5.3.4 `_get_reference_trajectory(x0, y0, current_speed)` +#### 4.2.3.4 `_get_reference_trajectory(x0, y0, current_speed)` ```python def _get_reference_trajectory(self, x0, y0, current_speed): @@ -484,9 +484,9 @@ where $s_0$ is the cumulative arc-length at the vehicle's current nearest point, --- -## 5.4 Public Methods +### 4.2.4 Public Methods -### 5.4.1 `update` +#### 4.2.4.1 `update` ```python def update(self, state, time_s): @@ -541,7 +541,7 @@ $$ --- -### 5.4.2 Getter Methods +#### 4.2.4.2 Getter Methods ```python def get_target_accel_mps2(self): @@ -558,7 +558,7 @@ These three getter methods expose the computed control outputs. They are called --- -### 5.4.3 `draw` +#### 4.2.4.3 `draw` ```python def draw(self, axes, elems): diff --git a/doc/4_path_tracking/4_3_mppi_controller.md b/doc/4_path_tracking/4_3_mppi_controller.md index 9fcbe57c..1273b8c5 100644 --- a/doc/4_path_tracking/4_3_mppi_controller.md +++ b/doc/4_path_tracking/4_3_mppi_controller.md @@ -1,4 +1,4 @@ -# 6. MPPI Controller +## 4.3 MPPI Controller In this chapter, the MPPI (Model Predictive Path Integral) path tracking controller class is implemented. This class implements the MPPI path tracking algorithm, which computes a steering angle and acceleration command by **sampling thousands of random control sequences**, rolling each one forward through the vehicle dynamics, scoring them by cost, and computing a weighted average update. @@ -39,7 +39,7 @@ Ref: https://dilithjay.com/blog/mppi --- -## 6.1 MppiController Class +### 4.3.1 MppiController Class The controller class is located at: [mppi_controller.py](/src/components/control/mppi/mppi_controller.py) @@ -71,7 +71,7 @@ This class imports trigonometric functions from Python's `math` module and `nump --- -### 6.1.1 `_StateView` Helper Class +#### 4.3.1.1 `_StateView` Helper Class ```python class _StateView: @@ -94,7 +94,7 @@ During rollouts, the simulated position of each sample is a plain numpy array, n --- -### 6.1.2 Constructor +#### 4.3.1.2 Constructor ```python def __init__(self, spec, course=None, color="g", @@ -140,15 +140,15 @@ The constructor takes a `VehicleSpecification` object and an optional `CubicSpli | `target_accel_mps2` | 0.0 | Computed acceleration command [m/s²] | | `target_steer_rad` | 0.0 | Computed steering angle command [rad] | | `target_yaw_rate_rps` | 0.0 | Computed yaw rate command [rad/s] | -| `target_speed_mps` | 0.0 | Current speed echoed back (not a planned target - see §6.4.1) | +| `target_speed_mps` | 0.0 | Current speed echoed back (not a planned target - see §4.3.4.1) | > **Note on `param_gamma`**: With the default `param_alpha = 1.0`, `param_gamma = 0` and the control cost term in the trajectory cost vanishes entirely. This means the default configuration is pure tracking with no penalty for deviating from the warm-start control. Set `param_alpha < 1.0` only when you want to discourage large perturbations from the previous solution. --- -## 6.2 Algorithm Background +### 4.3.2 Algorithm Background -### 6.2.1 State and Control Vectors +#### 4.3.2.1 State and Control Vectors MPPI operates on a state vector $x_t$ and control vector $u_t$: @@ -170,7 +170,7 @@ $$ --- -### 6.2.2 Sampling - Exploitation and Exploration +#### 4.3.2.2 Sampling - Exploitation and Exploration At each step, K noise sequences are drawn from a zero-mean Gaussian: @@ -195,11 +195,11 @@ Exploration samples (last param_exploration × K): The exploitation samples refine the previous solution. The exploration samples venture further afield and can discover better solutions when the warm start has drifted off-course. The `param_exploration` parameter controls the ratio. -> **Important**: `v[k,t]` stores the **clipped** perturbed control used for rollout. The raw unclipped noise `epsilon[k,t]` is stored separately. The weighted update later uses `epsilon`, not `v` - this asymmetry is intentional and explained in section 6.2.5. +> **Important**: `v[k,t]` stores the **clipped** perturbed control used for rollout. The raw unclipped noise `epsilon[k,t]` is stored separately. The weighted update later uses `epsilon`, not `v` - this asymmetry is intentional and explained in section 4.3.2.5. --- -### 6.2.3 Trajectory Cost S(k) +#### 4.3.2.3 Trajectory Cost S(k) Each sample trajectory $k$ accumulates cost across all T steps plus a terminal cost: @@ -226,7 +226,7 @@ param_alpha = 0.0 -> γ = λ (full control cost - conservative) --- -### 6.2.4 Information-Theoretic Weighting +#### 4.3.2.4 Information-Theoretic Weighting Once all K trajectory costs are computed, the weights are calculated in three steps: @@ -248,7 +248,7 @@ The subtraction of $\rho$ before the exponential is a **numerical stability tric --- -### 6.2.5 Control Update - Why `epsilon` Not `v` +#### 4.3.2.5 Control Update - Why `epsilon` Not `v` The weighted perturbation sum is computed using the **raw unclipped noise** `epsilon`, not the clipped `v`: @@ -281,9 +281,9 @@ The last element is **repeated rather than zeroed** because zeroing would force --- -## 6.3 Private Methods +### 4.3.3 Private Methods -### 6.3.1 `_get_nearest_waypoint(x, y, update_prev_idx)` +#### 4.3.3.1 `_get_nearest_waypoint(x, y, update_prev_idx)` ```python def _get_nearest_waypoint(self, x, y, update_prev_idx=False): @@ -309,7 +309,7 @@ It is called in two distinct contexts: --- -### 6.3.2 `_g(v)` - Control Clipping +#### 4.3.3.2 `_g(v)` - Control Clipping ```python def _g(self, v): @@ -320,11 +320,11 @@ def _g(self, v): After adding noise to the warm-start control, `_g()` clips the result to the physical limits of the vehicle. This is MPPI's only mechanism for enforcing control bounds - there are no hard constraints as in MPC. Samples that would require $|\delta| > \delta_{\max}$ are simply clipped to the limit before being rolled out. -The clipped result is stored in `v[k,t]`. Critically, the raw unclipped noise `epsilon[k,t]` is preserved separately and is used in the weighted update - see section 6.2.6 for why this separation matters. +The clipped result is stored in `v[k,t]`. Critically, the raw unclipped noise `epsilon[k,t]` is preserved separately and is used in the weighted update - see section 4.3.2.5 for why this separation matters. --- -### 6.3.3 `_F(x_t, v_t)` - One-Step Dynamics Rollout +#### 4.3.3.3 `_F(x_t, v_t)` - One-Step Dynamics Rollout ```python def _F(self, x_t, v_t): @@ -349,7 +349,7 @@ This function is called $K \times T$ times per `update()` step - once for each s --- -### 6.3.4 `_c(x_t)` - Stage Cost +#### 4.3.3.4 `_c(x_t)` - Stage Cost ```python def _c(self, x_t): @@ -376,7 +376,7 @@ Without the first step, vehicles near $\pm\pi$ can receive arbitrarily large hea --- -### 6.3.5 `_phi(x_T)` - Terminal Cost +#### 4.3.3.5 `_phi(x_T)` - Terminal Cost ```python def _phi(self, x_T): @@ -392,7 +392,7 @@ The terminal cost is evaluated only at the **last state** $x_T^k$ of each sample --- -### 6.3.6 `_calc_epsilon()` +#### 4.3.3.6 `_calc_epsilon()` ```python def _calc_epsilon(self): @@ -413,7 +413,7 @@ The off-diagonal zeros mean steering and acceleration noise are drawn independen --- -### 6.3.7 `_compute_weights(S)` +#### 4.3.3.7 `_compute_weights(S)` ```python def _compute_weights(self, S): @@ -429,7 +429,7 @@ The output $w$ is a $(K,)$ array that sums to 1.0, acting as a proper probabilit --- -### 6.3.8 `_moving_average_filter(xx, window_size)` +#### 4.3.3.8 `_moving_average_filter(xx, window_size)` ```python def _moving_average_filter(self, xx, window_size): @@ -451,9 +451,9 @@ Applies a moving-average filter to each column of the $(T, 2)$ weighted perturba --- -## 6.4 Public Methods +### 4.3.4 Public Methods -### 6.4.1 `update` +#### 4.3.4.1 `update` ```python def update(self, state, time_s): @@ -556,7 +556,7 @@ update() --- -### 6.4.2 Getter Methods +#### 4.3.4.2 Getter Methods ```python def get_target_accel_mps2(self): @@ -573,7 +573,7 @@ These three getter methods expose the computed control outputs. They are called --- -### 6.4.3 `draw` +#### 4.3.4.3 `draw` ```python def draw(self, axes, elems): @@ -606,7 +606,7 @@ The `self.weights` consumed here are set inside `update()` as `self.weights = w. --- -## 6.5 Comparison with MPC +### 4.3.5 Comparison with MPC | Aspect | MPC | MPPI | |---|---|---| diff --git a/doc/4_path_tracking/4_path_tracking.md b/doc/4_path_tracking/4_path_tracking.md new file mode 100644 index 00000000..2f7216ab --- /dev/null +++ b/doc/4_path_tracking/4_path_tracking.md @@ -0,0 +1,13 @@ +# 4. Path Tracking +This chapter explains the design and implementation of the path tracking controllers used in this project. + +The documents in this chapter are: + +## 4.1 [Stanley Controller](/doc/4_path_tracking/4_1_stanley_controller.md) +Covers the Stanley steering controller, which corrects heading error and cross-track error to keep the front axle on a reference course. + +## 4.2 [MPC Controller](/doc/4_path_tracking/4_2_mpc_controller.md) +Covers the model predictive controller, which solves a finite-horizon optimization problem for steering and acceleration at every time step. + +## 4.3 [MPPI Controller](/doc/4_path_tracking/4_3_mppi_controller.md) +Covers the model predictive path integral controller, which samples control sequences, rolls them forward, and combines them by cost. diff --git a/doc/DESIGN_DOCUMENT.md b/doc/DESIGN_DOCUMENT.md index 8b20133e..0f1b03b1 100644 --- a/doc/DESIGN_DOCUMENT.md +++ b/doc/DESIGN_DOCUMENT.md @@ -5,4 +5,4 @@ This document provides an explanation of each Python programs design in this rep 1. [World Visualization](/doc/1_world_visualization/1_world_visualization.md) 2. [Vehicle Model](/doc/2_vehicle_model/2_vehicle_model.md) 3. [Sensor Models](/doc/3_sensor_models/3_sensor_models.md) -4. [Path Tracking](/doc/4_path_tracking/4_staley_controller.md) \ No newline at end of file +4. [Path Tracking](/doc/4_path_tracking/4_path_tracking.md) \ No newline at end of file