#ifndef __FILTERED_INS2_H__
#define __FILTERED_INS2_H__

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

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

class Filtered_INS2_Property {
  public:
    static const unsigned P_SIZE
#if defined(_MSC_VER)
        = 10
#endif
        ; ///< P行列(システム誤差共分散行列)の大きさ
    static const unsigned Q_SIZE
#if defined(_MSC_VER)
        = 7
#endif
        ; ///< Q行列(入力誤差共分散行列)の大きさ
};

#if !defined(_MSC_VER)
const unsigned Filtered_INS2_Property::P_SIZE = 10;

const unsigned Filtered_INS2_Property::Q_SIZE = 7;
#endif

/**
 * @brief 他の航法装置を統合する為のINS拡張クラス(Multiplicative)
 * 
 * カルマンフィルタを通じて、INSを他の航法装置と統合を行うのに
 * 必要となる諸行列演算について定義をしたINS拡張クラス。
 * ただし、クォータニオンの線形化は積算型(Multiplicative)で行っています。
 * すなわち
 * @f[
 *    \Tilde{q} + \Delta \Tilde{q}
 *      \equiv \begin{Bmatrix} 
 *          1 \\
 *          \Delta \vec{u}
 *        \end{Bmatrix} \begin{Bmatrix} 
 *          q_{0} \\
 *          \vec{q}
 *        \end{Bmatrix} 
 * @f]
 * で定義されています。
 * 
 * @param FloatT 演算精度
 * @param Filter カルマンフィルタ
 */
template <class FloatT, typename Filter = KalmanFilterUD<FloatT> >
class Filtered_INS2 : public INS<FloatT>, public Filtered_INS2_Property {
  public:
    using Filtered_INS2_Property::P_SIZE;
    using Filtered_INS2_Property::Q_SIZE;
    
  protected:
    Filter m_filter;  ///< カルマンフィルタ本体
    
#define R_STRICT ///< 曲率半径を厳密に計算するかのスイッチ、この場合計算する
    
    using INS<FloatT>::get;
    
