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

What’s New?

Open sourced in:
https://github.com/HongshiTan/RTIMULib2

概述

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

原理

由heading的计算方法：

$$thead=arctg\left(\frac{h_x^l}{h_y^l}\right)$$
$$\psi =\{\begin{array}{ll}thead,& \left(h_x^l⩾0,h_y^l>0\right)\\ \pi +thead,& \left(h_y^l<0\right)\\ 2\pi +thead,& \left(h_x^l<0,h_y^l>0\right)\\ \frac{\pi }{2},& \left(h_x^l>0,h_y^l=0\right)\\ \frac{3\pi }{2},& \left(h_x^l<0,h_y^l=0\right)\end{array}$$

$$\begin{array}{rl}& \text{其中: }\\ & \psi \text{ 为heading }\\ & h_x^l\text{为当地水平坐标系下x轴方向的磁力值}\\ & h_y^l\text{为当地水平坐标系下y轴方向的磁力值}\\ & \text{由上可知heading只与}{h}_x^l\text{和}{h}_y^l\text{的比例有关}\end{array}$$
地磁计噪声模型：
$$\begin{gathered} h_m^b=M_k{M}_o{M}_s{h}^b+b+n=M{h}^b+b+n \\ \begin{array}{rl}& \text{其中：}\\ & h_m^b\text{: real measurements of sensors.}\\ & h^b\text{: no interference measurements of sensors.}\\ & M_k\text{: the sensitivities of the individual sensors.}\\ & M_o\text{: the nonorthogonality and misalignment of the sensors.}\\ & M_s\text{: the sum of soft iron errors fixed to the body frame.}\\ & b\text{: constant offset.}\\ & n\text{: the noise of sensors.}\\ & \text{当}{h}^b\text{为球面点集时}{h}_m^b\text{为椭球面。}\end{array} \end{gathered}$$
椭球方程：
$$A{x}^2+B{y}^2+C{z}^2+2Dxy+2Exz+2Fyz+2Gx+2Hy+2Iz=1$$
可以用最小二乘法求取系数,由于该方程可以表示任意二次曲面，故要加相应的约束条件保证解是椭圆，但当数据点足够多的，并且在很多的姿态上都有采样的话就不必添加约束条件，添加约束条件的方法可见论文"Fitting conic sections to scattered data",和"A Novel Calibration Method of Magnetic Compass Based on Ellipsod Fitting" 以后会详细说明。
以上表明未标定的地磁计数据会是一个椭球面的形状，因此需要求解该椭球面的系数并将其变换为球面，这https://github.com/HongshiTan/RTIMULib2个变换矩阵就是矫正矩阵

C++实现

Details in github