MPC: Tracking breathing sine-wave in the cartpole system

Constant Sine-Wave: In Figure 1, we track a constant sine wave reference with amplitude $A_0=0.4$m and frequency $f=0.2$ Hz. We use MPC with hyperparameters $Q=\mathrm{diag}(120,75,2500,250)$, $R=0.05$, $H=100$, and \textcolor{blue}{\texttt{max\_iters=5}}. The cart follows the reference position and velocity with RMSE $0.052$m and $0.079$m/s respectively. From panels (a) and (b), we observe that the tracking is good overall, but the cart shows a small delay (phase lag) and its peaks are slightly smaller than the reference, especially near the maximum and minimum points. Panel (c) shows that the pole stays close to upright (with peak angle about $2.77^\circ$), and panel (d) shows that the control input stays within the limit $u\in[-1,1]$. Finally, panel (e) shows a nearly closed loop in the $(x,\dot{x})$ phase plot, which is consistent with steady periodic wave tracking. Note: Since our main goal was breathing sine wave tracking, we didn't attempt perfect tracking of a constant sine wave and proceeded after obtaining satisfactory results.

Breathing Sine-Wave (Low Envelope Frequency): In Figure 2, we track a breathing sine wave reference with a slowly varying amplitude envelope (low envelope frequency $f_{\mathrm{amp}}=0.05$ Hz) and main sine frequency $f=0.15$ Hz. We've visualized the envelope by plotting the bounds $\pm r(t)A(t)$ around the reference. Overall, we see that the tracking is strong: the cart position and velocity closely follow the reference in panels (a) and (b). There's only a small delay near the peaks, but the errors are low (RMSE $x=0.024$ m and RMSE $\dot{x}=0.030$ m/s). The low envelope frequency makes the tracking easier because the amplitude changes slowly, so over short time windows ($T=500$ steps or $10$s) the reference looks close to a constant-amplitude sine wave. Panel (c) shows that the pole remains close to upright (peak angle about $2.44^\circ$). Similarly, panel (d) shows that the control input stays within the limits $u\in[-1,1]$, with a smoother control. Finally, the $(x,\dot{x})$ phase plot in panel (e) is not perfectly closed. This is expected because the motion is not strictly periodic when the amplitude is changing over time.

Breathing Sine-Wave (High Envelope Frequency): In Figure 3, we increase the difficulty by using a faster breathing reference ($f=0.25$ Hz and a higher envelope frequency $f_{\mathrm{amp}}=0.10$ Hz). We also use a longer horizon $H=120$ (other parameters are shown in panel (f)). Compared to the low-frequency case in Figure 2, the tracking is noticeably worse: panels (a) and (b) show larger mismatches near the peaks. This lag is reflected in the higher errors (RMSE $x=0.058$ m and RMSE $\dot{x}=0.097$ m/s). We also observe that the controller works harder: panel (d) shows the input coming close to the limits $u=\pm 1$. Panel (c) indicates the pole is still kept near upright, but with a larger oscillation (peak angle about $4.20^\circ$). Finally, the $(x,\dot{x})$ phase portrait in panel (e) does not form a single closed loop. This is again expected because the amplitude is changing more quickly. We can clearly see that at around $6$-$8$ seconds, the loop in panel (e) is smaller and the corresponding position and velocity in panels (a) and (b) are also smaller. The same pattern is seen for bigger loop around $2$-$4$ seconds. This behavior clearly demonstrates that the cart is performing the breathing behavior.