diff --git a/README.md b/README.md index f3d09c47..d1f55376 100644 --- a/README.md +++ b/README.md @@ -27,7 +27,7 @@ Refer to the documentation to quickly integrate and utilize the library's signal | [Math](doc/math/README.md) | CORDIC, Quaternion, MatrixNorms, Step Response Metrics, MatrixExponential | | [Solvers](doc/solvers/README.md) | Gaussian Elimination, Levinson-Durbin, Durand-Kerner, Cholesky, DARE, Runge-Kutta ODE Integrators (RK4 + Dormand-Prince), Spectral Radius & Discrete Stability Margin, QR Decomposition (Householder / Givens), LU Decomposition with Partial Pivoting, Singular Value Decomposition (Golub-Kahan) | | [Nonlinear Control](doc/nonlinear_control/README.md) | Feedback Linearization, Backstepping Control | -| [Robust Control](doc/robust_control/README.md) | Active Disturbance Rejection Control (ADRC + ESO), Sliding Mode Control (SMC), Disturbance Observer (DOB) | +| [Robust Control](doc/robust_control/README.md) | Active Disturbance Rejection Control (ADRC + ESO), Sliding Mode Control (SMC), Disturbance Observer (DOB), H∞ State-Feedback Control | | [Performance Optimization](doc/performance-optimization/README.md) | Compiler optimizations, SIMD | Each category page lists its algorithms with a brief description and links to the detailed documentation. diff --git a/ROADMAP.md b/ROADMAP.md index 7cf2cc51..68db83d7 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -27,7 +27,6 @@ Difficulty legend: | # | Component | Target module | Difficulty | |----|------------------------------------------------------|---------------------------|------------| | 45 | IIR filter design (Butterworth/Chebyshev + bilinear) | `filters/passive` | ★★★★★ | -| 46 | H∞ state-feedback control | `robust_control` (new) | ★★★★★ | | 47 | Model Reference Adaptive Control (MRAC) | `nonlinear_control` (new) | ★★★★★ | Items 48–52 are the **evaluation & metrics primitives** — reusable quantities the per-family @@ -248,7 +247,7 @@ the library does not yet expose. Detailed below under - **Algorithm / paper:** T. W. Parks, C. S. Burrus, *Digital Filter Design* (1987); bilinear transform — Oppenheim & Schafer, *DTSP*. - **Reuses:** Item 15 (biquad target), `math::ComplexNumber`, item 23 (root placement). -### 46. H∞ state-feedback control *(float-first)* +### ~~46. H∞ state-feedback control~~ *(float-first)* ✓ Done - **What:** Robust optimal control minimizing the worst-case disturbance-to-error gain via a Riccati/LMI solution. - **Embedded value:** Guaranteed performance under bounded model uncertainty for safety-critical loops. - **Algorithm / paper:** J. Doyle, K. Glover, P. Khargonekar, B. Francis, "State-Space Solutions to Standard H₂ and H∞ Control Problems," *IEEE Trans. AC*, 34(8), 1989. diff --git a/doc/robust_control/HInfinityStateFeedback.md b/doc/robust_control/HInfinityStateFeedback.md new file mode 100644 index 00000000..8489ac39 --- /dev/null +++ b/doc/robust_control/HInfinityStateFeedback.md @@ -0,0 +1,148 @@ +# H∞ State-Feedback Control + +## Overview & Motivation + +Robust control guarantees performance under worst-case conditions rather than average ones. H∞ +state-feedback synthesizes a linear gain that minimizes the largest possible ratio of output energy +to disturbance energy across all bounded disturbances — the induced $\mathcal{L}_2$ gain. This +provable attenuation bound is what safety-critical and certification-driven embedded systems require: +not a best-guess response, but a hard upper limit on how badly an unknown disturbance can degrade +performance. + +## Mathematical Theory + +### Generalized Plant + +The design is framed around the generalized plant + +$$ +x_{k+1} = A x_k + B_1 w_k + B_2 u_k, \quad z_k = C_1 x_k + D_{12} u_k +$$ + +where $w \in \mathbb{R}^{n_w}$ is the exogenous disturbance, $u \in \mathbb{R}^{n_u}$ is the +control input, and $z \in \mathbb{R}^{n_z}$ is the performance (error) output. + +### H∞ Performance Criterion + +The controller objective is to find $u = -K x$ such that + +$$ +\sup_{w \neq 0} \frac{\|z\|_2}{\|w\|_2} < \gamma +$$ + +where $\gamma > 0$ is the prescribed attenuation level. Finding the smallest feasible $\gamma$ +(the optimal $\gamma^*$) determines the best achievable robustness. + +### Game-Theoretic Discrete Algebraic Riccati Equation (GARE) + +The H∞ state-feedback gain is derived from the discrete Riccati equation + +$$ +X = A^\top X A - A^\top X B \tilde{R}^{-1} B^\top X A + Q +$$ + +with the augmented input matrix $B = [B_2 \mid B_1]$ and the **indefinite** weight matrix + +$$ +\tilde{R} = \begin{pmatrix} I_{n_u} & 0 \\ 0 & -\gamma^2 I_{n_w} \end{pmatrix}. +$$ + +The negative block encodes the adversarial role of the disturbance: the disturbance player +maximizes while the control player minimizes. A positive-semidefinite stabilizing solution $X \geq 0$ +exists if and only if $\gamma$ is above the optimal level $\gamma^*$. + +### Gain Extraction + +From the Riccati solution $X$, the augmented gain is + +$$ +K_{\text{full}} = (\tilde{R} + B^\top X B)^{-1} B^\top X A. +$$ + +Only the top $n_u$ rows — the control block — form the feedback gain $K$, and the closed-loop +map is $A_{\text{cl}} = A - B_2 K$. + +### Feasibility Conditions + +A given $\gamma$ is feasible when: +1. The GARE has a positive-semidefinite solution $X \geq 0$. +2. The disturbance block $-\gamma^2 I + B_1^\top X B_1 \prec 0$ is negative definite, confirming + that the disturbance remains a genuine maximizer rather than a destabilizing force. + +### Bisection for Optimal $\gamma$ + +Neither condition holds for $\gamma < \gamma^*$; both hold for $\gamma > \gamma^*$. A standard +bisection on $[\gamma_{\min}, \gamma_{\max}]$ converges to $\gamma^*$ at a linear rate. + +## Complexity Analysis + +| Phase | Time Complexity | Space | Notes | +|----------------|-------------------------------------------------------------|----------|------------------------------------| +| Synthesize | $O(\log((\gamma_{\max}-\gamma_{\min})/\epsilon) \cdot n^3)$ | $O(n^2)$ | Dominated by iterative DARE solves | +| ComputeControl | $O(n_u \cdot n)$ | $O(1)$ | Single matrix-vector multiply | + +## Step-by-Step Walkthrough + +Consider a 2-state discrete plant with one disturbance and one control input. + +1. **Bisect**: choose $g = (\gamma_{\min} + \gamma_{\max}) / 2$; stack $B = [B_2 \mid B_1]$; + build $\tilde{R} = \text{diag}(1, -g^2)$. +2. **Solve GARE**: run the iterative DARE solver with indefinite $\tilde{R}$ until convergence. +3. **Check feasibility**: verify $X_{ii} \geq 0$ for all $i$ and $-g^2 + (B_1^\top X B_1)_{ii} < 0$. +4. **Update bisection**: if feasible, tighten ($\gamma_{\max} \leftarrow g$); otherwise relax + ($\gamma_{\min} \leftarrow g$). +5. **Finalize**: at convergence, solve GARE at $\gamma_{\max}$, extract the control rows of + $K_{\text{full}}$, and verify $A - B_2 K$ is Schur-stable (all eigenvalues inside the unit disk). + +## Pitfalls & Edge Cases + +- **Ill-conditioned GARE near $\gamma^*$**: the Riccati solution blows up as $\gamma \to \gamma^*$ + from above. The bisection tolerance should not be driven below the float precision of the + Riccati solver. +- **Indefinite $\tilde{R}$**: the standard DARE assumes positive-definite $R$; using $\tilde{R}$ + with a negative block is valid only when the full augmented pair $(A, B)$ is stabilizable and + the game saddle-point condition holds. Infeasibility manifests as non-PSD $X$ or violated + disturbance-block condition. +- **Disturbance block check**: a numerically PSD $X$ does not guarantee feasibility; the + disturbance block condition must also be verified explicitly. +- **Float precision**: accumulated rounding in many DARE iterations can erode the convergence + criterion; using the iterative formulation with a conservative tolerance (relative to `1e-3f`) + prevents premature acceptance of a diverged iterate. + +## Variants & Generalizations + +- **H∞ output feedback (H∞ LQG)**: replaces the state $x$ with an observer-based estimate; requires + a second (filter) Riccati equation to solve the full information-state problem. +- **Continuous-time H∞**: replaces the DARE with the continuous algebraic Riccati equation; + directly applicable to analog plants or zero-order-hold designs. +- **Mixed H₂/H∞**: constrains the H∞ norm while minimizing the H₂ (LQG) cost — trades average + and worst-case performance on a Pareto frontier. + +## Applications + +- Safety-critical motion control where actuator saturation or load shifts make average-case design + insufficient. +- Vibration suppression under unknown broadband disturbances. +- Robust attitude control of spacecraft or UAVs subject to unmodeled flexible modes. +- Robust stabilization of plants with parametric uncertainty encoded as bounded disturbances. + +## Connections to Other Algorithms + +- **LQR** ($\gamma \to \infty$): as the adversary weakens, the H∞ gain converges to the LQR gain + for the same $(A, B_2, Q, I)$ weights. H∞ is the robust generalization of LQR. +- **DiscreteAlgebraicRiccatiEquation**: the inner computational engine; H∞ passes an indefinite + weight to it. +- **LQG / Kalman Filter**: the stochastic average-case counterpart; H∞ and LQG bound opposite ends + of the robustness-optimality trade-off. +- **Sliding Mode Control**: a nonlinear alternative to H∞ that achieves robust disturbance rejection + without solving a Riccati equation, at the cost of chattering and switching nonlinearity. +- **DurandKerner**: used post-synthesis to verify that all eigenvalues of $A - B_2 K$ lie inside + the unit disk. + +## References & Further Reading + +- J. Doyle, K. Glover, P. Khargonekar, B. Francis, "State-Space Solutions to Standard H₂ and H∞ + Control Problems," *IEEE Trans. Automatic Control*, 34(8), pp. 831–847, 1989. +- B. A. Francis, *A Course in H∞ Control Theory*, Lecture Notes in Control and Information + Sciences, Springer, 1987. +- K. Zhou, J. C. Doyle, K. Glover, *Robust and Optimal Control*, Prentice-Hall, 1996. diff --git a/doc/robust_control/README.md b/doc/robust_control/README.md index 4962b6cd..db9d29e9 100644 --- a/doc/robust_control/README.md +++ b/doc/robust_control/README.md @@ -9,3 +9,4 @@ Algorithms for robust control design: controllers that explicitly account for di | [Active Disturbance Rejection Control](ActiveDisturbanceRejection.md) | Near model-free controller pairing an Extended State Observer with bandwidth-parameterized PD feedback to estimate and cancel total disturbance in real time | | [Sliding Mode Control](SlidingModeControl.md) | Variable-structure controller driving the state onto a sliding surface with a boundary layer to suppress chattering — robust to matched disturbances and parameter uncertainty | | [Disturbance Observer](DisturbanceObserver.md) | Estimates lumped disturbance and model mismatch via the nominal plant inverse and a Q-filter, cancelling the disturbance to make the real plant behave like the nominal model | +| [H∞ State-Feedback Control](HInfinityStateFeedback.md) | Game-theoretic robust state-feedback gain synthesized via bisection on the game-theoretic DARE with an indefinite weight, guaranteeing a hard L2 disturbance-to-error attenuation bound | diff --git a/numerical/robust_control/CMakeLists.txt b/numerical/robust_control/CMakeLists.txt index e98a9704..fc2f02a9 100644 --- a/numerical/robust_control/CMakeLists.txt +++ b/numerical/robust_control/CMakeLists.txt @@ -9,18 +9,21 @@ target_link_libraries(numerical.robust_control ${NUMERICAL_VISIBILITY} numerical.math numerical.filters.passive numerical.solver + numerical.controllers.implementations infra.util ) target_sources(numerical.robust_control PRIVATE ActiveDisturbanceRejection.hpp DisturbanceObserver.hpp + HInfinityStateFeedback.hpp SlidingModeControl.hpp ) numerical_add_coverage_sources(numerical.robust_control ActiveDisturbanceRejection.cpp DisturbanceObserver.cpp + HInfinityStateFeedback.cpp SlidingModeControl.cpp ) diff --git a/numerical/robust_control/HInfinityStateFeedback.cpp b/numerical/robust_control/HInfinityStateFeedback.cpp new file mode 100644 index 00000000..9681e6b4 --- /dev/null +++ b/numerical/robust_control/HInfinityStateFeedback.cpp @@ -0,0 +1,6 @@ +#include "numerical/robust_control/HInfinityStateFeedback.hpp" + +namespace robust_control +{ + template class HInfinityStateFeedback; +} diff --git a/numerical/robust_control/HInfinityStateFeedback.hpp b/numerical/robust_control/HInfinityStateFeedback.hpp new file mode 100644 index 00000000..bdb77a94 --- /dev/null +++ b/numerical/robust_control/HInfinityStateFeedback.hpp @@ -0,0 +1,219 @@ +#pragma once + +#if defined(__GNUC__) || defined(__clang__) +#pragma GCC optimize("O3", "fast-math") +#endif + +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Matrix.hpp" +#include "numerical/solvers/DiscreteAlgebraicRiccatiEquation.hpp" +#include "numerical/solvers/GaussianElimination.hpp" +#include "numerical/solvers/SpectralRadius.hpp" +#include +#include +#include + +namespace robust_control +{ + template + struct GeneralizedPlant + { + static_assert(std::is_floating_point_v, "GeneralizedPlant supports floating-point types"); + + using StateMatrix = math::SquareMatrix; + using DisturbanceMatrix = math::Matrix; + using ControlMatrix = math::Matrix; + using ErrorStateMatrix = math::Matrix; + using ErrorControlMatrix = math::Matrix; + + StateMatrix A{}; + DisturbanceMatrix B1{}; + ControlMatrix B2{}; + ErrorStateMatrix C1{}; + ErrorControlMatrix D12{}; + }; + + template + class HInfinityStateFeedback + { + static_assert(std::is_floating_point_v, "HInfinityStateFeedback supports floating-point types"); + + static constexpr std::size_t AugInputSize = ControlSize + DisturbanceSize; + + public: + using StateVector = math::Vector; + using InputVector = math::Vector; + using GainMatrix = math::Matrix; + using RiccatiMatrix = math::SquareMatrix; + using Plant = GeneralizedPlant; + + explicit HInfinityStateFeedback(const Plant& gplant); + + bool Synthesize(T gammaMin, T gammaMax, T tol); + + OPTIMIZE_FOR_SPEED InputVector ComputeControl(const StateVector& x) const; + + [[nodiscard]] const GainMatrix& Gain() const; + [[nodiscard]] T Gamma() const; + + private: + using AugInputMatrix = math::Matrix; + using AugWeightMatrix = math::SquareMatrix; + using AugGainMatrix = math::Matrix; + + struct Augmented + { + AugInputMatrix B{}; + AugWeightMatrix Rtilde{}; + RiccatiMatrix Q{}; + }; + + Augmented BuildAugmented(T g) const; + AugGainMatrix FullGain(const Augmented& aug, const RiccatiMatrix& Xsol) const; + bool RiccatiFeasible(T g) const; + void SolveGameRiccati(T g); + + Plant plant{}; + GainMatrix K{}; + RiccatiMatrix X{}; + T gamma{}; + }; + + template + HInfinityStateFeedback::HInfinityStateFeedback( + const Plant& gplant) + : plant{ gplant } + {} + + template + typename HInfinityStateFeedback::Augmented + HInfinityStateFeedback::BuildAugmented(T g) const + { + Augmented aug{}; + aug.B.SetBlock(plant.B2, 0, 0); + aug.B.SetBlock(plant.B1, 0, ControlSize); + + auto controlWeight = math::SquareMatrix::Identity(); + auto disturbanceWeight = math::SquareMatrix::Identity(); + disturbanceWeight *= -(g * g); + aug.Rtilde.SetBlock(controlWeight, 0, 0); + aug.Rtilde.SetBlock(disturbanceWeight, ControlSize, ControlSize); + + aug.Q = plant.C1.Transpose() * plant.C1; + return aug; + } + + template + typename HInfinityStateFeedback::AugGainMatrix + HInfinityStateFeedback::FullGain( + const Augmented& aug, const RiccatiMatrix& Xsol) const + { + auto BtX = aug.B.Transpose() * Xsol; + auto S = aug.Rtilde + BtX * aug.B; + return solvers::SolveSystem(S, BtX * plant.A); + } + + template + bool HInfinityStateFeedback::RiccatiFeasible(T g) const + { + const Augmented aug = BuildAugmented(g); + + solvers::DiscreteAlgebraicRiccatiEquation dare{}; + auto Xcandidate = dare.Solve(plant.A, aug.B, aug.Q, aug.Rtilde); + + for (std::size_t i = 0; i < StateSize; ++i) + if (Xcandidate.at(i, i) < T{ 0 }) + return false; + + auto distBlock = plant.B1.Transpose() * (Xcandidate * plant.B1); + const T gammaSq = g * g; + for (std::size_t i = 0; i < DisturbanceSize; ++i) + if (distBlock.at(i, i) >= gammaSq) + return false; + + auto AtX = plant.A.Transpose() * Xcandidate; + auto Xresid = AtX * plant.A - AtX * aug.B * FullGain(aug, Xcandidate) + aug.Q; + + for (std::size_t i = 0; i < StateSize; ++i) + for (std::size_t j = 0; j < StateSize; ++j) + if (std::abs(Xresid.at(i, j) - Xcandidate.at(i, j)) > T{ 1 }) + return false; + + return true; + } + + template + void HInfinityStateFeedback::SolveGameRiccati(T g) + { + const Augmented aug = BuildAugmented(g); + + solvers::DiscreteAlgebraicRiccatiEquation dare{}; + X = dare.Solve(plant.A, aug.B, aug.Q, aug.Rtilde); + + K = FullGain(aug, X).template GetBlock(0, 0); + } + + template + bool HInfinityStateFeedback::Synthesize( + T gammaMin, T gammaMax, T tol) + { + T gLo{ gammaMin }; + T gHi{ gammaMax }; + bool anyFeasible{ false }; + + while (gHi - gLo > tol) + { + const T gMid = (gLo + gHi) / T{ 2 }; + if (RiccatiFeasible(gMid)) + { + gHi = gMid; + anyFeasible = true; + } + else + gLo = gMid; + } + + if (!anyFeasible && !RiccatiFeasible(gHi)) + return false; + + gamma = gHi; + SolveGameRiccati(gamma); + + auto closedLoop = plant.A - plant.B2 * K; + return solvers::SpectralRadius{}.IsSchurStable(closedLoop); + } + + template + OPTIMIZE_FOR_SPEED + typename HInfinityStateFeedback::InputVector + HInfinityStateFeedback::ComputeControl( + const StateVector& x) const + { + return K * x * T{ -1 }; + } + + template + const typename HInfinityStateFeedback::GainMatrix& + HInfinityStateFeedback::Gain() const + { + return K; + } + + template + T HInfinityStateFeedback::Gamma() const + { + return gamma; + } + +#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD + extern template class HInfinityStateFeedback; +#endif +} diff --git a/numerical/robust_control/test/CMakeLists.txt b/numerical/robust_control/test/CMakeLists.txt index 7fb58925..737e1bb2 100644 --- a/numerical/robust_control/test/CMakeLists.txt +++ b/numerical/robust_control/test/CMakeLists.txt @@ -10,5 +10,6 @@ target_link_libraries(numerical.robust_control_test PUBLIC target_sources(numerical.robust_control_test PRIVATE TestActiveDisturbanceRejection.cpp TestDisturbanceObserver.cpp + TestHInfinityStateFeedback.cpp TestSlidingModeControl.cpp ) diff --git a/numerical/robust_control/test/TestHInfinityStateFeedback.cpp b/numerical/robust_control/test/TestHInfinityStateFeedback.cpp new file mode 100644 index 00000000..71485bbf --- /dev/null +++ b/numerical/robust_control/test/TestHInfinityStateFeedback.cpp @@ -0,0 +1,207 @@ +#include "numerical/controllers/implementations/Lqr.hpp" +#include "numerical/math/Matrix.hpp" +#include "numerical/math/Tolerance.hpp" +#include "numerical/robust_control/HInfinityStateFeedback.hpp" +#include +#include + +namespace +{ + using Plant = robust_control::GeneralizedPlant; + using HInf = robust_control::HInfinityStateFeedback; + + Plant MakeGeneralizedPlant() + { + Plant p{}; + p.A.at(0, 0) = 1.0f; + p.A.at(0, 1) = 0.1f; + p.A.at(1, 0) = 0.0f; + p.A.at(1, 1) = 0.9f; + + p.B1.at(0, 0) = 0.0f; + p.B1.at(1, 0) = 0.1f; + + p.B2.at(0, 0) = 0.0f; + p.B2.at(1, 0) = 1.0f; + + p.C1.at(0, 0) = 1.0f; + p.C1.at(0, 1) = 0.0f; + + p.D12.at(0, 0) = 0.0f; + return p; + } + + class TestHInfinityStateFeedback : public ::testing::Test + { + protected: + Plant plant{ MakeGeneralizedPlant() }; + HInf hinf{ plant }; + }; +} + +TEST_F(TestHInfinityStateFeedback, reduces_to_lqr_as_gamma_large) +{ + const bool ok = hinf.Synthesize(0.5f, 1000.0f, 1e-3f); + EXPECT_TRUE(ok); + + math::SquareMatrix Q{ plant.C1.Transpose() * plant.C1 }; + math::SquareMatrix R{}; + R.at(0, 0) = 1.0f; + controllers::Lqr lqr{ plant.A, plant.B2, Q, R }; + + const auto& Khinf = hinf.Gain(); + const auto& Klqr = lqr.GetGain(); + + for (std::size_t c = 0; c < 2; ++c) + EXPECT_NEAR(Khinf.at(0, c), Klqr.at(0, c), 5e-2f); +} + +TEST_F(TestHInfinityStateFeedback, closed_loop_is_schur_stable) +{ + const bool ok = hinf.Synthesize(0.5f, 100.0f, 1e-3f); + EXPECT_TRUE(ok); + + const auto& K = hinf.Gain(); + auto clA = plant.A - plant.B2 * K; + + std::array charPoly{}; + charPoly[0] = 1.0f; + charPoly[1] = -(clA.at(0, 0) + clA.at(1, 1)); + charPoly[2] = clA.at(0, 0) * clA.at(1, 1) - clA.at(0, 1) * clA.at(1, 0); + + solvers::DurandKerner dk{}; + auto roots = dk.Solve(std::span{ charPoly.data(), 3 }); + + for (const auto& root : roots) + EXPECT_LT(std::abs(root), 1.0f); +} + +TEST_F(TestHInfinityStateFeedback, achieves_target_gamma) +{ + const float gTarget = 5.0f; + const bool ok = hinf.Synthesize(0.5f, gTarget * 2.0f, 1e-3f); + EXPECT_TRUE(ok); + + const auto& K = hinf.Gain(); + auto clA = plant.A - plant.B2 * K; + auto clB = plant.B1; + const auto& C1 = plant.C1; + + float outputEnergy{ 0.0f }; + float inputEnergy{ 0.0f }; + math::Vector x{}; + + const float wMag = 1.0f; + for (int step = 0; step < 200; ++step) + { + math::Vector w{}; + w.at(0, 0) = (step % 2 == 0) ? wMag : -wMag; + + auto z = C1 * x; + outputEnergy += z.at(0, 0) * z.at(0, 0); + inputEnergy += w.at(0, 0) * w.at(0, 0); + + x = clA * x + clB * w; + } + + if (inputEnergy > 0.0f) + EXPECT_LE(outputEnergy / inputEnergy, hinf.Gamma() * hinf.Gamma() + 1.0f); +} + +TEST_F(TestHInfinityStateFeedback, bisection_finds_minimal_gamma) +{ + const bool ok = hinf.Synthesize(0.01f, 20.0f, 1e-2f); + EXPECT_TRUE(ok); + EXPECT_LT(hinf.Gamma(), 20.0f); + EXPECT_GT(hinf.Gamma(), 0.01f); +} + +TEST_F(TestHInfinityStateFeedback, infeasible_below_gamma_optimum) +{ + const bool ok = hinf.Synthesize(0.01f, 20.0f, 1e-2f); + EXPECT_TRUE(ok); + + const float gammaStar = hinf.Gamma(); + HInf hinfLow{ plant }; + const bool okLow = hinfLow.Synthesize(0.0f, gammaStar * 0.5f, 1e-3f); + EXPECT_FALSE(okLow); +} + +TEST_F(TestHInfinityStateFeedback, gain_matches_gare_solution) +{ + const bool ok = hinf.Synthesize(0.5f, 100.0f, 1e-3f); + EXPECT_TRUE(ok); + + constexpr std::size_t AugInputSize = 2; + math::Matrix B{}; + B.at(0, 0) = plant.B2.at(0, 0); + B.at(1, 0) = plant.B2.at(1, 0); + B.at(0, 1) = plant.B1.at(0, 0); + B.at(1, 1) = plant.B1.at(1, 0); + + const float g = hinf.Gamma(); + math::SquareMatrix Rtilde{}; + Rtilde.at(0, 0) = 1.0f; + Rtilde.at(1, 1) = -(g * g); + + auto Q = plant.C1.Transpose() * plant.C1; + solvers::DiscreteAlgebraicRiccatiEquation dare{}; + auto Xref = dare.Solve(plant.A, B, Q, Rtilde); + + auto BtX = B.Transpose() * Xref; + auto S = Rtilde + BtX * B; + auto BtXA = BtX * plant.A; + auto Kfull = solvers::SolveSystem(S, BtXA); + + const auto& K = hinf.Gain(); + EXPECT_NEAR(K.at(0, 0), Kfull.at(0, 0), 1e-3f); + EXPECT_NEAR(K.at(0, 1), Kfull.at(0, 1), 1e-3f); +} + +TEST_F(TestHInfinityStateFeedback, rejects_worst_case_disturbance) +{ + const bool ok = hinf.Synthesize(0.5f, 100.0f, 1e-3f); + EXPECT_TRUE(ok); + + const float g = hinf.Gamma(); + const auto& K = hinf.Gain(); + auto clA = plant.A - plant.B2 * K; + const auto& C1 = plant.C1; + + math::Vector x{}; + x.at(0, 0) = 1.0f; + + float outputEnergy{ 0.0f }; + float inputEnergy{ 0.0f }; + const float wMag = 0.5f; + + for (int step = 0; step < 100; ++step) + { + math::Vector w{}; + w.at(0, 0) = (step % 2 == 0) ? wMag : -wMag; + + auto z = C1 * x; + outputEnergy += z.at(0, 0) * z.at(0, 0); + inputEnergy += w.at(0, 0) * w.at(0, 0); + + x = clA * x + plant.B1 * w; + } + + EXPECT_LE(outputEnergy, g * g * inputEnergy + 10.0f); +} + +TEST_F(TestHInfinityStateFeedback, compute_control_is_negative_feedback) +{ + const bool ok = hinf.Synthesize(0.5f, 100.0f, 1e-3f); + EXPECT_TRUE(ok); + + math::Vector x{}; + x.at(0, 0) = 1.0f; + x.at(1, 0) = 0.5f; + + const auto u = hinf.ComputeControl(x); + const auto& K = hinf.Gain(); + + const float expected = -(K.at(0, 0) * x.at(0, 0) + K.at(0, 1) * x.at(1, 0)); + EXPECT_NEAR(u.at(0, 0), expected, math::Tolerance()); +} diff --git a/numerical/solvers/DiscreteAlgebraicRiccatiEquation.cpp b/numerical/solvers/DiscreteAlgebraicRiccatiEquation.cpp index 51c86de5..2377c554 100644 --- a/numerical/solvers/DiscreteAlgebraicRiccatiEquation.cpp +++ b/numerical/solvers/DiscreteAlgebraicRiccatiEquation.cpp @@ -4,5 +4,6 @@ namespace solvers { template class DiscreteAlgebraicRiccatiEquation; template class DiscreteAlgebraicRiccatiEquation; + template class DiscreteAlgebraicRiccatiEquation; template class DiscreteAlgebraicRiccatiEquation; } diff --git a/numerical/solvers/DiscreteAlgebraicRiccatiEquation.hpp b/numerical/solvers/DiscreteAlgebraicRiccatiEquation.hpp index 00dc5840..b0443769 100644 --- a/numerical/solvers/DiscreteAlgebraicRiccatiEquation.hpp +++ b/numerical/solvers/DiscreteAlgebraicRiccatiEquation.hpp @@ -87,6 +87,7 @@ namespace solvers #ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD extern template class DiscreteAlgebraicRiccatiEquation; extern template class DiscreteAlgebraicRiccatiEquation; + extern template class DiscreteAlgebraicRiccatiEquation; extern template class DiscreteAlgebraicRiccatiEquation; #endif } diff --git a/numerical/solvers/SpectralRadius.cpp b/numerical/solvers/SpectralRadius.cpp index 8027108d..cf105327 100644 --- a/numerical/solvers/SpectralRadius.cpp +++ b/numerical/solvers/SpectralRadius.cpp @@ -2,5 +2,6 @@ namespace solvers { + template class SpectralRadius; template class SpectralRadius; } diff --git a/numerical/solvers/SpectralRadius.hpp b/numerical/solvers/SpectralRadius.hpp index a8e459cd..e8d8b1c3 100644 --- a/numerical/solvers/SpectralRadius.hpp +++ b/numerical/solvers/SpectralRadius.hpp @@ -93,6 +93,7 @@ namespace solvers } #ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD + extern template class SpectralRadius; extern template class SpectralRadius; #endif } diff --git a/roadmap/robust_control/HInfinityStateFeedback/explanation.md b/roadmap/robust_control/HInfinityStateFeedback/explanation.md deleted file mode 100644 index bc6180bc..00000000 --- a/roadmap/robust_control/HInfinityStateFeedback/explanation.md +++ /dev/null @@ -1,34 +0,0 @@ -# H∞ State-Feedback Control — Overview - -## What it is -A robust optimal state-feedback law that minimizes the **worst-case** gain from disturbance to -performance error. Instead of assuming a known disturbance (like LQR/LQG), it assumes an adversary -picks the disturbance to hurt you the most, and designs the gain that best defends against it — the -guaranteed `L2` gain from disturbance `w` to error `z` is kept below a level `γ`. - -## Why it matters (embedded) -When a loop must **not fail** under bounded but unknown model error or disturbance — actuators near -limits, safety-critical motion, plants that shift with load — H∞ gives a *provable* performance -bound rather than an average-case one. That guarantee is exactly what certification and -safety-critical embedded control ask for. - -## How it works (intuition) -Frame the design as a two-player game: the controller minimizes error energy, a fictitious disturbance -maximizes it. The equilibrium is captured by a **game-theoretic Riccati equation** — the same algebraic -Riccati machinery as LQR, but with an *indefinite* weight that encodes the disturbance's antagonistic -role (a `−γ²` block). Solving it yields the feedback gain. A **bisection on `γ`** then squeezes the -attenuation level down to the smallest value for which a stabilizing solution still exists — the -sub-optimal `γ*`. As `γ` grows the adversary weakens and the whole thing gracefully becomes LQR. - -## Key parameters -- **`γ` (attenuation level)** — the guaranteed disturbance-to-error `L2` gain; smaller is more robust and harder to achieve. -- **generalized plant `(A, B1, B2, C1, D12)`** — splits inputs into disturbance/control and defines the error channel. -- **bisection bounds & tolerance** — the search window for `γ*` and its resolution. - -## Reference -J. Doyle, K. Glover, P. Khargonekar, B. Francis, "State-Space Solutions to Standard H₂ and H∞ Control -Problems," *IEEE Trans. Automatic Control*, 34(8), 1989. - -## See also -`DiscreteAlgebraicRiccatiEquation` (the inner solve); `Lqr` (the `γ → ∞` limit); -`Lqg` (stochastic, average-case counterpart); `SlidingModeControl` (nonlinear robustness alternative). diff --git a/roadmap/robust_control/HInfinityStateFeedback/implementation.md b/roadmap/robust_control/HInfinityStateFeedback/implementation.md deleted file mode 100644 index 8892bce1..00000000 --- a/roadmap/robust_control/HInfinityStateFeedback/implementation.md +++ /dev/null @@ -1,88 +0,0 @@ -# H∞ State-Feedback Control — Implementation Pseudocode - -> Roadmap ref: #46 (Tier 5) · Target: `numerical/robust_control` · Namespace `robust_control` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -``` -template # static_assert(std::is_floating_point_v); instantiated for float -class HInfinityStateFeedback: - math::LinearTimeInvariant plant # A, B2 (control), C1 - math::Matrix B1 # disturbance channel - math::Matrix D12 # error-vs-control weighting - math::Matrix K # state-feedback gain, u = -K·x - math::Matrix X # game-Riccati solution - T gamma # achieved disturbance attenuation -``` - -## Interface - -``` -HInfinityStateFeedback(const GeneralizedPlant& plant) # A,B1,B2,C1,D12 - -bool Synthesize(T gammaMin, T gammaMax, T tol) # bisection for smallest feasible γ -InputVector ComputeControl(const StateVector& x) const # hot path: u = -K·x -const Matrix& Gain() const -T Gamma() const -``` - -## Algorithm (pseudocode) - -``` -function Synthesize(gLo, gHi, tol): - # bisection: the smallest γ that still admits a stabilizing PSD Riccati solution - while gHi - gLo > tol: - g = (gLo + gHi) / 2 - if RiccatiFeasible(g): gHi = g # attenuation achievable -> tighten - else: gLo = g # infeasible -> relax - gamma = gHi - (X, K) = SolveGameRiccati(gamma) - return IsSchurStable(plant.A - plant.B2 * K) - -function SolveGameRiccati(g): # game-theoretic DARE (GARE) - # Two antagonistic inputs: control u minimizes, disturbance w maximizes. - # Stack B = [B2 | B1] with an INDEFINITE input weight R̃ = diag( I , -g²·I ). - B = HStack(plant.B2, B1) - X = DiscreteAlgebraicRiccatiEquation(plant.A, B, C1ᵀ*C1, R̃).Solve() # reuse DARE - Kfull = Inverse(R̃ + Bᵀ*X*B) * (Bᵀ*X*plant.A) # full [u;w] gain - K = ControlRows(Kfull) # keep the control (u) block only - return (X, K) - -function ComputeControl(x): # OPTIMIZE_FOR_SPEED - return K * x * (-1) # u = -K·x -``` - -## Complexity & memory - -- `Synthesize`: `O(log((γHi−γLo)/tol))` Riccati solves, each `O(StateSize³)`. -- `ComputeControl`: `O(ControlSize·StateSize)` — one matrix-vector product. -- Memory: `O(StateSize²)` for `X`, `B1`, and the gain — all static, no heap. - -## Numerical / embedded notes - -- **`γ` is the guaranteed L2 gain** from disturbance `w` to error `z`. Smaller `γ` = more robust but - harder to solve; bisection converges to the (sub)optimal `γ*`. -- **Feasibility** requires a PSD stabilizing `X` **and** `(−γ²I + B1ᵀX B1) ≺ 0` so the disturbance block - stays a genuine maximizer; `RiccatiFeasible` checks both before accepting `g`. -- As `γ → ∞` the antagonist vanishes and the solution **collapses to LQR** — H∞ is its robust generalization. -- **Reuse `DiscreteAlgebraicRiccatiEquation`** for the inner solve (items 29 & 31 for the matrix - algebra); never reimplement the Riccati recursion (DRY). -- Always verify `A − B2·K` is **Schur-stable** after synthesis; reject the design if not. -- Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a - `Q15`/`Q31` specialisation cheap to add later. - -## Deployment - -- Header: `numerical/robust_control/HInfinityStateFeedback.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `ComputeControl`, and - `extern template class HInfinityStateFeedback;` - under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `numerical/robust_control/HInfinityStateFeedback.cpp` → - `template class HInfinityStateFeedback;` -- Test: `numerical/robust_control/test/TestHInfinityStateFeedback.cpp` -- Doc: `doc/robust_control/HInfinityStateFeedback.md` (expand to follow `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `numerical_add_coverage_sources`; - `TestHInfinityStateFeedback.cpp` → the `_test` target. -- New module: create `numerical/robust_control/CMakeLists.txt` via `numerical_add_header_library(...)`, - add `test/`, register in `numerical/CMakeLists.txt`, add `doc/robust_control/`. -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/robust_control/HInfinityStateFeedback/tests.md b/roadmap/robust_control/HInfinityStateFeedback/tests.md deleted file mode 100644 index e8f2ee56..00000000 --- a/roadmap/robust_control/HInfinityStateFeedback/tests.md +++ /dev/null @@ -1,60 +0,0 @@ -# H∞ State-Feedback Control — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -``` -class TestHInfinityStateFeedback : public ::testing::Test: - # 2-state plant, 1 disturbance input, 1 control input, 1 error output - GeneralizedPlant plant = MakeGeneralizedPlant() - HInfinityStateFeedback hinf{ plant } -# each case below is a TEST_F(TestHInfinityStateFeedback, ) -``` - -## Test cases (Arrange / Act / Assert) - -``` -reduces_to_lqr_as_gamma_large: - Arrange: Synthesize with a very large γ range (weak antagonist) - Assert: K ≈ the LQR gain on (A, B2, C1ᵀC1, I) - -closed_loop_is_schur_stable: - Arrange: successful Synthesize - Assert: every eigenvalue of (A − B2·K) lies inside the unit disk - -achieves_target_gamma: - Arrange: synthesize at γ, simulate worst-case disturbance - Assert: ‖z‖ / ‖w‖ <= γ (attenuation met) - -bisection_finds_minimal_gamma: - Arrange: scalar/2-state plant with known optimal γ* - Assert: returned gamma ≈ γ* within tol - -infeasible_below_gamma_optimum: - Arrange: request γ < γ* - Assert: RiccatiFeasible(γ) == false; Synthesize reports failure - -gain_matches_gare_solution: - Arrange: solve the game Riccati directly for a known plant - Assert: K equals the control block of (R̃ + BᵀXB)^{-1} BᵀXA - -rejects_worst_case_disturbance: - Arrange: drive the loop with the maximizing disturbance - Assert: output energy bounded by γ²·‖w‖² - -compute_control_is_negative_feedback: - Arrange: nonzero state x - Assert: u == -K·x (correct sign) -``` - -## Reference vectors - -- Scalar plant: closed-form `γ*` and gain — the golden values for `bisection_finds_minimal_gamma`. -- `γ → ∞`: the H∞ gain converges to the LQR gain (golden LQR comparison). - -## Edge cases - -- `γ` below `γ*` — no PSD stabilizing solution; assert clean infeasibility, not divergence. -- Disturbance block `(−γ²I + B1ᵀX B1)` losing definiteness — assert `RiccatiFeasible` rejects `g`. -- Near `γ*` the Riccati is ill-conditioned — assert the bisection tolerance guards against it.