#ifndef __CALIBRATION_H__
#define __CALIBRATION_H__

#include "util/util.h"
#include "param/vector3.h"

#include <cstring>
#include <cstdlib>
#include <cctype>

class Calibrator {
  
  public:
    Calibrator() {}
    virtual ~Calibrator() {}
  
    /**
     * 加速度の値をVector3形式で得る
     * 単位はm/s^2
     */
    virtual Vector3<float_sylph_t> raw2accel(const int ch[9]) const = 0;
    
    /**
     * 角速度の値をVector3形式で得る
     * 単位はrad/sec
     */
    virtual Vector3<float_sylph_t> raw2gyro(const int ch[9]) const = 0;
    
    /**
     * 加速度計X,Y,Zの分散(単位はm/s^2)
     * 
     */
    virtual Vector3<float_sylph_t> sigma_accel() const = 0;
    
    
    /**
     * ジャイロX,Y,Zの分散(単位はrad/s)
     * 
     */
    virtual Vector3<float_sylph_t> sigma_gyro() const = 0;
};

class RotatedCalibrator : public Calibrator {
  public:
    struct rotate_spec_t {
      int xyz[3];
      int xyz_sign[3];
      
      enum Index {
          X_INDEX = 0,  ///< X要素はインデックス0
          Y_INDEX,      ///< Y要素はインデックス1
          Z_INDEX};     ///< Z要素はインデックス2
      
      Vector3<float_sylph_t> rotate(const Vector3<float_sylph_t> &orig) const {
        return Vector3<float_sylph_t>(
            orig.get(xyz[X_INDEX]) * xyz_sign[X_INDEX],
            orig.get(xyz[Y_INDEX]) * xyz_sign[Y_INDEX],
            orig.get(xyz[Z_INDEX]) * xyz_sign[Z_INDEX]);
      }
      
      Matrix<float_sylph_t> rotate_mat() const {
        Matrix<float_sylph_t> res(3, 3);
        for(int i(0); i < res.rows(); i++){
          for(int j(0); j < res.columns(); j++){
            res(i, j) = (xyz[i] == j) ? xyz_sign[i] : 0;
          }
        }     
        return res;
      }
    };
  protected:
    const Calibrator &orig;
    rotate_spec_t rotate_spec;
    
  public:
    static bool decode(
        const char *spec, 
        rotate_spec_t &decoded){
      //右手系で許される組み合わせ(全組み合わせは2*3*2*2*2*1=48通り、そのうち半分が右手系)
      const char *valid_specs[] = {
          "+x+y+z", "+x-z+y", "+x-y-z", "+x+z-y",
          "-x-y+z", "-x+z+y", "-x+y-z", "-x-z+y"
          "+y+z+x", "+y-x+z", "+y-z-x", "+y+x-z",
          "-y-z+x", "-y+x+z", "-y+z-x", "-y-x-z",
          "+z+y-x", "+z-x-y", "+z-y+x", "+z+x+y",
          "-z-y-x", "-z+x-y", "-z+y+x", "-z-x+y"};
      bool valid_spec(false);
      for(int i(0); i < sizeof(valid_specs) / sizeof(valid_specs[0]); ++i){
        if(std::strcmp(valid_specs[i], spec) == 0){
          valid_spec = true;
          break;
        }
      }
      
      if(valid_spec){
        for(int i(0), j(0); i < 3; i++, j+= 2){
          decoded.xyz[i] = spec[j+1] - 'x';
          decoded.xyz_sign[i] = (spec[j] == '-' ? -1 : 1);
        }
      }
      
      return valid_spec;
    }
    
    RotatedCalibrator(const Calibrator &_orig, const char *spec) 
        : Calibrator(), orig(_orig), rotate_spec() {
      
      if(!decode(spec, rotate_spec)){ // 不正な文字が入っていた場合は+x+y+zで初期化
        for(int i(0); 
            i < sizeof(rotate_spec.xyz) / sizeof(rotate_spec.xyz[0]); 
            ++i){
          rotate_spec.xyz[i] = i;
          rotate_spec.xyz_sign[i] = 1;
        }
      }
    }
    RotatedCalibrator(
        const Calibrator &_orig, 
        const rotate_spec_t &spec) 
        : Calibrator(), orig(_orig), rotate_spec() {
      
      for(int i(0); 
          i < sizeof(rotate_spec.xyz) / sizeof(rotate_spec.xyz[0]); 
          ++i){
        rotate_spec.xyz[i] = (spec.xyz[i] * ((spec.xyz[i] >= 0) ? 1 : -1)) % 3;
        rotate_spec.xyz_sign[i] = (spec.xyz_sign[i] > 0) ? 1 : -1;
      }
    }
    virtual ~RotatedCalibrator(){}
    
    Vector3<float_sylph_t> rotate(const Vector3<float_sylph_t> &orig) const {
      return rotate_spec.rotate(orig);
    }
    
    Matrix<float_sylph_t> rotate_mat() const {
      return rotate_spec.rotate_mat();
    }
    
    /**
     * 加速度の値をVector3形式で得る
     * 単位はm/s^2
     */
    virtual Vector3<float_sylph_t> raw2accel(const int ch[9]) const {
      return rotate(orig.raw2accel(ch));
    }
    
    /**
     * 角速度の値をVector3形式で得る
     * 単位はrad/sec
     */
    virtual Vector3<float_sylph_t> raw2gyro(const int ch[9]) const {
      return rotate(orig.raw2gyro(ch));
    }
    
    /**
     * 加速度計X,Y,Zの分散(単位はm/s^2)
     * 
     */
    virtual Vector3<float_sylph_t> sigma_accel() const {
      return rotate(orig.sigma_accel());
    }
    
    /**
     * ジャイロX,Y,Zの分散(単位はrad/s)
     * 
     */
    virtual Vector3<float_sylph_t> sigma_gyro() const {
      return rotate(orig.sigma_gyro());
    }
};

class TestCalibrator : public Calibrator {
  
  public:
    static const char *id; 
    TestCalibrator() : Calibrator() {}
    ~TestCalibrator(){}
    
    Vector3<float_sylph_t> raw2accel(const int ch[9]) const{
      return Vector3<float_sylph_t>(ch[0], ch[1], ch[2]);
    }
    
    Vector3<float_sylph_t> raw2gyro(const int ch[9]) const{
      return Vector3<float_sylph_t>(ch[3], ch[4], ch[5]);
    }
    
    Vector3<float_sylph_t> sigma_accel() const {
      return Vector3<float_sylph_t>(1, 1, 1);
    }
    
    Vector3<float_sylph_t> sigma_gyro() const {
      return Vector3<float_sylph_t>(deg2rad(1.), deg2rad(1.), deg2rad(1.));
    }
};
const char *TestCalibrator::id = "test";

#ifndef PI
#ifdef M_PI
#define PI M_PI
#else
#define PI 3.14159265358979
#endif
#endif

class StandardCalibrator : public Calibrator {
  
  public:
    unsigned int index_base;
    unsigned int index_temp_ch;
  
    StandardCalibrator() : Calibrator(), index_base(0), index_temp_ch(8){}
    virtual ~StandardCalibrator(){}
    
    virtual Vector3<float_sylph_t> acc_bias(int temp_ch) const = 0;
    virtual Vector3<float_sylph_t> acc_sf(int temp_ch) const = 0;
    virtual Matrix<float_sylph_t> acc_mis() const = 0;
    virtual Vector3<float_sylph_t> gyro_bias(int temp_ch) const = 0;
    virtual Vector3<float_sylph_t> gyro_sf(int temp_ch) const = 0;
    virtual Matrix<float_sylph_t> gyro_mis() const = 0;
  
    /**
     * 加速度の値をVector3形式で得る
     * 単位はm/s^2
     */
    Vector3<float_sylph_t> raw2accel(const int ch[9]) const{
      
      /* バイアス除去ならびに
       * スケールファクタ積算による正規化
       * (単位をm/s^2にする作業)
       */
      Vector3<float_sylph_t> bias(acc_bias(ch[index_temp_ch]));
      Vector3<float_sylph_t> sf(acc_sf(ch[index_temp_ch]));
      Vector3<float_sylph_t> before_mis;
      for(unsigned int i = 0; 
          i < Vector3<float_sylph_t>::OUT_OF_INDEX;
          i++){
        before_mis[i]
          = (((float_sylph_t)ch[i + index_base] - bias[i]) / sf[i]);
      }
      
      /* ミスアライメント補正
       * 直交していない場合はこの変換行列を定めることで対応
       * その後、最終的に検出されたX,Y,Z軸の加速度とする
       */
      return acc_mis() * before_mis;
    }
    
    /**
     * 角速度の値をVector3形式で得る
     * 単位はrad/sec
     */
    Vector3<float_sylph_t> raw2gyro(const int ch[9]) const{
      /* バイアス除去ならびに
       * スケールファクタ積算による正規化
       * (単位をrad/sにする作業)
       */
      Vector3<float_sylph_t> bias(gyro_bias(ch[index_temp_ch]));
      Vector3<float_sylph_t> sf(gyro_sf(ch[index_temp_ch]));
      Vector3<float_sylph_t> before_mis;
      for(unsigned int i = 0;
          i < Vector3<float_sylph_t>::OUT_OF_INDEX; 
          i++){
        before_mis[i] 
          = (((float_sylph_t)ch[i + 3 + index_base] - bias[i]) / sf[i]);
      }
      
      /* ミスアライメント補正
       * 直交していない場合はこの変換行列を定めることで対応
       * その後、最終的に検出されたX,Y,Z軸の加速度とする
       */
      return gyro_mis() * before_mis;
    }
};

class IMU_T0 : public StandardCalibrator{
  public:
    static const char *id;
    IMU_T0() : StandardCalibrator() {
      StandardCalibrator::index_base = 2;
    }
    ~IMU_T0(){}
    
    Vector3<float_sylph_t> acc_bias(int temp_ch) const {
      return Vector3<float_sylph_t>(
          8488558.18  + ((float_sylph_t)temp_ch - 383.5) * 2460.805471,
          8654766.327 + ((float_sylph_t)temp_ch - 383.5) * 1100.370850,
          8619335.387 + ((float_sylph_t)temp_ch - 383.5) * 8426.074735
        ); 
        // zero <= ma.exe(temp_ch@average(X,0deg/sec))
        // slope <= tb.pdf
    }
    Vector3<float_sylph_t> acc_sf(int temp_ch) const {
      return Vector3<float_sylph_t>(
          350748.3348, 
          347999.8231,
          -359479.2142
        );
        // <= ma.exe
    }
    Matrix<float_sylph_t> acc_mis() const {
      const float_sylph_t mis[3][3] = {
          { 0.9999766561,    -0.00680183347,  -0.000649848462},
          { 0.006797369404,   0.9999548457,   -0.006640951458},
          { 0.0006949897645,  0.006636379172,  0.9999777375}};
      
      return Matrix<float_sylph_t>(
          sizeof(mis) / sizeof(mis[0]),
          sizeof(mis[0]) / sizeof(mis[0][0]),
          (float_sylph_t *)mis);
      // <= ma.exe
    }
    Vector3<float_sylph_t> gyro_bias(int temp_ch) const {
      return Vector3<float_sylph_t>(
          (float_sylph_t)3628.223982 * temp_ch + 6213201.347913,
          (float_sylph_t)-3835.833629 * temp_ch + 9760881.047663, 
          (float_sylph_t)1024.648189 * temp_ch + 7987901.193722
        );
        // <= tb.pdf
    }
    Vector3<float_sylph_t> gyro_sf(int temp_ch) const {
      return Vector3<float_sylph_t>(
          ((float_sylph_t)41590.50546) * 180 / PI,  // rad/sec 
          ((float_sylph_t)42660.69728) * 180 / PI,  // rad/sec 
          ((float_sylph_t)41818.04588) * 180 / PI   // rad/sec
        );
        // ma.exe
    }
    Matrix<float_sylph_t> gyro_mis() const {
      const float_sylph_t mis[3][3] = {
          { 1.000009615,      0.0005032535034,  -0.007384590231},
          {-0.005309183072,   1.000255406,      -0.0179208169},
          { 0.002061394568,  -0.004617731355,    1.000080445}};
      
      return Matrix<float_sylph_t>(
          sizeof(mis) / sizeof(mis[0]),
          sizeof(mis[0]) / sizeof(mis[0][0]),
          (float_sylph_t *)mis);
      // ma.exe
    }

