加速度计内参模型
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]
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;
推导过程见上;
最终结果
参考:
深蓝学院多传感器融合课件