linear_system.h

#pragma once
 
#include "core/math/eigen_interface.h"
#include "core/math/systems/discretization.h"

template <int STATES, int INPUTS, int OUTPUTS>
class LinearSystem {
  public:
    using MatrixA = EMat<STATES, STATES>;
    using MatrixB = EMat<STATES, INPUTS>;
    using MatrixC = EMat<OUTPUTS, STATES>;
    using MatrixD = EMat<OUTPUTS, INPUTS>;
 
    using VectorX = EVec<STATES>;
    using VectorU = EVec<INPUTS>;
    using VectorY = EVec<OUTPUTS>;

    LinearSystem(const MatrixA &A, const MatrixB &B, const MatrixC &C, const MatrixD &D) : m_Ac(A), m_Bc(B), m_C(C), m_D(D) {}

    MatrixA A() { return m_Ac; }

    MatrixB B() { return m_Bc; }

    const std::tuple<std::tuple<MatrixA, MatrixB>> &discAB(const double &dt) { return discretize_AB(m_Ac, m_Bc, dt); }

    MatrixC C() { return m_C; }

    MatrixD D() { return m_D; }

    VectorX compute_X(const VectorX &x, const VectorU &u, double dt) {
        // Discretize A and B
        auto [Ad, Bd] = discretize_AB(m_Ac, m_Bc, dt);
 
        return Ad * x + Bd * u;
    }

    VectorY compute_Y(const VectorX &x, const VectorU &u) {
        return m_C * x + m_D * u;
    }
 
  private:
    // Continuous system matrix
    MatrixA m_Ac;
    // Continuous input matrix
    MatrixB m_Bc;
    // Output matrix
    MatrixC m_C;
    // Feedthrough matrix
    MatrixD m_D;
};