    Vector3<float_sylph_t> sigma_accel() const {
      Vector3<float_sylph_t> sf(acc_sf(0));
      Vector3<float_sylph_t> raw(
          1E4 / sf[0],
          1E4 / sf[1],
          1E4 / sf[2]);
      
      // estimation from ma_mu_sigma.xls
      return acc_mis() * raw;
    }
    
    Vector3<float_sylph_t> sigma_gyro() const {
      Vector3<float_sylph_t> sf(gyro_sf(0));
      Vector3<float_sylph_t> raw(
          (35232.28195 + 19106.62465 + 20216.83883) / 3 / sf[0],
          (20006.43482 + 19293.97923 + 20211.75002) / 3 / sf[1],
          (21667.17818 + 22023.13098 + 22325.15644) / 3 / sf[2]);
      // ma_mu_sigma.xls @ X, 0deg/sec
      return gyro_mis() * raw;
    }
};
const char *IMU_T0::id = "IMU_T0";

class IMU_T1 : public StandardCalibrator{
  public:
    static const char *id;
    IMU_T1() : StandardCalibrator() {
      StandardCalibrator::index_base = 0;
    }
    ~IMU_T1(){}
    
    Vector3<float_sylph_t> acc_bias(int temp_ch) const {
      return Vector3<float_sylph_t>(
          8644020.521 + ((float_sylph_t)temp_ch - 390.4) * 1842.002130,
          8221985.563 + ((float_sylph_t)temp_ch - 390.4) * 387.381803,
          8896843.941 + ((float_sylph_t)temp_ch - 390.4) * 6126.597434
        );
    }
    Vector3<float_sylph_t> acc_sf(int temp_ch) const {
      return Vector3<float_sylph_t>(
          353483.5819,
          353462.519,
          -350120.7474
        );
    }
    Matrix<float_sylph_t> acc_mis() const {
      const float_sylph_t mis[3][3] = {
          { 0.9999971144,    -0.0008220286881,  0.002257299495},
          { 0.0008259290744,  0.9999981668,    -0.001727511099},
          {-0.002255875293,   0.001729370484,   0.9999959601}};
      return Matrix<float_sylph_t>(
          sizeof(mis) / sizeof(mis[0]),
          sizeof(mis[0]) / sizeof(mis[0][0]),
          (float_sylph_t *)mis);
    }
    Vector3<float_sylph_t> gyro_bias(int temp_ch) const {
      return Vector3<float_sylph_t>(
          (float_sylph_t)-913.013327 * temp_ch + 10475152.072652,
          (float_sylph_t)23579.268223 * temp_ch + 909984.532229, 
          (float_sylph_t)-2086.095313 * temp_ch + 11045103.597762
        );
    }
    Vector3<float_sylph_t> gyro_sf(int temp_ch) const {
      return Vector3<float_sylph_t>(
          ((float_sylph_t)58707.57752) * 180 / PI,  // rad/sec
          ((float_sylph_t)58466.98749) * 180 / PI,  // rad/sec 
          ((float_sylph_t)59476.27511) * 180 / PI   // rad/sec
        );
    }
    Matrix<float_sylph_t> gyro_mis() const {
      const float_sylph_t mis[3][3] = {
          { 1.185709041,  -0.2475056504,  -0.2333186845},
          {-0.2116587924,  1.177835346,   -0.2659847294},
          {-0.2760560323, -0.1972282096,   1.184157712}};
      return Matrix<float_sylph_t>(
          sizeof(mis) / sizeof(mis[0]),
          sizeof(mis[0]) / sizeof(mis[0][0]),
          (float_sylph_t *)mis);
    }

    Vector3<float_sylph_t> sigma_accel() const {
      Vector3<float_sylph_t> sf(acc_sf(0));
      Vector3<float_sylph_t> raw(
          1E4 / sf[0],
          1E4 / sf[1],
          1E4 / sf[2]);
      return acc_mis() * raw;
    }
    
    Vector3<float_sylph_t> sigma_gyro() const {
      Vector3<float_sylph_t> sf(gyro_sf(0));
      Vector3<float_sylph_t> raw(
          (31471.80811 + 30084.59774 + 29482.26399) / 3 / sf[0],
          (32467.19626 + 32157.63166 + 31833.89811) / 3 / sf[1],
          (27721.00184 + 27866.58007 + 27499.16041) / 3 / sf[2]);
      return gyro_mis() * raw;
    }
    
    /*Vector3<float_sylph_t> raw2accel(const int ch[9]) const{
      Vector3<float_sylph_t> orig(StandardCalibrator::raw2accel(ch));
      Vector3<float_sylph_t> mod(0.0613888040017289, 0.105127488788353, -0.35643123673503);
      return orig - mod;
      // T1 - E0 (mean, sigma)
      // 0: 0.0613888040017289, 0.0711747111737325
      // 1: 0.105127488788353, 0.0116939325166733
      // 2: -0.35643123673503, 0.0414350247251706
    }
    
    virtual Vector3<float_sylph_t> raw2gyro(const int ch[9]) const{
      Vector3<float_sylph_t> orig(StandardCalibrator::raw2gyro(ch));
      Vector3<float_sylph_t> mod(0.00845510390501084, 0.136559221813616, 0.0125198411230387);
      return orig - mod;
      // T1 - E0 (mean, sigma)
      // 3: 0.00845510390501084, 0.000192520440800412
      // 4: 0.136559221813616, 0.00151463052248204
      // 5: 0.0125198411230387, 0.000162961216719998
    }*/
};
const char *IMU_T1::id = "IMU_T1";

class IMU_E0 : public StandardCalibrator{
  public:
    static const char *id;
    IMU_E0() : StandardCalibrator() {
      StandardCalibrator::index_base = 0;
    }
    ~IMU_E0(){}
    
    Vector3<float_sylph_t> acc_bias(int temp_ch) const {
      return Vector3<float_sylph_t>(
          8384137.06  + ((float_sylph_t)temp_ch - 382.7) * 2879.895303,
          8582977.329 + ((float_sylph_t)temp_ch - 382.7) * 495.602553,
          8592260.272 + ((float_sylph_t)temp_ch - 382.7) * 8717.642473
        );
    }
    Vector3<float_sylph_t> acc_sf(int temp_ch) const {
      return Vector3<float_sylph_t>(
          -345829.9706,
          -356561.8403,
          357889.067
        ) * -1; // -gで計測していたのを忘れるな!!(11/14)
    }
    Matrix<float_sylph_t> acc_mis() const {
      const float_sylph_t mis[3][3] = {
          { 0.9997624156,   -0.00297374952,   -0.02159326518},
          { 0.002981766819,  0.999995497,      0.0003390996509},
          { 0.02159215955,  -0.0004034051678,  0.9997667808}};
      return Matrix<float_sylph_t>(
          sizeof(mis) / sizeof(mis[0]),
          sizeof(mis[0]) / sizeof(mis[0][0]),
          (float_sylph_t *)mis);
    }
    Vector3<float_sylph_t> gyro_bias(int temp_ch) const {
      return Vector3<float_sylph_t>(
          (float_sylph_t)-202.153864 * temp_ch + 8442886.885330,
          (float_sylph_t)-422.614729 * temp_ch + 8592680.836395, 
          (float_sylph_t)-238.637566 * temp_ch + 8433408.021407
        );
    }
    Vector3<float_sylph_t> gyro_sf(int temp_ch) const {
      return Vector3<float_sylph_t>(
          ((float_sylph_t)41248.8932) * 180 / PI,  // rad/sec
          ((float_sylph_t)41264.78029) * 180 / PI,  // rad/sec 
          ((float_sylph_t)41782.51423) * 180 / PI   // rad/sec
        );
    }
    Matrix<float_sylph_t> gyro_mis() const {
      const float_sylph_t mis[3][3] = {
          {  0.9988364217,    0.0223773125,   -0.0265951597},
          { -0.002837741235,  0.9999439655,   -0.008181823492},
          { 0.06444209849,    0.004972402211,  1.000340948}};
      return Matrix<float_sylph_t>(
          sizeof(mis) / sizeof(mis[0]),
          sizeof(mis[0]) / sizeof(mis[0][0]),
          (float_sylph_t *)mis);
    }

    Vector3<float_sylph_t> sigma_accel() const {
      Vector3<float_sylph_t> sf(acc_sf(0));
      Vector3<float_sylph_t> raw(
          1E4 / sf[0],
          1E4 / sf[1],
          1E4 / sf[2]);
      return acc_mis() * raw;
    }
    
    Vector3<float_sylph_t> sigma_gyro() const {
      Vector3<float_sylph_t> sf(gyro_sf(0));
      Vector3<float_sylph_t> raw(
          4E3 / sf[0],
          4E3 / sf[1],
          4E3 / sf[2]);
      // est. from ma_mu_sigma.xls
      return gyro_mis() * raw;
    }
};
const char *IMU_E0::id = "IMU_E0";

class IMU_E1 : public StandardCalibrator{
  public:
    static const char *id;
    IMU_E1() : StandardCalibrator() {
      StandardCalibrator::index_base = 0;
    }
    ~IMU_E1(){}
    
    Vector3<float_sylph_t> acc_bias(int temp_ch) const {
      return Vector3<float_sylph_t>(
          8630184.276 + ((float_sylph_t)temp_ch - 390.1) * 2036.371157,
          8718104.793 + ((float_sylph_t)temp_ch - 390.1) * 1092.026892,
          9231189.202 + ((float_sylph_t)temp_ch - 390.1) * 7677.174791
        );
    }
    Vector3<float_sylph_t> acc_sf(int temp_ch) const {
      return Vector3<float_sylph_t>(
          -355323.0125,
          -359711.6493,
          361113.7008
        ) * -1; // -gで計測していたのを忘れるな!!(11/14)
    }
    Matrix<float_sylph_t> acc_mis() const {
      const float_sylph_t mis[3][3] = {
          { 0.999896328,    -0.0140202137,    0.003281297177},
          { 0.01400988323,   0.9998968938,    0.003150380419},
          {-0.003325127862, -0.003104083223,  0.999989654}};
      return Matrix<float_sylph_t>(
          sizeof(mis) / sizeof(mis[0]),
          sizeof(mis[0]) / sizeof(mis[0][0]),
          (float_sylph_t *)mis);
    }
    Vector3<float_sylph_t> gyro_bias(int temp_ch) const {
      return Vector3<float_sylph_t>(
          (float_sylph_t)-362.827133 * temp_ch + 8482795.827132,
          (float_sylph_t)90.034111 * temp_ch + 8392196.460149, 
          (float_sylph_t)-234.56717 * temp_ch + 8580756.390961
        );
    }
    Vector3<float_sylph_t> gyro_sf(int temp_ch) const {
      return Vector3<float_sylph_t>(
          ((float_sylph_t)41644.7604) * 180 / PI,  // rad/sec
          ((float_sylph_t)41465.4576) * 180 / PI,  // rad/sec 
          ((float_sylph_t)41352.30616) * 180 / PI   // rad/sec
        );
    }
    Matrix<float_sylph_t> gyro_mis() const {
      const float_sylph_t mis[3][3] = {
          { 0.999863297,    -0.003371996897,  -0.01004608045},
          {-0.006810545144,  1.00135929,      -0.04028177391},
          { 0.02145649341,  -0.01262872112,    1.00059739}};
      return Matrix<float_sylph_t>(
          sizeof(mis) / sizeof(mis[0]),
          sizeof(mis[0]) / sizeof(mis[0][0]),
          (float_sylph_t *)mis);
    }