    /**
     * 慣性航法方程式(所謂システム方程式)において、
     * その状態量の誤差に対して線形化した場合の式、
     * すなわち誤差システム方程式における行列 Aを返します。
     * 詳しくはgetA(const Vector3<FloatT> &, const Vector3<FloatT> &)を参照してください。  
     * 
     * @param accel 加速度
     * @param gyro 角速度
     * @param dcm_e2n @f$ \mathrm{DCM} \left( \Tilde{q}_{e}^{n} \right) @f$
     * @param dcm_n2b @f$ \mathrm{DCM} \left( \Tilde{q}_{n}^{b} \right) @f$
     * @return (Matrix<FloatT>) A行列
     * @see getA(const Vector3<FloatT> &, const Vector3<FloatT> &)
     */
    Matrix<FloatT> getA(
        const Vector3<FloatT> &accel, 
        const Vector3<FloatT> &gyro, 
        const Matrix<FloatT> &dcm_e2n, 
        const Matrix<FloatT> &dcm_n2b) const {
      
#define dcm_e2n(r, c) (const_cast<Matrix<FloatT> *>(&dcm_e2n)->operator()(r, c))
#define dcm_n2b(r, c) (const_cast<Matrix<FloatT> *>(&dcm_n2b)->operator()(r, c))
     
#ifndef pow2
#define pow2(x) ((x) * (x))
#else
#define ALREADY_POW2_DEFINED
#endif

      //行列Aの計算
      FloatT A_serialized[P_SIZE][P_SIZE] = {{FloatT(0)}};
#define A(i, j) A_serialized[i][j]
      {
       
        Vector3<FloatT> omega_1(this->omega_e2i_4n * 2 + this->omega_n2e_4n);

#ifdef R_STRICT
        FloatT Rn_1(Earth::R_normal(this->phi) + get(7));
        FloatT Rm_1(Earth::R_meridian(this->phi) + get(7));
        FloatT Rn_2(pow2(Rn_1));
        FloatT Rm_2(pow2(Rm_1));
#else
        FloatT R(Earth::R_e + get(7));
        FloatT R2(pow2(R));
  #define Rn_1 R
  #define Rm_1 R
  #define Rn_2 R2
  #define Rm_2 R2
#endif
        
        {
          A(0, 0) =  get(2) / Rm_1;
          A(0, 1) =  omega_1[2];
          A(0, 2) = -omega_1[1];
    
          A(0, 3) = Earth::Omega_Earth * 4 * (dcm_e2n(2, 1) * get(1) - dcm_e2n(1, 1) * get(2));
          A(0, 4) = Earth::Omega_Earth * 4 * (dcm_e2n(1, 0) * get(2) - dcm_e2n(2, 0) * get(1));
    
          A(0, 6) = -get(0) * get(2) / Rm_2;
    
          //A(0, 7) = 0;
          A(0, 8) = (dcm_n2b(0, 2) * accel.get(0) + dcm_n2b(1, 2) * accel.get(1) + dcm_n2b(2, 2) * accel.get(2)) *  2;
          A(0, 9) = (dcm_n2b(0, 1) * accel.get(0) + dcm_n2b(1, 1) * accel.get(1) + dcm_n2b(2, 1) * accel.get(2)) * -2;
        }
    
        {
          A(1, 0) = -omega_1[2];
          A(1, 1) =  get(2) / Rn_1;
          A(1, 2) =  omega_1[0];
          
          A(1, 3) = Earth::Omega_Earth * 4 * (dcm_e2n(0, 1) * get(2) - dcm_e2n(2, 1) * get(0));
          A(1, 4) = Earth::Omega_Earth * 4 * (dcm_e2n(2, 0) * get(0) - dcm_e2n(0, 0) * get(2));
    
          A(1, 6) = -get(1) * get(2) / Rn_2;
    
          A(1, 7) = -A(0, 8);
          //A(1, 8) = 0;
          A(1, 9) = (dcm_n2b(0, 0) * accel.get(0) + dcm_n2b(1, 0) * accel.get(1) + dcm_n2b(2, 0) * accel.get(2)) *  2;
        }
    
        {
          A(2, 0) =  omega_1[1] - get(0) / Rm_1;
          A(2, 1) = -omega_1[0] - get(1) / Rn_1;
    
          A(2, 3) = Earth::Omega_Earth * 4 * (dcm_e2n(1, 1) * get(0) - dcm_e2n(0, 1) * get(1));
          A(2, 4) = Earth::Omega_Earth * 4 * (dcm_e2n(0, 0) * get(1) - dcm_e2n(1, 0) * get(0));
    
          A(2, 6) = pow2(get(0)) / Rm_2 + pow2(get(1)) / Rn_2;

          A(2, 7) = -A(0, 9);
          A(2, 8) = -A(1, 9);
          //A(2, 9) = 0;
        }
        
#define CENTRIPETAL ///< 求心力誤差を考慮するかのスイッチ、この場合考慮する
#ifdef CENTRIPETAL
        {
#define by_delta_r(i, j) A(i + 0, j + 3)
          {
            by_delta_r(0, 0) =  dcm_e2n(0, 1) * dcm_e2n(2, 2) + dcm_e2n(0, 2) * dcm_e2n(2, 1);
            by_delta_r(0, 1) = -dcm_e2n(0, 0) * dcm_e2n(2, 2) - dcm_e2n(0, 2) * dcm_e2n(2, 0);
            
            by_delta_r(1, 0) =  dcm_e2n(1, 1) * dcm_e2n(2, 2) + dcm_e2n(1, 2) * dcm_e2n(2, 1);
            by_delta_r(1, 1) = -dcm_e2n(1, 0) * dcm_e2n(2, 2) - dcm_e2n(1, 2) * dcm_e2n(2, 0);
            
            by_delta_r(2, 0) =  dcm_e2n(2, 1) * dcm_e2n(2, 2) + dcm_e2n(2, 2) * dcm_e2n(2, 1);
            by_delta_r(2, 1) = -dcm_e2n(2, 0) * dcm_e2n(2, 2) - dcm_e2n(2, 2) * dcm_e2n(2, 0);
            
            FloatT coef(pow2(Earth::Omega_Earth) * 2 * Rn_1);
            by_delta_r(0, 0) *= coef;
            by_delta_r(0, 1) *= coef;
            
            by_delta_r(1, 0) *= coef;
            by_delta_r(1, 1) *= coef;
            
            by_delta_r(2, 0) *= coef;
            by_delta_r(2, 1) *= coef;
          }
#undef by_delta_r
          
          //Matrix<FloatT> by_delta_r(
           //   Matrix<FloatT>(P_SIZE, P_SIZE, _A).partial(3, 2, 0, 3));
          //cout << "centriprtal : " << by_delta_r << endl;
          
          A(0, 6) -= (dcm_e2n(0, 0) * dcm_e2n(2, 0) - dcm_e2n(0, 1) * dcm_e2n(2, 1)) * pow2(Earth::Omega_Earth);
          A(1, 6) -= (dcm_e2n(1, 0) * dcm_e2n(2, 0) - dcm_e2n(1, 1) * dcm_e2n(2, 1)) * pow2(Earth::Omega_Earth);
          A(2, 6) -= (dcm_e2n(2, 0) * dcm_e2n(2, 0) - dcm_e2n(2, 1) * dcm_e2n(2, 1)) * pow2(Earth::Omega_Earth);
        }
#endif
    
        {
          A(3, 0) = -dcm_e2n(1, 0) / 2 / Rm_1;
          A(3, 1) =  dcm_e2n(0, 0) / 2 / Rn_1;
          
          A(3, 6) =  (dcm_e2n(1, 0) * get(0) / Rm_2 - dcm_e2n(0, 0) * get(1) / Rn_2) / 2;
        }
    
        {
          A(4, 0) = -dcm_e2n(1, 1) / 2 / Rm_1;
          A(4, 1) =  dcm_e2n(0, 1) / 2 / Rn_1;
          
          A(4, 6) =  (dcm_e2n(1, 1) * get(0) / Rm_2 - dcm_e2n(0, 1) * get(1) / Rn_2) / 2;
        }
        
        {
          A(5, 0) = -dcm_e2n(1, 2) / 2 / Rm_1;
          A(5, 1) =  dcm_e2n(0, 2) / 2 / Rn_1;
          
          A(5, 6) =  (dcm_e2n(1, 2) * get(0) / Rm_2 - dcm_e2n(0, 2) * get(1) / Rn_2) / 2;
        }
    
        {
          A(6, 2) = -1;
        }
    
        Vector3<FloatT> omega_2(this->omega_e2i_4n + this->omega_n2e_4n);
        {
          A(7, 1) = -1. / Rn_1 / 2;
      
          A(7, 3) = -Earth::Omega_Earth * dcm_e2n(0, 1);
          A(7, 4) =  Earth::Omega_Earth * dcm_e2n(0, 0);
      
          A(7, 6) = get(1) / Rn_2 / 2;
      
          A(7, 8) =  omega_2[2];
          A(7, 9) = -omega_2[1];
        }
    
        {
          A(8, 0) =  1. / Rm_1 / 2;
      
          A(8, 3) = -Earth::Omega_Earth * dcm_e2n(1, 1);
          A(8, 4) =  Earth::Omega_Earth * dcm_e2n(1, 0);
      
          A(8, 6) = -get(0) / Rm_2 / 2;
      
          A(8, 7) = -omega_2[2];
          A(8, 9) =  omega_2[0];
        }
    
        {
          A(9, 3) = -Earth::Omega_Earth * dcm_e2n(2, 1);
          A(9, 4) =  Earth::Omega_Earth * dcm_e2n(2, 0);
      
          A(9, 7) =  omega_2[1];
          A(9, 8) = -omega_2[0];
        }
      }
#ifdef ALREADY_POW2_DEFINED
#undef ALREADY_POW2_DEFINED
#else
#undef pow2
#endif

#undef dcm_e2n
#undef dcm_n2b
#undef A

      return Matrix<FloatT>(
          sizeof(A_serialized) / sizeof(A_serialized[0]),
          sizeof(A_serialized[0]) / sizeof(A_serialized[0][0]), 
          (FloatT *)A_serialized);
    }
    
