Finite-Horizon LQR for MPC

Problem Formulation

Minimize cost over horizon N:

J = Σ(k=0 to N-1) [x'Qx + u'Ru] + x_N' P x_N

Backward Riccati Recursion

Initialize: P_N = Q (or LQR solution for stability)

For k = N-1 down to 0:

K_k = inv(R + B'P_{k+1}B) @ B'P_{k+1}A
P_k = Q + A'P_{k+1}(A - B @ K_k)