hinf_state_feedback.hpp

Go to the documentation of this file.

/*
 * Copyright © 2025-2026 Akreon LLC
 * SPDX-License-Identifier: Apache-2.0
 */
 
#pragma once
 
#include "akreon/linalg/assert.hpp"
#include "akreon/control/feedback/gain_recovery.hpp"
#include "akreon/control/model/plant_model.hpp"
#include "akreon/convex/composed/weighted_sum.hpp"
#include "akreon/convex/convex_problem.hpp"
#include "akreon/convex/intersection.hpp"
#include "akreon/convex/sets/lmi.hpp"
#include "akreon/convex/variable.hpp"
#include "akreon/linalg/operator/matrix_inverse.hpp"
#include "akreon/linalg/operator/matrix_pencil.hpp"
 
namespace control {
 
template <ContinuousPlantModelLike SFModel>
struct HinfStateFeedback {
  using Model = linalg::remove_cvref_t<SFModel>;
 
  using T = plant_type_t<Model>;
 
  static constexpr std::size_t StateDim = plant_traits<Model>::StateDim;
  static constexpr PortRole ExogenousPortRole = PortRole::EXOGENOUS;
  static constexpr std::size_t InputDim = plant_traits<Model>::ControlDim;
  static constexpr std::size_t ExogenousDim = plant_traits<Model>::ExogenousDim;
  static constexpr std::size_t OutputDim = plant_traits<Model>::PerformanceDim;
 
  using QMat = linalg::Matrix<T, StateDim, StateDim>;
  using YMat = linalg::Matrix<T, InputDim, StateDim>;
  using KMat = linalg::Matrix<T, InputDim, StateDim>;
 
  static constexpr std::size_t QDim = StateDim * (StateDim + 1) / 2;
  static constexpr std::size_t YDim = InputDim * StateDim;
  static constexpr std::size_t GammaDim = 1;
  static constexpr std::size_t DecVarDim = QDim + YDim + GammaDim;
 
  using DecVarSpace = linalg::RowVector<T, DecVarDim>;
 
  template <convex::AtomLike DecVar>
  constexpr auto operator()(const SFModel& model, const DecVar& Z_hat,
                            T margin = 1e-6) const {
    return apply(model, Z_hat, margin);
  }
 
  template <convex::AtomLike DecVar>
  constexpr auto apply(const SFModel& model, const DecVar& Z_hat,
                       T margin = 1e-6) const {
    static_assert(convex::is_var_type_v<DecVar>, "Input atom is not a var");
    using DecVarDom = convex::meta_dom_t<convex::atom_meta_t<DecVar>>;
    static_assert(convex::dom_vartype_v<DecVarDom> == convex::VarType::FREE,
                  "Merged decision var must be free");
    static_assert(std::same_as<DecVarSpace, convex::dom_space_t<DecVarDom>>,
                  "Expected merged decision size [svec(Q); vec(Y); gamma]");
 
    const auto q_lmi = q_constraint(Z_hat, margin);
    const auto brl_lmi = brl_constraint(model, Z_hat, margin);
    return convex::Intersection(q_lmi, brl_lmi);
  }
 
  template <convex::AtomLike DecVar>
  constexpr auto q_constraint(const DecVar& Z_hat, T margin = 1e-6) const {
    using FMat = QMat;
    using Fs = linalg::Vector<FMat, DecVarDim>;
 
    const auto F_q =
        linalg::MatrixPencil<FMat, QDim>{}.template apply_sym_mat<QMat>(
            [](const QMat& E) -> FMat { return E; });
 
    Fs F_blks{};
    for (std::size_t i = 0; i < QDim; ++i) F_blks(i) = F_q(i);
 
    const auto I = linalg::make_identity<T, StateDim>();
    return convex::LMI(F_blks, T(-1) * margin * I, Z_hat);
  }
 
