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

ref:https://blog.csdn.net/u011070641/article/details/49851481

概述

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

原理

可见论文”Fitting conic sections to scattered data”,和”A Novel Calibration Method of Magnetic Compass Based on Ellipsod Fitting” 以后会详细说明。
以上表明未标定的地磁计数据会是一个椭球面的形状，因此需要求解该椭球面的系数并将其变换为球面，这个变换矩阵就是矫正矩阵
 

    //动态大小矩阵
    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;