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