倾斜补偿的电子罗盘(3)：椭球拟合，磁传感器软磁干扰和硬磁干扰的9参数校准

背景

之前提到磁传感器的误差来源，并介绍了消除硬磁干扰的3参数校准。倾斜补偿的电子罗盘(2)：磁传感器的误差来源、硬磁干扰的校准（3个参数）、实验验证

硬磁干扰的影响相当于零偏，导致数据点所在的球面发生平移。而软磁干扰会导致球面变为椭球，如下图，同时存在硬磁和软磁干扰的情况：

Layout Recommendations for PCBs Using a Magnetometer Sensor (nxp.com.cn)

同样，获得磁传感器读数后，需要通过校准消除软磁和硬磁干扰的影响，校准后的数据然后再用于后续处理实现电子罗盘功能，见 倾斜补偿的电子罗盘(1)：地磁场，磁传感器，倾斜补偿

电子罗盘使用中的校准流程：

1. 进行一次任意旋转，尽量包含多个方向，使得拟合更准确（比如手机的指南针应用，启动后有时需要把手机旋转几次才能继续使用，可能就是这个原因？）
2. 用获得的数据进行拟合，根据拟合结果计算 ${\mathbf{W}}^{-1}$和$\mathbf{V}$
3. 对于新获得的读数$\mathbf{h}$，用${\mathbf{h}}_0={\mathbf{W}}^{-1}(\mathbf{h}-\mathbf{V})$进行校准，然后基于${\mathbf{h}}_0$可以使用倾斜补偿来获得方位

本文介绍计算 ${\mathbf{W}}^{-1}$和$\mathbf{V}$的过程，并用样机做了校准+倾斜补偿的测试。

这里只是跟着参考资料操作了拟合的每个步骤，但是对于椭球拟合部分具体的原理其实不太理解（好久没接触线代了），不过看起来效果还不错。

椭球拟合

椭球的表达式

首先，椭球面表达式：
$${\mathbf{h}}_0^{\mathbf{T}}{\mathbf{h}}_0=[{\mathbf{W}}^{-1}(\mathbf{h}-\mathbf{V}){]}^T{\mathbf{W}}^{-1}(\mathbf{h}-\mathbf{V})=B^2$$
其中，${\mathbf{W}}^{-1}$是对称矩阵。

展开后：
$$\begin{gathered} {\mathbf{h}}^T({\mathbf{W}}^{-1}{)}^T{\mathbf{W}}^{-1}\mathbf{h}-2{\mathbf{h}}^T({\mathbf{W}}^{-1}{)}^T{\mathbf{W}}^{-1}\mathbf{V}+{\mathbf{V}}^T({\mathbf{W}}^{-1}{)}^T{\mathbf{W}}^{-1}\mathbf{V}-B^2=0 \\ \to {\mathbf{h}}^T\mathbf{Mh}+2{\mathbf{h}}^T\mathbf{n}+d=0 \end{gathered}$$
其中，
$$\begin{gathered} \mathbf{M}=({\mathbf{W}}^{-1}{)}^T{\mathbf{W}}^{-1}=({\mathbf{W}}^{-1}{)}^2 \\ \mathbf{n}=-\mathbf{MV} \\ d={\mathbf{V}}^T\mathbf{MV}-B^2 \end{gathered}$$
同时，用xyz表示：
$$\begin{gathered} a{x}^2+b{y}^2+c{z}^2+2fyz+2gxz+2hxy+2px+2qy+2rz+d={\mathbf{v}}^T\mathbf{x}=0, \\ \mathbf{v}={\left[\begin{array}{cccccccccc}a& b& c& f& g& h& p& q& r& d\end{array}\right]}^T \\ \mathbf{x}={\left[\begin{array}{cccccccccc}{x}^2& y^2& z^2& 2yz& 2xz& 2xy& 2x& 2y& 2z& 1\end{array}\right]}^T \end{gathered}$$
则系数的对应关系为：
$$\mathbf{M}=\left[\begin{array}{ccc}a& h& g\\ h& b& f\\ g& f& c\end{array}\right],\mathbf{n}=\left[\begin{array}{c}p\\ q\\ r\end{array}\right]$$

椭球拟合实际上就是要获得$\mathbf{v}$的10个系数。

简单验证系数的对应关系：

拟合过程

此处只介绍计算过程，也是基于最小二乘法，详见参考资料。

