TwoInertiaStateObsrv.hh

[詳解]

 
//
// Copyright (C) 2011-2020 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 TWOINERTIASTATEOBSRV
#define TWOINERTIASTATEOBSRV
 
#include <cassert>
#include <tuple>
#include <Matrix.hh>
#include <Discret.hh>
#include "TwoInertiaParamDef.hh"
#include "Observer.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 = 1>
class TwoInertiaStateObsrv {
    public:
        TwoInertiaStateObsrv(const struct TwoInertiaParamDef& Params, const double Bandwidth, const double SmplTime)
            : Kt(Params.Kt),
              Jm(Params.Jm),
              Dm(Params.Dm),
              Jl(Params.Jl),
              Dl(Params.Dl),
              Ks(Params.Ks),
              Rg(Params.Rg),
              g(Bandwidth),
              Ts(SmplTime),
              A(),
              b(),
              c(),
              k(),
              StateObsrv()
        {
            CalcPlantStateModel();  // プラント状態空間モデルの計算
            CalcObserverGain();     // オブザーバゲインの計算
            StateObsrv.SetPlantModelAndGain(A, b, c, k, Ts);    // オブザーバの状態空間モデルの計算
            PassedLog();
        }
        
        TwoInertiaStateObsrv(TwoInertiaStateObsrv&& r)
            : Kt(r.Kt),
              Jm(r.Jm),
              Dm(r.Dm),
              Jl(r.Jl),
              Dl(r.Dl),
              Ks(r.Ks),
              Rg(r.Rg),
              g(r.g),
              Ts(r.Ts),
              A(r.A),
              b(r.b),
              c(r.c),
              k(r.k),
              StateObsrv(std::move(r.StateObsrv))
        {
            
        }
        
        ~TwoInertiaStateObsrv(){
            PassedLog();
        }
        
        void GetEstimatedVect(const double Current, const double Velocity, Matrix<1,3>& xhat){
            const Matrix<1,2> u = {Current, Velocity};
            return StateObsrv.Estimate(u, xhat);
        }
        
        Matrix<1,3> GetEstimatedVect(const double Current, const double Velocity){
            const Matrix<1,2> u = {Current, Velocity};
            return StateObsrv.Estimate(u);
        }
        
        std::tuple<double,double,double> GetEstimatedVars(const double Current, const double Velocity){
            const Matrix<1,2> u = {Current, Velocity};
            Matrix<1,3> xhat = StateObsrv.Estimate(u);
            return std::forward_as_tuple(xhat[1], xhat[2], xhat[3]);
        }
        
    private:
        TwoInertiaStateObsrv(const TwoInertiaStateObsrv&) = delete;                 
        const TwoInertiaStateObsrv& operator=(const TwoInertiaStateObsrv&) = delete;
        double Kt;  
        double Jm;  
        double Dm;  
        double Jl;  
        double Dl;  
        double Ks;  
        double Rg;  
        double g;   
        double Ts;  
        Matrix<3,3> A;  
        Matrix<1,3> b;  
        Matrix<3,1> c;  
        Matrix<1,3> k;  
        Observer<3> StateObsrv; 
        
        void CalcPlantStateModel(void){
            // プラントの状態方程式 (x = [ωl θs ωm]^T, u = iqref, y = ωm)
            A.Set(
                -Dl/Jl,  Ks/Jl      ,  0        ,
                -1    ,  0          ,  1.0/Rg   ,
                 0    , -Ks/(Jm*Rg) , -Dm/Jm    
            );
            b.Set(
                0,
                0,
                Kt/Jm
            );
            c.Set(0, 0, 1);
        }
        
        void CalcObserverGain(void){
            // 推定帯域gで重根配置されるオブザーバゲインを計算
            const double k1 = -(Jm*Rg*(Dl*Dl*Dl - 3.0*Dl*Dl*Jl*g + 3.0*Dl*Jl*Jl*g*g - 2.0*Ks*Dl*Jl - Jl*Jl*Jl*g*g*g + 3.0*Ks*Jl*Jl*g))/(Jl*Jl*Jl*Ks);   // オブザーバゲイン1
            const double k2 = (- Jm*Dl*Dl*Rg*Rg + 3.0*Jm*Dl*Jl*Rg*Rg*g - 3.0*Jm*Jl*Jl*Rg*Rg*g*g + Ks*Jl*Jl + Jm*Ks*Jl*Rg*Rg)/(Jl*Jl*Ks*Rg);             // オブザーバゲイン2
            const double k3 = -(Dl*Jm + Dm*Jl - 3.0*Jl*Jm*g)/(Jl*Jm);   // オブザーバゲイン3
            
            // オブザーバゲインベクトルにセット
            k.Set(
                k1,
                k2,
                k3
            );
        }
};
}
 
#endif