Doppler-Tuned MPC: Neural Motion Planning

• Doppler-Tuned MPC is a control framework that integrates Doppler LiDAR data with adaptive model predictive control for real-time trajectory planning.
• It leverages a D-KalmanNet module that uses RNN-based adaptive gain computation to rapidly and accurately track fast-moving obstacles.
• Empirical results demonstrate up to 12 dB NMSE improvement in both highway and city scenarios, highlighting enhanced robustness in dynamic environments.

Doppler Planning Network (DPNet) is a neural network-based architecture designed for real-time motion planning in highly dynamic environments using Doppler LiDAR sensors. DPNet integrates advanced state estimation—via a custom Doppler Kalman network (D-KalmanNet)—with a Doppler-tuned model predictive control (DT-MPC) framework, enabling rapid and accurate tracking and reaction to rapid-moving obstacles. This design leverages Doppler LiDAR’s unique ability to measure both range and instantaneous point velocities, and addresses the challenges posed by high-frequency, high-variation obstacle dynamics that confound conventional planners and filters (Zuo et al., 29 Nov 2025).

1. Doppler LiDAR Perception and Measurement Model

DPNet is fundamentally designed around the extended capabilities of Doppler LiDARs, which simultaneously provide distance and radial velocity measurements for each point return. For each tracked obstacle $n$, the underlying latent state vector at time $t$ is

$$\mathbf{x}_t^n = \begin{bmatrix} x_t^n \ \cos\theta_t^n\,v_t^n \ \cos\theta_t^n\,a_t^n \ y_t^n \ \sin\theta_t^n\,v_t^n \ \sin\theta_t^n\,a_t^n \end{bmatrix} \in \mathbb{R}^6,$$

capturing position, velocity, and acceleration components in the obstacle’s heading-aligned frame. The measurement model fuses Doppler-velocity projections from segmented LiDAR returns into a planar velocity estimate for each obstacle, structured as

$$\mathbf{y}_t^n = \mathbf{U} \mathbf{x}_t^n + v_t^n, \quad v_t^n \sim \mathcal{N}(0, \mathbf{R}),$$

with

$$\mathbf{y}_t^n = [x_t^n,\, y_t^n,\, v_{x,t}^n,\, v_{y,t}^n]^\top.$$

The Doppler-fusion pipeline aggregates multiple noisy, potentially misaligned Doppler measurements into a single low-variance planar velocity estimate by appropriately projecting, averaging, and rectifying the set of radial velocities $I_n$ associated with the segmented obstacle. This reduces the impact of noisy or outlier points and provides high-frequency velocity information not accessible from range data alone.

2. Gaussian State-Space Model and Baseline Filtering

Obstacle motion is modelled as constant-acceleration Gaussian state-space processes. The dynamics are given by

$$\mathbf{x}_{t+1}^n = \mathbf{T} \mathbf{x}_t^n + w_t^n,\quad w_t^n \sim \mathcal{N}(0, \mathbf{Q}),$$

where $\mathbf{T}$ is a block-diagonal matrix propagating position, velocity, and acceleration according to the discretized motion equations (with sample interval $\Delta t$). A standard Kalman filter can be constructed by propagating the estimated state and error covariance: $$\begin{aligned} &\text{Predict:} && \hat{\mathbf{x}}_{t|t-1} = \mathbf{T} \hat{\mathbf{x}}_{t-1}, \ &&& \mathbf{P}_{t|t-1} = \mathbf{T} \mathbf{P}_{t-1} \mathbf{T}^\top + \mathbf{Q}, \ &\text{Update:} && \mathbf{K}_t = \mathbf{P}_{t|t-1} \mathbf{U}^\top (\mathbf{U} \mathbf{P}_{t|t-1} \mathbf{U}^\top + \mathbf{R})^{-1}, \ &&& \hat{\mathbf{x}}_{t} = \hat{\mathbf{x}}_{t|t-1} + \mathbf{K}_t (\mathbf{y}_t - \mathbf{U} \hat{\mathbf{x}}_{t|t-1}), \ &&& \mathbf{P}_t = (\mathbf{I} - \mathbf{K}_t \mathbf{U}) \mathbf{P}_{t|t-1}. \end{aligned}$$

While effective for moderate or predictable dynamics, this approach is sensitive to mismodelling in rapidly varying or partially observable regimes—especially as encountered in highly dynamic traffic scenes.

3. D-KalmanNet Architecture and Learning

DPNet's core innovation is the D-KalmanNet module, which replaces the static, model-derived Kalman gain with a data-driven, adaptive gain generated via a recurrent neural network. The gain computation is:

$$\mathbf{K}_t = \mathrm{RNN}\left(\hat{\mathbf{x}}_{t|t-1},\, \hat{\mathbf{y}}_{t|t-1},\, \mathbf{y}_t,\, \mathbf{h}_{t-1}\right),$$

