#ifndef __FILTERED_INS_H__
#define __FILTERED_INS_H__

/** @file
 * @brief 慣性航法装置(INS)との統合の橋渡し(Additive)
 * 
 * 慣性航法装置(INS)と他の航法装置を
 * カルマンフィルタを通じて統合するのに必要となる
 * 行列演算について記述したファイル。
 * クォータニオンの線形化は加算型(Additive)で行っています。
 */

#include "INS.h"
#include "param/matrix.h"
#include "algorithm/kalman.h"

/**
 * @brief 他の航法装置を統合する為のINS拡張クラス(Additive)
 * 
 * カルマンフィルタを通じて、INSを他の航法装置と統合を行うのに
 * 必要となる諸行列演算について定義をしたINS拡張クラス。
 * ただし、クォータニオンの線形化は加算型(Additive)で行っています。
 * すなわち
 * @f[
 *    \Tilde{q} + \Delta \Tilde{q}
 *      \equiv \begin{Bmatrix} 
 *          q_{0} + \Delta q_{0} \\
 *          \vec{q} + \Delta \vec{q}
 *        \end{Bmatrix} 
 * @f]
 * で定義されています。
 * 
 * @param FloatT 演算精度
 * @param Filter カルマンフィルタ
 */
template <class FloatT, typename Filter = KalmanFilterUD<FloatT> >
class Filtered_INS : public INS<FloatT>{
   
  protected:
    Filter m_filter; ///< カルマンフィルタ本体
  