    /**
     * 慣性航法方程式(所謂システム方程式)において、
     * その状態量の誤差に対して線形化した場合の式、
     * すなわち誤差システム方程式
     * @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) const {
      return getA(accel, gyro, this->q_e2n.getDCM(), this->q_n2b.getDCM());  
    }
    
    /**
     * 慣性航法方程式(所謂システム方程式)において、
     * その状態量の誤差に対して線形化した場合の式、
     * すなわち誤差システム方程式における行列 Bを返します。
     * 詳しくはgetB(const Vector3<FloatT> &, const Vector3<FloatT> &)を参照してください。  
     * 
     * @param accel 加速度
     * @param gyro 角速度
     * @param dcm_n2b @f$ \mathrm{DCM} \left( \Tilde{q}_{n}^{b} \right) @f$
     * @return (Matrix<FloatT>) A行列
     * @see getB(const Vector3<FloatT> &, const Vector3<FloatT> &)
     */
    Matrix<FloatT> getB(
        const Vector3<FloatT> &accel, 
        const Vector3<FloatT> &gyro, 
        const Matrix<FloatT> &dcm_n2b) const {

#define dcm_n2b(r, c) (const_cast<Matrix<FloatT> *>(&dcm_n2b))->operator()(r, c)

      //行列Bの計算
      FloatT B_serialized[P_SIZE][Q_SIZE] = {{FloatT(0)}};
#define B(i, j) B_serialized[i][j]
      {
        B(0, 0) = dcm_n2b(0, 0);
        B(0, 1) = dcm_n2b(1, 0);
        B(0, 2) = dcm_n2b(2, 0);
    
        B(1, 0) = dcm_n2b(0, 1);
        B(1, 1) = dcm_n2b(1, 1);
        B(1, 2) = dcm_n2b(2, 1);
    
        B(2, 0) = dcm_n2b(0, 2);
        B(2, 1) = dcm_n2b(1, 2);
        B(2, 2) = dcm_n2b(2, 2);
        
        B(2, 6) = FloatT(1); // 重力誤差Zのみ
    
        B(7, 3) = dcm_n2b(0, 0) / 2;
        B(7, 4) = dcm_n2b(1, 0) / 2;
        B(7, 5) = dcm_n2b(2, 0) / 2;
    
        B(8, 3) = dcm_n2b(0, 1) / 2;
        B(8, 4) = dcm_n2b(1, 1) / 2;
        B(8, 5) = dcm_n2b(2, 1) / 2;
    
        B(9, 3) = dcm_n2b(0, 2) / 2;
        B(9, 4) = dcm_n2b(1, 2) / 2;
        B(9, 5) = dcm_n2b(2, 2) / 2;
      }
#undef B
#undef dcm_n2b
      
      return Matrix<FloatT>(
          sizeof(B_serialized) / sizeof(B_serialized[0]),
          sizeof(B_serialized[0]) / sizeof(B_serialized[0][0]), 
          (FloatT *)B_serialized);
    }
    