    Vector3<float_sylph_t> sigma_accel() const {
      Vector3<float_sylph_t> sf(acc_sf(0));
      Vector3<float_sylph_t> raw(
          1E4 / sf[0],
          1E4 / sf[1],
          1E4 / sf[2]);
      return acc_mis() * raw;
    }
    
    Vector3<float_sylph_t> sigma_gyro() const {
      Vector3<float_sylph_t> sf(gyro_sf(0));
      Vector3<float_sylph_t> raw(
          4E3 / sf[0],
          4E3 / sf[1],
          4E3 / sf[2]);
      // est. form ma_mu_sigma.xls
      return gyro_mis() * raw;
    }
};
const char *IMU_E1::id = "IMU_E1";

class IMU_E1_6G : public IMU_E1{
  public:
    static const char *id;
    IMU_E1_6G() : IMU_E1() {}
    ~IMU_E1_6G(){}
    
    Vector3<float_sylph_t> acc_bias(int temp_ch) const {
      return Vector3<float_sylph_t>(
          8105894.261 + ((float_sylph_t)temp_ch - 350) * (-44.086154 + -80.840906 + -80.840906) / 3,
          8161611.743 + ((float_sylph_t)temp_ch - 350) * (-100.453660 + 14.308807 + -152.768509) / 3,
          8271243.639 + ((float_sylph_t)temp_ch - 350) * (1824.190528 + 1632.108976 + 1682.758485) / 3);
    }
    Vector3<float_sylph_t> acc_sf(int temp_ch) const {
      return Vector3<float_sylph_t>(
          119058.3252,
          119475.6048,
          -114936.7297);
    }
    Matrix<float_sylph_t> acc_mis() const {
      const float_sylph_t mis[3][3] = {
          { 0.9999367694,    0.01027336584, -0.004573306933},
          {-0.01038333417,   0.9996407582,  -0.02470912426},
          { 0.004317818138,  0.02475504806,  0.9996842222}};
      return Matrix<float_sylph_t>(
          sizeof(mis) / sizeof(mis[0]),
          sizeof(mis[0]) / sizeof(mis[0][0]),
          (float_sylph_t *)mis);
    }
    Vector3<float_sylph_t> gyro_bias(int temp_ch) const {
      return Vector3<float_sylph_t>(
          ((float_sylph_t)(-137.819800 + -392.230207 + -398.392793) * temp_ch 
              + (8357524.174605 + 8490165.967736 + 8492564.478805)) / 3,
          ((float_sylph_t)(-361.958921 + -554.961403 + -1011.819176) * temp_ch 
              + (8556497.850003 + 8667798.701869 + 8774625.273953)) / 3, 
          ((float_sylph_t)(-323.887618 + -298.122546 + -543.891833) * temp_ch 
              + (8609583.247294 + 8597366.402354 + 8727850.764068)) / 3);
    }
    Vector3<float_sylph_t> gyro_sf(int temp_ch) const {
      return Vector3<float_sylph_t>(
          ((float_sylph_t)41585.32458) * 180 / PI,  // rad/sec
          ((float_sylph_t)41286.97602) * 180 / PI,  // rad/sec 
          ((float_sylph_t)41422.18239) * 180 / PI);   // rad/sec
    }
    Matrix<float_sylph_t> gyro_mis() const {
      const float_sylph_t mis[3][3] = {
          { 0.9998567366,   -0.0002372130275, -0.01024637463},
          {-0.007695797439,  1.000154958,     -0.01040756316},
          { 0.01933702178,  -0.006691481378,   1.000080297}};
      return Matrix<float_sylph_t>(
          sizeof(mis) / sizeof(mis[0]),
          sizeof(mis[0]) / sizeof(mis[0][0]),
          (float_sylph_t *)mis);
    }
};
const char *IMU_E1_6G::id = "IMU_E1_6G";

class IMU_E2 : public StandardCalibrator{
  public:
    static const char *id;
    IMU_E2() : StandardCalibrator() {
      StandardCalibrator::index_base = 0;
    }
    ~IMU_E2(){}
    
    Vector3<float_sylph_t> acc_bias(int temp_ch) const {
      return Vector3<float_sylph_t>(
          8717340.916 + ((float_sylph_t)temp_ch - 350) * (132.858992 + 965.508010 + 640.408598) / 3,
          8766588.307 + ((float_sylph_t)temp_ch - 350) * (-422.456301 + 132.161753 + -685.883818) / 3,
          8592743.432 + ((float_sylph_t)temp_ch - 350) * (4407.360137 + 4432.515546 + 3869.906907) / 3);
    }
    Vector3<float_sylph_t> acc_sf(int temp_ch) const {
      return Vector3<float_sylph_t>(
          339436.0896,
          367725.5434,
          -339990.2401);
    }
    Matrix<float_sylph_t> acc_mis() const {
      const float_sylph_t mis[3][3] = {
          { 0.9989046844,   0.04565529804, -0.01024818259},
          {-0.04618453239,  0.9971706329,  -0.05931035126},
          { 0.007511354953, 0.05971869522,  0.998186985}};
      return Matrix<float_sylph_t>(
          sizeof(mis) / sizeof(mis[0]),
          sizeof(mis[0]) / sizeof(mis[0][0]),
          (float_sylph_t *)mis);
    }
    Vector3<float_sylph_t> gyro_bias(int temp_ch) const {
      return Vector3<float_sylph_t>(
          ((float_sylph_t)(566.517668 + 446.953330 + 451.157572) * temp_ch 
              + (8265338.797637 + 8350334.966438 + 8348506.312291)) / 3,
          ((float_sylph_t)(1064.460933 + 979.852772 + 1069.002950) * temp_ch 
              + (8063453.119965 + 8135450.198866 + 8063265.848317)) / 3, 
          ((float_sylph_t)(686.154208 + 690.331663 + 689.125398) * temp_ch 
              + (8209452.469206 + 8208466.599620 + 8254107.388526)) / 3);
    }
    Vector3<float_sylph_t> gyro_sf(int temp_ch) const {
      return Vector3<float_sylph_t>(
          ((float_sylph_t)41788.20384) * 180 / PI,  // rad/sec
          ((float_sylph_t)41449.16294) * 180 / PI,  // rad/sec 
          ((float_sylph_t)42027.17568) * 180 / PI   // rad/sec
        );
    }
    Matrix<float_sylph_t> gyro_mis() const {
      const float_sylph_t mis[3][3] = {
          {0.9999831369,-0.003530575325,0.006219479975},
          {0.003660629886,1.000683106,-0.03143548082},
          {-0.00472032661,-0.006196150989,1.000196256}};
      return Matrix<float_sylph_t>(
          sizeof(mis) / sizeof(mis[0]),
          sizeof(mis[0]) / sizeof(mis[0][0]),
          (float_sylph_t *)mis);
    }

    Vector3<float_sylph_t> sigma_accel() const {
      Vector3<float_sylph_t> sf(acc_sf(0));
      Vector3<float_sylph_t> raw(
          1E4 / sf[0],
          1E4 / sf[1],
          1E4 / sf[2]);
      return acc_mis() * raw;
    }
    
    Vector3<float_sylph_t> sigma_gyro() const {
      Vector3<float_sylph_t> sf(gyro_sf(0));
      Vector3<float_sylph_t> raw(
          4E3 / sf[0],
          4E3 / sf[1],
          4E3 / sf[2]);
      // est. form ma_mu_sigma.xls
      return gyro_mis() * raw;
    }
};
const char *IMU_E2::id = "IMU_E2";

class IMU_EM : public StandardCalibrator{
  public:
    static const char *id;
    IMU_EM() : StandardCalibrator() {
      StandardCalibrator::index_base = 0;
    }
    ~IMU_EM(){}
    
    Vector3<float_sylph_t> acc_bias(int temp_ch) const {
      return Vector3<float_sylph_t>(
          8677889.025 + ((float_sylph_t)temp_ch - 350) * (2278.618697 + 2684.458315 + 2514.844076) / 3,
          8570180.797 + ((float_sylph_t)temp_ch - 350) * (169.278559 + 469.411790 + 6.871192) / 3,
          8447692.787 + ((float_sylph_t)temp_ch - 350) * (6542.927214 + 5906.372858 + 6012.844603) / 3);
    }
    Vector3<float_sylph_t> acc_sf(int temp_ch) const {
      return Vector3<float_sylph_t>(
          355160.4738,
          358644.6501,
          -347053.1274);
    }
    Matrix<float_sylph_t> acc_mis() const {
      const float_sylph_t mis[3][3] = {
          { 0.9997456741,    0.02254900532,  0.0003598797152},
          {-0.02254855523,   0.9997450177,  -0.001209203912},
          {-0.0003870542977, 0.001200781613, 0.9999992042}};
      return Matrix<float_sylph_t>(
          sizeof(mis) / sizeof(mis[0]),
          sizeof(mis[0]) / sizeof(mis[0][0]),
          (float_sylph_t *)mis);
    }
    Vector3<float_sylph_t> gyro_bias(int temp_ch) const {
      return Vector3<float_sylph_t>(
          ((float_sylph_t)(344.487590 + 133.703438 + -15.986864) * temp_ch 
              + (8350721.277271 + 8456664.180487 + 8511516.990986)) / 3,
          ((float_sylph_t)(-251.607837 + -146.993454 + -147.033519) * temp_ch 
              + (8526459.244259 + 8448160.093824 + 8448084.067118)) / 3, 
          ((float_sylph_t)(1079.288098 + 1029.856015 + 1160.604843) * temp_ch 
              + (8092439.796614 + 8065591.571260 + 8027891.484784)) / 3);
    }
    Vector3<float_sylph_t> gyro_sf(int temp_ch) const {
      return Vector3<float_sylph_t>(
          ((float_sylph_t)41663.13454) * 180 / PI,  // rad/sec
          ((float_sylph_t)41951.17013) * 180 / PI,  // rad/sec 
          ((float_sylph_t)42427.87716) * 180 / PI   // rad/sec
        );
    }
    Matrix<float_sylph_t> gyro_mis() const {
      const float_sylph_t mis[3][3] = {
          { 1.000036739,    -0.01617323824, -0.007155302738},
          { 0.008853249227,  0.999935006,   -0.006139747009},
          {-0.003122453067, -0.003357634051, 1.000053636}};
      return Matrix<float_sylph_t>(
          sizeof(mis) / sizeof(mis[0]),
          sizeof(mis[0]) / sizeof(mis[0][0]),
          (float_sylph_t *)mis);
    }

    Vector3<float_sylph_t> sigma_accel() const {
      Vector3<float_sylph_t> sf(acc_sf(0));
      Vector3<float_sylph_t> raw(
          1E4 / sf[0],
          1E4 / sf[1],
          1E4 / sf[2]);
      return acc_mis() * raw;
    }
    