    /**
     * 慣性航法方程式(所謂システム方程式)において、
     * その状態量の誤差に対して線形化した場合の式、
     * すなわち誤差システム方程式
     * @f[
     *    \frac{d}{dt} \Bar{x} = A \Bar{x} + B \Bar{u}
     * @f]
     * における行列 Aを返します。
     * この式において @f$ \Bar{x} @f$は状態量、つまり速度・位置・姿勢、の誤差、
     * @f$ \Bar{u} @f$は入力、すなわち加速度や角速度(、加えてここでは重力も)、の誤差を表します。  
     * 
     * @param accel 加速度
     * @param gyro 角速度
     * @return (Matrix<FloatT>) A行列
     */
    Matrix<FloatT> getA(const Vector3<FloatT> &accel, const Vector3<FloatT> &gyro){

#ifndef pow2
#define pow2(x) ((x) * (x))
#else
#define ALREADY_POW2_DEFINED
#endif
      //行列Aの計算
      Matrix<FloatT> A(12, 12);
      {
   
        FloatT R = Earth::R_normal(INS<FloatT>::phi) + INS<FloatT>::h;
        FloatT R2 = pow2(R);
        Vector3<FloatT> omega_1(this->omega_e2i_4n * 2 + this->omega_n2e_4n);
        
        {
          A(0, 0) =  (*this)[2] / R;
          A(0, 1) =  omega_1[2];
          A(0, 2) = -omega_1[1];
    
          A(0, 3) = Earth::Omega_Earth * (-(*this)[4] * (*this)[2] + (*this)[3] * (*this)[1]) * 4;
          A(0, 4) = Earth::Omega_Earth * (-(*this)[3] * (*this)[2] - (*this)[4] * (*this)[1]) * 4;
          A(0, 5) = Earth::Omega_Earth * (-(*this)[6] * (*this)[2] - (*this)[5] * (*this)[1]) * 4;
          A(0, 6) = Earth::Omega_Earth * (-(*this)[5] * (*this)[2] + (*this)[6] * (*this)[1]) * 4;
    
          A(0, 7) = -(*this)[0] * (*this)[2] / R2;
    
          A(0, 8)  = ( (*this)[8]  * accel.get(0) - (*this)[11] * accel.get(1) + (*this)[10] * accel.get(2)) * 2;
          A(0, 9)  = ( (*this)[9]  * accel.get(0) + (*this)[10] * accel.get(1) + (*this)[11] * accel.get(2)) * 2;
          A(0, 10) = (-(*this)[10] * accel.get(0) + (*this)[9]  * accel.get(1) + (*this)[8]  * accel.get(2)) * 2;
          A(0, 11) = (-(*this)[11] * accel.get(0) - (*this)[8]  * accel.get(1) + (*this)[9]  * accel.get(2)) * 2;
        }
    
        {
          A(1, 0) = -omega_1[2];
          A(1, 1) =  (*this)[2] / R;
          A(1, 2) =  omega_1[0];
    
          A(1, 3) = Earth::Omega_Earth * (-(*this)[3] * (*this)[0] - (*this)[5] * (*this)[2]) * 4;
          A(1, 4) = Earth::Omega_Earth * ( (*this)[4] * (*this)[0] + (*this)[6] * (*this)[2]) * 4;
          A(1, 5) = Earth::Omega_Earth * ( (*this)[5] * (*this)[0] - (*this)[3] * (*this)[2]) * 4;
          A(1, 6) = Earth::Omega_Earth * (-(*this)[6] * (*this)[0] + (*this)[4] * (*this)[2]) * 4;
    
          A(1, 7) = -(*this)[1] * (*this)[2] / R2;
    
          A(1, 8)  = ( (*this)[11] * accel.get(0) + (*this)[8]  * accel.get(1) - (*this)[9]  * accel.get(2)) * 2;
          A(1, 9)  = ( (*this)[10] * accel.get(0) - (*this)[9]  * accel.get(1) - (*this)[8]  * accel.get(2)) * 2;
          A(1, 10) = ( (*this)[9]  * accel.get(0) + (*this)[10] * accel.get(1) + (*this)[11] * accel.get(2)) * 2;
          A(1, 11) = ( (*this)[8]  * accel.get(0) - (*this)[11] * accel.get(1) + (*this)[10] * accel.get(2)) * 2;
        }
    
        {
          A(2, 0) =  omega_1[1] - (*this)[0] / R;
          A(2, 1) = -omega_1[0] - (*this)[1] / R;
    
          A(2, 3) = Earth::Omega_Earth * ( (*this)[5] * (*this)[1] + (*this)[4] * (*this)[0]) * 4;
          A(2, 4) = Earth::Omega_Earth * (-(*this)[6] * (*this)[1] + (*this)[3] * (*this)[0]) * 4;
          A(2, 5) = Earth::Omega_Earth * ( (*this)[3] * (*this)[1] + (*this)[6] * (*this)[0]) * 4;
          A(2, 6) = Earth::Omega_Earth * (-(*this)[4] * (*this)[1] + (*this)[5] * (*this)[0]) * 4;
    
          A(2, 7) = (pow2((*this)[1]) + pow2((*this)[2])) / R2;
    
          A(2, 8)  = (-(*this)[10] * accel.get(0) + (*this)[9]  * accel.get(1) + (*this)[8]  * accel.get(2)) * 2;
          A(2, 9)  = ( (*this)[11] * accel.get(0) + (*this)[8]  * accel.get(1) - (*this)[9]  * accel.get(2)) * 2;
          A(2, 10) = (-(*this)[8]  * accel.get(0) + (*this)[11] * accel.get(1) - (*this)[10] * accel.get(2)) * 2;
          A(2, 11) = ( (*this)[9]  * accel.get(0) + (*this)[10] * accel.get(1) + (*this)[11] * accel.get(2)) * 2;
        }

#define CENTRIPETAL ///< 求心力誤差を考慮するかのスイッチ、この場合考慮する
#ifdef CENTRIPETAL       
        {
          FloatT centripetal_f(pow2(Earth::Omega_Earth) * R);
          Vector3<FloatT> centripetal_vector(
                        (*this)[3] * (*this)[5] + (*this)[4] * (*this)[6],
                        (*this)[6] * (*this)[5] - (*this)[4] * (*this)[3],
                        0
                      );
          centripetal_vector *= centripetal_f * -2;
          
          FloatT centripetal_lambda(
                (*this)[6] * (pow2((*this)[4]) + pow2((*this)[5]))
              );
          centripetal_lambda *= centripetal_f * 2 * (-2);
          Vector3<FloatT> centripetal_omega(
                 (*this)[5] * (pow2((*this)[3]) + pow2((*this)[6])),
                -(*this)[4] * (pow2((*this)[3]) + pow2((*this)[6])),
                 (*this)[3] * (pow2((*this)[4]) + pow2((*this)[5]))
              );
          centripetal_omega *= centripetal_f * 2 * (-2);
          
          A(0, 3) += centripetal_omega[0];
          A(1, 3) += centripetal_omega[1];
          A(2, 3) += centripetal_omega[2];
          
          A(0, 4) += centripetal_lambda;
          A(0, 5) -= centripetal_omega[2];
          A(0, 6) += centripetal_omega[1];
          A(1, 4) += centripetal_omega[2];
          A(1, 5) += centripetal_lambda;
          A(1, 6) -= centripetal_omega[0];
          A(2, 4) -= centripetal_omega[1];
          A(2, 5) += centripetal_omega[0];
          A(2, 6) += centripetal_lambda;
          
          Matrix<FloatT> dcm_e2n(INS<FloatT>::q_e2n.getDCM());
          Matrix<FloatT> centripetal_linerized(3, 4);
          {
            centripetal_linerized(0, 0) =  (*this)[5];
            centripetal_linerized(0, 1) =  (*this)[6];
            centripetal_linerized(0, 2) =  (*this)[3];
            centripetal_linerized(0, 3) =  (*this)[4];
            centripetal_linerized(1, 0) = -(*this)[4];
            centripetal_linerized(1, 1) = -(*this)[3];
            centripetal_linerized(1, 2) =  (*this)[6];
            centripetal_linerized(1, 3) =  (*this)[5];
          }
          centripetal_linerized *= centripetal_f * -2;
          A.pivotMerge(0, 3, dcm_e2n * centripetal_linerized);
          
          A(0, 7) -= (dcm_e2n(0, 0) * dcm_e2n(2, 0) - dcm_e2n(0, 1) * dcm_e2n(2, 1)) * pow2(Earth::Omega_Earth);
          A(1, 7) -= (dcm_e2n(1, 0) * dcm_e2n(2, 0) - dcm_e2n(1, 1) * dcm_e2n(2, 1)) * pow2(Earth::Omega_Earth);
          A(2, 7) -= (dcm_e2n(2, 0) * dcm_e2n(2, 0) - dcm_e2n(2, 1) * dcm_e2n(2, 1)) * pow2(Earth::Omega_Earth);
        }
#endif
    
        {
          A(3, 0) =  (*this)[5] / R / 2;
          A(3, 1) = -(*this)[4] / R / 2;
    
          A(3, 4) = -(this->omega_n2e_4n)[0] / 2;
          A(3, 5) = -(this->omega_n2e_4n)[1] / 2;
          A(3, 6) = -(this->omega_n2e_4n)[2] / 2;
    
          A(3, 7) = (-(*this)[5] * (*this)[0] + (*this)[4] * (*this)[1]) / R2 / 2;
        }
    
        {
          A(4, 0) =  (*this)[6] / R / 2;
          A(4, 1) =  (*this)[3] / R / 2;
    
          A(4, 3) =  (this->omega_n2e_4n)[0] / 2;
          A(4, 5) =  (this->omega_n2e_4n)[2] / 2;
          A(4, 6) = -(this->omega_n2e_4n)[1] / 2;
    
          A(4, 7) = (-(*this)[6] * (*this)[0] - (*this)[3] * (*this)[1]) / R2 / 2;
        }
    
        {
          A(5, 0) = -(*this)[3] / R / 2;
          A(5, 1) =  (*this)[6] / R / 2;
    
          A(5, 3) =  (this->omega_n2e_4n)[1] / 2;
          A(5, 4) = -(this->omega_n2e_4n)[2] / 2;
          A(5, 6) =  (this->omega_n2e_4n)[0] / 2;
    
          A(5, 7) = ((*this)[3] * (*this)[0] - (*this)[6] * (*this)[1]) / R2 / 2;
        }
    
        {
          A(6, 0) = -(*this)[4] / R / 2;
          A(6, 1) = -(*this)[5] / R / 2;
    
          A(6, 3) =  (this->omega_n2e_4n)[2] / 2;
          A(6, 4) =  (this->omega_n2e_4n)[1] / 2;
          A(6, 5) = -(this->omega_n2e_4n)[0] / 2;
    
          A(6, 7) =  ((*this)[4] * (*this)[0] + (*this)[5] * (*this)[1]) / R2 / 2;
        }
    
        {
          A(7, 2) = -1;
        }
    
        Vector3<FloatT> omega_2(this->omega_e2i_4n + this->omega_n2e_4n);
        {
          A(8, 0) = -(*this)[10] / R / 2;
          A(8, 1) =  (*this)[9] / R / 2;
      
          A(8, 3) = Earth::Omega_Earth * (-(*this)[9] * (*this)[5] + (*this)[10] * (*this)[4] + (*this)[11] * (*this)[3]);
          A(8, 4) = Earth::Omega_Earth * ( (*this)[9] * (*this)[6] + (*this)[10] * (*this)[3] - (*this)[11] * (*this)[4]);
          A(8, 5) = Earth::Omega_Earth * (-(*this)[9] * (*this)[3] + (*this)[10] * (*this)[6] - (*this)[11] * (*this)[5]);
          A(8, 6) = Earth::Omega_Earth * ( (*this)[9] * (*this)[4] + (*this)[10] * (*this)[5] + (*this)[11] * (*this)[6]);
      
          A(8, 7) = ((*this)[10] * (*this)[0] - (*this)[9] * (*this)[1]) / R2 / 2;
      
          A(8, 9)  = (-gyro.get(0) + omega_2[0]) / 2;
          A(8, 10) = (-gyro.get(1) + omega_2[1]) / 2;
          A(8, 11) = (-gyro.get(2) + omega_2[2]) / 2;
        }
    
        {
          A(9, 0) =  (*this)[11] / R / 2;
          A(9, 1) = -(*this)[8] / R / 2;
      
          A(9, 3) = Earth::Omega_Earth * ( (*this)[8] * (*this)[5] - (*this)[11] * (*this)[4] + (*this)[10] * (*this)[3]);
          A(9, 4) = Earth::Omega_Earth * (-(*this)[8] * (*this)[6] - (*this)[11] * (*this)[3] - (*this)[10] * (*this)[4]);
          A(9, 5) = Earth::Omega_Earth * ( (*this)[8] * (*this)[3] - (*this)[11] * (*this)[6] - (*this)[10] * (*this)[5]);
          A(9, 6) = Earth::Omega_Earth * (-(*this)[8] * (*this)[4] - (*this)[11] * (*this)[5] + (*this)[10] * (*this)[6]);
      
          A(9, 7) = (-(*this)[11] * (*this)[0] + (*this)[8] * (*this)[1]) / R2 / 2;
      
          A(9, 8)  = ( gyro.get(0) - omega_2[0]) / 2;
          A(9, 10) = ( gyro.get(2) + omega_2[2]) / 2;
          A(9, 11) = (-gyro.get(1) - omega_2[1]) / 2;
        }
    
        {
          A(10, 0) =  (*this)[8] / R / 2;
          A(10, 1) =  (*this)[11] / R / 2;
      
          A(10, 3) = Earth::Omega_Earth * (-(*this)[11] * (*this)[5] - (*this)[8] * (*this)[4] - (*this)[9] * (*this)[3]);
          A(10, 4) = Earth::Omega_Earth * ( (*this)[11] * (*this)[6] - (*this)[8] * (*this)[3] + (*this)[9] * (*this)[4]);
          A(10, 5) = Earth::Omega_Earth * (-(*this)[11] * (*this)[3] - (*this)[8] * (*this)[6] + (*this)[9] * (*this)[5]);
          A(10, 6) = Earth::Omega_Earth * ( (*this)[11] * (*this)[4] - (*this)[8] * (*this)[5] - (*this)[9] * (*this)[6]);
      
          A(10, 7) = (-(*this)[8] * (*this)[0] - (*this)[11] * (*this)[1]) / R2 / 2;
      
          A(10, 8)  = ( gyro.get(1) - omega_2[1]) / 2;
          A(10, 9)  = (-gyro.get(2) - omega_2[2]) / 2;
          A(10, 11) = ( gyro.get(0) + omega_2[0]) / 2;
        }
    
        {
          A(11, 0) = -(*this)[9] / R / 2;
          A(11, 1) = -(*this)[10] / R / 2;
      
          A(11, 3) = Earth::Omega_Earth * ( (*this)[10] * (*this)[5] + (*this)[9] * (*this)[4] - (*this)[8] * (*this)[3]);
          A(11, 4) = Earth::Omega_Earth * (-(*this)[10] * (*this)[6] + (*this)[9] * (*this)[3] + (*this)[8] * (*this)[4]);
          A(11, 5) = Earth::Omega_Earth * ( (*this)[10] * (*this)[3] + (*this)[9] * (*this)[6] + (*this)[8] * (*this)[5]);
          A(11, 6) = Earth::Omega_Earth * ( (*this)[10] * (*this)[4] - (*this)[9] * (*this)[5] + (*this)[8] * (*this)[6]);
      
          A(11, 7) = ((*this)[9] * (*this)[0] + (*this)[10] * (*this)[1]) / R2 / 2;
      
          A(11, 8)  = ( gyro.get(2) - omega_2[2]) / 2;
          A(11, 9)  = ( gyro.get(1) + omega_2[1]) / 2;
          A(11, 10) = (-gyro.get(0) - omega_2[0]) / 2;
        }
      }
#ifdef ALREADY_POW2_DEFINED
#undef ALREADY_POW2_DEFINED
#else
#undef pow2
#endif
      
      return A;      
    }
    
