通州网站制作/seo是什么职位简称
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");//构建矩阵Dfor (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;