    /**
     * 慣性航法方程式(所謂システム方程式)において、
     * その状態量の誤差に対して線形化した場合の式、
     * すなわち誤差システム方程式
     * @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) const {
      return getB(accel, gyro, (this->q_n2b).getDCM());
    }
  
  public:
    
    /**
     * コンストラクタ。
     * 行列P(システム誤差共分散行列)や
     * Q(入力誤差共分散行列)の作成は内部的に行われます。
     * 
     */
    Filtered_INS2() 
        : INS<FloatT>(), 
          m_filter(Matrix<FloatT>::getI(P_SIZE), Matrix<FloatT>::getI(Q_SIZE)){
    }
    
    /**
     * コンストラクタ。
     * 行列P(システム誤差共分散行列)や
     * Q(入力誤差共分散行列)を指定して初期化を完了します。
     * 
     * @param P P行列(システム誤差共分散行列)
     * @param Q Q行列(入力誤差共分散行列)
     */
    Filtered_INS2(const Matrix<FloatT> &P, const Matrix<FloatT> &Q) 
        : INS<FloatT>(), m_filter(P, Q) {}
    
    /**
     * コピーコンストラクタ
     * 
     * @param orig コピー元
     * @param deepcopy ディープコピーを作成するかどうか
     */
    Filtered_INS2(const Filtered_INS2 &orig, const bool deepcopy = false)
        : INS<FloatT>(orig, deepcopy),
          m_filter(orig.m_filter, deepcopy){
      //cout << "FINS2" << endl;
    }
    
