MovingDifferentiator.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 MOVINGDIFFERENTIATOR
#define MOVINGDIFFERENTIATOR
 
#include <cmath>
#include "RingBuffer.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 W, typename T = double>
class MovingDifferentiator {
    public:
        MovingDifferentiator()
            : VarWindow(), TimeWindow(), FirstTime(true)
        {
            
        }
 
        MovingDifferentiator(MovingDifferentiator&& r)
            : VarWindow(r.VarWindow), TimeWindow(r.TimeWindow), FirstTime(r.FirstTime)
        {
            
        }
 
        ~MovingDifferentiator(){
            
        }
        
        double GetSignal(const double Var, const double Time){
            if(FirstTime){
                // 初めて呼ばれた場合は，
                VarWindow.FillBuffer(Var);      // 初期位置でバッファを埋めておく
                TimeWindow.FillBuffer(Time);    // 初期時刻でバッファを埋めておく
                FirstTime = false;              // フラグリセット
                return 0;                       // 最初の1回目は速度を計算できないのでゼロ
            }else{
                // 2回目以降は，
                const T dx = Var - VarWindow.GetFinalValue();       // 現在位置と位置リングバッファの最後尾との偏差の計算
                const double dt = Time - TimeWindow.GetFinalValue();// 現在時刻と時刻リングバッファの最後尾との偏差の計算
                const T v = dx/dt;              // 微分の計算
                VarWindow.SetFirstValue(Var);   // 位置リングバッファの先頭に現在位置を詰める
                TimeWindow.SetFirstValue(Time); // 時刻リングバッファの先頭に現在時刻を詰める
                if(std::isfinite(v)){
                    return v;   // 微分結果が有限値なら計算結果を返す
                }else{
                    return 0;   // ゼロ割のときはゼロにしておく
                }
            }
        }
        
        template<size_t NN, size_t MM>
        Matrix<NN,MM> GetSignal(const Matrix<NN,MM>& Var, const double Time){
            if(FirstTime){
                // 初めて呼ばれた場合は，
                VarWindow.FillBuffer(Var);      // 初期位置でバッファを埋めておく
                TimeWindow.FillBuffer(Time);    // 初期時刻でバッファを埋めておく
                FirstTime = false;              // フラグリセット
                return Matrix<NN,MM>::zeros();  // 最初の1回目は速度を計算できないのでゼロ
            }else{
                // 2回目以降は，
                const T dx = Var - VarWindow.GetFinalValue();       // 現在位置と位置リングバッファの最後尾との偏差の計算
                const double dt = Time - TimeWindow.GetFinalValue();// 現在時刻と時刻リングバッファの最後尾との偏差の計算
                const T v = dx/dt;              // 微分の計算
                VarWindow.SetFirstValue(Var);   // 位置リングバッファの先頭に現在位置を詰める
                TimeWindow.SetFirstValue(Time); // 時刻リングバッファの先頭に現在時刻を詰める
                return v;                       // 計算結果を返す
            }
        }
        
        void Reset(void){
            FirstTime = true;
        }
        
    private:
        MovingDifferentiator(const MovingDifferentiator&) = delete;                 
        const MovingDifferentiator& operator=(const MovingDifferentiator&) = delete;
        RingBuffer<T, W, false> VarWindow;          
        RingBuffer<double, W, false> TimeWindow;    
        bool FirstTime;                             
};
}
 
#endif