Observer.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 OBSERVER
#define OBSERVER
 
#include <cassert>
#include <complex>
#include "Matrix.hh"
#include "StateSpaceSystem.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>
class Observer {
    public:
        Observer(void)
            : ObsrvSys()
        {
            PassedLog();
        }
        
        Observer(const Matrix<N,N>& A, const Matrix<1,N>& b, const Matrix<N,1>& c)
            : ObsrvSys()
        {
            Matrix<1,N,std::complex<double>> lambda = eigen(A);
            PrintMat(tp(lambda));
            PassedLog();
        }
        
        Observer(Observer&& r)
                : ObsrvSys(std::move(r.ObsrvSys))
        {
            
        }
 
        ~Observer(){
            PassedLog();
        }
        
        void SetPlantModelAndGain(const Matrix<N,N>& A, const Matrix<1,N>& b, const Matrix<N,1>& c, const Matrix<1,N>& k, const double Ts){
            const Matrix<N,N> Ao = A - k*c;         // オブザーバの連続系A行列
            Matrix<2,N> Bo;                         // オブザーバの連続系B行列
            setcolumn(Bo, b, 1);                    // オブザーバの連続系B行列1列目
            setcolumn(Bo, k, 2);                    // オブザーバの連続系B行列2列目
            const auto Co = Matrix<N,N>::eye();     // オブザーバの出力行列
            ObsrvSys.SetContinuous(Ao, Bo, Co, Ts); // オブザーバの状態空間モデルを設定
        }
        
        void Estimate(const Matrix<1,2>& u, Matrix<1,N>& xhat){
            ObsrvSys.GetResponses(u, xhat);         // 推定演算
        }
        
        Matrix<1,N> Estimate(const Matrix<1,2>& u){
            return ObsrvSys.GetResponses(u);        // 推定演算
        }
        
    private:
        Observer(const Observer&) = delete;                 
        const Observer& operator=(const Observer&) = delete;
        StateSpaceSystem<N,2,N> ObsrvSys;   
};
}
 
#endif