    virtual ~Filtered_INS2(){}

  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> dcm_e2n((this->q_e2n).getDCM());
      Matrix<FloatT> dcm_n2b((this->q_n2b).getDCM());
    
      Matrix<FloatT> A(getA(accel, gyro, dcm_e2n, dcm_n2b));
      Matrix<FloatT> B(getB(accel, gyro, dcm_n2b));
      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){
      
      // 速度
      for(unsigned i = 0; i < 3; i++){(*this)[i] -= x_hat(i, 0);}
      
      // 2D位置修正
#ifndef pow2
#define pow2(x) ((x) * (x))
#else
#define ALREADY_POW2_DEFINED
#endif
      /*Quarternion<FloatT> delta_q_e2n(
                                  sqrt(1. - (pow2(x_hat(3, 0)) + pow2(x_hat(4, 0)) + pow2(x_hat(5, 0)))), 
                                  -x_hat(3, 0), 
                                  -x_hat(4, 0), 
                                  -x_hat(5, 0)
                                );*/
      Quarternion<FloatT> delta_q_e2n(1, -x_hat(3, 0), -x_hat(4, 0), -x_hat(5, 0));
      //cout << delta_q_e2n << endl;
      (this->q_e2n) = delta_q_e2n * (this->q_e2n);
      
      // z位置
      (*this)[7] -= x_hat(6, 0);
      
      // 姿勢修正
      /*Quarternion<FloatT> delta_q_n2b(
                                  sqrt(1. - (pow2(x_hat(7, 0)) + pow2(x_hat(8, 0)) + pow2(x_hat(9, 0)))),
                                  -x_hat(7, 0), 
                                  -x_hat(8, 0), 
                                  -x_hat(9, 0)
                                );*/
      Quarternion<FloatT> delta_q_n2b(1, -x_hat(7, 0), -x_hat(8, 0), -x_hat(9, 0));
      (this->q_n2b) = delta_q_n2b * (this->q_n2b);
      
      /*T norm(delta_q_n2b.abs());
      for(int i = 7; i < 10 ; i++){
        for(int j = 7; j < 10 ; j++){
          m_filter.getP()(i, j) *= norm;
        }
      }*/ 
#ifdef ALREADY_POW2_DEFINED
#undef ALREADY_POW2_DEFINED
#else
#undef pow2
#endif

      for(unsigned i = 10; i < x_hat.rows(); i++){(*this)[i + 2] -= x_hat(i, 0);}

      INS<FloatT>::recalc();
    }

  public:
//#define DEBUG_DUMP_CORRECTION
//#define DEBUG_DUMP_Q_N2B
    /**
     * 観測更新(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);

#ifdef DEBUG_DUMP_CORRECTION
      static bool is_first(true);
      
      if(is_first){
        for(int i = 0; i < z.rows(); i++) cerr << "z" << i << '\t';
        for(int i = 0; i < 10; i++) cerr << "x_hat" << i << '\t';
        cerr << endl;
        is_first = false;
      }
     
      for(int i = 0; i < z.rows(); i++) cerr << const_cast<Matrix<FloatT> *>(&z)->operator()(i, 0) << '\t';
      for(int i = 0; i < x_hat.rows(); i++) cerr << x_hat(i, 0) << '\t';
      cerr << endl;
#endif
      
      correct_INS(x_hat);
      
#ifdef DEBUG_DUMP_Q_N2B
      for(int i = 0; i < 4; i++) cerr << this->q_n2b[i] << ",";
      for(int i = 0; i < 3; i++) cerr << (m_filter.getP())(i + 7, i + 7) << ",";
      cerr << endl;
#endif
    }
    /**
     * フィルターを取得します。
     * P行列やQ行列が欲しい場合はgetFilter().getP()などとしてください。
     * 
     * @return (Filter &) フィルター
     */
    Filter &getFilter(){return m_filter;}

    struct StandardDeviations {
      FloatT v_north_ms, v_east_ms, v_down_ms;
      FloatT longitude_rad, latitude_rad, height_m;
      FloatT heading_rad, pitch_rad, roll_rad;
    };

    /**
     * 標準偏差を馴染みの形で求めます。
     *
     * @return (StandardDeviations) [北/東/下方向速度、経/緯度、高度、ヘディング、ピッチ、ロール]
     * で構成された標準偏差
     */
    StandardDeviations getSigma() const {
      StandardDeviations sigma;

      Matrix<FloatT> &P(const_cast<Matrix<FloatT> &>(
          const_cast<Filtered_INS2 *>(this)->getFilter().getP()));

      // 速度
      sigma.v_north_ms = std::sqrt(P(0, 0));
      sigma.v_east_ms = std::sqrt(P(1, 1));
      sigma.v_down_ms = std::sqrt(P(2, 2));

      // 位置
      {
        FloatT cl(std::cos(INS<FloatT>::lambda)), sl(std::sin(INS<FloatT>::lambda));

        Matrix<FloatT> M(2, 3);

        M(0, 0) = 0;
        M(0, 1) = 0;
        M(0, 2) = 1;

        M(1, 0) = - sl * cl * 2;
        M(1, 1) = cl * cl * 2 - 1;
        M(1, 2) = 0;

        Matrix<FloatT> P_euler_e2n(M * P.partial(3, 3, 3, 3) * M.transpose());

        sigma.longitude_rad = std::sqrt(P_euler_e2n(0, 0)) * 2; // 経度
        sigma.latitude_rad = std::sqrt(P_euler_e2n(1, 1)) * 2; // 緯度

        sigma.height_m = std::sqrt(P(6, 6)); // 高度
      }

      // 姿勢
      {
        FloatT psi(INS<FloatT>::euler_psi()), theta(INS<FloatT>::euler_theta()), phi(INS<FloatT>::euler_phi());
#if 1 // 最小二乗法(事前計算)
        FloatT cpsi(std::cos(psi)), spsi(std::sin(psi));
        FloatT ctheta(std::cos(theta)), ttheta(std::tan(theta));
        Matrix<FloatT> M_conv(3, 3);
        {
          M_conv(0, 0) =  cpsi * ttheta; M_conv(0, 1) = spsi * ttheta; M_conv(0, 2) = 1;
          M_conv(1, 0) = -spsi;          M_conv(1, 1) = cpsi;          M_conv(1, 2) = 0;
          M_conv(2, 0) =  cpsi / ctheta; M_conv(2, 1) = spsi / ctheta; M_conv(2, 2) = 0;
        }
#else
        FloatT cpsi(std::cos(psi / 2)), ctheta(std::cos(theta / 2)), cphi(std::cos(phi / 2));
        FloatT spsi(std::sin(psi / 2)), stheta(std::sin(theta / 2)), sphi(std::sin(phi / 2));

        FloatT q0(get(8)), q0s(q0 - (spsi * stheta * sphi* 2));
        FloatT q1(get(9)), q1s(q1 + (spsi * stheta * cphi* 2));
        FloatT q2(get(10)), q2s(q2 - (spsi * ctheta * sphi* 2));
        FloatT q3(get(11)), q3s(q3 + (cpsi * stheta * sphi* 2));
#if 1 // 最小二乗法
        Matrix<FloatT> M_euler(4, 3);
        {
          M_euler(0, 0) = -q3; M_euler(0, 1) = -q2s; M_euler(0, 2) = -q1;
          M_euler(1, 0) = -q2; M_euler(1, 1) = -q3s; M_euler(1, 2) =  q0;
          M_euler(2, 0) =  q1; M_euler(2, 1) =  q0s; M_euler(2, 2) =  q3;
          M_euler(3, 0) =  q0; M_euler(3, 1) = -q1s; M_euler(3, 2) = -q2;
        }
        Matrix<FloatT> M_euler_trans(M_euler.transpose());

        Matrix<FloatT> M_u(4, 3);
        {
          M_u(0, 0) = -q1; M_u(0, 1) = -q2; M_u(0, 2) = -q3;
          M_u(1, 0) =  q0; M_u(1, 1) =  q3; M_u(1, 2) = -q2;
          M_u(2, 0) = -q3; M_u(2, 1) =  q0; M_u(2, 2) =  q1;
          M_u(3, 0) =  q2; M_u(3, 1) = -q1; M_u(3, 2) =  q0;
        }

        Matrix<FloatT> M_conv((M_euler_trans * M_euler).inverse() * M_euler_trans * M_u);
#else // TODO 直接逆行列を使う(結果があやしい)
        Matrix<FloatT> M_euler(4, 4);
        {
          M_euler(0, 0) = q0; M_euler(0, 1) = -q3; M_euler(0, 2) = -q2s; M_euler(0, 3) = -q1;
          M_euler(1, 0) = q1; M_euler(1, 1) = -q2; M_euler(1, 2) = -q3s; M_euler(1, 3) =  q0;
          M_euler(2, 0) = q2; M_euler(2, 1) =  q1; M_euler(2, 2) =  q0s; M_euler(2, 3) =  q3;
          M_euler(3, 0) = q3; M_euler(3, 1) =  q0; M_euler(3, 2) = -q1s; M_euler(3, 3) = -q2;
        }

        Matrix<FloatT> M_u(4, 4);
        {
          M_u(0, 0) = q0; M_u(0, 0) = -q1; M_u(0, 1) = -q2; M_u(0, 2) = -q3;
          M_u(1, 0) = q1; M_u(1, 0) =  q0; M_u(1, 1) =  q3; M_u(1, 2) = -q2;
          M_u(2, 0) = q2; M_u(2, 0) = -q3; M_u(2, 1) =  q0; M_u(2, 2) =  q1;
          M_u(3, 0) = q3; M_u(3, 0) =  q2; M_u(3, 1) = -q1; M_u(3, 2) =  q0;
        }

        Matrix<FloatT> M_conv_all(M_euler.inverse() * M_u);
        Matrix<FloatT> M_conv(M_conv_all.partial(3, 3, 1, 1));
#endif
#endif
        Matrix<FloatT> P_euler_n2b(M_conv * P.partial(3, 3, 7, 7) * M_conv.transpose());

        sigma.heading_rad = std::sqrt(P_euler_n2b(0, 0)) * 2; // ヨー
        sigma.pitch_rad = std::sqrt(P_euler_n2b(1, 1)) * 2; // ピッチ
        sigma.roll_rad = std::sqrt(P_euler_n2b(2, 2)) * 2; // ロール
      }

      return sigma;
    }
};