  template <convex::AtomLike DecVar>
  constexpr auto brl_constraint(const SFModel& model, const DecVar& Z_hat,
                                T margin = 1e-6) const {
    constexpr std::size_t StateEnd = StateDim;
    constexpr std::size_t DisturbanceEnd = StateDim + ExogenousDim;
    constexpr std::size_t LmiDim = StateDim + ExogenousDim + OutputDim;
 
    using FMat = linalg::Matrix<T, LmiDim, LmiDim>;
    using Fs = linalg::Vector<FMat, DecVarDim>;
 
    auto M_q = [&](const QMat& E) -> FMat {
      FMat F{};
      const auto A = model.dynamics_matrix();
      const auto C_z = model.performance_matrix();
      F = linalg::set_block<0, StateEnd, 0, StateEnd>(
          F, T(-1) * (A * E + E * linalg::transpose(A)));
      F = linalg::set_block<0, StateEnd, DisturbanceEnd, LmiDim>(
          F, T(-1) * linalg::transpose(C_z * E));
      F = linalg::set_block<DisturbanceEnd, LmiDim, 0, StateEnd>(
          F, T(-1) * C_z * E);
      return F;
    };
 
    auto M_y = [&](const YMat& E) -> FMat {
      FMat F{};
      const auto B_u = model.control_matrix();
      const auto D_u =
          model
              .template feedthrough<PortRole::PERFORMANCE, PortRole::CONTROL>();
      F = linalg::set_block<0, StateEnd, 0, StateEnd>(
          F, T(-1) * (B_u * E + linalg::transpose(E) * linalg::transpose(B_u)));
      F = linalg::set_block<0, StateEnd, DisturbanceEnd, LmiDim>(
          F, T(-1) * linalg::transpose(D_u * E));
      F = linalg::set_block<DisturbanceEnd, LmiDim, 0, StateEnd>(
          F, T(-1) * D_u * E);
      return F;
    };
 
    auto M_gamma = [](const linalg::Matrix<T, 1, 1>& E) -> FMat {
      FMat F{};
      const auto gamma = E(0, 0);
      const auto I_w = linalg::make_identity<T, ExogenousDim>();
      const auto I_z = linalg::make_identity<T, OutputDim>();
      F = linalg::set_block<StateEnd, DisturbanceEnd, StateEnd, DisturbanceEnd>(
          F, gamma * I_w);
      F = linalg::set_block<DisturbanceEnd, LmiDim, DisturbanceEnd, LmiDim>(
          F, gamma * I_z);
      return F;
    };
 
    const auto F_q =
        linalg::MatrixPencil<FMat, QDim>{}.template apply_sym_mat<QMat>(M_q);
    const auto F_y =
        linalg::MatrixPencil<FMat, YDim>{}.template apply_full<YMat>(M_y);
    const auto F_gamma =
        linalg::MatrixPencil<FMat, GammaDim>{}
            .template apply_full<linalg::Matrix<T, 1, 1>>(M_gamma);
 
    Fs F_blks{};
    for (std::size_t i = 0; i < QDim; ++i) F_blks(i) = F_q(i);
    for (std::size_t i = 0; i < YDim; ++i) F_blks(QDim + i) = F_y(i);
    F_blks(QDim + YDim) = F_gamma(0);
 
    FMat F_0{};
    const auto B_w = model.exogenous_matrix();
    const auto D_w =
        model
            .template feedthrough<PortRole::PERFORMANCE, PortRole::EXOGENOUS>();
    F_0 = linalg::set_block<0, StateEnd, StateEnd, DisturbanceEnd>(F_0,
                                                                   T(-1) * B_w);
    F_0 = linalg::set_block<StateEnd, DisturbanceEnd, 0, StateEnd>(
        F_0, T(-1) * linalg::transpose(B_w));
    F_0 = linalg::set_block<StateEnd, DisturbanceEnd, DisturbanceEnd, LmiDim>(
        F_0, T(-1) * linalg::transpose(D_w));
    F_0 = linalg::set_block<DisturbanceEnd, LmiDim, StateEnd, DisturbanceEnd>(
        F_0, T(-1) * D_w);
 
    const auto I = linalg::make_identity<T, LmiDim>();
    return convex::LMI(F_blks, F_0 + T(-1) * margin * I, Z_hat);
  }
 
  template <convex::AtomLike DecVar>
  constexpr auto objective(const DecVar& Z_hat) const {
    linalg::Vector<T, DecVarDim> weights{};
    weights(QDim + YDim) = T(1);
    return convex::make_weighted_sum(weights, Z_hat);
  }
 
  constexpr auto K(const QMat& Q, const YMat& Y) const {
    return recover_state_feedback_gain(Q, Y);
  }
};
 
template <ContinuousPlantModelLike SFModel, convex::AtomLike DecVar>
constexpr auto make_hinf_state_feedback_problem(
    const SFModel& model, const DecVar& Z_hat,
    plant_type_t<SFModel> margin = 1e-6) {
  const auto hinf = HinfStateFeedback<decltype(model)>();
  const auto lmis = hinf(model, Z_hat, margin);
  const auto prob = convex::make_problem(Z_hat).in_set(lmis);
  const auto obj = hinf.objective(Z_hat);
  return prob.minimize(obj);
}
 
} // namespace control