where $\mathbf{h}_{t-1}$ denotes the recurrent hidden state, and the input is a concatenation of the predicted state ($6$D), predicted observation ($4$D), current observation ($4$D), total $14$D.

The network utilizes a gated recurrent unit (GRU) layer (hidden size $64$), followed by two-layer multilayer perceptron ($64\rightarrow32\rightarrow24$), outputting vectorized gain parameters, reshaped into $\mathbb{R}^{6\times 4}$. The gain adaptively modulates the update step:

$$\hat{\mathbf{x}}_t = \hat{\mathbf{x}}_{t|t-1} + \mathbf{K}_t (\mathbf{y}_t - \hat{\mathbf{y}}_{t|t-1}),$$

thus allowing real-time compensation for model errors and rapid state changes.

Editor's term: "D-KalmanNet" refers specifically to this hybrid of physical state-space modelling with RNN-learned adaptive gain, distinct from conventional KalmanNet architectures.

4. Planning Integration via Doppler-Tuned MPC (DT-MPC)

DPNet leverages D-KalmanNet’s predictions to inform a Doppler-tuned model predictive control (DT-MPC) module that plans ego-vehicle motion. DT-MPC incorporates obstacle tracking predictions directly into its cost and constraint structure and auto-tunes its controller parameters in real time according to the learned uncertainty and velocity of surrounding traffic.

This integration is essential for operation in traffic scenarios where rapid motion, partial observability, and sensor noise can cause drastic short-horizon changes in feasible motion plans. DT-MPC's Doppler adaptivity enables trajectory plans that are robust to uncertainty in both the velocity and anticipated future locations of obstacles.

5. Training Procedures and Empirical Results

D-KalmanNet is trained using real-world Doppler LiDAR data (AevaScenes) with access to ground-truth trajectories. For batch windows of $H$ steps, the loss is:

$$\mathcal{L} = \sum_n \sum_{h=1}^H \|\hat{x}_{t+h|t}^n - x_{t+h}^n\|^2 + \lambda \sum_h \|\mathbf{K}_{t+h}\|_F^2,$$

with the second term providing $L_2$ regularization on the learnable gain. Optimization uses Adam (learning rate $10^{-3}$, weight decay $10^{-5}$), batch size $32$ over $2000$ epochs, gradient clipping at norm $1.0$, and early stopping on validation set MSE.

Empirical evaluations show pronounced performance gains: on AevaScenes, with $H=5$ at $10$ Hz, DPNet yields normalized MSE (NMSE) of $-35.80 \pm 8.62$ dB (highway) and $-45.00 \pm 7.35$ dB (city), representing $\sim12$ dB improvement over the best Doppler-Kalman filter baseline. At lower rates ($2$ Hz) and longer horizons ($H=10$), D-KalmanNet maintains $5$–$10$ dB improvements, with especially marked superiority at short forecast steps ($-48.8$ dB vs $-16.5$ dB in city, step $1$).

D-KalmanNet's computational efficiency is evidenced by 100 Hz update rates for single obstacles on Jetson Orin NX, with $15+$ Hz for tracking $10$ obstacles, requiring only $107$ MB GPU memory.

Scenario	Horizon (H)	NMSE (dB)	Baseline Gain (dB)
Highway	5	$-35.8\pm8.62$	$\sim -23.8$
City	5	$-45.0\pm7.35$	$\sim -33.0$

6. Methodological Innovations and Significance

DPNet introduces several methodological advances beyond prior approaches:

• Doppler-based velocity rectification: By aggregating noisy high-dimensional radial velocities to a low-variance planar estimate, DPNet overcomes the unobservability of non-radial velocity components inherent to Doppler sensors.
• Partial observability and tractable prediction: Structured state augmentation with heading-aligned velocity/acceleration enables representation of orientation and heading uncertainty, preserving linear propagation in the transition model.
• Learned adaptive gain: Model-free gain computation via RNN allows DPNet to self-correct for model mismatch, temporal nonstationarities, and rapid acceleration events without explicit multi-modal modelling or filter bank selection.

This hybrid of physics-based and data-driven filtering results in both high-frequency operation and improved accuracy, substantially advancing tracking and planning in settings where rapid obstacle motion is prevalent (Zuo et al., 29 Nov 2025).

7. Relationship to Related Work

Prior to DPNet, channel tracking with neural-augmented Kalman filtering (e.g., Hypernetwork Kalman Filter, (Pratik et al., 2021)) sought to generalize across a range of system dynamics using neural nets to parameterize adaptation. DPNet is distinguished by the explicit use of Doppler fusion in the measurement model, planning integration via DT-MPC, and an RNN-based gain output tailored for real-time tracking in autonomous driving.

A plausible implication is that the DPNet methodology could be generalized to other domains involving partially observed, rapidly varying dynamical systems where fusion of unconventional sensor modalities is possible.