    /**
     * 慣性航法方程式(所謂システム方程式)において、
     * その状態量の誤差に対して線形化した場合の式、
     * すなわち誤差システム方程式
     * @f[
     *    \frac{d}{dt} \Bar{x} = A \Bar{x} + B \Bar{u}
     * @f]
     * における行列 Bを返します。
     * この式において @f$ \Bar{x} @f$は状態量、つまり速度・位置・姿勢、の誤差、
     * @f$ \Bar{u} @f$は入力、すなわち加速度や角速度(、加えてここでは重力も)、の誤差を表します。  
     * 
     * @param accel 加速度
     * @param gyro 角速度
     * @return (Matrix<FloatT>) B行列
     */
    Matrix<FloatT> getB(const Vector3<FloatT> &accel, const Vector3<FloatT> &gyro){
     
#ifndef pow2
#define pow2(x) ((x) * (x))
#else
#define ALREADY_POW2_DEFINED
#endif
      //行列Bの計算
      Matrix<FloatT> B(12, 7);
      {
        B(0, 0) = 1. - (pow2((*this)[10]) + pow2((*this)[11])) * 2;
        B(0, 1) = ((*this)[9] * (*this)[10] - (*this)[8] * (*this)[11]) * 2;
        B(0, 2) = ((*this)[9] * (*this)[11] + (*this)[8] * (*this)[10]) * 2;
    
        B(1, 0) = ((*this)[9] * (*this)[10] + (*this)[8] * (*this)[11]) * 2;
        B(1, 1) = 1. - (pow2((*this)[9]) + pow2((*this)[11])) * 2;
        B(1, 2) = ((*this)[10] * (*this)[11] - (*this)[8] * (*this)[9]) * 2;
    
        B(2, 0) = ((*this)[9] * (*this)[11] - (*this)[8] * (*this)[10]) * 2;
        B(2, 1) = ((*this)[10] * (*this)[11] + (*this)[8] * (*this)[9]) * 2;
        B(2, 2) = 1. - (pow2((*this)[9]) + pow2((*this)[10])) * 2;
        B(2, 6) = 1;
    
        B(8, 3) = -(*this)[9] / 2;
        B(8, 4) = -(*this)[10] / 2;
        B(8, 5) = -(*this)[11] / 2;
    
        B(9, 3) =  (*this)[8] / 2;
        B(9, 4) = -(*this)[11] / 2;
        B(9, 5) =  (*this)[10] / 2;
    
        B(10, 3) =  (*this)[11] / 2;
        B(10, 4) =  (*this)[8] / 2;
        B(10, 5) = -(*this)[9] / 2;
    
        B(11, 3) = -(*this)[10] / 2;
        B(11, 4) =  (*this)[9] / 2;
        B(11, 5) =  (*this)[8] / 2;
      }
#ifdef ALREADY_POW2_DEFINED
#undef ALREADY_POW2_DEFINED
#else
#undef pow2
#endif
      
      return B;      
    }
  
