StateSpace.hpp

Go to the documentation of this file.

#pragma once
 
// std
#include <tuple>
#include <concepts>
 
// eigen
#include <Eigen/Core>
 
// controlpp
#include <controlpp/math.hpp>
#include <controlpp/TransferFunction.hpp>
 
namespace controlpp
{
 
    template<class T, int NStates, int NInputs, int NOutputs>
    class StateSpace{
        public:
            using value_type = T;
 
            using A_matrix_type = Eigen::Matrix<T, NStates, NStates>;
            using B_matrix_type = Eigen::Matrix<T, NStates, NInputs>;
            using C_matrix_type = Eigen::Matrix<T, NOutputs, NStates>;
            using D_matrix_type = Eigen::Matrix<T, NOutputs, NInputs>;
 
            //constexpr static int number_of_states = NStates;
            //constexpr static int number_of_inputs = NInputs;
            //constexpr static int number_of_outputs = NOutputs;
 
        private:
            A_matrix_type _A;
            B_matrix_type _B;
            C_matrix_type _C;
            D_matrix_type _D;
 
        public:
            StateSpace() = default;
            StateSpace(const StateSpace&) = default;
            StateSpace& operator=(const StateSpace&) = default;
 
            StateSpace(
                const Eigen::Matrix<T, NStates, NStates>& A,
                const Eigen::Matrix<T, NStates, NInputs>& B,
                const Eigen::Matrix<T, NOutputs, NStates>& C,
                const Eigen::Matrix<T, NOutputs, NInputs>& D
            )
                : _A(A)
                , _B(B)
                , _C(C)
                , _D(D){}
 
            std::tuple<Eigen::Vector<T, NStates>, Eigen::Vector<T, NOutputs>> eval(const Eigen::Vector<T, NStates>& x, const Eigen::Vector<T, NInputs>& u) const {
                const Eigen::Vector<T, NStates> result_x = this->A() * x + this->B() * u;
                const Eigen::Vector<T, NOutputs> result_y = this->C() * x + this->D() * u;
                return std::tuple(result_x, result_y);
            }
 
            template<std::convertible_to<T> U>
                requires(NInputs == 1 && NOutputs != 1)
            std::tuple<Eigen::Vector<U, NStates>, Eigen::Vector<U, NOutputs>> eval(const Eigen::Vector<U, NStates>& x, const U& u_scalar) const {
                const Eigen::Vector<T, 1> u(static_cast<T>(u_scalar));
                return this->eval(x, u);
            }
 
            template<std::convertible_to<T> U>
                requires(NInputs == 1 && NOutputs == 1)
            std::tuple<Eigen::Vector<U, NStates>, U> eval(const Eigen::Vector<U, NStates>& x, const U& u_scalar) const {
                const Eigen::Vector<T, 1> u(static_cast<T>(u_scalar));
                const auto [new_x, y] = this->eval(x, u);
                return {new_x, y(0)};
            }
 
            const A_matrix_type& A() const {return this->_A;}
            const B_matrix_type& B() const {return this->_B;}
            const C_matrix_type& C() const {return this->_C;}
            const D_matrix_type& D() const {return this->_D;}
 
            A_matrix_type& A() {return this->_A;}
            B_matrix_type& B() {return this->_B;}
            C_matrix_type& C() {return this->_C;}
            D_matrix_type& D() {return this->_D;}
 
            friend std::ostream& operator<<(std::ostream& stream, const StateSpace& state_space){
                stream << "A:\n" << state_space.A() << '\n';
                stream << "B:\n" << state_space.B() << '\n';
                stream << "C:\n" << state_space.C() << '\n';
                stream << "D:\n" << state_space.D() << '\n';
                return stream;
            }
    };
 
    template<class T, int NumOrder, int DenOrder>
    StateSpace<T, DenOrder, 1, 1> to_state_space_controller_norm(const TransferFunction<T, NumOrder, DenOrder>& rp){
        StateSpace<T, DenOrder, 1, 1> result;
        const T a_n = rp.den()[rp.den().order()];
 
        // normalise
        const Polynom<T, DenOrder> a = -(rp.den() / a_n);
        const Polynom<T, NumOrder> b = rp.num() / a_n;
        
        const T bn = (DenOrder > 0 && a.order() == b.order()) ? b.at(b.size()-1) : static_cast<T>(0);
 
        // write A matrix
        if constexpr (DenOrder > 0){
            const auto I = Eigen::Matrix<T, DenOrder-1, DenOrder-1>::Identity();
            result.A().template block<DenOrder-1, DenOrder-1>(0, 1) = I; 
            result.A().col(0).head(DenOrder-1).setZero(); 
            result.A().row(DenOrder-1) = a.vector().head(a.vector().size()-1);
        }
 
        // write B matrix
        if constexpr (DenOrder > 0){
            result.B().col(0).head(DenOrder-1).setZero();
            result.B()(DenOrder-1, 0) = T(1);
        }
        
        // write C matrix
        if constexpr (DenOrder > 0){
            const int l = (b.size() < DenOrder) ? b.size() : DenOrder;
            if(b.size() > (DenOrder)){
                result.C().row(0).head(l) = b.vector().head(l) + a.vector().head(l) * bn;
            }else{
                result.C().row(0).head(l) = b.vector().head(l);
                result.C().row(0).tail(DenOrder - b.size()).setZero();
            }
        }
        
        // write D matrix
        result.D()(0, 0) = (b.size() > (DenOrder)) ? bn : T(0);
        return result;
    }
 
