融合定位学习--惯性器件误差分析及标定

1.环境问题

框架编译

cd imu_tk
mkdir build
cd build
cmake  ..
make

查看CMakelists，依赖还是qt4，但是Ubuntu 20.04 LTS中弃用了Qt4，可以下载并安装Qt5并进行改写。

sudo apt-get install build-essential
sudo apt-get install qtbase5-dev qtchooser qt5-qmake qtbase5-dev-tools
sudo apt-get install qtcreator
sudo apt-get install qt5*

整体进行改写比较麻烦，很多Qt4的方法已经被启用，但是仍然可以获得Qt4库作为软件依赖项

sudo add-apt-repository ppa:rock-core/qt4
sudo apt update
sudo apt-get install libqt4-dev
sudo apt-get install libqt4-opengl-dev

如果需要卸载qt4

sudo add-apt-repository --remove ppa:rock-core/qt4
sudo apt remove --autoremove libqt4* libqtcore4 libqtgui4 libqtwebkit4 qt4*

2. 加速度计对应残差的雅克比

残差：
其中：
展开a

待求参数：

根据链式求导法则：
对参数分别求偏导数：

3.修改imu_tk中的上三角矩阵为下三角矩阵

主要是三处修改：
第一处初始化：

      // TODO: implement lower triad model here
      //
      // 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] 

第二处:

    //
    // TODO: implement lower triad model here
    //
    acc_calib_params[0] = init_acc_calib_.misXZ();
    acc_calib_params[1] = init_acc_calib_.misXY();
    acc_calib_params[2] = init_acc_calib_.misYX();

第三处：

 acc_calib_ = CalibratedTriad_<_T>( 
    //
    // TODO: implement lower triad model here
    // 
    0,0,0,
    min_cost_calib_params[0],
    min_cost_calib_params[1],
    min_cost_calib_params[2],
    min_cost_calib_params[3],
    min_cost_calib_params[4],
    min_cost_calib_params[5],
    min_cost_calib_params[6],
    min_cost_calib_params[7],
    min_cost_calib_params[8] 
  );

运行过程中，可能出现以下错误：

解决方法是使用gcc的堆栈保护功能：
在CMakelists中加入

add_compile_options(-g  -ggdb -O0 -Wall   -fstack-protector-all -march=native -fnon-call-exceptions)

运行结果：

4.ceres 非线性优化，求解加速度计内参

替换calibration.cpp的原本自动求导的MultiPosAccResidual类为下面的解析求导类代码：

template<typename _T1>
class MultiPosAccResidual  : public ceres::SizedCostFunction<1,9>
{
  public:
    MultiPosAccResidual(const _T1 &g_mag, const Eigen::Matrix< _T1, 3 , 1> &sample 
    ) : g_mag_(g_mag), sample_(sample){}
   
   virtual ~MultiPosAccResidual() {}
   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)) 
    );
    /* Apply undistortion transform to accel measurements
         mis_mat_ <<  _T(1)   , -mis_yz  ,  mis_zy  ,
                       mis_xz ,  _T(1)   , -mis_zx  ,  
                      -mis_xy ,  mis_yx  ,  _T(1)   ; */
    CalibratedTriad_<double> calib_triad( 
      //
      // TODO: implement lower triad model here
      //
      // 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] 
    );
    
    // apply undistortion transform:
    Eigen::Matrix< double, 3 , 1> calib_samp = calib_triad.unbiasNormalize( raw_samp );
    
    residuals[0] = g_mag_*g_mag_ - calib_samp.squaredNorm();
    // residuals[0] = g_mag_- calib_samp.norm();

		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;
	}

static ceres::CostFunction* Create ( const _T1 &g_mag, const Eigen::Matrix< _T1, 3 , 1> &sample )
  {
    return ( new MultiPosAccResidual<_T1>( g_mag, sample ) );
  }
protected:
	const _T1 g_mag_;
  const Eigen::Matrix< _T1, 3 , 1> sample_;
};

解析求导结果和自动求导一致：