    Vector3<float_sylph_t> sigma_gyro() const {
      Vector3<float_sylph_t> sf(gyro_sf(0));
      Vector3<float_sylph_t> raw(
          4E3 / sf[0],
          4E3 / sf[1],
          4E3 / sf[2]);
      // est. form ma_mu_sigma.xls
      return gyro_mis() * raw;
    }
};
const char *IMU_EM::id = "IMU_EM";

class IMU_S0 : public StandardCalibrator{
  public:
    static const char *id;
    IMU_S0() : StandardCalibrator() {
      StandardCalibrator::index_base = 0;
    }
    ~IMU_S0(){}
    
    Vector3<float_sylph_t> acc_bias(int temp_ch) const {
      return Vector3<float_sylph_t>(
          4372097.691 + ((float_sylph_t)temp_ch - 350) * (6881.481995 + 7637.360674 + 7369.642143) / 3,
          4548764.729 + ((float_sylph_t)temp_ch - 350) * (7833.042137 + 8326.376412 + 8516.933420) / 3,
          5344869.566 + ((float_sylph_t)temp_ch - 350) * (8220.989669 + 8471.375155 + 8704.710279) / 3);
    }
    Vector3<float_sylph_t> acc_sf(int temp_ch) const {
      return Vector3<float_sylph_t>(
          247442.7862,
          249377.2138,
          250255.9313); // Z軸はmisで逆転している
    }
    Matrix<float_sylph_t> acc_mis() const {
      const float_sylph_t mis[3][3] = {
          {-0.006567580235,  0.9999547707,   -0.006879210742},
          { 0.9999775265,    0.006576682716,  0.001301401883},
          { 0.001346585408, -0.006870509082, -0.9999754911}};
      return Matrix<float_sylph_t>(
          sizeof(mis) / sizeof(mis[0]),
          sizeof(mis[0]) / sizeof(mis[0][0]),
          (float_sylph_t *)mis);
    }
    Vector3<float_sylph_t> gyro_bias(int temp_ch) const {
      return Vector3<float_sylph_t>(
          ((float_sylph_t)(353.972076 + 373.749397 + 341.357132) * temp_ch 
              + (5448425.416608 + 5432900.452148 + 5443160.462424)) / 3,
          ((float_sylph_t)(113.829560 + 157.209129 + 141.784354) * temp_ch 
              + (5371003.112958 + 5343475.389057 + 5359723.892273)) / 3, 
          ((float_sylph_t)(347.627710 + 267.290034 + 383.293433) * temp_ch 
              + (5156136.067150 + 5184959.264085 + 5133155.098564)) / 3);
    }
    Vector3<float_sylph_t> gyro_sf(int temp_ch) const {
      return Vector3<float_sylph_t>(
          ((float_sylph_t)10451.22522) * 180 / PI,  // rad/sec
          ((float_sylph_t)11622.38118) * 180 / PI,  // rad/sec 
          ((float_sylph_t)10940.59935) * 180 / PI   // rad/sec
        );
    }
    Matrix<float_sylph_t> gyro_mis() const {
      const float_sylph_t mis[3][3] = {
          {-1.000218658,    -0.002437579057, -0.009275488041},
          {0.00453654898,   -1.000031039,    -0.005300914796},
          {-0.01985291786,  -0.003241196028, -1.00040351}};
      return Matrix<float_sylph_t>(
          sizeof(mis) / sizeof(mis[0]),
          sizeof(mis[0]) / sizeof(mis[0][0]),
          (float_sylph_t *)mis);
    }

    Vector3<float_sylph_t> sigma_accel() const {
      Vector3<float_sylph_t> sf(acc_sf(0));
      Vector3<float_sylph_t> raw(
          1E5 / sf[0],
          1E5 / sf[1],
          1E5 / sf[2]);
      return acc_mis() * raw;
    }
    
    Vector3<float_sylph_t> sigma_gyro() const {
      Vector3<float_sylph_t> sf(gyro_sf(0));
      Vector3<float_sylph_t> raw(
          1E4 / sf[0],
          1E4 / sf[1],
          1E4 / sf[2]);
      // est. form ma_mu_sigma.xls
      return gyro_mis() * raw;
    }
};
const char *IMU_S0::id = "IMU_S0";

class IMU_9001 : public StandardCalibrator{
  public:
    static const char *id;
    IMU_9001() : StandardCalibrator() {
      StandardCalibrator::index_base = 0;
    }
    ~IMU_9001(){}
    
    Vector3<float_sylph_t> acc_bias(int temp_ch) const {
      return Vector3<float_sylph_t>(
           5019479.919 + ((float_sylph_t)temp_ch - 350) * (-309.536036 - 93.986171 - 102.864231) / 3,
           5053957.623 + ((float_sylph_t)temp_ch - 350) * (-766.303951 - 467.163447 - 694.608488) / 3,
           5027919.787 + ((float_sylph_t)temp_ch - 350) * (-239.156453 - 311.522659 + 83.888547) / 3);
    }
    Vector3<float_sylph_t> acc_sf(int temp_ch) const {
      return Vector3<float_sylph_t>(
          137622.1062,
          136251.8485,
          136063.7737);
    }
    Matrix<float_sylph_t> acc_mis() const {
      const float_sylph_t mis[3][3] = {
          {0.9998831263,   -0.01318105197,  -0.00774555268},
          {0.01322701495,   0.9998950348,    0.00591315349},
          {0.007666798083, -0.006014912939,  0.9999525194}};
      return Matrix<float_sylph_t>(
          sizeof(mis) / sizeof(mis[0]),
          sizeof(mis[0]) / sizeof(mis[0][0]),
          (float_sylph_t *)mis);
    }
    Vector3<float_sylph_t> gyro_bias(int temp_ch) const {
      return Vector3<float_sylph_t>(
          ((float_sylph_t)(322.787485 + 322.589396 + 270.314462) * temp_ch 
              + (8194576.933879 + 8194966.463965 + 8213513.917172)) / 3,
          ((float_sylph_t)(525.255417 + 523.460584 + 587.675823) * temp_ch 
              + (8232437.344435 + 8233115.329416 + 8210862.804092)) / 3, 
          ((float_sylph_t)(-228.746649 - 231.571508 - 249.062847) * temp_ch 
              + (8551853.553061 + 8552826.097056 + 8559226.746187)) / 3);
    }
    Vector3<float_sylph_t> gyro_sf(int temp_ch) const {
      return Vector3<float_sylph_t>(
          ((float_sylph_t)33218.1068) * 180 / PI,  // rad/sec
          ((float_sylph_t)33338.00821) * 180 / PI,  // rad/sec 
          ((float_sylph_t)33748.90469) * 180 / PI   // rad/sec
        );
    }
    Matrix<float_sylph_t> gyro_mis() const {
      const float_sylph_t mis[3][3] = {
          {-1.000093134,     0.005311123711, -0.005073849361},
          { 0.006710531132, -1.000132088,    -0.01103833847},
          {-0.005877000916, -0.001071490571, -1.000059942}};
      return Matrix<float_sylph_t>(
          sizeof(mis) / sizeof(mis[0]),
          sizeof(mis[0]) / sizeof(mis[0][0]),
          (float_sylph_t *)mis);
    }

    Vector3<float_sylph_t> sigma_accel() const {
      Vector3<float_sylph_t> sf(acc_sf(0));
      Vector3<float_sylph_t> raw(
          1E4 / sf[0],
          1E4 / sf[1],
          1E4 / sf[2]);
      return acc_mis() * raw;
    }
    
    Vector3<float_sylph_t> sigma_gyro() const {
      Vector3<float_sylph_t> sf(gyro_sf(0));
      Vector3<float_sylph_t> raw(
          4E3 / sf[0],
          4E3 / sf[1],
          4E3 / sf[2]);
      // est. form ma_mu_sigma.xls
      return gyro_mis() * raw;
    }
};
const char *IMU_9001::id = "IMU_9001";

class IMU_9002 : public StandardCalibrator{
  public:
    static const char *id;
    IMU_9002() : StandardCalibrator() {
      StandardCalibrator::index_base = 0;
    }
    ~IMU_9002(){}
    
    Vector3<float_sylph_t> acc_bias(int temp_ch) const {
      return Vector3<float_sylph_t>(
          4964202.571 + ((float_sylph_t)temp_ch - 350) * (-286.277426 - 161.025415 - 83.088600) / 3,
          5074411.757 + ((float_sylph_t)temp_ch - 350) * (-572.984844 - 429.604454 - 559.772025) / 3,
          5054452.064 + ((float_sylph_t)temp_ch - 350) * (-465.357482 - 492.577991 - 41.141805) / 3);
    }
    Vector3<float_sylph_t> acc_sf(int temp_ch) const {
      return Vector3<float_sylph_t>(
          137215.0159,
          136614.7921,
          136032.4311);
    }
    Matrix<float_sylph_t> acc_mis() const {
      const float_sylph_t mis[3][3] = {
          {0.9999545722,   -0.00883338483,  -0.00358118547},
          {0.008865547679,  0.9999195932,    0.009066933463},
          {0.003500805806, -0.009098270742,  0.9999524818}};
      return Matrix<float_sylph_t>(
          sizeof(mis) / sizeof(mis[0]),
          sizeof(mis[0]) / sizeof(mis[0][0]),
          (float_sylph_t *)mis);
    }
    Vector3<float_sylph_t> gyro_bias(int temp_ch) const {
      return Vector3<float_sylph_t>(
          ((float_sylph_t)(97.340880 + 99.377601 + 97.520411) * temp_ch 
              + (8332191.415035 + 8331417.565502 + 8332201.751243)) / 3,
          ((float_sylph_t)(713.580705 + 719.980225 + 712.792494) * temp_ch 
              + (8116457.827952 + 8113931.091342 + 8116702.543250)) / 3, 
          ((float_sylph_t)(211.150986 + 210.098319 + 207.388144) * temp_ch 
              + (8322385.455063 + 8322714.497547 + 8323739.798047)) / 3);
          /*((float_sylph_t)(101.228099 + 101.538732 + 99.780667) * temp_ch 
              + (8330547.662287 + 8330524.732713 + 8331352.121952)) / 3,
          ((float_sylph_t)(721.438278 + 720.164489 + 714.188417) * temp_ch 
              + (8113121.754767 + 8113663.101248 + 8115991.342237)) / 3, 
          ((float_sylph_t)(209.963490 + 211.376119 + 208.434988) * temp_ch 
              + (8322696.881671 + 8322128.614253 + 8323320.625581)) / 3);*/ // +/- 500deg から
    }
    Vector3<float_sylph_t> gyro_sf(int temp_ch) const {
      return Vector3<float_sylph_t>(
          ((float_sylph_t)13390.54884) * 180 / PI,  // rad/sec
          ((float_sylph_t)13345.24541) * 180 / PI,  // rad/sec
          ((float_sylph_t)13341.74469) * 180 / PI   // rad/sec
          /*((float_sylph_t)13390.29886) * 180 / PI,  // rad/sec
          ((float_sylph_t)13346.66395) * 180 / PI,  // rad/sec 
          ((float_sylph_t)13343.41013) * 180 / PI   // rad/sec */ // +/- 500deg から
        );
    }
    Matrix<float_sylph_t> gyro_mis() const {
      const float_sylph_t mis[3][3] = {
          {-1.000087854,      0.01296177243,   0.005959169402},
          { 0.0007815835913, -1.000296292,    -0.01675958086},
          {-0.003836248224,  -0.008636184887, -1.000167714}};
          /*{-1.000085674,      0.01301338941,   0.005907133768}, // +/- 500degから求めた
          { 0.0005234674579, -1.000292478,    -0.01674707602},
          {-0.003739723879,  -0.008635345803, -1.000167966}*/
      return Matrix<float_sylph_t>(
          sizeof(mis) / sizeof(mis[0]),
          sizeof(mis[0]) / sizeof(mis[0][0]),
          (float_sylph_t *)mis);
    }

