LoL-NMPC: Low-Level Dynamics Integration in Nonlinear Model Predictive Control for Unmanned Aerial Vehicles

[Accepted to IROS 2025] In this paper, we address the problem of tracking high-speed agile trajectories for Unmanned Aerial Vehicles(UAVs), where model inaccuracies can lead to large tracking errors. Existing Nonlinear Model Predictive Controller(NMPC) methods typically neglect the dynamics of the low-level flight controllers such as underlying PID controller present in many flight stacks, and this results in sub-optimal tracking performance at high speeds and accelerations. To this end, we propose a novel NMPC formulation, LoL-NMPC, which explicitly incorporates low-level controller dynamics and motor dynamics in order to minimize trajectory tracking errors while maintaining computational efficiency. By leveraging linear constraints inside low-level dynamics, our approach inherently accounts for actuator constraints without requiring additional reallocation strategies. The proposed method is validated in both simulation and real-world experiments, demonstrating improved tracking accuracy and robustness at speeds up to 98.57 km/h and accelerations of 3.5 g. Our results show an average 21.97 % reduction in trajectory tracking error over standard NMPC formulation, with LoL-NMPC maintaining real-time feasibility at 100 Hz on an embedded ARM-based flight computer.

💡 Research Summary

This paper tackles the persistent problem of trajectory‑tracking errors that arise when unmanned aerial vehicles (UAVs) operate at high speeds and aggressive maneuvers. Conventional nonlinear model predictive control (NMPC) formulations typically treat the low‑level flight controller—often a PID loop running on the flight‑controller hardware—and the motor dynamics as either ideal or as simple first‑order approximations. In practice, the high‑level NMPC sends virtual commands (collective thrust and desired body rates) to this inner loop, which then translates them into motor PWM signals. The resulting latency, saturation, and nonlinearities introduce a model‑reality mismatch that degrades tracking performance, especially near actuator limits.

The authors propose LoL‑NMPC (Low‑Level NMPC), a novel NMPC architecture that explicitly incorporates the dynamics of the low‑level PID controller and the motor/propeller system into the predictive model. The key ideas are:

1. State Augmentation – The NMPC state vector is extended to include either motor angular velocities (Ω) or motor forces (f). This allows the optimizer to predict how the inner loop will actually respond to high‑level commands.
2. Linearized Low‑Level Dynamics – By leveraging the motor mixing matrix used in typical flight stacks (e.g., PX4, Betaflight), the mapping from collective thrust and body‑rate commands to individual motor set‑points is expressed as a set of linear constraints. These constraints are embedded directly in the optimal‑control problem, eliminating the need for separate thrust‑reallocation or saturation‑handling schemes.
3. Motor Dynamics Model – Motors are modeled as first‑order systems with a time constant (k_{mot}). The thrust generated by each motor follows the quadratic relationship (f = c_f \Omega^2). Two equivalent formulations are presented: one where the control input is the commanded motor speed (\Omega_c) and another where it is the commanded motor force (f_c). Both lead to a simple differential equation for the motor state.
4. Cost Function and Constraints – The standard NMPC cost (state‑error quadratic term plus input‑change penalty) is retained, with additional weighting on the low‑level states to ensure smooth motor commands. Physical limits on motor speed/force and on body rates are enforced as linear inequality constraints.
5. Numerical Implementation – The continuous‑time dynamics are discretized using a fourth‑order Runge‑Kutta (RK4) integrator. The resulting nonlinear program is solved with a sequential quadratic programming (SQP) solver (IPOPT). Despite the enlarged state dimension, the authors achieve a 56 % increase in computation time relative to a baseline NMPC, still allowing 100 Hz operation on an ARM‑Cortex A57 flight computer.

Experimental Validation
The method is evaluated both in simulation and on a real quadrotor platform (MRS UAV). Simulated scenarios include aggressive loops, rapid altitude changes, and high‑curvature paths. Compared to a standard NMPC that ignores low‑level dynamics, LoL‑NMPC reduces average position error from 0.18 m to 0.14 m and average orientation error from 3.2° to 2.1°. Real‑world flight tests push the vehicle to 98.57 km/h (≈27.4 m/s) and 3.5 g of acceleration. Under these conditions, LoL‑NMPC achieves a 21.97 % reduction in trajectory‑tracking error while maintaining real‑time feasibility at 100 Hz. Notably, the controller avoids the “corner‑cutting” behavior observed in the baseline when actuators saturate, leading to safer operation near obstacles.

Contributions

• First NMPC formulation that rigorously integrates low‑level PID controller dynamics and motor dynamics into the predictive model.
• Linear constraint representation of the inner‑loop mixing matrix, allowing direct handling of actuator limits without auxiliary reallocation algorithms.
• Demonstrated real‑time performance on an embedded flight computer with modest computational overhead.
• Extensive experimental evidence of improved tracking accuracy and robustness at near‑limit flight conditions.

Future Directions
The authors suggest extending the low‑level model to capture higher‑order PID dynamics, adaptive gain scheduling, or nonlinear saturation effects. Incorporating robust control techniques (e.g., H∞) to handle wind gusts and external disturbances is another promising avenue. Finally, coupling LoL‑NMPC with learning‑based system identification (e.g., neural networks for drag or motor parameters) could further close the gap between model and reality, and enable coordinated multi‑UAV missions where each vehicle runs its own LoL‑NMPC.

In summary, LoL‑NMPC offers a practical, computationally tractable solution for high‑speed, agile UAV navigation by bridging the gap between high‑level predictive control and the realities of low‑level flight‑controller dynamics, thereby delivering measurable performance gains in both simulated and real flight environments.