  public:
    static const unsigned P_SIZE = 12;  ///< P行列(システム誤差共分散行列)の大きさ
    static const unsigned Q_SIZE = 7;   ///< Q行列(入力誤差共分散行列)の大きさ
    
    /**
     * コンストラクタ。
     * 行列P(システム誤差共分散行列)や
     * Q(入力誤差共分散行列)の作成は内部的に行われます。
     * 
     */
    Filtered_INS()
      : INS<FloatT>(), 
        m_filter(Matrix<FloatT>::getI(P_SIZE), Matrix<FloatT>::getI(Q_SIZE)){
    }
    
    /**
     * コンストラクタ。
     * 行列PやQを指定して初期化を完了します。
     * 
     * @param P P行列(システム誤差共分散行列)
     * @param Q Q行列(入力誤差共分散行列)
     */
    Filtered_INS(const Matrix<FloatT> &P, const Matrix<FloatT> &Q) 
      : INS<FloatT>(), 
        m_filter(P, Q){}

    /**
     * コピーコンストラクタ
     * 
     * @param orig コピー元
     * @param deepcopy ディープコピーを作成するかどうか
     */
    Filtered_INS(const Filtered_INS &orig, const bool deepcopy = false)
      : INS<FloatT>(orig, deepcopy),
        m_filter(orig.m_filter, deepcopy){
    }
    