    Vector3<float_sylph_t> sigma_accel() const {
      Vector3<float_sylph_t> sf(acc_sf(0));
      Vector3<float_sylph_t> raw(
          1E4 / sf[0],
          1E4 / sf[1],
          1E4 / sf[2]);
      return acc_mis() * raw;
    }
    
    Vector3<float_sylph_t> sigma_gyro() const {
      Vector3<float_sylph_t> sf(gyro_sf(0));
      Vector3<float_sylph_t> raw(
          4E3 / sf[0],
          4E3 / sf[1],
          4E3 / sf[2]);
      // est. form ma_mu_sigma.xls
      return gyro_mis() * raw;
    }
};
const char *IMU_9002::id = "IMU_9002";

class IMU_9003 : public StandardCalibrator{
  public:
    static const char *id;
    IMU_9003() : StandardCalibrator() {
      StandardCalibrator::index_base = 0;
    }
    ~IMU_9003(){}
    
    Vector3<float_sylph_t> acc_bias(int temp_ch) const {
      return Vector3<float_sylph_t>(
           4982381.508 + ((float_sylph_t)temp_ch - 350) * (-303.063648 - 123.907700 - 67.336852) / 3,
           5041528.647 + ((float_sylph_t)temp_ch - 350) * (-1165.883293 - 988.652183 - 1120.429278) / 3,
           5021116.615 + ((float_sylph_t)temp_ch - 350) * (-294.879439 - 295.356582 + 98.677010) / 3);
    }
    Vector3<float_sylph_t> acc_sf(int temp_ch) const {
      return Vector3<float_sylph_t>(
          136559.9917,
          135314.9841,
          136699.3181);
    }
    Matrix<float_sylph_t> acc_mis() const {
      const float_sylph_t mis[3][3] = {
          {0.9999568175,   -0.007153113347, -0.005932624234},
          {0.007242865242,  0.9998575155,    0.01524760956},
          {0.005822711048, -0.01528992033,   0.9998661482}};
      return Matrix<float_sylph_t>(
          sizeof(mis) / sizeof(mis[0]),
          sizeof(mis[0]) / sizeof(mis[0][0]),
          (float_sylph_t *)mis);
    }
    Vector3<float_sylph_t> gyro_bias(int temp_ch) const {
      return Vector3<float_sylph_t>(
          ((float_sylph_t)(1268.299452 + 1282.250727 + 1281.288181) * temp_ch 
              + (7951114.990924 + 7946041.271469 + 7947000.971285)) / 3,
          ((float_sylph_t)(422.782492 + 404.236055 + 431.688460) * temp_ch 
              + (8194136.255147 + 8200849.199475 + 8192057.544939)) / 3, 
          ((float_sylph_t)(1475.419495 + 1484.619767 + 1486.824957) * temp_ch 
              + (7823802.454239 + 7820219.065504 + 7820706.323882)) / 3);
    }
    Vector3<float_sylph_t> gyro_sf(int temp_ch) const {
      return Vector3<float_sylph_t>(
          ((float_sylph_t)34113.19644) * 180 / PI,  // rad/sec
          ((float_sylph_t)33553.26656) * 180 / PI,  // rad/sec 
          ((float_sylph_t)33403.53523) * 180 / PI   // rad/sec
        );
    }
    Matrix<float_sylph_t> gyro_mis() const {
      const float_sylph_t mis[3][3] = {
          {-0.9998330137,    0.01034596734, -0.0001361293039},
          {-0.01790456667,  -0.9998251568,  -0.00510296287},
          { 0.003656678735,  0.01557615856, -0.9999869705}};
      return Matrix<float_sylph_t>(
          sizeof(mis) / sizeof(mis[0]),
          sizeof(mis[0]) / sizeof(mis[0][0]),
          (float_sylph_t *)mis);
    }

    Vector3<float_sylph_t> sigma_accel() const {
      Vector3<float_sylph_t> sf(acc_sf(0));
      Vector3<float_sylph_t> raw(
          1E4 / sf[0],
          1E4 / sf[1],
          1E4 / sf[2]);
      return acc_mis() * raw;
    }
    
    Vector3<float_sylph_t> sigma_gyro() const {
      Vector3<float_sylph_t> sf(gyro_sf(0));
      Vector3<float_sylph_t> raw(
          4E3 / sf[0],
          4E3 / sf[1],
          4E3 / sf[2]);
      // est. form ma_mu_sigma.xls
      return gyro_mis() * raw;
    }
};
const char *IMU_9003::id = "IMU_9003";

class IMU_9004 : public StandardCalibrator{
  public:
    static const char *id;
    IMU_9004() : StandardCalibrator() {
      StandardCalibrator::index_base = 0;
    }
    ~IMU_9004(){}
    
    Vector3<float_sylph_t> acc_bias(int temp_ch) const {
      return Vector3<float_sylph_t>(
          4903683.923 + ((float_sylph_t)temp_ch - 350) * (287.602 + 35.2134 + 96.3862) / 3,
          5071675.972 + ((float_sylph_t)temp_ch - 350) * (-642.697 - 452.301 - 645.181) / 3,
          4989463.333 + ((float_sylph_t)temp_ch - 350) * (-29.38 - 15.4393 + 363.842) / 3);
    }
    Vector3<float_sylph_t> acc_sf(int temp_ch) const {
      return Vector3<float_sylph_t>(
          137348.5742,
          136595.9485,
          136187.4833);
    }
    Matrix<float_sylph_t> acc_mis() const {
      const float_sylph_t mis[3][3] = {
          {0.9999948859,   -0.002129600268, -0.002385995268},
          {0.002139596206,  0.9999889131,    0.00419473002},
          {0.002377035716, -0.004199813634,  0.9999883556}};
      return Matrix<float_sylph_t>(
          sizeof(mis) / sizeof(mis[0]),
          sizeof(mis[0]) / sizeof(mis[0][0]),
          (float_sylph_t *)mis);
    }
    Vector3<float_sylph_t> gyro_bias(int temp_ch) const {
      return Vector3<float_sylph_t>(
          ((float_sylph_t)(932.979918 + 944.282212 + 942.313672) * temp_ch 
              + (7970056.947508 + 7966011.032832 + 7966555.923422)) / 3,
          ((float_sylph_t)(1082.342304 + 1077.994236 + 1082.039181) * temp_ch 
              + (7996277.401250 + 7997962.687642 + 7996181.341989)) / 3, 
          ((float_sylph_t)(481.705770 + 477.207017 + 477.110932) * temp_ch 
              + (8167243.028089 + 8168915.335786 + 8168889.031519)) / 3);
    }
    Vector3<float_sylph_t> gyro_sf(int temp_ch) const {
      return Vector3<float_sylph_t>(
          ((float_sylph_t)33838.00116) * 180 / PI,  // rad/sec
          ((float_sylph_t)33714.26953) * 180 / PI,  // rad/sec 
          ((float_sylph_t)33441.82624) * 180 / PI   // rad/sec
        );
    }
    Matrix<float_sylph_t> gyro_mis() const {
      const float_sylph_t mis[3][3] = {
          {-1.000166492,     0.007729514306, -0.005729319529},
          { 0.001112494227, -1.000261292,    -0.01585241307},
          {-0.01889308619,  -0.007804273861, -1.000444424}};
      return Matrix<float_sylph_t>(
          sizeof(mis) / sizeof(mis[0]),
          sizeof(mis[0]) / sizeof(mis[0][0]),
          (float_sylph_t *)mis);
    }

    Vector3<float_sylph_t> sigma_accel() const {
      Vector3<float_sylph_t> sf(acc_sf(0));
      Vector3<float_sylph_t> raw(
          1E4 / sf[0],
          1E4 / sf[1],
          1E4 / sf[2]);
      return acc_mis() * raw;
    }
    
    Vector3<float_sylph_t> sigma_gyro() const {
      Vector3<float_sylph_t> sf(gyro_sf(0));
      Vector3<float_sylph_t> raw(
          4E3 / sf[0],
          4E3 / sf[1],
          4E3 / sf[2]);
      // est. form ma_mu_sigma.xls
      return gyro_mis() * raw;
    }
};
const char *IMU_9004::id = "IMU_9004";

class IMU_9005 : public StandardCalibrator{
  public:
    static const char *id;
    IMU_9005() : StandardCalibrator() {
      StandardCalibrator::index_base = 0;
    }
    ~IMU_9005(){}
    
    Vector3<float_sylph_t> acc_bias(int temp_ch) const {
      return Vector3<float_sylph_t>(
          5012403.612 + ((float_sylph_t)temp_ch - 350) * (-216.276 - 80.0634 + 114.399) / 3,
          5058846.113 + ((float_sylph_t)temp_ch - 350) * (-944.71 - 747.964 - 814.378) / 3,
          4994406.186 + ((float_sylph_t)temp_ch - 350) * (-16.6907 - 89.7227 + 311.358) / 3);
    }
    Vector3<float_sylph_t> acc_sf(int temp_ch) const {
      return Vector3<float_sylph_t>(
          136728.3033,
          136315.2255,
          135766.2404);
    }
    Matrix<float_sylph_t> acc_mis() const {
      const float_sylph_t mis[3][3] = {
          {0.9999379414,   -0.009400800375, -0.005978158597},
          {0.009453625046,  0.99991597,      0.008870280133},
          {0.00589426852,  -0.008926244926,  0.9999427882}};
      return Matrix<float_sylph_t>(
          sizeof(mis) / sizeof(mis[0]),
          sizeof(mis[0]) / sizeof(mis[0][0]),
          (float_sylph_t *)mis);
    }
    Vector3<float_sylph_t> gyro_bias(int temp_ch) const {
      return Vector3<float_sylph_t>(
          ((float_sylph_t)(1318.807358 + 1313.922383 + 1301.791730) * temp_ch 
              + (7816191.990147 + 7817764.557533 + 7822189.268293)) / 3,
          ((float_sylph_t)(180.169664 + 179.862307 + 188.344078) * temp_ch 
              + (8269383.273112 + 8269001.688854 + 8267019.583494)) / 3, 
          ((float_sylph_t)(612.895204 + 605.616019 + 601.113317) * temp_ch 
              + (8172759.817105 + 8175357.780282 + 8176785.875086)) / 3);
    }
    Vector3<float_sylph_t> gyro_sf(int temp_ch) const {
      return Vector3<float_sylph_t>(
          ((float_sylph_t)33731.41287) * 180 / PI,  // rad/sec
          ((float_sylph_t)33599.63475) * 180 / PI,  // rad/sec 
          ((float_sylph_t)33764.38889) * 180 / PI   // rad/sec
        );
    }
    Matrix<float_sylph_t> gyro_mis() const {
      const float_sylph_t mis[3][3] = {
          {-1.000171732,     0.01341635712,  -0.001965721717},
          { 0.003852250294, -1.000149517,    -0.009023837752},
          {-0.01328651776,  -0.005268784865, -1.000178644}};
      return Matrix<float_sylph_t>(
          sizeof(mis) / sizeof(mis[0]),
          sizeof(mis[0]) / sizeof(mis[0][0]),
          (float_sylph_t *)mis);
    }

    Vector3<float_sylph_t> sigma_accel() const {
      Vector3<float_sylph_t> sf(acc_sf(0));
      Vector3<float_sylph_t> raw(
          1E4 / sf[0],
          1E4 / sf[1],
          1E4 / sf[2]);
      return acc_mis() * raw;
    }
    
