加速度计内参模型

1、零偏

概念：加速度计输出中的常值偏移，及bias

2、刻度系数误差

概念：器件的输出往往为脉冲值或模数转换得到的值,需要乘以一个刻度系数才能转换成角速度或加速度值,若该系数不准,便存在刻度系数误差

3、安装误差

概念：如右图所示,b坐标系是正交的imu坐标系,g坐标系的三个轴是分别对应三个陀螺仪。由于加工工艺原因,陀螺仪的三个轴并不正交,而且和b坐标系的轴不重合,二者之间的偏差即为安装误差。

4、不需要转台的标定内参模型

上下三角模型转换：

构造函数中进行如下更改

CalibratedTriad_<_T2> calib_triad( 
      //
      // TODO: implement lower triad model here
      //
      // mis_yz, mis_zy, mis_zx:
      //params[0], params[1], params[2],//取消此行注释成为上三角
      _T2(0), _T2(0), _T2(0),//此行注释后为上三角
      // mis_xz, mis_xy, mis_yx:
     params[0], params[1], params[2],//此行注释后为上三角
     // _T2(0), _T2(0), _T2(0),//取消此行注释成为上三角
      //    s_x,    s_y,    s_z:
      params[3], params[4], params[5], 
      //    b_x,    b_y,    b_z: 
      params[6], params[7], params[8] 
    );
    

  // TODO: implement lower triad model here
    //上三角
    // acc_calib_params[0] = init_acc_calib_.misYZ();
    // acc_calib_params[1] = init_acc_calib_.misZY();
    // acc_calib_params[2] = init_acc_calib_.misZX();
    //下三角
    acc_calib_params[0] = init_acc_calib_.misYX();
    acc_calib_params[1] = init_acc_calib_.misZX();
    acc_calib_params[2] = init_acc_calib_.misZY();

 

内参标定整体思路

加速度输入(重力加速度)是已知的,已知值与测量值的差异作为残差,通过优化,估计内参

残差函数

其中g为重力加速度，

a为通过测量值与内参运算得到的真实值

在不断的迭代优化过程中，残差函数逐渐接近0。内参便逐渐得到优化结算。

由测量值和内参计算得到真实值

残差函数雅可比

代码实现(解析求导)

源代码使用的是自动求导方式，需要改成解析求导：

首先：

将calibration.cpp中的结构体注释，
template <typename _T1> struct MultiPosAccResidual
并添加解析求导类
template<typename _T1>  
class MultiPosAccResidual_Analytical : public ceres::SizedCostFunction<1, 9>

 其中参数1，代表残差块维数是1

参数9，代表参数块维数是9。

自动求导转换为解析求导

 // ceres::CostFunction* cost_function = MultiPosAccResidual<_T>::Create ( 
      //   g_mag_, static_samples[i].data() 
      // );
 
      // problem.AddResidualBlock ( 
      //   cost_function,           /* error fuction */
      //   NULL,                    /* squared loss */
      //   acc_calib_params.data()  /* accel deterministic error params */
      // ); 
      ceres::CostFunction *cost_function=new MultiPosAccResidual_Analytical<_T>( g_mag_, static_samples[i].data() );
      problem.AddResidualBlock(
        cost_function,
        NULL,
        acc_calib_params.data()
      );

template<typename _T1>  
class MultiPosAccResidual_Analytical : public ceres::SizedCostFunction<1, 9>
{
  //g_mag：重力加速度，sample：测量值
  public: MultiPosAccResidual_Analytical(const _T1 &g_mag,const Eigen::Matrix<_T1,3,1>&sample)
  :g_mag_(g_mag),sample_(sample){}
 
  virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians)const
  {
    Eigen::Matrix<double,3,1>raw_samp(double(sample_(0)),double(sample_(1)),double(sample_(2)));
  //初始化下三角
  CalibratedTriad_<double>calib_triad(
     //
//       // mis_yz, mis_zy, mis_zx:
           double(0), double(0), double(0),
//       // mis_xz, mis_xy, mis_yx:
          parameters[0][0],parameters[0][1], parameters[0][2],
//       //    s_x,    s_y,    s_z:
          parameters[0][3], parameters[0][4], parameters[0][5], 
//       //    b_x,    b_y,    b_z: 
          parameters[0][6], parameters[0][7],parameters[0][8] 
  );
 
  Eigen::Matrix<double,3,1>calib_samp=calib_triad.unbiasNormalize(raw_samp);
  //残差函数
  residuals[0]=g_mag_*g_mag_-calib_samp.transpose()*calib_samp;
 
  if(jacobians!=NULL)
  {
    if(jacobians[0]!=NULL)
    {
      double S1=parameters[0][0];
      double S2=parameters[0][1];
      double S3=parameters[0][2];
 
      double K1=parameters[0][3];
      double K2=parameters[0][4];
      double K3=parameters[0][5];
 
      double b1=parameters[0][6];
      double b2=parameters[0][7];
      double b3=parameters[0][8];
 
      double A1=sample_(0);
      double A2=sample_(1);
      double A3=sample_(2);
 
      Eigen::MatrixXd a(3,1);
      //真实值
      a << K1*(A1-b1), S1*K1*(A1-b1)+K2*(A2-b2), -S2*K1*(A1-b1)+S3*K2*(A2-b2)+K3*(A3-b3);
     //雅可比矩阵
      Eigen::MatrixXd da_dTheta(3,9);
      da_dTheta<<0,0,0,
                                  A1-b1,0,0,
                                  -K1,0,0,
                                  K1*(A1-b1),0,0,
                                  S1*(A1-b1),A2-b2,0,
                                  -S1*K1,-K2,0,
                                  0,-K1*(A1-b1),K2*(A2-b2),-S2*(A1-b1),
                                  S3*(A2-b2),A3-b3,S2*K1,-S3*K2,-K3;
      Eigen::Map<Eigen::Matrix<double,1,9,Eigen::RowMajor>>Jacob(jacobians[0]);
      Jacob.setZero();
      Jacob=-2*calib_samp.transpose()*da_dTheta;
    }
  }
    return true;
  }
protected:
	const _T1 g_mag_;
  const Eigen::Matrix< _T1, 3 , 1> sample_;
};

详解

// mis_yz, mis_zy, mis_zx:
   double(0), double(0), double(0),
// mis_xz, mis_xy, mis_yx:
   parameters[0][0],parameters[0][1], parameters[0][2],

parameters[0][3], parameters[0][4], parameters[0][5], 
//    b_x,    b_y,    b_z: 
parameters[0][6], parameters[0][7],parameters[0][8] 

1. Eigen::Matrix<double,3,1>calib_samp=calib_triad.unbiasNormalize(raw_samp);


inline Eigen::Matrix< _T, 3 , 1> unbiasNormalize( const Eigen::Matrix< _T, 3 , 1> &raw_data ) const
  {
    return ms_mat_*(raw_data - bias_vec_); 
  };

 //残差函数
 residuals[0]=g_mag_*g_mag_-calib_samp.transpose()*calib_samp;

 Eigen::MatrixXd da_dTheta(3,9);
      da_dTheta<<0,0,0,
                 A1-b1,0,0,
                 -K1,0,0,
                 K1*(A1-b1),0,0,
                 S1*(A1-b1),A2-b2,0,
                 -S1*K1,-K2,0,
                 0,-K1*(A1-b1),K2*(A2-b2),-S2*(A1-b1),
                 S3*(A2-b2),A3-b3,S2*K1,-S3*K2,-K3;
Eigen::Map<Eigen::Matrix<double,1,9,Eigen::RowMajor>>Jacob(jacobians[0]);
Jacob.setZero();
Jacob=-2*calib_samp.transpose()*da_dTheta;

 推导过程见上；

最终结果

 参考：

深蓝学院多传感器融合课件