    /**
     * デストラクタ。
     * 
     */
    ~Filtered_INS(){}

  protected:
    /**
     * 時間更新において後処理をするためのコールバック関数。
     * デフォルトでは何もしません。
     * 
     * @param A A行列
     * @param B B行列
     * @param deltaT 時間間隔
     */
    virtual inline void before_update_INS(
        const Matrix<FloatT> &A, const Matrix<FloatT> &B, 
        const FloatT &deltaT
      ){}

  public:
    /**
     * 時間更新(Time Update)
     * 
     * @param accel 加速度
     * @param gyro 角速度
     * @param deltaT 時間間隔
     */
    void update(const Vector3<FloatT> &accel, const Vector3<FloatT> &gyro, const FloatT &deltaT){
    
      Matrix<FloatT> A(getA(accel, gyro));
      Matrix<FloatT> B(getB(accel, gyro));
      m_filter.predict(A, B, deltaT);
      before_update_INS(A, B, deltaT);
      INS<FloatT>::update(accel, gyro, deltaT);
    }
    
  protected:
    /**
     * 観測更新(Measurement Update)において後処理をするためのコールバック関数。
     * デフォルトでは何もしません。
     * 
     * @param H H行列
     * @param R R行列
     * @param K K行列、カルマンゲイン
     * @param v (z - H x)ここではEKFでxがゼロであるためzに等しい
     * @param x_hat 修正量
     */
    virtual inline void before_correct_INS(
        const Matrix<FloatT> &H,
        const Matrix<FloatT> &R,
        const Matrix<FloatT> &K,
        const Matrix<FloatT> &v,
        Matrix<FloatT> &x_hat
      ){}
    
