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

背景

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

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

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

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

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

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

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

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

椭球拟合

椭球的表达式

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

展开后：
hT(W−1)TW−1h−2hT(W−1)TW−1V+VT(W−1)TW−1V−B2=0→hTMh+2hTn+d=0\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 \\ \rightarrow \mathbf{h}^T\mathbf{M}\mathbf{h}+2\mathbf{h}^T\mathbf{n}+d=0 hT(W−1)TW−1h−2hT(W−1)TW−1V+VT(W−1)TW−1V−B2=0→hTMh+2hTn+d=0
其中，
M=(W−1)TW−1=(W−1)2n=−MVd=VTMV−B2\mathbf{M} = (\mathbf{W}^{-1})^{T}\mathbf{W}^{-1} = (\mathbf{W}^{-1})^2 \\ \mathbf{n} = -\mathbf{M} \mathbf{V} \\ d = \mathbf{V}^T\mathbf{M}\mathbf{V}-B^2 M=(W−1)TW−1=(W−1)2n=−MVd=VTMV−B2
同时，用xyz表示：
ax2+by2+cz2+2fyz+2gxz+2hxy+2px+2qy+2rz+d=vTx=0,v=[abcfghpqrd]Tx=[x2y2z22yz2xz2xy2x2y2z1]Tax^2+by^2+cz^2+2fyz+2gxz+2hxy+2px+2qy+2rz+d=\mathbf{v}^T\mathbf{x}=0, \\ \mathbf{v} = \left[ \begin{matrix} a &b &c &f &g &h &p &q &r &d \end{matrix} \right]^T \\ \mathbf{x} = \left[ \begin{matrix} x^2 &y^2 &z^2 &2yz &2xz &2xy &2x &2y &2z &1 \end{matrix} \right]^T ax2+by2+cz2+2fyz+2gxz+2hxy+2px+2qy+2rz+d=vTx=0,v=[abcfghpqrd]Tx=[x2y2z22yz2xz2xy2x2y2z1]T
则系数的对应关系为：
M=[ahghbfgfc],n=[pqr]\mathbf{M} = \left[ \begin{matrix} a &h &g\\ h &b &f\\ g &f &c \end{matrix} \right], \mathbf{n} = \left[ \begin{matrix} p \\ q \\ r \end{matrix} \right] M=ahghbfgfc,n=pqr

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

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

拟合过程

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

使用m组测量数据构建10*m矩阵D=[x1,x2,...,xm]\mathbf{D}=[\mathbf{x_1},\mathbf{x_2},...,\mathbf{x_m}]D=[x1,x2,...,xm]。根据我们选用的模型，
ax2+by2+cz2+2fyz+2gxz+2hxy+2px+2qy+2rz+d=vTx=0ax^2+by^2+cz^2+2fyz+2gxz+2hxy+2px+2qy+2rz+d=\mathbf{v}^T\mathbf{x}=0 ax2+by2+cz2+2fyz+2gxz+2hxy+2px+2qy+2rz+d=vTx=0
需要获得一组v\mathbf{v}v，实现min(∣∣Dv∣∣2)min(||\mathbf{D}\mathbf{v}||^2)min(∣∣Dv∣∣2)。后续的算法是把这个优化问题转化后再进行求解。（不太理解具体是怎么转化的。。）
计算S=DDT\mathbf{S}=\mathbf{D}\mathbf{D}^TS=DDT
对S\mathbf{S}S和v\mathbf{v}v进行分块：

S=DDT=[(S11)6×6(S12)6×4(S12)4×6T(S22)4×4],v=[(v1)6×1(v2)4×1]\mathbf{S}=\mathbf{D}\mathbf{D}^T=\left[ \begin{matrix} (S_{11})_{6\times6} & (S_{12})_{6\times4}\\ (S_{12})^T_{4\times6} & (S_{22})_{4\times4}\\ \end{matrix} \right], \mathbf{v} = \left[ \begin{matrix} (\mathbf{v}_1)_{6\times1} \\ (\mathbf{v}_2)_{4\times1} \end{matrix} \right] S=DDT=[(S11)6×6(S12)4×6T(S12)6×4(S22)4×4],v=[(v1)6×1(v2)4×1]

直接计算v1,v2\mathbf{v}_1,\mathbf{v}_2v1,v2，优化问题被转化为：
(S11−λC1)v1+S12v2=0(1)S12Tv1+S22v2=0(2)(S_{11}-\lambda C_1)\mathbf{v}_1 + S_{12}\mathbf{v}_2 = 0 \quad (1)\\ S_{12}^T\mathbf{v}_1 + S_{22}\mathbf{v}_2 = 0 \quad (2) (S11−λC1)v1+S12v2=0(1)S12Tv1+S22v2=0(2)
其中，
C1=[−11−10001−1100011−1000000−4000000−4000000−4]C_1 = \left[ \begin{matrix} -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{matrix} \right] C1=−1110001−11000−11−1000000−4000000−4000000−4
根据(2)，v2=−S22−1S12Tv1\mathbf{v}_2=-S_{22}^{-1}S_{12}^T\mathbf{v}_1v2=−S22−1S12Tv1，代入(1)：
C1−1(S11−S12S22−1S12T)v1=λv1C_1^{-1} (S_{11}-S_{12}S_{22}^{-1}S_{12}^T)\mathbf{v}_1 = \lambda \mathbf{v}_1 C1−1(S11−S12S22−1S12T)v1=λv1
从这个公式可以看出v1\mathbf{v}_1v1是矩阵C1−1(S11−S12S22−1S12T)C_1^{-1} (S_{11}-S_{12}S_{22}^{-1}S_{12}^T)C1−1(S11−S12S22−1S12T)的特征向量。