class Filtered_INS2A_Property {
  public:
    static const unsigned Q_SIZE
#if defined(_MSC_VER)
        = 9
#endif
        ; ///< Q行列(入力誤差共分散行列)の大きさ
};

#if !defined(_MSC_VER)
const unsigned Filtered_INS2A_Property::Q_SIZE = 9;
#endif

/**
 * @brief 他の航法装置を統合する為のINS拡張クラス2(Multiplicative)
 * 
 * Filtered_INS2と基本は同じですが、
 * 重力誤差をスカラー量ではなく3次元ベクトル量とした場合の定義です。
 * そのためQ行列のサイズが7から9に変更されています。
 * 
 * @param FloatT 演算精度
 * @param Filter カルマンフィルタ
 * @see Filtered_INS2
 */
template <class FloatT, typename Filter = KalmanFilterUD<FloatT> >
class Filtered_INS2A 
    : public Filtered_INS2<FloatT, Filter>, 
    public Filtered_INS2A_Property {
  public:
    using Filtered_INS2<FloatT, Filter>::P_SIZE;
    using Filtered_INS2A_Property::Q_SIZE;
   
  protected:
    /**
     * システム誤差方程式におけるB行列を返します。
     * 
     * @return B行列
     * @see Filtered_INS2::getB(const Vector3<FloatT> &, const Vector3<FloatT> &, Matrix<FloatT> &)
     */
    Matrix<FloatT> getB(const Vector3<FloatT> &accel, const Vector3<FloatT> &gyro, Matrix<FloatT> &dcm_n2b){

      //行列Bの計算
      FloatT B_serialized[P_SIZE][Q_SIZE] = {{FloatT(0)}};
#define B(i, j) B_serialized[i][j]
      {
        B(0, 0) = dcm_n2b(0, 0);
        B(0, 1) = dcm_n2b(1, 0);
        B(0, 2) = dcm_n2b(2, 0);
    
        B(1, 0) = dcm_n2b(0, 1);
        B(1, 1) = dcm_n2b(1, 1);
        B(1, 2) = dcm_n2b(2, 1);
    
        B(2, 0) = dcm_n2b(0, 2);
        B(2, 1) = dcm_n2b(1, 2);
        B(2, 2) = dcm_n2b(2, 2);
        
        B(0, 6) = 1;  // 重力誤差X
        B(1, 7) = 1;  // 重力誤差Y
        B(2, 8) = 1;  // 重力誤差Z
    
        B(7, 3) = dcm_n2b(0, 0) / 2;
        B(7, 4) = dcm_n2b(1, 0) / 2;
        B(7, 5) = dcm_n2b(2, 0) / 2;
    
        B(8, 3) = dcm_n2b(0, 1) / 2;
        B(8, 4) = dcm_n2b(1, 1) / 2;
        B(8, 5) = dcm_n2b(2, 1) / 2;
    
        B(9, 3) = dcm_n2b(0, 2) / 2;
        B(9, 4) = dcm_n2b(1, 2) / 2;
        B(9, 5) = dcm_n2b(2, 2) / 2;
      }
#undef B
      
      return Matrix<FloatT>(
          sizeof(B_serialized) / sizeof(B_serialized[0]),
          sizeof(B_serialized[0]) / sizeof(B_serialized[0][0]),           
          (FloatT *)B_serialized);
    }
  
  public:
    /**
     * コンストラクタ。
     * 
     * @see Filtered_INS2::Filtered_INS2()
     */
    Filtered_INS2A() 
      : Filtered_INS2<FloatT, Filter>(
            Matrix<FloatT>::getI(P_SIZE), 
            Matrix<FloatT>::getI(Q_SIZE)
          ){}
    
    /**
     * コピーコンストラクタ
     * 
     * @param orig コピー元
     * @param deepcopy ディープコピーを作成するかどうか
     */
    Filtered_INS2A(const Filtered_INS2A &orig, const bool deepcopy = false) :
      Filtered_INS2<FloatT, Filter>(orig, deepcopy){
      
    }
    
    /**
     * デストラクタ。
     * 
     */
    ~Filtered_INS2A(){};
};

#endif /* __FILTERED_INS2_H__ */