smarts.core.controllers.trajectory_tracking_controller module

class smarts.core.controllers.trajectory_tracking_controller.TrajectoryTrackingController

A trajectory tracking controller.

static MPC(trajectory, heading_error, lateral_error, dt, state_matrix, input_matrix, drift_matrix, prediction_horizon=5)

Implementation of MPC, please see the following ref: Convex Optimization – Boyd and Vandenberghe https://github.com/markcannon/markcannon.github.io/blob/c3344814c9c79d5f0d5f3cf8fc2e7ef14cb4fad3/assets/downloads/teaching/C21_Model_Predictive_Control/mpc_notes.pdf https://markcannon.github.io/teaching/

static calculate_heading_lateral_error(vehicle, trajectory, initial_look_ahead_distant, speed_reduction_activation)

Determine vehicle heading error and lateral error in regards to the given trajectory.

static calculate_raw_throttle_feedback(vehicle, state, trajectory, velocity_gain, velocity_integral_gain, integral_velocity_error, velocity_damping_gain, windup_gain, traction_gain, speed_reduction_activation, throttle_filter_constant, dt_sec)

Applies filters to the throttle for stability.

static curvature_calculation(trajectory, offset=0, num_points=5)

Attempts to approximate the curvature radius of a trajectory

static mpc_drift_matrix(vehicle, trajectory, prediction_horizon=1)

TODO: Clarify

static perform_trajectory_tracking_MPC(trajectory: Tuple[Sequence[float], Sequence[float], Sequence[float], Sequence[float]], vehicle, state, dt_sec: float, prediction_horizon: int = 5)

Attempts model predictive control for the given vehicle given an expected trajectory.

static perform_trajectory_tracking_PD(trajectory, vehicle, state, dt_sec)

Attempts proportional derivative control for the vehicle given an expected trajectory.

class smarts.core.controllers.trajectory_tracking_controller.TrajectoryTrackingControllerState

Controller state