Figure 1 below summarizes this end-to-end architecture, from high-rate sensor inputs through perception\/localization and fusion to planning, control, and actuation, so the main data and compute flow is visible at a glance.<\/p>\n
![\"\"](\"https:\/\/www.infoq.com\/articles\/optimization-in-automated-driving\/articles\/optimization-in-automated-driving\/en\/resources\/191figure-1-1774348228376.jpg\")
<\/p>\n
Figure 1: High-level AV software architecture<\/p>\n
[Click here to expand image above to full-size<\/a>]<\/p>\n Typical Data Throughput Volumes<\/p>\n
Optimizing the Perception Pipeline: Dynamic Resource Allocation<\/p>\n The perception layer is responsible for turning raw data into a world model. A naive approach processes every sensor at full resolution and maximum frequency. However, processing gigabytes of data every second at full fidelity would saturate the computational resources of any vehicle.<\/p>\n Context-Aware Sensor Prioritization<\/p>\n Optimization in perception often means context-aware prioritization: adjusting sensing, preprocessing, and inference effort to match the current Operational Design Domain (ODD). Stacks frequently model the computational cost of key pipeline stages and apply policies (or optimization-based controllers) that trade off accuracy, latency, and resource usage.<\/p>\n Highway Scenario<\/p>\n The Region of Interest (ROI) narrows and long-range precision is critical. Consequently, stacks often prioritize forward-looking LiDAR and long-range cameras, while reducing load from side-facing sensors via downsampling, reduced frame\/scan rates, or selective ROI processing.<\/p>\n Urban Scenario<\/p>\n Peripheral coverage becomes more important for cross-traffic, vulnerable road users, and complex interactions. Stacks often prioritize wide-angle cameras and side-looking sensors, and may allocate more compute to semantic perception and tracking.<\/p>\n Figure 2: Dynamic Sensor Weighting Logic<\/p>\n [Click here to expand image above to full-size<\/a>]<\/p>\n Technical Implementation: From Preprocessing to Fusion<\/p>\n In production AV programs, perception pipelines need this kind of flexible allocation. Many teams go beyond single-stage object lists and design pipelines that manage high-rate sensor streams with low latency, starting from early preprocessing through inference and tracking. In practice, this fusion typically involves two complementary controls. First, there are processing knobs (rate, ROI, resolution, model choice) to manage compute load. Next are fusion weights that scale measurement uncertainty (e.g., the measurement covariance, R) in tracking.<\/p>\n
As Ahn et al., n.d.<\/a> show, combining data-parallel execution with selective GPU offload can improve end-to-end throughput and latency in perception pipelines while maintaining accuracy targets.<\/p>\n To visualize how this logic looks in the tracking\/fusion layer, consider a context-aware weight manager. In a Kalman-filter-based tracker, sensor trust can be represented via the measurement covariance R (often per sensor and per measurement type): Higher covariance reduces the filter\u2019s reliance on a measurement, while lower covariance increases it.<\/p>\n Pseudocode: Dynamic Sensor Weighting (Python)<\/p>\n
Trajectory Planning: The Mathematics of MPC<\/p>\n While perception deals with probabilities, planning deals with constraints. The planning module generates a feasible trajectory, typically parameterized over a horizon as a sequence of states and controls \\(\\{x_k, u_k\\}_{k=0}^N\\) , at a fixed control cadence.<\/p>\n It is commonly on the order of tens of milliseconds to approximately one hundred milliseconds depending on the stack and platform. Missing this deadline degrades responsiveness.<\/p>\n The Optimization Problem<\/p>\n Trajectory generation is commonly framed as a Model Predictive Control (MPC) problem. Instead of hard-coding rules, engineers define a Cost Function (J) that the solver minimizes.<\/p>\n \\(J = \\sum_{k=0}^{N-1} \\left( \\|x_k – x_k^{\\mathrm{ref}}\\|_Q^2 + \\|u_k\\|_R^2 \\right) + \\|x_N – x_N^{\\mathrm{ref}}\\|_P^2\\)<\/p>\n Here \\(\\{x_k^{\\mathrm{ref}}\\}_{k=0}^N\\) denotes the reference trajectory over the prediction horizon.<\/p>\n
Solving Under Constraints<\/p>\n The solver must find the minimum J subject to hard constraints.<\/p>\n Actuation and dynamics limits<\/p>\n \\(| | \\leq \\delta_{max} \\text{ (Steer)} \\quad |\\alpha| \\leq \\alpha_{max} \\text{ (Accel)}\\), plus rate limits where applicable.<\/p>\n Safety Corridors<\/p>\n The planned ego footprint must remain within the drivable region and maintain separation from obstacles (often expressed via corridor boundaries, signed-distance constraints, or convex approximations of collision geometry).<\/p>\n Figure 3: MPC Control Loop<\/p>\n [Click here to expand image above to full-size<\/a>]<\/p>\n Solvers and Algorithms<\/p>\n In autonomous driving, MPC has been used to balance speed and comfort while reacting safely. To meet embedded deadlines, teams typically rely on warm-started solvers: QP solvers such as OSQP<\/a> for convex MPC formulations, and nonlinear programming solvers (e.g., Ipopt) or real-time NMPC toolchains for nonlinear formulations.<\/p>\n Research by Allamaa et al. 2024<\/a> illustrates how advanced MPC formulations and hybrid optimization techniques provide safe, agile decision-making.Earlier work by Zhang, Rossi, and Pavone 2015<\/a> provides a broader example of MPC as receding-horizon decision-making in autonomous mobility systems at the fleet coordination level, rather than ego-vehicle trajectory control. Additionally, Arrigoni, Braghin, and Cheli 2021<\/a> research explores an alternative approach in which an NMPC trajectory planner is solved using a genetic algorithm strategy. In production (often C++) implementations, the optimization loop must be highly efficient, predictable, and instrumented for worst-case performance.<\/p>\n Pseudocode: MPC Cost Function (C++)<\/p>\n
Real-Time Compute Budget and Middleware<\/p>\n An AV stack is a “busy ecosystem.” Localization, perception, prediction, and control all run in parallel, competing for the same CPU and GPU resources. If perception takes too long to process an image, the planning module might miss its update window.<\/p>\n Deterministic Scheduling<\/p>\n To prevent this problem, many teams treat the compute budget itself as an engineering optimization problem: They measure execution times, allocate cores, set priorities, and tune QoS so the right work happens at the right time. Worst-Case Execution Time (WCET): Each node has a measured (or conservatively estimated) WCET and an explicit deadline budget. Deterministic Scheduling Policies: Real-time scheduling is enforced either via an RTOS in safety-\/control-critical domains or via real-time scheduling configurations on general-purpose operating systems. Fixed-priority preemptive scheduling is common; protocols such as priority inheritance help bound blocking on shared resources and protect deadline-critical tasks.<\/p>\n In practice, these budgets are multi-rate: high-level planning often runs at ~10-20 Hz (50-100 ms), while low-level control loops can run at ~50-100 Hz (10-20 ms) on dedicated controllers; exact rates depend on platform and safety architecture.<\/p>\n
Table 1: Example Latency Budget for a 100ms Control Cycle (Illustrative)<\/p>\n
\n\tLiDAR<\/a>: ~0.3\u20132.6 million points\/sec (often ~35\u2013255 Mbps per sensor depending on configuration).
\n\tCameras<\/a>: 4K\/60fps streams (full-color uncompressed video can require ~12 Gbps; Production systems typically rely on RAW formats and\/or compression).
\n\tRadar: Sparse detections\/tracks (typically low bandwidth, high refresh rate).
<\/p>\n![\"\"](\"https:\/\/www.infoq.com\/articles\/optimization-in-automated-driving\/articles\/optimization-in-automated-driving\/en\/resources\/143figure-2-1774348228376.jpg\")
<\/p>\n
\n\tLiDAR Processing
\n\tRaw point clouds (x, y, z, intensity) are typically discretized (voxelization or pillarization) and then consumed by 3D detection networks such as VoxelNet-family approaches or PointPillars. The voxel\/pillar resolution is a critical trade-off between spatial fidelity and inference latency\/compute.
\n\tRadar Processing
\n\tRadar measurements (range, angle, range-rate) are often leveraged for robust velocity cues and adverse weather operation; uncertainty can be adjusted by context and clutter characteristics.
\n\tTooling
\n\tDeployment pipelines often use inference accelerators such as TensorRT to optimize and run deep learning models on embedded GPU platforms (for example, NVIDIA Xavier-class systems; Newer generations also target Orin-class hardware). Model choices vary by stack, but standard backbones (e.g., ResNet) and detector families (e.g., YOLO-style) are widely used in computer vision alongside 3D-\/BEV-specific architectures in AV.
<\/p>\n
\nclass SensorFusionManager:
\n\u00a0 \u00a0 def update_weights(self, vehicle_state, environment_context):
\n\u00a0 \u00a0 \u00a0 \u00a0 “””
\n\u00a0 \u00a0 \u00a0 \u00a0 Dynamically adjusts sensor trust (covariance) based on context.
\n\u00a0 \u00a0 \u00a0 \u00a0 Low covariance = High Trust.
\n\u00a0 \u00a0 \u00a0 \u00a0 “””
\n\u00a0 \u00a0 \u00a0 \u00a0 # Base configuration (illustrative scalar variances \/ scale factors)
\n\u00a0 \u00a0 \u00a0 \u00a0 lidar_cov = 0.1
\n\u00a0 \u00a0 \u00a0 \u00a0 camera_cov = 0.2
\n\u00a0 \u00a0 \u00a0 \u00a0 radar_cov = 0.3
\n
\n\u00a0 \u00a0 \u00a0 \u00a0 # SCENARIO: High-Speed Highway
\n\u00a0 \u00a0 \u00a0 \u00a0 # Trust long-range Radar\/LiDAR more; Cameras may suffer motion blur
\n\u00a0 \u00a0 \u00a0 \u00a0 if vehicle_state.speed > 100.0: \u00a0# km\/h
\n\u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 radar_cov = 0.1 \u00a0# Increase trust in Radar for velocity
\n\u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 camera_cov = 0.5 # Decrease trust in Camera
\n\u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0\u00a0
\n\u00a0 \u00a0 \u00a0 \u00a0 # SCENARIO: Urban \/ Congested
\n\u00a0 \u00a0 \u00a0 \u00a0 # Trust Cameras for semantic understanding (pedestrians, signs)
\n\u00a0 \u00a0 \u00a0 \u00a0 elif environment_context.type == ‘URBAN_DENSE’:
\n\u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 lidar_cov = 0.05 # Max trust in LiDAR for close-range geometry
\n\u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 camera_cov = 0.1 # High trust for object classification
\n\u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 radar_cov = 0.4 \u00a0# Radar can be less reliable for some cues\/associations in dense clutter
\n
\n\u00a0 \u00a0 \u00a0 \u00a0 return self.kalman_filter.update_covariance(
\n\u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 lidar=lidar_cov,\u00a0
\n\u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 camera=camera_cov,\u00a0
\n\u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 radar=radar_cov
\n\u00a0 \u00a0 \u00a0 \u00a0 )<\/p>\n
\n\tWhere
\n\t\\(x_k\\): State vector at step k (position, velocity, yaw).
\n\t\\(u_k\\): Control input at step k (steering angle, acceleration).
\n\t\\(x_k^{\\mathrm{ref}}\\): reference state at step k (the desired position\/heading\/velocity at that point along the horizon), typically provided by a higher-level planner, route, or behavior module.
\n\t\\(x_N^{\\mathrm{ref}}\\): terminal reference state at the end of the horizon (the reference at step N), used to encourage convergence toward the desired end-of-horizon condition
\n\tQ, R, and P: Weight matrices. By tuning Q and R and P, engineers optimize for “assertiveness” vs. “comfort”.
<\/p>\n![\"\"](\"https:\/\/www.infoq.com\/articles\/optimization-in-automated-driving\/articles\/optimization-in-automated-driving\/en\/resources\/120figure-3-1774348228376.jpg\")
<\/p>\n
\n\/\/ Simplified MPC Cost Calculation Loop
\ndouble calculate_cost(const std::vector& preds,
\n\u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 const Trajectory& ref_traj,\u00a0
\n\u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 const std::vector& u_seq) {
\n\u00a0 \u00a0 double total_cost = 0.0;
\n
\n\u00a0 \u00a0 \/\/ Weights for tuning behavior (Comfort vs. Tracking)
\n\u00a0 \u00a0 const double W_POS = 10.0; \u00a0 \/\/ Penalty for position error
\n\u00a0 \u00a0 const double W_JERK = 50.0; \u00a0\/\/ High penalty for jerky steering \u00a0 \u00a0 \u00a0 (Comfort\/smoothness \u0394steer)
\n\u00a0 \u00a0 const double W_VEL = 1.0; \u00a0 \u00a0\/\/ Penalty for speed deviation
\n
\n\u00a0 \u00a0 for (int t = 0; t < HORIZON_N; ++t) {
\n\u00a0 \u00a0 \u00a0 \u00a0 \/\/ 1. State Deviation Cost (Tracking Accuracy)
\n\u00a0 \u00a0 \u00a0 \u00a0 const double pos_error = (preds[t].x – ref_traj[t].x);
\n\u00a0 \u00a0 \u00a0 \u00a0 const double vel_error = (preds[t].v – ref_traj[t].v);
\n\u00a0 \u00a0 \u00a0 \u00a0\u00a0
\n\u00a0 \u00a0 \u00a0 \u00a0 total_cost += W_POS * (pos_error * pos_error);
\n\u00a0 \u00a0 \u00a0 \u00a0 total_cost += W_VEL * (vel_error * vel_error);
\n
\n\u00a0 \u00a0 \u00a0 \u00a0 \/\/ 2. Control Input Cost (Passenger Comfort)
\n\u00a0 \u00a0 \u00a0 \u00a0 \/\/ Penalize large changes in steering (delta_delta)
\n\u00a0 \u00a0 \u00a0 \u00a0 if (t > 0) {
\n\u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 const double steering_delta_penalty = u_seq[t].steer – u_seq[t-1].steer;
\n\u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 total_cost += W_JERK * (steering_delta_penalty * steering_delta_penalty);
\n\u00a0 \u00a0 \u00a0 \u00a0 }
\n\u00a0 \u00a0 }
\n
\n\u00a0 \u00a0 return total_cost;
\n}<\/p>\n
\n\t
\n\t\t
\n\t\t\tModule
\n\t\t\tAllocated Time
\n\t\t\tHardware Target
\n\t\t\tFunction
\n\t\t
\n\t\t
\n\t\t\tSensor Acquisition
\n\t\t\t0 – 10 ms
\n\t\t\tFPGA \/ NIC
\n\t\t\tTimestamping & Packetization
\n\t\t
\n\t\t
\n\t\t\tPre-Processing
\n\t\t\t10 – 25 ms
\n\t\t\tGPU (CUDA)
\n\t\t\tPointCloud filtering, Image resizing
\n\t\t
\n\t\t
\n\t\t\tPerception Inference
\n\t\t\t25 – 55 ms
\n\t\t\tNPU \/ GPU
\n\t\t\tCNN inference (YOLO\/PointPillars)
\n\t\t
\n\t\t
\n\t\t\tFusion & Tracking
\n\t\t\t55 – 65 ms
\n\t\t\tCPU
\n\t\t\tKalman Filtering, Object ID association
\n\t\t
\n\t\t
\n\t\t\tPrediction & Plan
\n\t\t\t65 – 85 ms
\n\t\t\tCPU
\n\t\t\tIntent prediction, trajectory optimization (e.g., MPC)
\n\t\t
\n\t\t
\n\t\t\tSafety Check
\n\t\t\t85 – 90 ms
\n\t\t\tSafety Core
\n\t\t\tRule checks, constraint validation, fallback triggering
\n\t\t
\n\t\t
\n\t\t\tControl & Actuation
\n\t\t\t90 – 100 ms
\n\t\t\tECU
\n\t\t\tCAN bus command transmission
\n\t\t
\n\t
<\/p>\n