#ifndef __SENSOR_ERR_H__
#define __SENSOR_ERR_H__

/** @file
 * @brief 慣性センサの誤差
 * 
 * 慣性センサの誤差について記述したファイル。
 */

#include <math.h>
#include "util/util.h"

/**
 * @brief センサの誤差モデル(ランダムドリフト成分)
 * 
 * センサの誤差モデルのうちランダムドリフト成分について定義した抽象クラス。
 * 1次のマルコフ過程を採用しています。
 * すなわち、バイアス成分 @f$ x_{\mathrm{bias}} @f$は
 * @f[
 *    \frac{d}{dt} x_{\mathrm{bias}}
 *      = - \beta x_{\mathrm{bias}} (t) + w (t)
 * @f]
 * です。ただし、$f@ w(t) $f@は 
 * @f[
 *    E \left[ w(t) w( \tau ) \right] = N \delta(t - \tau)
 * @f]
 * を満たすホワイトノイズです。
 * ここで、離散化すると
 * @f[
 *    x_{\mathrm{bias}} {}_{k+1}
 *      = - \beta x_{\mathrm{bias}} {}_{k} \Delta t
 *        + \sqrt{N} u_{k} \Delta \sqrt{t}
 * @f]
 * となります。@f$ u_{k} @f$は単位ホワイトノイズ(偏差が1)です。
 * 
 * @param T 演算精度
 */
template <class T>
class SensorErrorModel{

  private:
    const T m_sqrt_N, ///< @f$ \sqrt{N} @f$、ランダムドリフトの大きさ 
            m_beta;   ///< @f$ \beta @f$、ランダムドリフトの減衰効果
    T m_Current;  ///< ランダムドリフトの現在値、@f$ x_{\mathrm{bias}} {}_{k} @f$
  protected:
    /**
     * 次の時間におけるランダムドリフト成分を返します。
     * 
     * @param delta 時間間隔
     * @return (T) ランダムドリフト成分
     */
    T next(const T &delta){
      return (m_Current += (-m_beta * m_Current * delta
                + rand_regularized(0, m_sqrt_N) * sqrt(delta)));
    }

  public:
    /**
     * コンストラクタ。
     * @f$ \sqrt{N} @f$と@f$ \beta @f$を指定して初期化します。
     * 
     * @param sqrt_N @f$ \sqrt{N} @f$、ランダムドリフトの大きさ
     * @param beta @f$ \beta @f$、ランダムドリフトの減衰効果
     */
    SensorErrorModel(const T &sqrt_N, const T &beta)
       : m_sqrt_N(sqrt_N), m_beta(beta), m_Current(0){}
    /**
     * コンストラクタ。
     * @f$ \sqrt{N} @f$を指定して初期化します。
     * @f$ \beta @f$は0で初期されます。
     * すなわち完全なランダムウォークを表現します。
     * 
     * @param sqrt_N @f$ \sqrt{N} @f$、ランダムドリフトの大きさ
     */
    SensorErrorModel(const T &sqrt_N)
       : m_sqrt_N(sqrt_N), m_beta(0), m_Current(0){}
    /**
     * 真値に対してノイズを加算し、シミュレーション値を生成します。
     * 
     * @param true_value 真値
     * @param delta 時間間隔
     * @return (T) シミュレーション値
     */
    virtual T convert(const T &true_value, const T &delta) = 0;
};

#endif /* __SENSOR_ERR_H__ */