    Vector3<float_sylph_t> sigma_gyro() const {
      Vector3<float_sylph_t> sf(gyro_sf(0));
      Vector3<float_sylph_t> raw(
          4E3 / sf[0],
          4E3 / sf[1],
          4E3 / sf[2]);
      // est. form ma_mu_sigma.xls
      return gyro_mis() * raw;
    }
};
const char *IMU_9005::id = "IMU_9005";

class IMU_9006 : public StandardCalibrator{
  public:
    static const char *id;
    IMU_9006() : StandardCalibrator() {
      StandardCalibrator::index_base = 0;
    }
    ~IMU_9006(){}
    
    Vector3<float_sylph_t> acc_bias(int temp_ch) const {
      return Vector3<float_sylph_t>(
          4924513.732 + ((float_sylph_t)temp_ch - 350) * (-74.403827 + 150.940795 + 159.168432) / 3,
          5062618.425 + ((float_sylph_t)temp_ch - 350) * (-228.348928 + 39.700157 - 44.435484) / 3,
          4991893.875 + ((float_sylph_t)temp_ch - 350) * (63.284110 + 128.236392 + 455.053177) / 3);
    }
    Vector3<float_sylph_t> acc_sf(int temp_ch) const {
      return Vector3<float_sylph_t>(
          138391.1121,
          136518.6158,
          136751.7184);
    }
    Matrix<float_sylph_t> acc_mis() const {
      const float_sylph_t mis[3][3] = {
          {0.9999929455,    -0.003625580056, -0.0009818765534},
          {0.003632717326,   0.9999662612,    0.007367490166},
          {0.0009551320006, -0.007371005072,  0.9999723776}};
      return Matrix<float_sylph_t>(
          sizeof(mis) / sizeof(mis[0]),
          sizeof(mis[0]) / sizeof(mis[0][0]),
          (float_sylph_t *)mis);
    }
    Vector3<float_sylph_t> gyro_bias(int temp_ch) const {
      return Vector3<float_sylph_t>(
          ((float_sylph_t)(-443.666822 - 435.441026 - 437.469752) * temp_ch 
              + (8610237.982347 + 8607764.912579 + 8608515.115891)) / 3,
          ((float_sylph_t)(1039.355042 + 1041.587280 + 1047.347834) * temp_ch 
              + (7926195.387290 + 7925283.750084 + 7923173.454908)) / 3, 
          ((float_sylph_t)(860.781413 + 860.057728 + 857.051765) * temp_ch 
              + (8011361.239902 + 8011294.947127 + 8012643.701646)) / 3);
    }
    Vector3<float_sylph_t> gyro_sf(int temp_ch) const {
      return Vector3<float_sylph_t>(
          ((float_sylph_t)33732.87805) * 180 / PI,  // rad/sec
          ((float_sylph_t)33814.54527) * 180 / PI,  // rad/sec 
          ((float_sylph_t)34101.665) * 180 / PI   // rad/sec
        );
    }
    Matrix<float_sylph_t> gyro_mis() const {
      const float_sylph_t mis[3][3] = {
          {-1.000033486,     0.01085533074,   0.006644362996},
          { 0.002698521363, -1.000271698,    -0.01838719138},
          {-0.01165776064,  -0.003658196816, -1.000067272}};
      return Matrix<float_sylph_t>(
          sizeof(mis) / sizeof(mis[0]),
          sizeof(mis[0]) / sizeof(mis[0][0]),
          (float_sylph_t *)mis);
    }

    Vector3<float_sylph_t> sigma_accel() const {
      Vector3<float_sylph_t> sf(acc_sf(0));
      Vector3<float_sylph_t> raw(
          1E4 / sf[0],
          1E4 / sf[1],
          1E4 / sf[2]);
      return acc_mis() * raw;
    }
    
    Vector3<float_sylph_t> sigma_gyro() const {
      Vector3<float_sylph_t> sf(gyro_sf(0));
      Vector3<float_sylph_t> raw(
          4E3 / sf[0],
          4E3 / sf[1],
          4E3 / sf[2]);
      // est. form ma_mu_sigma.xls
      return gyro_mis() * raw;
    }
};
const char *IMU_9006::id = "IMU_9006";

class IMU_9007 : public StandardCalibrator{
  public:
    static const char *id;
    IMU_9007() : StandardCalibrator() {
      StandardCalibrator::index_base = 0;
    }
    ~IMU_9007(){}
    
    Vector3<float_sylph_t> acc_bias(int temp_ch) const {
      return Vector3<float_sylph_t>(
          5001021.264 + ((float_sylph_t)temp_ch - 350) * (149.453647 + 295.748951 + 501.058861) / 3,
          5083119.406 + ((float_sylph_t)temp_ch - 350) * (-648.871882 - 473.722823 - 558.110811) / 3,
          5026221.682 + ((float_sylph_t)temp_ch - 350) * (-368.857940 - 325.896173 + 118.153270) / 3);
    }
    Vector3<float_sylph_t> acc_sf(int temp_ch) const {
      return Vector3<float_sylph_t>(
          137411.1888,
          136059.7314,
          136379.8185);
    }
    Matrix<float_sylph_t> acc_mis() const {
      const float_sylph_t mis[3][3] = {
          {0.9999807296,   -0.00582522908,  -0.002146401182},
          {0.005838865661,  0.9999624558,    0.006402702232},
          {0.00210902339,  -0.006415111397,  0.9999771989}};
      return Matrix<float_sylph_t>(
          sizeof(mis) / sizeof(mis[0]),
          sizeof(mis[0]) / sizeof(mis[0][0]),
          (float_sylph_t *)mis);
    }
    Vector3<float_sylph_t> gyro_bias(int temp_ch) const {
      return Vector3<float_sylph_t>(
          ((float_sylph_t)(-302.339271 - 297.481672 - 293.767900) * temp_ch 
              + (8507169.140964 + 8505434.003757 + 8504564.413235)) / 3,
          ((float_sylph_t)(-583.762574 - 594.147433 - 573.147558) * temp_ch 
              + (8522437.724939 + 8526060.352239 + 8519546.717762)) / 3, 
          ((float_sylph_t)(-479.176991 - 487.154850 - 478.140954) * temp_ch 
              + (8539267.622536 + 8541716.840945 + 8539386.051785)) / 3);
    }
    Vector3<float_sylph_t> gyro_sf(int temp_ch) const {
      return Vector3<float_sylph_t>(
          ((float_sylph_t)33288.02129) * 180 / PI,  // rad/sec
          ((float_sylph_t)33993.39154) * 180 / PI,  // rad/sec 
          ((float_sylph_t)33478.78853) * 180 / PI   // rad/sec
        );
    }
    Matrix<float_sylph_t> gyro_mis() const {
      const float_sylph_t mis[3][3] = {
          {-0.9998584932,    0.008066572465,  0.01875215383},
          { 0.002160232019, -1.00002186,     -0.0008466164808},
          {-0.01956499284,  -0.00199938733,  -0.9998286491}};
      return Matrix<float_sylph_t>(
          sizeof(mis) / sizeof(mis[0]),
          sizeof(mis[0]) / sizeof(mis[0][0]),
          (float_sylph_t *)mis);
    }

    Vector3<float_sylph_t> sigma_accel() const {
      Vector3<float_sylph_t> sf(acc_sf(0));
      Vector3<float_sylph_t> raw(
          1E4 / sf[0],
          1E4 / sf[1],
          1E4 / sf[2]);
      return acc_mis() * raw;
    }
    
    Vector3<float_sylph_t> sigma_gyro() const {
      Vector3<float_sylph_t> sf(gyro_sf(0));
      Vector3<float_sylph_t> raw(
          4E3 / sf[0],
          4E3 / sf[1],
          4E3 / sf[2]);
      // est. form ma_mu_sigma.xls
      return gyro_mis() * raw;
    }
};
const char *IMU_9007::id = "IMU_9007";

class IMU_9008 : public StandardCalibrator{
  public:
    static const char *id;
    IMU_9008() : StandardCalibrator() {
      StandardCalibrator::index_base = 0;
    }
    ~IMU_9008(){}
    
    Vector3<float_sylph_t> acc_bias(int temp_ch) const {
      return Vector3<float_sylph_t>(
          4997982.146 + ((float_sylph_t)temp_ch - 350) * (-88.546082 + 117.647434 + 255.156941) / 3,
          5040071.209 + ((float_sylph_t)temp_ch - 350) * (-957.277576 - 730.328800 - 808.004677) / 3,
          4990716.72 + ((float_sylph_t)temp_ch - 350) * (-55.880917 - 164.277538 + 282.038921) / 3);
    }
    Vector3<float_sylph_t> acc_sf(int temp_ch) const {
      return Vector3<float_sylph_t>(
          136885.4962,
          137061.0173,
          135952.7969);
    }
    Matrix<float_sylph_t> acc_mis() const {
      const float_sylph_t mis[3][3] = {
          {0.9998610735,   -0.01391903353,  -0.009170292375},
          {0.01394643489,   0.9998984482,    0.002930913259},
          {0.009128565635, -0.003058398963,  0.9999536567}};
      return Matrix<float_sylph_t>(
          sizeof(mis) / sizeof(mis[0]),
          sizeof(mis[0]) / sizeof(mis[0][0]),
          (float_sylph_t *)mis);
    }
    Vector3<float_sylph_t> gyro_bias(int temp_ch) const {
      return Vector3<float_sylph_t>(
          ((float_sylph_t)(510.714509 + 673.718055 + 676.336270) * temp_ch 
              + (8069220.731740 + 8007823.349387 + 8006987.631218)) / 3,
          ((float_sylph_t)(-53.402233 - 62.430345 - 51.525978) * temp_ch 
              + (8364188.173372 + 8367338.104600 + 8364564.396678)) / 3, 
          ((float_sylph_t)(204.709575 + 200.204183 + 189.037845) * temp_ch 
              + (8277218.533497 + 8278739.782684 + 8283105.089580)) / 3);
    }
    Vector3<float_sylph_t> gyro_sf(int temp_ch) const {
      return Vector3<float_sylph_t>(
          ((float_sylph_t)33325.11265) * 180 / PI,  // rad/sec
          ((float_sylph_t)33840.28501) * 180 / PI,  // rad/sec 
          ((float_sylph_t)33739.44473) * 180 / PI   // rad/sec
        );
    }
    Matrix<float_sylph_t> gyro_mis() const {
      const float_sylph_t mis[3][3] = {
          {-1.00001399,      0.006819953876, -0.005583636867},
          {-0.008741545098, -1.0000679,      -0.01953967659},
          {-0.005939775968,  0.005151894488, -0.9999645181}};
      return Matrix<float_sylph_t>(
          sizeof(mis) / sizeof(mis[0]),
          sizeof(mis[0]) / sizeof(mis[0][0]),
          (float_sylph_t *)mis);
    }

    Vector3<float_sylph_t> sigma_accel() const {
      Vector3<float_sylph_t> sf(acc_sf(0));
      Vector3<float_sylph_t> raw(
          1E4 / sf[0],
          1E4 / sf[1],
          1E4 / sf[2]);
      return acc_mis() * raw;
    }
    
    Vector3<float_sylph_t> sigma_gyro() const {
      Vector3<float_sylph_t> sf(gyro_sf(0));
      Vector3<float_sylph_t> raw(
          4E3 / sf[0],
          4E3 / sf[1],
          4E3 / sf[2]);
      // est. form ma_mu_sigma.xls
      return gyro_mis() * raw;
    }
};
const char *IMU_9008::id = "IMU_9008";

