StateSpaceSystem.hh

[詳解]

 
//
// Copyright (C) 2011-2023 Yokokura, Yuki
// MIT License. For details, see the LICENSE file.
 
#ifndef STATESPACESYSTEM
#define STATESPACESYSTEM
 
#include <cassert>
#include "Matrix.hh"
#include "Discret.hh"
#include "ArcsControl.hh"
 
// ARCS組込み用マクロ
#ifdef ARCS_IN
    // ARCSに組み込まれる場合
    #include "ARCSassert.hh"
    #include "ARCSeventlog.hh"
#else
    // ARCSに組み込まれない場合
    #define arcs_assert(a) (assert(a))
    #define PassedLog()
    #define EventLog(a)
    #define EventLogVar(a)
#endif
 
namespace ARCS {    // ARCS名前空間
template <size_t N, size_t I = 1, size_t O = 1>
class StateSpaceSystem {
    public:
        StateSpaceSystem(void)
            : Ad(), Bd(), Cd(), Dd(), u(), x(), x_next(), y(), y_next(), DirectTerm(true)
        {
            PassedLog();
        }
        
        StateSpaceSystem(const Matrix<N,N>& A, const Matrix<I,N>& B, const Matrix<N,O>& C, const double Ts)
            : Ad(), Bd(), Cd(), Dd(), u(), x(), x_next(), y(), y_next(), DirectTerm(true)
        {
            SetContinuous(A, B, C, Ts);     // 連続系のA行列，B行列，C行列を設定して離散化
            PassedLog();
        }
        
        StateSpaceSystem(const Matrix<N,N>& A, const Matrix<I,N>& B, const Matrix<N,O>& C, const Matrix<I,O>& D, const double Ts)
            : Ad(), Bd(), Cd(), Dd(), u(), x(), x_next(), y(), y_next(), DirectTerm(true)
        {
            SetContinuous(A, B, C, D, Ts);  // 連続系のA行列，B行列，C行列，D行列を設定して離散化
            PassedLog();
        }
        
        StateSpaceSystem(StateSpaceSystem&& r)
            : Ad(r.Ad), Bd(r.Bd), Cd(r.Cd), Dd(r.Dd), u(r.u), x(r.x), x_next(r.x_next), y(r.y), y_next(r.y_next), DirectTerm(r.DirectTerm)
        {
            
        }
 
        ~StateSpaceSystem(){
            PassedLog();
        }
        
        void SetContinuous(const Matrix<N,N>& A, const Matrix<I,N>& B, const Matrix<N,O>& C, const double Ts){
            if constexpr(N == 1){
                // 次数が1次のときは平衡化せずに離散化
                auto Ah = A;
                auto Bh = B;
                auto Ch = C;
                std::tie(Ad, Bd) = ArcsControl::Discretize(Ah, Bh, Ts); // 離散化
                Cd = Ch;            // C行列はそのまま
            }else{
                // 次数が1次より高いときは平衡化してから離散化
                auto [Ah, Bh, Ch] = ArcsControl::BalanceReal(A, B, C);  // 平衡化
                std::tie(Ad, Bd) = ArcsControl::Discretize(Ah, Bh, Ts); // 離散化
                Cd = Ch;            // C行列は平衡化後そのまま
            }
            DirectTerm = false;     // 直達項は無し
        }
        
        void SetContinuous(const Matrix<N,N>& A, const Matrix<I,N>& B, const Matrix<N,O>& C, const Matrix<I,O>& D, const double Ts){
            SetContinuous(A, B, C, Ts);
            Dd = D;             // D行列は何もせずそのまま
            DirectTerm = true;  // 直達項は有り
        }
        
        void SetDiscrete(const Matrix<N,N>& A, const Matrix<I,N>& B, const Matrix<N,O>& C){
            Ad = A;
            Bd = B;
            Cd = C;
            DirectTerm = false; // 直達項は無し
        }
        
        void SetDiscrete(const Matrix<N,N>& A, const Matrix<I,N>& B, const Matrix<N,O>& C, const Matrix<I,O>& D){
            Ad = A;
            Bd = B;
            Cd = C;
            Dd = D;
            DirectTerm = true;  // 直達項は有り
        }
        
        void SetInput(const Matrix<1,I>& uin){
            u = uin;
        }
        
        void SetInput(const double uin){
            static_assert(I == 1);  // 1入力系かチェック
            u[1] = uin;
        }
        
        void SetInput(const double u1, const double u2){
            static_assert(I == 2);  // 2入力系かチェック
            u[1] = u1;
            u[2] = u2;
        }
        
