h2_state_observer.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/trace.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 ObsModel>
struct H2StateObserver {
  using Model = linalg::remove_cvref_t<ObsModel>;
 
  using T = plant_type_t<Model>;
 
  static constexpr std::size_t StateDim = plant_traits<Model>::StateDim;
  static constexpr std::size_t ExogenousDim = plant_traits<Model>::ExogenousDim;
  static constexpr std::size_t MeasurementDim =
      plant_traits<Model>::MeasurementDim;
  static constexpr std::size_t PerformanceDim =
      plant_traits<Model>::PerformanceDim;
 
  using QMat = linalg::Matrix<T, StateDim, StateDim>;
  using WMat = linalg::Matrix<T, StateDim, MeasurementDim>;
  using LMat = linalg::Matrix<T, StateDim, MeasurementDim>;
  using ZMat = linalg::Matrix<T, ExogenousDim, ExogenousDim>;
 
  static constexpr std::size_t QDim = StateDim * (StateDim + 1) / 2;
  static constexpr std::size_t WDim = StateDim * MeasurementDim;
  static constexpr std::size_t ZDim = ExogenousDim * (ExogenousDim + 1) / 2;
  static constexpr std::size_t DecVarDim = QDim + WDim + ZDim;
 
  using DecVarSpace = linalg::RowVector<T, DecVarDim>;
 
  template <convex::AtomLike DecVar>
  constexpr auto operator()(const ObsModel& model, const DecVar& Z_hat,
                            T margin = 1e-6) const {
    return apply(model, Z_hat, margin);
  }
 
  template <convex::AtomLike DecVar>
  constexpr auto apply(const ObsModel& 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(W); svec(Z)]");
 
    const auto stability_lmi = stability_constraint(model, Z_hat);
    const auto z_lmi = z_constraint(model, Z_hat, margin);
    return convex::Intersection(z_lmi, stability_lmi);
  }
 
  template <convex::AtomLike DecVar>
  constexpr auto stability_constraint(const ObsModel& model,
                                      const DecVar& Z_hat) const {
    using FMat = QMat;
    using Fs = linalg::Vector<FMat, DecVarDim>;
 
    auto M_q = [&](const QMat& E) -> FMat {
      const auto& A = model.dynamics_matrix();
      return T(-1) * (linalg::transpose(A) * E + E * A);
    };
 
    auto M_w = [&](const WMat& E) -> FMat {
      const auto& C = model.measurement_matrix();
      return linalg::transpose(C) * linalg::transpose(E) + E * C;
    };
 
    const auto C_e = model.performance_matrix();
    const auto F_0 = T(-1) * (linalg::transpose(C_e) * C_e);
 
    const auto F_q_local =
        linalg::MatrixPencil<FMat, QDim>{}.template apply_sym_mat<QMat>(M_q);
    const auto F_w_local =
        linalg::MatrixPencil<FMat, WDim>{}.template apply_full<WMat>(M_w);
 
    Fs F_blks{};
    for (std::size_t i = 0; i < QDim; ++i) F_blks(i) = F_q_local(i);
    for (std::size_t i = 0; i < WDim; ++i) F_blks(QDim + i) = F_w_local(i);
 
    return convex::LMI(F_blks, F_0, Z_hat);
  }
 
  template <convex::AtomLike DecVar>
  constexpr auto z_constraint(const ObsModel& model, const DecVar& Z_hat,
                              T margin = 1e-6) const {
    constexpr std::size_t LmiDim = ExogenousDim + StateDim;
    using FMat = linalg::Matrix<T, LmiDim, LmiDim>;
    using Fs = linalg::Vector<FMat, DecVarDim>;
    auto M_z = [](const ZMat& E) -> FMat {
      FMat F{};
      return linalg::set_block<0, ExogenousDim, 0, ExogenousDim>(F, E);
    };
    auto M_q = [&](const QMat& E) -> FMat {
      FMat F{};
      const auto B_w = model.exogenous_matrix();
      F = linalg::set_block<ExogenousDim, LmiDim, ExogenousDim, LmiDim>(F, E);
      F = linalg::set_block<0, ExogenousDim, ExogenousDim, LmiDim>(
          F, linalg::transpose(B_w) * E);
      F = linalg::set_block<ExogenousDim, LmiDim, 0, ExogenousDim>(F, E * B_w);
      return F;
    };
    auto M_w = [&](const WMat& E) -> FMat {
      FMat F{};
      const auto D_w = model.template feedthrough<PortRole::MEASUREMENT,
                                                  PortRole::EXOGENOUS>();
      F = linalg::set_block<0, ExogenousDim, ExogenousDim, LmiDim>(
          F, T(-1) * linalg::transpose(D_w) * linalg::transpose(E));
      F = linalg::set_block<ExogenousDim, LmiDim, 0, ExogenousDim>(
          F, T(-1) * E * D_w);
      return F;
    };
 
    const auto F_z_local =
        linalg::MatrixPencil<FMat, ZDim>{}.template apply_sym_mat<ZMat>(M_z);
    const auto F_q_local =
        linalg::MatrixPencil<FMat, QDim>{}.template apply_sym_mat<QMat>(M_q);
    const auto F_w_local =
        linalg::MatrixPencil<FMat, WDim>{}.template apply_full<WMat>(M_w);
 
    Fs F_blks{};
    for (std::size_t i = 0; i < QDim; ++i) F_blks(i) = F_q_local(i);
    for (std::size_t i = 0; i < WDim; ++i) F_blks(QDim + i) = F_w_local(i);
    for (std::size_t i = 0; i < ZDim; ++i)
      F_blks(QDim + WDim + i) = F_z_local(i);
 
    const auto I = linalg::make_identity<T, LmiDim>();
    return convex::LMI(F_blks, T(-1) * margin * I, Z_hat);
  }
 
  template <convex::AtomLike DecVar>
  constexpr auto objective(const DecVar& Z_hat) const {
    return convex::make_trace_block<QDim + WDim, linalg::SymmetricMatrix<ZMat>>(
        Z_hat);
  }
 
  constexpr auto L(const QMat& Q, const WMat& W) const {
    return recover_state_observer_gain(Q, W);
  }
};
 
template <ContinuousPlantModelLike ObsModel, convex::AtomLike DecVar>
constexpr auto make_h2_state_observer_problem(
    const ObsModel& model, const DecVar& Z_hat,
    plant_type_t<ObsModel> margin = 1e-6) {
  const auto h2 = H2StateObserver<decltype(model)>();
  const auto lmis = h2(model, Z_hat, margin);
  const auto prob = convex::make_problem(Z_hat).in_set(lmis);
  const auto obj = h2.objective(Z_hat);
  return prob.minimize(obj);
}
 
} // namespace control