class IMU_9009 : public StandardCalibrator{
  public:
    static const char *id;
    IMU_9009() : StandardCalibrator() {
      StandardCalibrator::index_base = 0;
    }
    ~IMU_9009(){}
    
    Vector3<float_sylph_t> acc_bias(int temp_ch) const {
      return Vector3<float_sylph_t>(
          4977645.642 + ((float_sylph_t)temp_ch - 350) * (-346.862702 - 13.640047 - 134.330036) / 3,
          5080886.759 + ((float_sylph_t)temp_ch - 350) * (-393.544707 - 200.425604 - 446.655786) / 3,
          5012030.411 + ((float_sylph_t)temp_ch - 350) * (-107.857720 - 124.009780 + 321.721467) / 3);
    }
    Vector3<float_sylph_t> acc_sf(int temp_ch) const {
      return Vector3<float_sylph_t>(
          137684.8531,
          136849.8132,
          137229.5172);
    }
    Matrix<float_sylph_t> acc_mis() const {
      const float_sylph_t mis[3][3] = {
          {0.9998646887,  -0.01032402728,  -0.01280698107},
          {0.01038585269,  0.9999346866,    0.004770396881},
          {0.0127568949,  -0.004902762811,  0.9999066079}};
      return Matrix<float_sylph_t>(
          sizeof(mis) / sizeof(mis[0]),
          sizeof(mis[0]) / sizeof(mis[0][0]),
          (float_sylph_t *)mis);
    }
    Vector3<float_sylph_t> gyro_bias(int temp_ch) const {
      return Vector3<float_sylph_t>(
          ((float_sylph_t)(592.000012 + 589.825792 + 594.020065) * temp_ch 
              + (8187676.495944 + 8188212.884350 + 8187218.996240)) / 3,
          ((float_sylph_t)(1052.068592 + 1054.585637 + 1050.205013) * temp_ch 
              + (7940216.128050 + 7939039.362392 + 7941766.341895)) / 3, 
          ((float_sylph_t)(20.835212 + 29.369803 + 32.546471) * temp_ch 
              + (8377808.308376 + 8374314.006641 + 8373935.906814)) / 3);
    }
    Vector3<float_sylph_t> gyro_sf(int temp_ch) const {
      return Vector3<float_sylph_t>(
          ((float_sylph_t)33650.9741) * 180 / PI,  // rad/sec
          ((float_sylph_t)33307.26265) * 180 / PI,  // rad/sec 
          ((float_sylph_t)34086.03904) * 180 / PI   // rad/sec
        );
    }
    Matrix<float_sylph_t> gyro_mis() const {
      const float_sylph_t mis[3][3] = {
          {-1.000002294,     0.01097061602,  -0.003805789291},
          {-0.002886201352, -0.9999763054,   -0.001184438877},
          {0.00879737526,   -0.002725978914, -1.000011824}};
      return Matrix<float_sylph_t>(
          sizeof(mis) / sizeof(mis[0]),
          sizeof(mis[0]) / sizeof(mis[0][0]),
          (float_sylph_t *)mis);
    }

    Vector3<float_sylph_t> sigma_accel() const {
      Vector3<float_sylph_t> sf(acc_sf(0));
      Vector3<float_sylph_t> raw(
          1E4 / sf[0],
          1E4 / sf[1],
          1E4 / sf[2]);
      return acc_mis() * raw;
    }
    
    Vector3<float_sylph_t> sigma_gyro() const {
      Vector3<float_sylph_t> sf(gyro_sf(0));
      Vector3<float_sylph_t> raw(
          4E3 / sf[0],
          4E3 / sf[1],
          4E3 / sf[2]);
      // est. form ma_mu_sigma.xls
      return gyro_mis() * raw;
    }
};
const char *IMU_9009::id = "IMU_9009";

class IMU_9010 : public StandardCalibrator{
  public:
    static const char *id;
    IMU_9010() : StandardCalibrator() {
      StandardCalibrator::index_base = 0;
    }
    ~IMU_9010(){}
    
    Vector3<float_sylph_t> acc_bias(int temp_ch) const {
      return Vector3<float_sylph_t>(
          4990458.2 + ((float_sylph_t)temp_ch - 350) * (97.299672 + 256.617562 + 252.668157) / 3,
          5080879.802 + ((float_sylph_t)temp_ch - 350) * (-85.725669 + 33.109995 - 78.195267) / 3,
          5017216.103 + ((float_sylph_t)temp_ch - 350) * (172.121993 + 199.290866 + 552.234847) / 3);
    }
    Vector3<float_sylph_t> acc_sf(int temp_ch) const {
      return Vector3<float_sylph_t>(
          136791.3978,
          135887.0476,
          136261.8153);
    }
    Matrix<float_sylph_t> acc_mis() const {
      const float_sylph_t mis[3][3] = {
          {0.9999370227,   -0.01003943089,  -0.005016017878},
          {0.01007893334,   0.999917896,     0.007913046688},
          {0.004936163558, -0.007963104455,  0.9999561107}};
      return Matrix<float_sylph_t>(
          sizeof(mis) / sizeof(mis[0]),
          sizeof(mis[0]) / sizeof(mis[0][0]),
          (float_sylph_t *)mis);
    }
    Vector3<float_sylph_t> gyro_bias(int temp_ch) const {
      return Vector3<float_sylph_t>(
          ((float_sylph_t)(1039.574753 + 1041.101761 + 1034.077957) * temp_ch 
              + (8038720.954465 + 8038006.231729 + 8041084.952244)) / 3,
          ((float_sylph_t)(741.865063 + 741.472122 + 727.945744) * temp_ch 
              + (8170424.469430 + 8170479.088362 + 8176057.614173)) / 3, 
          ((float_sylph_t)(457.147891 + 455.974007 + 455.464306) * temp_ch 
              + (8248361.852221 + 8248590.842019 + 8249261.808515)) / 3);
    }
    Vector3<float_sylph_t> gyro_sf(int temp_ch) const {
      return Vector3<float_sylph_t>(
          ((float_sylph_t)33256.69879) * 180 / PI,  // rad/sec
          ((float_sylph_t)33978.59823) * 180 / PI,  // rad/sec 
          ((float_sylph_t)33717.83903) * 180 / PI   // rad/sec
        );
    }
    Matrix<float_sylph_t> gyro_mis() const {
      const float_sylph_t mis[3][3] = {
          {-1.00011186,      0.01624888203,  -0.0005694071527},
          {-0.001303582742, -1.000051591,    -0.009613855491},
          {-0.001044088754, -0.002659837455, -1.000030448}};
      return Matrix<float_sylph_t>(
          sizeof(mis) / sizeof(mis[0]),
          sizeof(mis[0]) / sizeof(mis[0][0]),
          (float_sylph_t *)mis);
    }

    Vector3<float_sylph_t> sigma_accel() const {
      Vector3<float_sylph_t> sf(acc_sf(0));
      Vector3<float_sylph_t> raw(
          1E4 / sf[0],
          1E4 / sf[1],
          1E4 / sf[2]);
      return acc_mis() * raw;
    }
    
    Vector3<float_sylph_t> sigma_gyro() const {
      Vector3<float_sylph_t> sf(gyro_sf(0));
      Vector3<float_sylph_t> raw(
          4E3 / sf[0],
          4E3 / sf[1],
          4E3 / sf[2]);
      // est. form ma_mu_sigma.xls
      return gyro_mis() * raw;
    }
};
const char *IMU_9010::id = "IMU_9010";

class TinyFeather_DEMO : public StandardCalibrator{
  public:
    static const char *id;
    TinyFeather_DEMO() : StandardCalibrator() {
      StandardCalibrator::index_base = 0;
    }
    ~TinyFeather_DEMO(){}
    
    Vector3<float_sylph_t> acc_bias(int temp_ch) const {
      return Vector3<float_sylph_t>(
          (6707494.4345 + 6785486.0589 + 6706581.1063 + 6743161.0299) / 4,
          (6725233.6115 + 6750500.5362 + 6762172.8771 + 6761972.9751) / 4,
          (6766908.2567 + 6772220.6185 + 6872485.4751 + 6758977.2691) / 4);
    }
    Vector3<float_sylph_t> acc_sf(int temp_ch) const {
      return Vector3<float_sylph_t>(
          (8557518.4364 - 4877722.7209) / 2 / 9.80665,
          (8603621.1179 - 4904645.7508) / 2 / 9.80665,
          (8568510.0249 - 4984761.4785) / 2 / 9.80665);
    }
    Matrix<float_sylph_t> acc_mis() const {
      const float_sylph_t mis[3][3] = {
          { 1.0,  0.0,  0.0},
          { 0.0,  1.0,  0.0},
          { 0.0,  0.0,  1.0}};
      return Matrix<float_sylph_t>(
          sizeof(mis) / sizeof(mis[0]),
          sizeof(mis[0]) / sizeof(mis[0][0]),
          (float_sylph_t *)mis);
    }
    Vector3<float_sylph_t> gyro_bias(int temp_ch) const {
      return Vector3<float_sylph_t>(
          8402990.957, 8374723.9195, 8387875.9595);
    }
    Vector3<float_sylph_t> gyro_sf(int temp_ch) const {
      return Vector3<float_sylph_t>(
          ((float_sylph_t)33000) * 180 / PI / 5,  // rad/sec
          ((float_sylph_t)33000) * 180 / PI / 5,  // rad/sec 
          ((float_sylph_t)33000) * 180 / PI / 5   // rad/sec
        );
    }
    Matrix<float_sylph_t> gyro_mis() const {
      const float_sylph_t mis[3][3] = {
          {-1.0,  0.0,  0.0},
          { 0.0, -1.0,  0.0},
          { 0.0,  0.0, -1.0}};
      return Matrix<float_sylph_t>(
          sizeof(mis) / sizeof(mis[0]),
          sizeof(mis[0]) / sizeof(mis[0][0]),
          (float_sylph_t *)mis);
    }

    Vector3<float_sylph_t> sigma_accel() const {
      Vector3<float_sylph_t> sf(acc_sf(0));
      Vector3<float_sylph_t> raw(
          1E4 / sf[0],
          1E4 / sf[1],
          1E4 / sf[2]);
      return acc_mis() * raw;
    }
    
    Vector3<float_sylph_t> sigma_gyro() const {
      Vector3<float_sylph_t> sf(gyro_sf(0));
      Vector3<float_sylph_t> raw(
          4E3 / sf[0],
          4E3 / sf[1],
          4E3 / sf[2]);
      // est. form ma_mu_sigma.xls
      return gyro_mis() * raw;
    }
};
const char *TinyFeather_DEMO::id = "TinyFeather_DEMO";

class TinyFeather_N0 : public StandardCalibrator{
  public:
    static const char *id;
    TinyFeather_N0() : StandardCalibrator() {
      StandardCalibrator::index_base = 0;
      StandardCalibrator::index_temp_ch = 7;
    }
    ~TinyFeather_N0(){}
    