        void Update(void){
            x_next = Ad*x + Bd*u;           // 状態方程式
            if(DirectTerm == false){
                y = Cd*x;                   // 直達項なし版の出力方程式
                y_next = Cd*x_next;         // 直達項なし版の出力方程式(次の時刻の出力ベクトルを即時に返す場合用)
            }else{
                y = Cd*x + Dd*u;            // 直達項あり版の出力方程式
                y_next = Cd*x_next + Dd*u;  // 直達項あり版の出力方程式(次の時刻の出力ベクトルを即時に返す場合用)
            }
            x = x_next;                     // 状態ベクトルを更新
        }
        
        void GetOutput(Matrix<1,O>& yout){
            yout = y;
        }
        
        Matrix<1,O> GetOutput(void){
            return y;
        }
        
        double GetOutput(size_t i){
            return y[i];
        }
        
        void GetNextOutput(Matrix<1,O>& yout){
            yout = y_next;
        }
        
        Matrix<1,O> GetNextOutput(void){
            return y_next;
        }
        
        double GetNextOutput(size_t i){
            return y_next[i];
        }
        
        void GetResponses(const Matrix<1,I>& uin, Matrix<1,O>& yout){
            u = uin;    // 入力ベクトルを設定
            Update();   // 状態空間モデルの応答を計算して状態ベクトルを更新
            yout = y;   // 出力ベクトルを返す
        }
        
        Matrix<1,O> GetResponses(const Matrix<1,I>& uin){
            Matrix<1,O> yout;           // 出力ベクトル
            GetResponses(uin, yout);    // 応答計算
            return yout;                // 出力ベクトルを返す
        }
        
        double GetResponse(const double uin){
            static_assert(I == 1 && O == 1);// SISOかチェック
            Matrix<1,I> u_vec;              // 入力ベクトル
            Matrix<1,O> y_vec;              // 出力ベクトル
            u_vec[1] = uin;
            GetResponses(u_vec, y_vec);     // 応答計算
            return y_vec[1];                // 出力を返す
        }
        
        double GetResponse(const double u1, const double u2){
            static_assert(I == 2 && O == 1);// 2入力1出力かチェック
            Matrix<1,I> u_vec;              // 入力ベクトル
            Matrix<1,O> y_vec;              // 出力ベクトル
            u_vec[1] = u1;
            u_vec[2] = u2;
            GetResponses(u_vec, y_vec);     // 応答計算
            return y_vec[1];                // 出力を返す
        }
        
        void GetNextResponses(const Matrix<1,I>& uin, Matrix<1,O>& yout){
            u = uin;        // 入力ベクトルを設定
            Update();       // 状態空間モデルの応答を計算して状態ベクトルを更新
            yout = y_next;  // 次の時刻の出力ベクトルを返す
        }
        
        Matrix<1,O> GetNextResponses(const Matrix<1,I>& uin){
            Matrix<1,O> yout;           // 出力ベクトル
            GetNextResponses(uin, yout);    // 応答計算
            return yout;                // 出力ベクトルを返す
        }
        
        double GetNextResponse(const double uin){
            static_assert(I == 1 && O == 1);// SISOかチェック
            Matrix<1,I> u_vec;              // 入力ベクトル
            Matrix<1,O> y_vec;              // 出力ベクトル
            u_vec[1] = uin;
            GetNextResponses(u_vec, y_vec); // 応答計算
            return y_vec[1];                // 出力を返す
        }
        
        double GetNextResponse(const double u1, const double u2){
            static_assert(I == 2 && O == 1);// 2入力1出力かチェック
            Matrix<1,I> u_vec;              // 入力ベクトル
            Matrix<1,O> y_vec;              // 出力ベクトル
            u_vec[1] = u1;
            u_vec[2] = u2;
            GetNextResponses(u_vec, y_vec); // 応答計算
            return y_vec[1];                // 出力を返す
        }
        
        void ClearStateVector(void){
            x = Matrix<1,N>::zeros();
        }
        
    private:
        StateSpaceSystem(const StateSpaceSystem&) = delete;                 
        const StateSpaceSystem& operator=(const StateSpaceSystem&) = delete;
        Matrix<N,N> Ad;     
        Matrix<I,N> Bd;     
        Matrix<N,O> Cd;     
        Matrix<I,O> Dd;     
        Matrix<1,I> u;      
        Matrix<1,N> x;      
        Matrix<1,N> x_next; 
        Matrix<1,O> y;      
        Matrix<1,O> y_next; 
        bool DirectTerm;    
};
}
 
#endif