因此，求矩阵C1−1(S11−S12S22−1S12T)C_1^{-1} (S_{11}-S_{12}S_{22}^{-1}S_{12}^T)C1−1(S11−S12S22−1S12T)的所有特征向量，选择最大特征值对应的特征向量，就得到了v1\mathbf{v}_1v1。（不太理解为什么是最大特征值对应的特征向量。。）
使用v1\mathbf{v}_1v1计算v2\mathbf{v}_2v2，v2=−S22−1S12Tv1\mathbf{v}_2=-S_{22}^{-1}S_{12}^T\mathbf{v}_1v2=−S22−1S12Tv1
获得v=[(v1)6×1(v2)4×1]\mathbf{v} = \left[ \begin{matrix} (\mathbf{v}_1)_{6\times1} \\ (\mathbf{v}_2)_{4\times1} \end{matrix} \right]v=[(v1)6×1(v2)4×1]。

使用拟合数据进行校准

整理下之前的结果.

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

原始数据满足：h0Th0=[W−1(h−V)]TW−1(h−V)=B2\mathbf{h_0^T}\mathbf{h_0} = [\mathbf{W}^{-1}(\mathbf{h}-\mathbf{V})]^T\mathbf{W}^{-1}(\mathbf{h}-\mathbf{V})=B^2h0Th0=[W−1(h−V)]TW−1(h−V)=B2，展开化简后是hTMh+2hTn+d=0\mathbf{h}^T\mathbf{M}\mathbf{h}+2\mathbf{h}^T\mathbf{n}+d=0hTMh+2hTn+d=0。

其中，
M=(W−1)TW−1=(W−1)2n=−MVd=VTMV−B2\mathbf{M} = (\mathbf{W}^{-1})^{T}\mathbf{W}^{-1} = (\mathbf{W}^{-1})^2 \\ \mathbf{n} = -\mathbf{M} \mathbf{V} \\ d = \mathbf{V}^T\mathbf{M}\mathbf{V}-B^2 M=(W−1)TW−1=(W−1)2n=−MVd=VTMV−B2

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

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

W−1=M12V=−M−1nB=VTMV−d\mathbf{W}^{-1} = \mathbf{M}^{\frac{1}{2}} \\ \mathbf{V} = -\mathbf{M}^{-1} \mathbf{n} \\ B = \sqrt{\mathbf{V}^T\mathbf{M}\mathbf{V}-d} W−1=M21V=−M−1nB=VTMV−d

实验验证

拟合结果

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

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

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

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

校准实测

获得的W−1,V\mathbf{W}^{-1},\mathbf{V}W−1,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]'，计算如下：

先计算cali
然后atan2计算方位角为-53°
按之前的定义（俯视，以磁北为基准，顺时针旋转为正，逆时针旋转为负），磁传感器的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

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

椭球拟合的电子罗盘磁差补偿_椭球拟合的电子罗盘磁差补偿
椭球拟合的电子罗盘磁差补偿孙倩 ; 付虹 [摘要] 对电子罗盘磁差补偿的椭球拟合校准法进行深入研究 , 并分解为硬磁.软磁.比例系数校准和未对准误差校准 , 分别进行仿真分析 , 直观给出各部分 ...
基于椭球磁补偿 matlab,基于椭球拟合的三轴磁传感器快速标定补偿方法
第4期(总第173期) 2012年8月机械工程与自动化 MECHANICAL ENGINEERING & AUTOMATION No．4Aug．文章编号:1672-6413(2012)04 ...
基于椭球磁补偿 matlab,基于椭球拟合的三轴磁传感器误差补偿方法.pdf
第 2 5卷第7期 2 0 1 2年 7月传感技术学报 C HI NE S E J OU R NAL O F S E NS OR S AND A C T UA T OR S V0 1 2 ...
【51单片机快速入门指南】4.4.1：python串口接收磁力计数据并进行最小二乘法椭球拟合
目录硬知识 Python代码使用方法串口收集数据椭球拟合验证 STC15F2K60S2 16.384MHz Keil uVision V5.29.0.0 PK51 Prof.Develope ...
n维椭球体积公式_加速度计椭球校准（最小二乘法椭球拟合）
在搞自动控制中,很少有人能不和陀螺仪,加速度计这些打交道,当然还有些人还不免和地磁计打交道, 这类三轴传感器都有一个特性,三个轴的零飘不一样,三个轴的比例尺不一样,随机游走我们暂且不考虑, 那么这时候 ...
基于最小二乘法的磁力计椭球拟合方法
基于最小二乘法的磁力计椭球拟合方法在写飞控代码时,必然要对磁力计的测量数据进行校正,本文将介绍一种简单实用的校正方法–基于最小二乘法的椭球拟合方法. 本文椭球拟合部分来自博文IMU加速度.磁力计校正 ...
IMU加速度、磁力计校正－－椭球拟合
本文为博主"声时刻"原创文章,未经博主允许不得转载. 联系方式:shenshikexmu@163.com 问题考虑到IMU中,x,y,z轴的度量单位并不相同,假设各轴之间相互直. ...
椭球拟合的电子罗盘磁差补偿_基于椭球曲面拟合的三维磁罗盘误差补偿算法.pdf...
您所在位置:网站首页 > 海量文档 &nbsp>&nbsp学术论文&nbsp>&nbsp自然科学论文基于椭球曲面拟合的三维磁罗盘误差补偿算法.pdf ...
倾斜补偿的电子罗盘(2)：磁传感器的误差来源、硬磁干扰的校准（3个参数）、实验验证
电子罗盘(2):磁传感器的误差来源.硬磁干扰的校准(3个参数).实验验证文章目录理想情况误差来源内部外部误差模型硬磁干扰的校准(3个参数) 使用的模型最小二乘法实测结果总结代码和 ...