磁力计的标定方法
罗汉杰. 磁力计数据的处理方法和装置[P]. 中国专利: CN115327452A, 2022.11.11.
背景介绍
磁力计也叫地磁、磁感器，它拥有三个正交方向的霍尔传感器，能够测量出三个方向的磁场强度。由于地球的磁场像一个条形磁体一样由磁南极指向磁北极，通过合成三个方向的霍尔传感器磁场强度读数，可以计算出出设备的航向和姿态。磁力计被广泛地应用于手机，飞行器，机器人等设备中。
然而在一般情况下，地球的磁场十分微弱。如果磁场计周围存在磁性物质或者可以影响局部磁场强度的物质存在，会对磁场造成干扰，使得地磁读取数据产生偏差。再加上受制造安装工艺、敏感轴电气性不一致及零点偏移等因素影响，磁力仪本身存在三轴非正交、敏感轴灵敏度不一致及零偏误差等性能缺陷，使得磁测的准确性受到较大影响。
而各种传感器都有各自的坐标系。在实际应用中，往往需要将多种传感器的坐标对齐到同一个坐标系下。例如，我们一般将磁力计的坐标系对齐到加速度传感器的坐标系下。但由于安装误差的存在，磁力计坐标系与加速度坐标系没有相互重合，存在一个旋转误差。
因此，我们在使用磁力计前，一方面需要对其进行标定工作，获得真实的地磁方向数据;另一方面将地磁计的坐标系与其他坐标系进行对齐，以方便后续使用。
磁力计误差模型
磁力计误差一般包括硬铁误差，软铁误差，尺度误差，三轴非正交误差和零偏误差。
硬铁误差：永磁铁或者磁化金属会对磁场产生额外的磁场干扰，其干扰磁场的大小和相对于载体的方向一般保持不变。我们称这种干扰导致的误差为硬铁误差\(\textbf{b}_{n}\)。硬铁误差\(\textbf{b}_{n}\)相当于在磁场上添加一个偏移向量，我们定义为一个3×1向量(\(𝑇\)符号表示矩阵转置)：
\[\begin{equation} \label{eq:mag1} \textbf{b}_{n}=[b_{n,x},b_{n,y},b_{n,z}]^{T} \end{equation}\]
软铁误差：软铁磁场是由铁磁材料如铁、镍、PCB 板等对受地磁场或电磁场的磁化而产生的。软铁磁场会随时间和载体航向的变化而变化。我们定义软铁磁场造成的误差\(\textbf{A}_{s}\)为一个3×3矩阵：
\[\begin{equation} \label{eq:mag2} \textbf{A}_{s}=\left[{\begin{array}{c c c}{a_{00}}&{a_{01}}&{a_{02}} \newline {a_{10}}&{a_{11}}&{a_{12}} \newline {a_{20}}&{a_{21}}&{a_{22}}\end{array}}\right] \end{equation}\]
尺度误差\(\textbf{S}\)为一个3×3矩阵：
\[\begin{equation} \label{eq:mag3} \textbf{S}=\left[{\begin{array}{c c c}{s_{x}}&{0}&{0} \newline {0}&{s_{y}}&{0} \newline {0}&{0}&{s_{z}}\end{array}}\right] \end{equation}\]
三轴非正交误差\(\textbf{N}\)为一个3×3矩阵：
\[\begin{equation} \label{eq:mag4} \textbf{N}=\left[{\begin{array}{c c c}{n_{x,x}}&{n_{y,x}}&{n_{z,x}} \newline {n_{x,y}}&{n_{y,y}}&{n_{z,y}} \newline {n_{x,z}}&{n_{y,z}}&{n_{z,z}}\end{array}}\right] \end{equation}\]
零偏误差\(\textbf{b}_{m}\)为一个3×1向量：
\[\begin{equation} \label{eq:mag5} \textbf{b}_{m}=[b_{m,x},b_{m,y},b_{m,z}]^{T} \end{equation}\]
综上所述，磁力计的误差模型为：
\[\begin{equation} \label{eq:mag6} \textbf{h}=\textbf{S}\textbf{N}(\textbf{A}_{s}\textbf{h}' + \textbf{b}_{n}) + \textbf{b}_{m} \end{equation}\]
其中，\(\textbf{h}=[h_{x},h_{y},h_{z}]^{T}\)为磁力计未经标定的直接读取值，\(\textbf{h}'=[h_{x}',h_{y}',h_{z}']^{T}\)为磁力计经过标定后的地磁真实值，简化公式\(\eqref{eq:mag6}\)：
\[\begin{equation} \label{eq:mag7} \textbf{h}'=\textbf{A}(\textbf{h} - \textbf{b}) \end{equation}\]
其中，\(\textbf{A}=(\textbf{S}\textbf{N}\textbf{A}_{s})^{-1}\)，\(\textbf{b}=\textbf{S}\textbf{N}\textbf{b}_{n}+\textbf{b}_{m}\)为未知的误差模型参数。通过对磁力计进行标定，我们可以得到参数\(\textbf{A}\)和\(\textbf{b}\)的值，然后我们可以使用公式\(\eqref{eq:mag7}\)，将读取的，带误差的测量值\(\textbf{h}\)恢复为真实值\(\textbf{h}'\)。
椭圆体拟合
假设空间中有\(𝑛\)个三维点集\(\lbrace \textbf{X}_{i} = [X_{i}, Y_{i}, Z_{i}]^{T} \mid 0 \le i < n \rbrace\)，我们希望能够找到一个最佳椭圆体去拟合这些三维点。描述椭圆体的一般方程为：
\[\begin{equation} \label{eq:mag8} 𝑎𝑋^{2} +𝑏𝑌^{2} +𝑐𝑍^{2} + 2𝑑𝑋𝑌 + 2𝑒𝑋𝑍 + 2𝑓𝑌𝑍 + 2𝑔𝑋 + 2h𝑌 + 2𝑖𝑍 + 𝑗 = 0 \end{equation}\]
上式可以写为矩阵形式：
\[\begin{equation} \label{eq:mag9} [X \quad Y \quad Z] \left[{\begin{array}{c c c}{a}&{d}&{e} \newline {d}&{b}&{f} \newline {e}&{f}&{c}\end{array}}\right] \left[{\begin{array}{c}{X} \newline {Y} \newline {Z}\end{array}}\right] + [X \quad Y \quad Z] \left[{\begin{array}{c}{2g} \newline {2h} \newline {2i}\end{array}}\right] + j = 0 \end{equation}\]
其中，\(𝑎, 𝑏, 𝑐, 𝑑, 𝑒, 𝑓, 𝑔, h, 𝑖, 𝑗\)为描述椭圆体的参数，表示了椭圆体的中心位置，轴方向，和旋转等信息。椭圆体拟合，就是给定一个三维点集，求取满足公式\(\eqref{eq:mag9}\)的椭圆体的参数\(𝑎, 𝑏, 𝑐, 𝑑, 𝑒, 𝑓, 𝑔, h, 𝑖, 𝑗\)。椭圆体的拟合方法有很多，这里我们使用经典的最小二乘法。
设定未知的参数向量\(\textbf{v} = [𝑎, 𝑏, 𝑐, 𝑑, 𝑒, 𝑓, 𝑔, h, 𝑖, 𝑗]^{T}\)，对于每一个点\(\textbf{X}_{i} = [X_{i}, Y_{i}, Z_{i}]^{T}\)，定义一个临时向量\(\textbf{P}_{i}\)：
\[\begin{equation} \label{eq:mag10} \textbf{P}_{i} = (X_{i}^{2}, Y_{i}^{2}, Z_{i}^{2}, 2Y_{i}Z_{i}, 2X_{i}Z_{i}, 2X_{i}Y_{i}, 2X_{i}, 2Y_{i}, 2Z_{i}, 1)^{T} \end{equation}\]
定义10×n矩阵\(\textbf{D} = (\textbf{P}_{0}, \textbf{P}_{1}, \cdots, \textbf{P}_{n-1})\)，它包含了所有的三维点集的信息。
定义6×6大的临时矩阵\(\textbf{C}_{1}\)：
\[\begin{equation} \label{eq:mag11} \textbf{C}_{1} = \left[{\begin{array}{c c c c c c}{-1}&{1}&{1}&{0}&{0}&{0} \newline {1}&{-1}&{1}&{0}&{0}&{0} \newline {1}&{1}&{-1}&{0}&{0}&{0} \newline {0}&{0}&{0}&{-4}&{0}&{0} \newline {0}&{0}&{0}&{0}&{-4}&{0} \newline {0}&{0}&{0}&{0}&{0}&{-4}\end{array}}\right] \end{equation}\]
定义10×10大的约束矩阵\(\textbf{C}\)：
\[\begin{equation} \label{eq:mag12} \textbf{C} = \left[{\begin{array}{c c}{\textbf{C}_{1}}&{\textbf{0}_{6×4}} \newline {\textbf{0}_{4×6}}&{\textbf{0}_{4×4}}\end{array}}\right] \end{equation}\]
根据椭圆体的几何特性，拟合的椭圆体，在满足公式\(\eqref{eq:mag9}\)的前提下，还需满足约束公式\(\textbf{v}^{T}\textbf{C}\textbf{v}=1\)。将椭圆体拟合问题变成最小二乘优化问题，即求取\(\textbf{v}\)，使得：
\[\begin{equation} \label{eq:mag13} \min_{\textbf{v}} \lVert \textbf{D} \textbf{v} \rVert ^{2}, 并且\textbf{v}^{T}\textbf{C}\textbf{v}=1 \end{equation}\]
使用增广拉格朗日乘子法，从公式\(\eqref{eq:mag13}\)能够得到：
\[\begin{align} \textbf{D}\textbf{D}^{T}\textbf{v} &= \lambda \textbf{C}\textbf{v} \label{eq:mag14} \newline \textbf{v}^{T}\textbf{C}\textbf{v} &= 1 \label{eq:mag15} \end{align}\]
其中，\(\lambda\)为拉格朗日乘数。\(\textbf{D}\textbf{D}^{T}\)为一个10×10大的矩阵，\(\textbf{v}\)为一个10×1的向量。分解矩阵\(\textbf{D}\textbf{D}^{T}\)和向量\(\textbf{v}\)为：
\[\begin{equation} \label{eq:mag16} \textbf{D}\textbf{D}^{T} = \left[{\begin{array}{c c}{\textbf{S}_{11}}&{\textbf{S}_{12}} \newline {\textbf{S}_{12}}&{\textbf{S}_{22}}\end{array}}\right] \end{equation}\]
\[\begin{equation} \label{eq:mag17} \textbf{v} = \left[{\begin{array}{c}{\textbf{v}_{1}} \newline {\textbf{v}_{2}}\end{array}}\right] \end{equation}\]
其中\(\textbf{S}_{11}\)，\(\textbf{S}_{12}\)，\(\textbf{S}_{22}\)大小为6×6，6×4和4×4，向量\(\textbf{v}_{1}\)，\(\textbf{v}_{2}\)大小为6和4。公式\(\eqref{eq:mag14}\)可以改写为：
\[\begin{align} \textbf{C}_{1}^{-1}(\textbf{S}_{11}-\textbf{S}_{12}\textbf{S}_{22}^{-1}\textbf{S}_{12}^{T})\textbf{v}_{1} &= \lambda \textbf{v}_{1} \label{eq:mag18} \newline \textbf{v}_{2} &= - \textbf{S}_{22}^{-1}\textbf{S}_{12}^{T}\textbf{v}_{1} \label{eq:mag19} \end{align}\]
对公式\(\eqref{eq:mag18}\)中的矩阵\(\textbf{C}_{1}^{-1}(\textbf{S}_{11}-\textbf{S}_{12}\textbf{S}_{22}^{-1}\textbf{S}_{12}^{T})\textbf{v}_{1}\)进行奇异值分解，求取特征值和特征向量，则\(\textbf{v}_{1}\)的值为最大特征值所对应的特征向量。然后将\(\textbf{v}_{1}\)代入公式\(\eqref{eq:mag19}\)求取\(\textbf{v}_{2}\)，最终椭圆体的参数向量\(v\)可得。
磁力计标定
本磁力计标定方法包括两个部分，一方面是求取磁力计的误差模型，用来获取真实的地磁方向数据；另外一个方面将磁力计坐标系对齐到加速度坐标系下。
我们先采集数据。假定载体上有已经经过标定的加速度计和未经标定的磁力计。旋转载体，然后静置，采集在该静置姿态下的，已标定的加速度计读数\(\textbf{a}\)（由于是在静置姿态下，所以该读数即为重力加速度）和未经标定的磁力计读数\(\textbf{h}\)。重复以上操作\(𝑛\)次，获取载体在各种姿态下（尽量覆盖所有的方向）的重力加速度数据集\(\lbrace \textbf{a}_{i} = [a_{i,x}, a_{i,y}, a_{i,z}]^{T} \mid 0\le i < n \rbrace\)和地磁数据集\(\lbrace \textbf{h}_{i} = [h_{i,x}, h_{i,y}, h_{i,z}]^{T} \mid 0\le i < n \rbrace\)，\(𝑛\)为数据集的大小。
磁力计误差标定
公式\(\eqref{eq:mag7}\)为磁力计的误差模型，其中\(\textbf{h}\)为未经标定的磁力计值，\(\textbf{h}'\)为标定后的真实值。由于在实际应用中，一般只需要磁力计的矢量方向用来计算设备的航向角，而不关心磁场的大小，因此会对\(\textbf{h}'\)进行归一化处理，即有\(\textbf{h}'^{T} \textbf{h}'= {\lVert \textbf{h}' \rVert }^2\)。代入公式\(\eqref{eq:mag7}\)并展开，有：
\[\begin{equation} \label{eq:mag20} \textbf{h}^{T} \textbf{Q} \textbf{h} + \textbf{h}^{T} \textbf{n} + d = 0 \end{equation}\]
其中\(\textbf{Q}=\textbf{A}^{T}\textbf{A}\),\(\textbf{n}=-2\textbf{Q}\textbf{b}\),\(d=\textbf{b}^{T}\textbf{Q}\textbf{b} - 1\)。
对比公式\(\eqref{eq:mag20}\)和椭圆体公式\(\eqref{eq:mag9}\)，我们发现磁力计误差模型\(\eqref{eq:mag20}\)相当于一个椭圆体公式，即磁力计的读值\(\textbf{h}\)会落在了一个椭圆体上(见下图)：
去掉硬铁误差，软铁误差，尺度误差，零点误差等误差而得到的磁力计真实值\(\textbf{h}'\)，应该落在一个圆心在原点的球体上。将未经标定的磁力计数据集\(\lbrace \textbf{h}_{i} \mid 0\le i < n \rbrace\)当作椭圆体上的三维点集\(\lbrace \textbf{X}_{i} \mid 0\le i < n \rbrace\)，使用上述的椭圆体拟合法求取参数向量\(\textbf{v} = [𝑎, 𝑏, 𝑐, 𝑑, 𝑒, 𝑓, 𝑔, h, 𝑖, 𝑗]^{T}\)。根据公式\(\eqref{eq:mag9}\)和公式\(\eqref{eq:mag20}\)，我们能够获得磁力计的误差模型参数\(\textbf{Q}\)，\(\textbf{n}\)，\(d\)：
\[\begin{equation} \label{eq:mag21} \textbf{Q} = \left[{\begin{array}{c c c}{a}&{d}&{e} \newline {d}&{b}&{f} \newline {e}&{f}&{c}\end{array}}\right], \quad \textbf{n} = \left[{\begin{array}{c}{2g} \newline {2h} \newline {2i}\end{array}}\right], \quad d = j \end{equation}\]
实际上通过公式\(\eqref{eq:mag20}\)计算获得的误差模型参数\(\textbf{Q}\)，\(\textbf{n}\)，\(d\)并非是真实值，它们与真实值\(\overline{\textbf{Q}}\)，\(\overline{\textbf{n}}\)，\(\overline{d}\)之间存在一个矢量\(\alpha\)值，即\(\overline{\textbf{Q}} = \alpha \textbf{Q}\)，\(\overline{\textbf{n}} = \alpha \textbf{n}\)，\(\overline{d} = \alpha d\)（\(\textbf{Q}\)，\(\textbf{n}\)，\(d\)乘以一个任意矢量\(\alpha\)值都会满足公式\(\eqref{eq:mag20}\)，所以我们通过公式\(\eqref{eq:mag20}\)获 得的\(\textbf{Q}\)，\(\textbf{n}\)，\(d\)并非是唯一值）。我们将\(\alpha\)设定为尺度变量，根据定义有\(\overline{\textbf{n}}=-2\overline{\textbf{Q}}\textbf{b}\),\(\overline{d} = \textbf{b}^{T}\overline{\textbf{Q}} \textbf{b} - 1\)，进一步得出：
\[\begin{equation} \nonumber \begin{aligned} 1 &= \textbf{b}^{T}\overline{\textbf{Q}} - \overline{d} \newline &= (-0.5\overline{\textbf{n}}^{T}\overline{\textbf{Q}}^{-1}) \overline{\textbf{Q}} (-0.5\overline{\textbf{n}}^{T}\overline{\textbf{Q}}^{-1}) - \overline{d} \newline &= 0.25 \overline{\textbf{n}}^{T}\overline{\textbf{Q}}^{-1} \overline{\textbf{n}} - \overline{d} \newline &= \alpha(0.25 \textbf{n}^{T}\textbf{Q}^{-1}\textbf{n}-d) \end{aligned} \end{equation}\]
\[\begin{equation} \label{eq:mag22} \longrightarrow \quad \alpha = \frac{4}{\textbf{n}^{T}\textbf{Q}^{-1}\textbf{n}-d} \end{equation}\]
根据\(\overline{\textbf{n}}= − 2\overline{\textbf{Q}}\textbf{b}\)，\(\overline{\textbf{Q}} = \alpha\textbf{Q}\)，\(\overline{\textbf{n}} = \alpha\textbf{n}\)，有：
\[\begin{align} b &= -\frac{1}{2} \overline{\textbf{Q}}^{-1}\overline{\textbf{n}} \nonumber \newline &= -\frac{1}{2} \textbf{Q}^{-1} \textbf{n} \label{eq:mag23} \end{align}\]
根据\(\overline{\textbf{Q}}=\overline{\textbf{A}}^{T}\overline{\textbf{A}}\)，\(\overline{\textbf{Q}}=\alpha\textbf{Q}\)，有：
\[\begin{equation} \label{eq:mag24} \textbf{A} = (\alpha \textbf{Q})^{\frac{1}{2}} \end{equation}\]
则标定后的磁力计值\(\textbf{h}'=\textbf{A}(\textbf{h} - \textbf{b})\)，为一个半径为1的球体上的点，如下图：
坐标系对齐
设定有3×3旋转矩阵\(\textbf{R}\)，能够将磁力计坐标系下的，经过标定的地磁向量\(\textbf{h}'\)对齐到加速度坐标系下，即：
\[\begin{equation} \label{eq:mag25} \textbf{h}^{a} = \textbf{R}\textbf{h}' \end{equation}\]
在局部地理环境下，地磁向量\(\textbf{h}'\)与重力加速度\(\textbf{a}\)有固定的夹角\(\theta\)，根据几何关系有：
\[\begin{equation} \label{eq:mag26} \cos(\theta) - \frac{\textbf{a}^{T} \textbf{R} \textbf{h}'}{ {\lVert \textbf{a} \rVert}{\lVert \textbf{h}' \rVert} } = 0 \end{equation}\]
我们使用高斯-牛顿优化方法求取旋转矩阵\(\textbf{R}\)。为方便后续计算，我们使用李代数\(\boldsymbol{ \phi } = [{\phi}_{1}, {\phi}_{2}, {\phi}_{3}]\)表示旋转矩阵\(\textbf{R}\)，李代数\(\boldsymbol{ \phi }\)与旋转矩阵\(\textbf{R}\)有以下关系\(\textbf{R}(\boldsymbol{ \phi })={e}^{\boldsymbol{ \phi }^{\wedge}}\)。\(e\)为自然常数，\(\wedge\)符号表示以下操作：
\[\begin{equation} \label{eq:mag27} \boldsymbol{ \phi }^{\wedge} = ([{\phi}_{1}, {\phi}_{2}, {\phi}_{3}])^{\wedge} = \begin{bmatrix} {0}&{-\phi_{3}}&{\phi_{2}} \newline {\phi_{3}}&{0}&{-\phi_{1}} \newline {-\phi_{2}}&{\phi_{1}}&{0} \end{bmatrix} \end{equation}\]
根据公式\(\eqref{eq:mag26}\)，对重力加速度数据集\(\lbrace \textbf{a}_{i} \mid 0\le i < n \rbrace\)和标定后的地磁数据集\(\lbrace \textbf{h}_{i}' = \textbf{A} (\textbf{h}_{i} - \textbf{b}) \mid 0\le i < n \rbrace\)建立代价函数\(L(\textbf{x})\)，有：
\[\begin{equation} \label{eq:mag28} L(\textbf{x}) = L([\boldsymbol{ \phi },k]^{T})=\frac{1}{2}\sum_{i=0}^{n-1}(k-\overline{\textbf{a}}_{i}^{T}\textbf{R}(\boldsymbol{ \phi })\overline{\textbf{h}}_{i})^2 \end{equation}\]
其中\(\textbf{x}=[\boldsymbol{ \phi },k]^{T}=[{\phi}_{1}, {\phi}_{2}, {\phi}_{3}, k]^{T}\)，\(k=\cos(\theta)\)，\({\overline{\textbf{a}}_{i}} = {\textbf{a}_{i}} / {\lVert \textbf{a}_{i} \rVert}\)，\({\overline{\textbf{h}}_{i}} = {\textbf{h}_{i}'} / {\lVert \textbf{h}_{i}' \rVert}\)。我们希望通过最小化代价函数，即\(\min_{L(\textbf{x})}\)，计算得到\(\textbf{x}\)，来求取未知的旋转矩阵\(\textbf{R}(\boldsymbol{ \phi })\)和夹角变量\(k\)。
对于数据集\(\lbrace \textbf{a}_{i} \mid 0\le i < n \rbrace\)，\(\lbrace \textbf{h}_{i}' \mid 0\le i < n \rbrace\)，有n×4雅可比矩阵\(J(\textbf{x})\)：
\[\begin{equation} \label{eq:mag29} J(\textbf{x}) = \left[{\begin{array}{c c } {-(\textbf{R}(\boldsymbol{ \phi }) \overline{\textbf{h}}_{0})^{T}\overline{\textbf{a}}_{0}^{\wedge}}&{1} \newline {-(\textbf{R}(\boldsymbol{ \phi }) \overline{\textbf{h}}_{1})^{T}\overline{\textbf{a}}_{1}^{\wedge}}&{1} \newline {\vdots}&{\vdots} \newline {-(\textbf{R}(\boldsymbol{ \phi }) \overline{\textbf{h}}_{n-1})^{T}\overline{\textbf{a}}_{n-1}^{\wedge}}&{1} \end{array}}\right] \end{equation}\]
和n×1残差矩阵\(F(\textbf{x})\)：
\[\begin{equation} \label{eq:mag30} F(\textbf{x}) = \left[{\begin{array}{c} {k - \overline{\textbf{a}}_{0}^{T} \textbf{R}(\boldsymbol{ \phi }) \overline{\textbf{h}}_{0}} \newline {k - \overline{\textbf{a}}_{1}^{T} \textbf{R}(\boldsymbol{ \phi }) \overline{\textbf{h}}_{1}} \newline {\vdots} \newline {k - \overline{\textbf{a}}_{n-1}^{T} \textbf{R}(\boldsymbol{ \phi }) \overline{\textbf{h}}_{n-1}} \end{array}}\right] \end{equation}\]
高斯-牛顿优化方法的算法流程如下：
- 输入重力加速度数据集\(\lbrace \textbf{a}_{i} \mid 0\le i < n \rbrace\)和标定后的地磁数据集\(\lbrace \textbf{h}_{i}' \mid 0\le i < n \rbrace\)；初始化待求\(x=[0, 0, 0, 0]^{T}\)。
- 根据公式\(\eqref{eq:mag29}\)和\(\eqref{eq:mag30}\)，计算雅可比矩阵\(J(\textbf{x})\)和残差矩阵\(F(\textbf{x})\)。
- 计算增量\(\Delta \textbf{x} = -(J(\textbf{x})^{T}J(\textbf{x}))^{-1}J(\textbf{x})^{T}F(\textbf{x})\)。
- 更新\(\textbf{x} = \textbf{x} + \Delta \textbf{x}\)。
- 如果\(\lVert \Delta \textbf{x} \rVert > 1e^{-8}\)，则返回步骤c；如果否，则说明算法收敛，\(\textbf{x}\)为结果值，进入到下一个步骤。
- 因为\(\textbf{x}=[\boldsymbol{ \phi },k]^{T}=[{\phi}_{1}, {\phi}_{2}, {\phi}_{3}, k]^{T}\)，根据\(\textbf{R}(\boldsymbol{ \phi })={e}^{\boldsymbol{ \phi }^{\wedge}}\)得到旋转矩阵\(\textbf{R}\)。
最终标定结果
通过步骤1，我们获得磁力计的误差模型参数\(\textbf{A}\)和\(\textbf{b}\)。根据\(\eqref{eq:mag7}\)，可以将带误差的磁力计原始测量值\(\textbf{h}\)转换为真实的地磁值\(\textbf{h}'\)。
通过步骤2，我们获得地磁计坐标系与加速度计坐标系之间的旋转关系\(\textbf{R}\)， 并且根据公式\(\eqref{eq:mag25}\)，将地磁向量\(\textbf{h}'\)，对齐到加速度坐标系下，\(\textbf{h}^{a}\)为最终标定的结果。
开源代码：https://github.com/HanjieLuo/drone/tree/master/Client/magnetometer_calibration
罗汉杰. 磁力计数据的处理方法和装置[P]. 中国专利: CN115327452A, 2022.11.11.