    /**
     * Kalman Filterによって得られた@f$ \Hat{x} @f$を利用して、INSを修正します。
     * 
     * @param x_hat Kalman Filterによって得られたx_hat
     */
    inline void correct_INS(Matrix<FloatT> &x_hat){
#ifdef CORRECT_LIMIT_ATT
      for(unsigned i = 0; i < 8; i++){(*this)[i] -= x_hat(i, 0);}
      for(unsigned i = 8; i < 12; i++){
        FloatT delta = min_macro(fabs(x_hat(i, 0)), CORRECT_LIMIT_ATT);
        if(x_hat(i, 0) < 0){delta = -delta;}
        (*this)[i] -= delta;
      }
      for(unsigned i = 12; i < x_hat.rows(); i++){(*this)[i] -= x_hat(i, 0);}
#else
      for(unsigned i = 0; i < x_hat.rows(); i++){(*this)[i] -= x_hat(i, 0);}
#endif

#ifdef CORRECT_SIGN
      for(unsigned i = 3; i < 7; i++){if((*this)[i] > 1) (*this)[i] *= -1;}
      for(unsigned i = 8; i < 12; i++){if((*this)[i] > 1) (*this)[i] *= -1;}
#endif
      
      INS<FloatT>::recalc();
    }

//#define CORRECT_LIMIT_ATT 1E-4
//#define CORRECT_SIGN
  public:
    /**
     * 観測更新(Measurement Update)します。
     * 
     * @param H 観測行列
     * @param z 観測量
     * @param R 観測誤差共分散行列
     */
    void correct(const Matrix<FloatT> &H, const Matrix<FloatT> &z, const Matrix<FloatT> &R){
            
      //修正
      Matrix<FloatT> K(m_filter.correct(H, R)); //カルマンゲイン
      Matrix<FloatT> x_hat(K * z);
      before_correct_INS(H, R, K, z, x_hat);
      correct_INS(x_hat);
    }
    
    /**
     * フィルターを取得します。
     * P行列やQ行列が欲しい場合はgetFilter().getP()などとしてください。
     * 
     * @return (Filter &) フィルター
     */
    Filter &getFilter(){return m_filter;}
    
    /**
     * 標準偏差を馴染みの形で求めます。(工事中)
     * 
     * 戻り値はσ値で
     * [北方向速度、東方向速度、下方向速度、緯度、経度、高度、ヨー、ピッチ、ロール]
     */
    /*Matrix<FloatT> getSigma() {
      Matrix<FloatT> sigma(9, 1);
      
      Matrix<FloatT> &P(const_cast<Filtered_INS2 *>(this)->getFilter().getP());

      // 速度
      for(unsigned i = 0; i < 3; i++) sigma(i, 0) = sqrt(P(i, i));
      
      // 位置
      //sigma(3, 0); sigma(4, 0); // TODO: 緯度経度誤差をΔuから出す
      sigma(5, 0) = sqrt(P(7, 7));
      
      // 姿勢 // TODO, P.partial(4, 4, 8, 8)を使って求める

      return sigma;
    }*/
};

#endif /* __FILTERED_INS_H__ */