ArcsControl.hh

[詳解]

 
//
// Copyright (C) 2011-2022 Yokokura, Yuki
// This program is free software;
// you can redistribute it and/or modify it under the terms of the FreeBSD License.
// For details, see the License.txt file.
 
#ifndef ARCSCONTROL
#define ARCSCONTROL
 
#include <cassert>
#include <tuple>
#include "Matrix.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名前空間
class ArcsControl {
    public:
        template<size_t NN, size_t MM>
        static constexpr void Lyapunov(const Matrix<NN,MM,double>& A, const Matrix<NN,MM,double>& Q, Matrix<NN,MM,double>& X){
            static_assert(NN == MM);                    // 正方行列のみ対応
            constexpr auto I = Matrix<NN,NN>::eye();    // 単位行列
            X = Matrix<1,NN*MM>::template vecinv<NN,MM>( -inv( Kronecker(I,A) + Kronecker(A,I) )*vec(Q) );  // A*X + X*A^T + Q = 0 をXについて解く
        }
        
        template<size_t NN, size_t MM>
        static constexpr Matrix<NN,MM,double> Lyapunov(const Matrix<NN,MM,double>& A, const Matrix<NN,MM,double>& Q){
            Matrix<NN,MM,double> X;
            Lyapunov(A, Q, X);
            return X;
        }
        
        template<size_t NA, size_t MA, size_t NB>
        static constexpr void GramianCtrl(const Matrix<NA,MA>& A, const Matrix<NB,MA>& b, Matrix<NA,MA>& Wc){
            Lyapunov(A, b*tp(b), Wc);
        }
        
        template<size_t NA, size_t MA, size_t NB>
        static constexpr Matrix<NA,MA> GramianCtrl(const Matrix<NA,MA>& A, const Matrix<NB,MA>& b){
            Matrix<NA,MA> Wc;
            GramianCtrl(A, b, Wc);
            return Wc;
        }
        
        template<size_t NA, size_t MA, size_t MC>
        static constexpr void GramianObsrv(const Matrix<NA,MA>& A, const Matrix<NA,MC>& c, Matrix<NA,MA>& Wo){
            Lyapunov(tp(A), tp(c)*c, Wo);
        }
        
        template<size_t NA, size_t MA, size_t MC>
        static constexpr Matrix<NA,MA> GramianObsrv(const Matrix<NA,MA>& A, const Matrix<NA,MC>& c){
            Matrix<NA,MA> Wo;
            GramianObsrv(A, c, Wo);
            return Wo;
        }
        
        template<size_t NA, size_t MA, size_t NB, size_t MC>
        static constexpr void BalanceReal(const Matrix<NA,MA>& A, const Matrix<NB,MA>& b, const Matrix<NA,MC>& c, Matrix<NA,MA>& Ah, Matrix<NB,MA>& bh, Matrix<NA,MC>& ch){
            static_assert(NA == MA);    // A行列は正方行列
            
            // 1. グラミアンズの計算
            const Matrix<NA,MA> Wc = GramianCtrl(A, b);     // 可制御グラミアン
            const Matrix<NA,MA> Wo = GramianObsrv(A, c);    // 可観測グラミアン
            
            // 2. 1をコレスキー分解
            Matrix<NA,MA> Lc, Lo;
            Cholesky(Wc, Lc);
            Cholesky(Wo, Lo);
            
            // 3. 2を特異値分解する
            const auto [U, S, V] = SVD( tp(Lo)*Lc );
            
            // 4. 3から変換行列を計算する
            const Matrix<NA,MA> T  = inv(sqrte(S))*tp(U)*tp(Lo);
            const Matrix<NA,MA> Ti = Lc*V*inv(sqrte(S));
            
            // 5. 4を使って状態空間モデルを変換する
            Ah = T*A*Ti;
            bh = T*b;
            ch = c*Ti;
            
            // 下記はデバッグ用
            //PrintMat(T*Wc*tp(T) - tp(Ti)*Wo*Ti);  // 零行列になればOK
            //PrintMat(T*Ti);                       // 単位行列になればOK
        }
        
        template<size_t NA, size_t MA, size_t NB, size_t MC>
        static constexpr std::tuple<Matrix<NA,MA>, Matrix<NB,MA>, Matrix<NA,MC>> BalanceReal(const Matrix<NA,MA>& A, const Matrix<NB,MA>& b, const Matrix<NA,MC>& c){
            Matrix<NA,MA> Ah;
            Matrix<NB,MA> bh;
            Matrix<NA,MC> ch;
            BalanceReal(A, b, c, Ah, bh, ch);
            return {Ah, bh, ch};
        }
        
        template <size_t N, size_t M>
        static constexpr void Discretize(
            const Matrix<N,N>& Ac, const Matrix<M,N>& Bc, Matrix<N,N>& Ad, Matrix<M,N>& Bd, const double Ts, const size_t Npade = 13, const size_t Nint = 10000
        ){
            Ad = expm(Ac*Ts, Npade);                    // A行列の離散化
            Bd = integral_expm(Ac, Ts, Nint, Npade)*Bc; // B行列の離散化
        }
        
        template <size_t N, size_t M>
        static constexpr std::tuple<Matrix<N,N>, Matrix<M,N>> Discretize(
            const Matrix<N,N>& Ac, const Matrix<M,N>& Bc, const double Ts, const size_t Npade = 13, const size_t Nint = 10000
        ){
            Matrix<N,N> Ad;
            Matrix<M,N> Bd;
            Discretize(Ac, Bc, Ad, Bd, Ts, Npade, Nint);
            return {Ad, Bd};
        }
        
    private:
        ArcsControl() = delete;                     
        ArcsControl(ArcsControl&& r) = delete;      
        ~ArcsControl() = delete;                    
        ArcsControl(const ArcsControl&) = delete;   
        const ArcsControl& operator=(const ArcsControl&) = delete;  
};
}
 
#endif