1. 使用m组测量数据构建10*m矩阵$\mathbf{D}=[{\mathbf{x}}_1,{\mathbf{x}}_2,...,{\mathbf{x}}_{\mathbf{m}}]$。根据我们选用的模型，
   $$a{x}^2+b{y}^2+c{z}^2+2fyz+2gxz+2hxy+2px+2qy+2rz+d={\mathbf{v}}^T\mathbf{x}=0$$
   需要获得一组$\mathbf{v}$，实现$min(||\mathbf{Dv}|{|}^2)$。后续的算法是把这个优化问题转化后再进行求解。（不太理解具体是怎么转化的。。）

2. 计算$\mathbf{S}=\mathbf{D}{\mathbf{D}}^T$

3. 对$\mathbf{S}$和$\mathbf{v}$进行分块：

$$\mathbf{S}=\mathbf{D}{\mathbf{D}}^T=\left[\begin{array}{cc}(S_{11}{)}_{6\times 6}& (S_{12}{)}_{6\times 4}\\ (S_{12}{)}_{4\times 6}^T& (S_{22}{)}_{4\times 4}\end{array}\right],\mathbf{v}=\left[\begin{array}{c}({\mathbf{v}}_1{)}_{6\times 1}\\ ({\mathbf{v}}_2{)}_{4\times 1}\end{array}\right]$$

5. 直接计算${\mathbf{v}}_1,{\mathbf{v}}_2$，优化问题被转化为：
   $$\begin{gathered} (S_{11}-\lambda C_1){\mathbf{v}}_1+S_{12}{\mathbf{v}}_2=0(1) \\ {S}_{12}^T{\mathbf{v}}_1+S_{22}{\mathbf{v}}_2=0(2) \end{gathered}$$
   其中，
   $$C_1=\left[\begin{array}{cccccc}-1& 1& -1& 0& 0& 0\\ 1& -1& 1& 0& 0& 0\\ 1& 1& -1& 0& 0& 0\\ 0& 0& 0& -4& 0& 0\\ 0& 0& 0& 0& -4& 0\\ 0& 0& 0& 0& 0& -4\end{array}\right]$$
   根据(2)，${\mathbf{v}}_2=-S_{22}^{-1}{S}_{12}^T{\mathbf{v}}_1$，代入(1)：
   $$C_1^{-1}(S_{11}-S_{12}{S}_{22}^{-1}{S}_{12}^T){\mathbf{v}}_1=\lambda {\mathbf{v}}_1$$
   从这个公式可以看出${\mathbf{v}}_1$是矩阵$C_1^{-1}(S_{11}-S_{12}{S}_{22}^{-1}{S}_{12}^T)$的特征向量。

   因此，求矩阵$C_1^{-1}(S_{11}-S_{12}{S}_{22}^{-1}{S}_{12}^T)$的所有特征向量，选择最大特征值对应的特征向量，就得到了${\mathbf{v}}_1$。（不太理解为什么是最大特征值对应的特征向量。。）

6. 使用${\mathbf{v}}_1$计算${\mathbf{v}}_2$，${\mathbf{v}}_2=-S_{22}^{-1}{S}_{12}^T{\mathbf{v}}_1$

7. 获得$\mathbf{v}=\left[\begin{array}{c}({\mathbf{v}}_1{)}_{6\times 1}\\ ({\mathbf{v}}_2{)}_{4\times 1}\end{array}\right]$。

使用拟合数据进行校准

整理下之前的结果.

已获得$\mathbf{v}$：
$$\mathbf{v}={\left[\begin{array}{cccccccccc}a& b& c& f& g& h& p& q& r& d\end{array}\right]}^T$$

原始数据满足：${\mathbf{h}}_0^{\mathbf{T}}{\mathbf{h}}_0=[{\mathbf{W}}^{-1}(\mathbf{h}-\mathbf{V}){]}^T{\mathbf{W}}^{-1}(\mathbf{h}-\mathbf{V})=B^2$，展开化简后是${\mathbf{h}}^T\mathbf{Mh}+2{\mathbf{h}}^T\mathbf{n}+d=0$。

其中，
$$\begin{gathered} \mathbf{M}=({\mathbf{W}}^{-1}{)}^T{\mathbf{W}}^{-1}=({\mathbf{W}}^{-1}{)}^2 \\ \mathbf{n}=-\mathbf{MV} \\ d={\mathbf{V}}^T\mathbf{MV}-B^2 \end{gathered}$$

$\mathbf{M},\mathbf{n}$和$\mathbf{V}$的关系为：

$$\mathbf{M}=\left[\begin{array}{ccc}a& h& g\\ h& b& f\\ g& f& c\end{array}\right],\mathbf{n}=\left[\begin{array}{c}p\\ q\\ r\end{array}\right]$$
因此，把$\mathbf{M},\mathbf{n}$转换为最终需要的${\mathbf{W}}^{-1},\mathbf{V},B$：

