ptimization with a cost composed of control effort, trajectory tracking, distance penalties, and a log-barrier term enforcing obstacle avoidance. The discrete-time dynamics and input limits form the constraints.

• Continuously adapt its gradient to track, enabling faster convergence to the new optimal solution as the environment changes.
• Therefore, it enables MPC run in real-time in dynamic environment without simplification or dynamic model of the obstacle

Based on that, we established the cost function based on the formulation of MPC.

Pseudo-code of MPC

PCIP accelerates MPC by predicting the next solution using the previous optimal state, then performing a small number of Newton correction iterations. This warm-start strategy efficiently tracks smooth variations in the environment, reducing solve time by more than 40% compared to ECOS.

Correction: it runs a few Newton iteration to “correct” errors and satisfy new constraints. PCIP can converge with fewer iteration

Prediction: It allows prediction step. It gives a “warm-start” that is already close to optimal. It also uses previous solution to predict the new solution.

Because obstacles and reference trajectories evolve over time, the optimization problem itself changes at each control step. TVO provides a principled way to track the continuously shifting optimal solution $x^\*(t)$x\*(t), forming the foundation of our solver.

It borrowed principles from L1 Adaptive Control. L1AO augments PCIP by compensating for prediction errors in the TVO gradient, especially when obstacle motion is abrupt or non-smooth. It quickly adjusts the correction term at the iteration time scale, improving robustness under noise, model mismatch, and fast-changing constraints.

It mainly:

• detects when system behave differently than expected
• adapts parameters quickly and robustly

I specifically chose L1AO because it is:

• Fast: it adapts parameters at time scale of optimization iteration
• Robust: When large disturbance is detected, L1 keeps stability
• Plug&Play: it does not replace PCIP, but it wraps around it, adjusting parameters as needed

It is complementary with PCIP:

• L1AO handles non-smooth variation, which ensures the robustness
• PCIP exploits smooth time variation, which increases the efficiency

In the 2D simulations, we model the robot as a discrete-time double integrator navigating toward a moving target while avoiding multiple dynamic obstacles. We benchmark ECOS, PCIP, and our PCIP–L1AO solver across increasing obstacle density and target-path complexity to evaluate computation time, robustness, and success rate. The results show that L1AO improves stability and obstacle clearance under rapidly changing environments without increasing computation time.

We deploy our MPC–TVO–L1AO controller on a Crazyflie nano-quadrotor to evaluate its performance under real-world noise, communication delays, and model mismatch. The drone is commanded wirelessly in real time and tested in scenarios involving moving obstacles, including other quadrotors and humans. These experiments show that the L1AO-augmented controller maintains stable, collision-free trajectories despite unpredictable environment changes and imperfect state estimation. After graduating, I remained in the lab to write a full operation manual—since no one else knew how to run the system—and carried out two key experiments: one where an agent drone tracked a target drone while avoiding the others, and another where a drone followed a trajectory while safely navigating around a randomly walking person.

W

Through this project, I saw how quickly real-world autonomy breaks down once you leave clean simulations. MPC rollouts frequently diverged due to poor warm-starts, horizon misalignment, and numerical stiffness in the log-barrier formulation, while TVO gradients occasionally exploded when obstacle motion changed faster than the solver could track. The L1AO layer also proved sensitive to noise: its adaptive term often amplified errors from inaccurate ∇ₜΦ estimates, requiring retuning of filter bandwidth, revisions to the piecewise-constant update law, and careful balancing of responsiveness and stability.