    template<class T, int NumOrder, int DenOrder>
    StateSpace<T, DenOrder, 1, 1> to_state_space_observer_norm(const TransferFunction<T, NumOrder, DenOrder>& rp){
        StateSpace<T, DenOrder, 1, 1> result;
        const T a_n = rp.den()[rp.den().order()];
 
        // normalise
        const Polynom<T, DenOrder> a = -(rp.den() / a_n);
        const Polynom<T, NumOrder> b = rp.num() / a_n;
        
        const T bn = (DenOrder > 0 && a.order() == b.order()) ? b.at(b.size()-1) : static_cast<T>(0);
 
        // write A matrix
        if constexpr (DenOrder > 0){
            const auto I = Eigen::Matrix<T, DenOrder-1, DenOrder-1>::Identity();
            result.A().template block<DenOrder-1, DenOrder-1>(1, 0) = I; 
            result.A().row(0).head(DenOrder-1).setZero(); 
            result.A().col(DenOrder-1) = a.vector().head(a.vector().size()-1);
        }
 
        // write C matrix
        if constexpr (DenOrder > 0){
            result.C().row(0).head(DenOrder-1).setZero();
            result.C()(0, DenOrder-1) = T(1);
        }
        
        // write B matrix
        if constexpr (DenOrder > 0){
            const int l = (b.size() < DenOrder) ? b.size() : DenOrder;
            if(b.size() > (DenOrder)){
                result.B().col(0).head(l) = b.vector().head(l) + a.vector().head(l) * bn;
            }else{
                result.B().col(0).head(l) = b.vector().head(l);
                result.B().col(0).tail(DenOrder - b.size()).setZero();
            }
        }
        
        // write D matrix
        result.D()(0, 0) = (b.size() > (DenOrder)) ? bn : T(0);
        return result;
    }
 
    template<class T, int NumOrder, int DenOrder>
    StateSpace<T, DenOrder, 1, 1> to_state_space(const TransferFunction<T, NumOrder, DenOrder>& rp){
        return to_state_space_observer_norm(rp);
    }
 
    template<class T, int NRows, int NCols, int Opts, int NMaxRows, int NMaxCols>
    StateSpace<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic> 
    to_state_space(const Eigen::Matrix<TransferFunction<T, Eigen::Dynamic, Eigen::Dynamic>, NRows, NCols, Opts, NMaxRows, NMaxCols>& Mtf){
        // count the number of states for matrix pre-allocation
        int states = 0;
        for(int row = 0; row < Mtf.rows(); ++row){
            for(int col = 0; col < Mtf.cols(); ++col){
                states += Mtf.at(row, col).den().order();
            }
        }
 
        int inputs = Mtf.cols();
        int outputs = Mtf.rows();
 
        // allocate matrices
        Eigen::Matrix<T, Eigen::Dynamic,  Eigen::Dynamic> A(states, states); A.setZero();
        Eigen::Matrix<T, Eigen::Dynamic,  Eigen::Dynamic> B(states, inputs); B.setZero();
        Eigen::Matrix<T, Eigen::Dynamic,  Eigen::Dynamic> C(outputs, states); C.setZero();
        Eigen::Matrix<T, Eigen::Dynamic,  Eigen::Dynamic> D(outputs, inputs); D.setZero();
 
        // turn each transfer function into its state space representation and build the total system
        int state_itr = 0;
        for(int row = 0; row < Mtf.rows(); ++row){
            for(int col = 0; col < Mtf.cols(); ++col){
                const StateSpace<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic> ss = to_state_space(Mtf(row, col));
                A.block(state_itr, state_itr, ss.states(), ss.states()) = ss.A();
                B.block(state_itr, col, ss.states(), 1) = ss.B();
                C.block(row, state_itr, 1, ss.states()) = ss.C();
                D.block(row, col, 1, 1) = ss.D();
                state_itr += ss.states();
            }
        }
        return StateSpace<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic>(A, B, C, D);
    }
 
    template<class T, int states>
    TransferFunction<T, states+1, states+1> to_transfer_function(const StateSpace<T, states, 1, 1>& css){
        const FixedPolynom<T, states+1> s({0, 1});
        const auto I = Eigen::Matrix<T, states, states>::Identity();
        const Eigen::Matrix<FixedPolynom<T, states+1>, states, states> sI_min_A = s * I - css.A();
        const Eigen::Matrix<FixedPolynom<T, states+1>, states, states> adj_sI_min_A = controlpp::adj(sI_min_A);
        const FixedPolynom<T, states+1> num = (css.C() * adj_sI_min_A * css.B() + css.D())(0, 0);
        const FixedPolynom<T, states+1> den = sI_min_A.determinant();
        return TransferFunction<T, states+1, states+1>(num.vector(), den.vector());
    }
 
 
    
/*
    template<class T, int LStates, int Linputs, int Loutputs, int RStates, int Rinputs, int Routputs>
    StateSpace<...> operator+(const StateSpace<>& lhs, const StateSpace<>& rhs){
        // TODO:    
    }
 
    template<class T, int LStates, int Linputs, int Loutputs, int RStates, int Rinputs, int Routputs>
    StateSpace<...> operator-(const StateSpace<>& lhs, const StateSpace<>& rhs){
        // TODO:
    }
 
    template<class T, int LStates, int Linputs, int Loutputs, int RStates, int Rinputs, int Routputs>
    StateSpace<...> operator*(const StateSpace<>& lhs, const StateSpace<>& rhs){
        // TODO:
    }
 
    template<class T, int LStates, int Linputs, int Loutputs, int RStates, int Rinputs, int Routputs>
    StateSpace<...> operator/(const StateSpace<>& lhs, const StateSpace<>& rhs){
        // TODO:
    }
 
    template<class T, int LStates, int Linputs, int Loutputs, int RStates, int Rinputs, int Routputs>
    StateSpace<...> inverse(const StateSpace<>& Sys){
        // TODO:
    }
*/
} // nam