$$\begin{gathered} {\mathbf{W}}^{-1}={\mathbf{M}}^{\frac{1}{2}} \\ \mathbf{V}=-{\mathbf{M}}^{-1}\mathbf{n} \\ B=\sqrt{{\mathbf{V}}^T\mathbf{MV}-d} \end{gathered}$$

实验验证

拟合结果

之前3参数校准时，YZ画图如下，Z轴方向上，有不少点在拟合的球之外。

对同一组数据进行9参数校准，YZ画图，校准前后结果如下，用椭球拟合看起来效果更好一些。

把Y轴原始数据乘以2，看下椭球拟合的效果。

看下XY，XZ，YZ的数据：红色为校准前，蓝色为校准后，可见效果还不错。

校准实测

获得的${\mathbf{W}}^{-1},\mathbf{V}$数据：

W_inv = [    0.7329    0.0389   -0.0044;0.0389    0.8484   -0.0151;-0.0044   -0.0151    0.6555];
V = [27.5424; -60.3430;9.6232];

对于一组新的测量值raw = [41.66,-75.77,34.67]'，计算如下：

1. 先计算cali
2. 然后atan2计算方位角为-53°
3. 按之前的定义（俯视，以磁北为基准，顺时针旋转为正，逆时针旋转为负），磁传感器的x轴与磁北的夹角为-53°，说明此时x轴是朝向北偏西53°。

参考资料

资料

软磁和硬磁干扰：Layout Recommendations for PCBs Using a Magnetometer Sensor (nxp.com.cn)

用了这篇文章的拟合方法：Least squares ellipsoid specific fitting | IEEE Conference Publication | IEEE Xplore

椭球拟合对应的matlab代码：Ellipsoid Fitting - File Exchange - MATLAB Central (Mathworks.cn)

椭球拟合+校准对应的python代码：Teslabs Engineering - A way to calibrate a magnetometer

注意python代码中一个小错误：这个程序是完全按照文章中的方法计算的，在v和M的转换中，v_1[3]对应f，v_1[5]对应h，所以1,2和2,3这两项应该对调（同理，对称矩阵的另外两项2,1和3,2也要对调）。

代码

测量数据和完整代码在：电子罗盘磁传感器校准MATLAB代码（9参数椭球拟合）

以下是9参数拟合具体实现方法，是基于文献和参考链接的python修改而来的MATLAB代码：

D = [mag_x.^2, mag_y.^2, mag_z.^2, 2*mag_y.*mag_z, 2*mag_x.*mag_z, 2*mag_x.*mag_y, ...2*mag_x, 2*mag_y, 2*mag_z, ones(length(mag_x),1) ];
S = D' * D;
% 根据文章做分块
S11 = S(1:6, 1:6);
S12 = S(1:6, 7:10);
S21 = S(7:10, 1:6);
S22 = S(7:10, 7:10);% k=4
C = [-1, 1, 1, 0, 0, 0; 1, -1, 1, 0, 0, 0;1, 1, -1, 0, 0, 0;0, 0, 0, -4, 0, 0;0, 0, 0, 0, -4, 0;0, 0, 0, 0, 0, -4];M0 = inv(C) * (S11 - S12 * inv(S22) * S12');
[gevec, geval]=eig(M0); % M0 * gevec = gevec * geval%%% 以下是找唯一 正的特征值，对应的特征向量v1。
% geval是一个对角矩阵，diag(geval)返回的是主对角线的元素
% 找主对角线元素的最大值，和所在位置
[max_val, max_index] = max(diag(geval));
% 选择最大的特征值对应的特征向量
v1 = gevec(:,max_index);
if v1(1)<0 v1 = -v1;
end
% 这里有点看不懂了，v1为什么要取反？
% 但是如果不取反，M开根号后全是虚数了
v2 = - pinv(S22) * S12' * v1;% 还原M矩阵
M = [v1(1), v1(6), v1(5),;v1(6), v1(2), v1(4);v1(5), v1(4), v1(3)];
n = v2(1:3);
d = v2(4);% 获得最终需要的W^(-1)和V
V = - inv(M) * n;
W_inv = sqrtm(M);
B_est2 = sqrt(V'*M*V-d);data_raw = [mag_x, mag_y, mag_z]';
data_cali9 = zeros(size(data_raw));
% 对原始的每一组数据分别进行9参数校准
for i = 1:length(mag_x)h = data_raw(:,i);data_cali9(:,i) = W_inv * (h - V);
end