finite-horizon-lqr

Skill do rozwiązywania skończonego horyzontu LQR (Linear Quadratic Regulator) metodą dynamicznego programowania, dedykowany dla systemów sterowania predykcyjnego (MPC). Implementacja zawiera backward pass z rekursją Riccatiego oraz forward simulation do generowania optymalnych sygnałów sterowania. Narzędzie przyjmuje macierze dynamiki systemu (A, B), macierze wag (Q, R), długość horyzontu (N) i stan początkowy (x0), zwracając optymalny sygnał sterowania dla pierwszego kroku czasowego.

1. Przygotuj parametry systemu: macierz dynamiki A (nx × nx), macierz wejścia B (nx × nu), macierz wagi stanu Q (nx × nx), macierz wagi sterowania R (nu × nu), gdzie nx to liczba zmiennych stanu, a nu to liczba wejść sterujących.

2. Zdefiniuj horyzont predykcji N (liczba kroków czasowych do przodu) oraz wektor stanu początkowego x0 (nx × 1).

3. Wywołaj funkcję finite_horizon_lqr(A, B, Q, R, N, x0). Skill wykonuje backward pass: iteracyjnie oblicza macierze wzmocnienia K_k dla każdego kroku od N-1 do 0, rozwiązując równanie Riccatiego i aktualizując macierz P_k.

4. Skill zwraca optymalny sygnał sterowania u_0 dla bieżącego kroku czasowego. Zastosuj ten sygnał do systemu fizycznego lub symulacji.

5. W następnym kroku czasowym zmierz nowy stan x (wynikający z dynamiki x_{k+1} = A @ x_k + B @ u_k) i powtórz kroki 3–4 z nowym stanem jako x0, aby uzyskać kolejny optymalny sygnał sterowania.

6. Kontynuuj pętlę sterowania: pomiar stanu → rozwiązanie LQR → aplikacja pierwszego sygnału → przesunięcie horyzontu w czasie, aż do osiągnięcia celu sterowania lub końca zadania.