IMU中地磁计的椭球面拟合标定法与C++实现

概述

imu中的地磁计（准确的说是电子罗盘）用来在数据融合中提供方向信息，卫星上的地磁计的标定除了要去除各种软铁，硬铁干扰的影响外还需要准确的计算三个方向的比例系数，而在imu中往往只需要计算heading，heading的计算只与当地水平坐标系(local horizontal frame)下的x，y方向磁力值的比例有关。故相对与通常的最大最小值标定如下说明的椭球面拟合方法可以矫正非正交误差在heading计算中获得更好的精度。

原理

由heading的计算方法：

thead=arctg⎛⎝hlxhly⎞⎠

ψ=⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪thead,π+thead,2π+thead,π2,3π2,(hlx⩾0,hly>0)(hly<0)(hlx<0,hly>0)(hlx>0,hly=0)(hlx<0,hly=0)

其中: ψ 为heading hlx为当地水平坐标系下x轴方向的磁力值hly为当地水平坐标系下y轴方向的磁力值由上可知heading只与hlx和hly的比例有关

地磁计噪声模型：

hbm=MkMoMshb+b+n=Mhb+b+n其中：hbm: real measurements of sensors.hb: no interference measurements of sensors.Mk: the sensitivities of the individual sensors.Mo: the nonorthogonality and misalignment of the sensors.Ms: the sum of soft iron errors fixed to the body frame.b: constant offset.n: the noise of sensors.当hb为球面点集时hbm为椭球面。

椭球方程：

Ax2+By2+Cz2+2Dxy+2Exz+2Fyz+2Gx+2Hy+2Iz=1

可以用最小二乘法求取系数,由于该方程可以表示任意二次曲面，故要加相应的约束条件保证解是椭圆，但当数据点足够多的，并且在很多的姿态上都有采样的话就不必添加约束条件，添加约束条件的方法可见论文”Fitting conic sections to scattered data”,和”A Novel Calibration Method of Magnetic Compass Based on Ellipsod Fitting” 以后会详细说明。

以上表明未标定的地磁计数据会是一个椭球面的形状，因此需要求解该椭球面的系数并将其变换为球面，这个变换矩阵就是矫正矩阵

C++实现

使用了Eigen矩阵运算库

//动态大小矩阵
MatrixXd mat_D(mag_data_counter, 9);
MatrixXd mat_DT;
fp = fopen("magdata", "r");
//构建矩阵D
for (int i = 0; i < mag_data_counter; i++)
{

fscanf(fp, "%lf %lf %lf\n", &mag_x, &mag_y, &mag_z);
mat_D(i, 0) = mag_x * mag_x;
mat_D(i, 1) = mag_y * mag_y;
mat_D(i, 2) = mag_z * mag_z;
mat_D(i, 3) = 2 * mag_x * mag_y;
mat_D(i, 4) = 2 * mag_x * mag_z;
mat_D(i, 5) = 2 * mag_y * mag_z;
mat_D(i, 6) = 2 * mag_x;
mat_D(i, 7) = 2 * mag_y;
mat_D(i, 8) = 2 * mag_z;
}
fclose(fp);
mat_DT = mat_D.transpose();

MatrixXd mat_Ones = MatrixXd::Ones(mag_data_counter, 1);
//最小二乘法求解椭圆系数
MatrixXd mat_Result =  (mat_DT * mat_D).inverse() * (mat_DT * mat_Ones);

//构建系数矩阵用于求解椭圆的中心和矫正矩阵
Matrix<double, 4, 4>  mat_A_4x4;

mat_A_4x4(0, 0) = mat_Result(0, 0);
mat_A_4x4(0, 1) = mat_Result(3, 0);
mat_A_4x4(0, 2) = mat_Result(4, 0);
mat_A_4x4(0, 3) = mat_Result(6, 0);

mat_A_4x4(1, 0) = mat_Result(3, 0);
mat_A_4x4(1, 1) = mat_Result(1, 0);
mat_A_4x4(1, 2) = mat_Result(5, 0);
mat_A_4x4(1, 3) = mat_Result(7, 0);

mat_A_4x4(2, 0) = mat_Result(4, 0);
mat_A_4x4(2, 1) = mat_Result(5, 0);
mat_A_4x4(2, 2) = mat_Result(2, 0);
mat_A_4x4(2, 3) = mat_Result(8, 0);

mat_A_4x4(3, 0) = mat_Result(6, 0);
mat_A_4x4(3, 1) = mat_Result(7, 0);
mat_A_4x4(3, 2) = mat_Result(8, 0);
mat_A_4x4(3, 3) = -1.0;

MatrixXd mat_Center = -((mat_A_4x4.block(0, 0, 3, 3)).inverse() * mat_Result.block(6, 0, 3, 1));

Matrix<double, 4, 4>  mat_T_4x4;
mat_T_4x4.setIdentity();
mat_T_4x4.block(3, 0, 1, 3) = mat_Center.transpose();

MatrixXd mat_R = mat_T_4x4 * mat_A_4x4 * mat_T_4x4.transpose();

EigenSolver<MatrixXd> eig(mat_R.block(0, 0, 3, 3) / -mat_R(3, 3));
//mat_T_4x4(3,0)=mat_Center()
MatrixXd mat_Eigval(3, 1) ;
MatrixXd mat_Evecs(3, 3) ;
for (int i=0;i<3;i++)
{
for (int j=0;j<3;j++)
{
mat_Evecs(i,j)=(eig.eigenvectors())(i,j).real();
}
}
mat_Eigval(0, 0) = (eig.eigenvalues())(0, 0).real();
mat_Eigval(1, 0) = (eig.eigenvalues())(1, 0).real();
mat_Eigval(2, 0) = (eig.eigenvalues())(2, 0).real();
MatrixXd mat_Radii = (1.0 / mat_Eigval.array()).cwiseSqrt();
MatrixXd mat_Scale=MatrixXd::Identity(3, 3) ;
mat_Scale(0,0)=mat_Radii(0,0);
mat_Scale(1,1)=mat_Radii(1,0);
mat_Scale(2,2)=mat_Radii(2,0);
double min_Radii=mat_Radii.minCoeff();

mat_Scale=mat_Scale.inverse().array()*min_Radii;
MatrixXd mat_Correct =mat_Evecs*mat_Scale*mat_Evecs.transpose();

cout << "The Ellipsoid center is:" << endl << mat_Center << endl;
cout << "The Ellipsoid radii is:" << endl << mat_Radii << endl;
cout << "The scale matrix  is:" << endl << mat_Scale << endl;
cout << "The correct matrix  is:" << endl << mat_Correct << endl;

cmake:

cmake_minimum_required(VERSION 2.6 FATAL_ERROR)

project(eillipose_fit)

find_package( PkgConfig )
pkg_check_modules( EIGEN3 REQUIRED eigen3 )
include_directories( ${EIGEN3_INCLUDE_DIRS} )

add_executable (main main.cpp)
#target_link_libraries (main ${PCL_LIBRARIES})