    Vector3<float_sylph_t> acc_bias(int temp_ch) const {
      return Vector3<float_sylph_t>(
          6588959.949, 6914082.345, 6674092.236);
    }
    Vector3<float_sylph_t> acc_sf(int temp_ch) const {
      return Vector3<float_sylph_t>(
          178269.3091, 202002.1877, 175749.9598);
    }
    Matrix<float_sylph_t> acc_mis() const {
      const float_sylph_t mis[3][3] = {
          {0.9969818751, 0.05266875236, 0.05703633323},
          {-0.04987031551, 0.9975320605, -0.04942408186},
          {-0.05949867574, 0.04643049387, 0.9971479914}};
      return Matrix<float_sylph_t>(
          sizeof(mis) / sizeof(mis[0]),
          sizeof(mis[0]) / sizeof(mis[0][0]),
          (float_sylph_t *)mis);
    }
    Vector3<float_sylph_t> gyro_bias(int temp_ch) const {
      return Vector3<float_sylph_t>(
          ((7201258.521950 + 7203802.982831 + 7212012.207300)
              + ((0.135182 + 0.134891 + 0.133978) * temp_ch)) / 3,
          ((8751434.356704 + 8675174.915625 + 8650068.861081)
              + ((-0.042371 + -0.033807 + -0.031026) * temp_ch)) / 3,
          ((8185606.977555 + 8184176.974604 + 8185346.765573)
              + ((0.022774 + 0.022929 + 0.022790) * temp_ch)) / 3);
    }
    Vector3<float_sylph_t> gyro_sf(int temp_ch) const {
      return Vector3<float_sylph_t>(
          ((float_sylph_t)8153.224013) * 180 / PI,  // rad/sec
          ((float_sylph_t)8093.831027) * 180 / PI,  // rad/sec 
          ((float_sylph_t)8234.48421) * 180 / PI    // rad/sec
        );
    }
    Matrix<float_sylph_t> gyro_mis() const {
      const float_sylph_t mis[3][3] = {
          {-0.9999840513, 0.02306541526, 0.007240449992},
          {-0.01307868426, -0.9997440703, 0.002755445297},
          {-0.0007933004976, -0.01627597876, -1.000083802}};
      return Matrix<float_sylph_t>(
          sizeof(mis) / sizeof(mis[0]),
          sizeof(mis[0]) / sizeof(mis[0][0]),
          (float_sylph_t *)mis);
    }

    Vector3<float_sylph_t> sigma_accel() const {
      Vector3<float_sylph_t> sf(acc_sf(0));
      Vector3<float_sylph_t> raw(
          1E4 / sf[0],
          1E4 / sf[1],
          1E4 / sf[2]);
      return acc_mis() * raw;
    }
    
    Vector3<float_sylph_t> sigma_gyro() const {
      Vector3<float_sylph_t> sf(gyro_sf(0));
      Vector3<float_sylph_t> raw(
          4E3 / sf[0],
          4E3 / sf[1],
          4E3 / sf[2]);
      // est. form ma_mu_sigma.xls
      return gyro_mis() * raw;
    }
};
const char *TinyFeather_N0::id = "TinyFeather_N0";

class CustomStandardCalibrator : public StandardCalibrator{
  public:
    typedef struct {
      const char *name;
      StandardCalibrator *calibrator;
    } CalibratorItem;
    static const CalibratorItem calibrators[];
    static const int num_calibrators;
  protected:
    typedef Vector3<float_sylph_t> vector3_t;
    typedef Matrix<float_sylph_t> matrix_t;
    
    StandardCalibrator *_base_calib;
    vector3_t *_acc_bias;
    vector3_t *_acc_bias_sf;
    vector3_t *_acc_sf;
    matrix_t *_acc_mis;
    vector3_t *_gyro_bias;
    vector3_t *_gyro_bias_sf;
    vector3_t *_gyro_sf;
    matrix_t *_gyro_mis;
    vector3_t *_sigma_accel;
    vector3_t *_sigma_gyro;
  public:
    static const char *id;
    CustomStandardCalibrator() 
        : StandardCalibrator(),
          _base_calib(NULL),
          _acc_bias(NULL),
          _acc_bias_sf(NULL),
          _acc_sf(NULL),
          _acc_mis(NULL),
          _gyro_bias(NULL),
          _gyro_bias_sf(NULL),
          _gyro_sf(NULL),
          _gyro_mis(NULL),
          _sigma_accel(NULL),
          _sigma_gyro(NULL){
      StandardCalibrator::index_base = 0;
    }
    ~CustomStandardCalibrator(){
      delete _acc_bias;
      delete _acc_bias_sf;
      delete _acc_sf;
      delete _acc_mis;
      delete _gyro_bias;
      delete _gyro_bias_sf;
      delete _gyro_sf;
      delete _gyro_mis;
      delete _sigma_accel;
      delete _sigma_gyro;
    }

    static const char *get_value(const char *spec, const char *key){
      int offset(std::strlen(key));
      if(std::strncmp(spec, key, offset) != 0){return NULL;}
      if((spec[offset] == '\0') || std::isgraph(spec[offset])){return NULL;} // no value or different key.
      while(spec[++offset] != '\0'){
        if(std::isgraph(spec[offset])){return &spec[offset];}
      }
      return NULL; // no value
    }
 
    /**
     * カスタマイズする
     * 
     * @spam line 行指向でカスタマイズコマンドを作成すること
     * @return 設定に成功した場合、true
     */
    bool custom(const char *line){
      const char *value(NULL);
      if(value = get_value(line, "base")){
        for(int i = 0; i < num_calibrators; i++){
          if(!strcmp(value, calibrators[i].name)){
            _base_calib = calibrators[i].calibrator;
            return true;
          }
        }
        return false;
      }
      if(value = get_value(line, "index_base")){
        StandardCalibrator::index_base = std::atoi(value);
        return true;
      }
      if(value = get_value(line, "index_temp_ch")){
        StandardCalibrator::index_temp_ch = std::atoi(value);
        return true;
      }
#if ENABLE_IOSTREAM
#define make_debug(name) \
std::cerr << TO_STRING(name) << ": " << *CONCATENATE(_, name) << std::endl;
#else
#define make_debug(name)
#endif
#define check_alias(name) \
value = get_value(line, TO_STRING(name))
#define make_proc1(name) \
if(value || (value = get_value(line, TO_STRING(name)))){ \
  if(!CONCATENATE(_, name)){ \
    CONCATENATE(_, name) = new vector3_t(); \
  } \
  char *spec(const_cast<char *>(value)); \
  for(int i(0); i < 3; i++){ \
    (*CONCATENATE(_, name))[i] = std::strtod(spec, &spec); \
  } \
  make_debug(name); \
  return true; \
}
#define make_proc2(name) \
if(value || (value = get_value(line, TO_STRING(name)))){ \
  if(!CONCATENATE(_, name)){ \
    CONCATENATE(_, name) = new matrix_t(3, 3); \
  } \
  char *spec(const_cast<char *>(value)); \
  for(int i(0); i < 3; i++){ \
    for(int j(0); j < 3; j++){ \
      (*CONCATENATE(_, name))(i, j) = std::strtod(spec, &spec); \
    } \
  } \
  make_debug(name); \
  return true; \
}
      check_alias(acc_bias_tc); make_proc1(acc_bias_sf);
      make_proc1(acc_bias);
      make_proc1(acc_sf);
      make_proc2(acc_mis);
      check_alias(gyro_bias_tc); make_proc1(gyro_bias_sf);
      make_proc1(gyro_bias);
      make_proc1(gyro_sf);
      make_proc2(gyro_mis);
      make_proc1(sigma_accel);
      make_proc1(sigma_gyro);
#undef check_ailas
#undef make_proc1
#undef make_proc2
#undef make_debug

      return false;
    }


#define concat3(a, b, c) a##b##c
#define make_func1(name) \
vector3_t name(int temp_ch) const { \
  if(CONCATENATE(_, name)){ \
    vector3_t res(CONCATENATE(_, name)->copy()); \
    if(concat3(_, name, _sf)){ \
      res += *concat3(_, name, _sf) * temp_ch; \
    } \
    return res; \
  }else{ \
    return (_base_calib ? _base_calib->name(temp_ch) : vector3_t()); \
  } \
}
#define make_func2(name) \
matrix_t name() const { \
  if(CONCATENATE(_, name)){ \
    return *CONCATENATE(_, name); \
  }else{ \
    return (_base_calib ? _base_calib->name() : matrix_t(3, 3)); \
  } \
}
#define make_func3(name) \
vector3_t name() const { \
  if(CONCATENATE(_, name)){ \
    return *CONCATENATE(_, name); \
  }else{ \
    return (_base_calib ? _base_calib->name() : vector3_t()); \
  } \
}
#define make_func4(name) \
vector3_t name(int temp_ch) const { \
  if(CONCATENATE(_, name)){ \
    return *CONCATENATE(_, name); \
  }else{ \
    return (_base_calib ? _base_calib->name(temp_ch) : vector3_t()); \
  } \
}
    make_func1(acc_bias);
    make_func4(acc_sf);
    make_func2(acc_mis);
    make_func1(gyro_bias);
    make_func4(gyro_sf);
    make_func2(gyro_mis);
    make_func3(sigma_accel);
    make_func3(sigma_gyro);
#undef concat3
#undef make_func1
#undef make_func2
#undef make_func3
#undef make_func4
};
const char *CustomStandardCalibrator::id = "Custom";

#if !defined(CALIBRATOR_NO_INSTANCE_LIST)

const CustomStandardCalibrator::CalibratorItem 
    CustomStandardCalibrator::calibrators[] = {
  {IMU_T0::id, new IMU_T0()},
  {IMU_T1::id, new IMU_T1()},
  {IMU_E0::id, new IMU_E0()},
  {IMU_E1::id, new IMU_E1()},
  {IMU_E1_6G::id, new IMU_E1_6G()},
  {IMU_E2::id, new IMU_E2()},
  {IMU_EM::id, new IMU_EM()},
  {IMU_S0::id, new IMU_S0()},
  {IMU_9001::id, new IMU_9001()},
  {IMU_9002::id, new IMU_9002()},
  {IMU_9003::id, new IMU_9003()},
  {IMU_9004::id, new IMU_9004()},
  {IMU_9005::id, new IMU_9005()},
  {IMU_9006::id, new IMU_9006()},
  {IMU_9007::id, new IMU_9007()},
  {IMU_9008::id, new IMU_9008()},
  {IMU_9009::id, new IMU_9009()},
  {IMU_9010::id, new IMU_9010()},
  {TinyFeather_DEMO::id, new TinyFeather_DEMO()},
  {TinyFeather_N0::id, new TinyFeather_N0()},
};
const int CustomStandardCalibrator::num_calibrators 
    = sizeof(calibrators) / sizeof(CalibratorItem); 

typedef struct {
  const char *name;
  Calibrator *calibrator;
} CalibratorItem;

const CalibratorItem calibrators[] = {
  {TestCalibrator::id, (Calibrator *)new TestCalibrator()},
#define dup_from_sc_list(index) \
{CustomStandardCalibrator::calibrators[index].name, \
(Calibrator *)CustomStandardCalibrator::calibrators[index].calibrator}
  dup_from_sc_list(0),
  dup_from_sc_list(1),
  dup_from_sc_list(2),
  dup_from_sc_list(3),
  dup_from_sc_list(4),
  dup_from_sc_list(5),
  dup_from_sc_list(6),
  dup_from_sc_list(7),
  dup_from_sc_list(8),
  dup_from_sc_list(9),
  dup_from_sc_list(10),
  dup_from_sc_list(11),
  dup_from_sc_list(12),
  dup_from_sc_list(13),
  dup_from_sc_list(14),
  dup_from_sc_list(15),
  dup_from_sc_list(16),
  dup_from_sc_list(17),
  dup_from_sc_list(18),
  dup_from_sc_list(19),
  {CustomStandardCalibrator::id, (Calibrator *)new CustomStandardCalibrator()}
};

#endif

#endif /* __CALIBRATION_H__ */

#if !defined(CALIBRATOR_NO_INSTANCE_LIST)
extern const CalibratorItem calibrators[];
#endif