地球磁场简介

地球上某点的地磁场是一个三维空间矢量，从地磁南极出发到地磁北极。强度大约为23-66uT. 这个磁场强度在生活中属于非常小的量级。如果离一块小磁铁很近，那么磁铁产生的磁场可以轻轻松松超过地磁场上千倍。除了显而易见的磁铁，生活中的一些其他物品，包括你想到的想不到的，比如手机，电机，电子产品，耳机，内嵌金属的家具，办公桌，带电导线，甚至建筑钢架结构, 衣服里钥匙等等等等都会对地磁场产生严重干扰，总之：室内磁场分布非常复杂，地磁场在室内受干扰非常严重。

关于地磁场的一些更多知识和地磁传感器的基本概念，可以参看之前的笔记：杨熙：NXP传感器融合笔记04(IMU，AHRS 加速度计，陀螺仪，地磁计概念)​zhuanlan.zhihu.com

美国某地的地磁场具体参数，可以通过NOAA网站查询：可以看出，这个地方的地磁场大部分 分量是朝下的哦。。

硬磁和软磁干扰

硬磁干扰

硬磁干扰其实就相当于磁传感器的零偏，但是这个零偏不是由传感器自身引起。如果没有硬磁干扰，我们让磁传感器旋转所有可能的角度，把数据记录下来，会得到一个球，而且球心在0,0,0 原点，但由于硬磁干扰的影响。总会测得一个bias(球的圆心不在原点)，这个bias就是硬磁干扰。

软磁干扰

软磁是由于传感器周围的磁性物质扭曲磁场得到，如果向上面那样让传感器旋转然后采集点，软磁干扰表现为将球变成椭球：注意无论是硬磁还是软磁干扰，都指的是在传感器坐标系下，换句话说，干扰源和传感器在同一个设备里，一起运动，旋转。如果干扰源不再传感器坐标系下而在固定坐标系下，那么就属于空间磁干扰，是无法校准补偿的！(比如室内的固定干扰源，桌子椅子，钢筋等)。 这也是为啥室内地磁电子罗盘不准的原因

总结一下硬磁干扰和软磁干扰对于磁场的畸变影响：无干扰，硬磁干扰，软磁干扰

以及他们对磁传感器采样结果的影响：完美的圆(无干扰)，偏心的圆(只有硬磁干扰)，偏心椭圆(硬磁+软磁 )

数学知识- 椭圆/圆拟合

这块一部分时后来加进来，地磁校准和圆/椭圆/球/椭球拟合这个数学问题息息相关，所以这里打算着重大书特书一下有关的数据知识，主要是看到一篇非常好的英文文章：

最小二乘圆拟合

机器视觉，包括本章说要解决的地磁校准问题都会可以归结于圆/椭圆或者椭球/椭球的数据拟合问题。这里有两本比较老但非常不错的论文可以参考：

圆拟合问题的提出：

给定一些数据点(X,Y) 求圆形坐标和圆半径，给出的数据点要大于未知数的个数。

比如给出的数据点记作：P数据点

代数法求解

代数法求解原理简单直接，我们把圆方程写为代数形式：

设待求向量为

则有：

其中B：

一般情况下，我们需要加一个约束来转换成最小二层问题：

，由此可得最小二层问题：

最终转换为圆形和半径：

matlab代码：

clear;

clc;

close all;

P = [1 7; 2 6; 5 8; 7 7; 9 5; 3 7];

B = [(P.*P)*[1 1]', P(:,1), P(:,2)];

b = -ones(length(P), 1);

res = (B'*B)^(-1)*B'*b;

xc = -res(2)/(2*res(1));

yc = -res(3)/(2*res(1));

c = sqrt(1 - res(1)^2 - res(2)^2 - res(3)^2);

r = sqrt((res(2)^2 + res(3)^2)/(4*res(1)^2) - c/res(1));

plot(P(:,1), P(:,2), '*')

viscircles([xc, yc],r);

axis equal最小二乘法求解

这个结果可以说不怎么好，实际上，这种算法只是求得了圆形到这些数据点距离最近的一个圆，并没有考虑这些点的几何关系，所以拟合效果不佳，如果数据点比较少(比如上面这个例子)，那么效果会更差。

几何法求解

几何求解法直接优化(最小化) 各数据点与圆心所形成的向量的长度与圆半径的差值平方和，如上图所示。换成数学语言：设

，

则有：

或者写做：

如上所述，定义cost function:

. 现在，要解决的问题变成了非线性问题，不能用传统的最小二乘来解决了，我们采用高斯牛顿迭代来搞定它!

高斯牛顿迭代

高斯牛法采用迭代方法来实现优化：实际上就是梯度下降法

其中

是雅克比矩阵的逆，

是残差。当方程数多于未知数时，系统变成超定问题，雅克比矩阵逆通过伪逆求得：

雅克比矩阵怎么求呢，实际就是按每个变量求偏导数所组成的矩阵，不再赘述：雅克比矩阵

matlab代码：

clear;

clc;

close all;

P = [1 7; 2 6; 5 8; 7 7; 9 5; 3 7];

%给定初值

xc = 5.3794;

yc = 7.2532;

r = 3.0370;

res = [xc; yc; r];

max_iters = 20;

max_dif = 10^(-6); % max difference between consecutive results

for i = 1 : max_iters

J = Jacobian(res(1), res(2), P);

R = Residual(res(1), res(2), res(3), P);

prev = res;

res = res - (J'*J)^(-1)*J'*R;

dif = abs((prev - res)./res);

if dif < max_dif

fprintf('Convergence met after %d iterations\n', i);

break;

end

end

if i == max_iters

fprintf('Convergence not reached after %d iterations\n', i);

end

xc = res(1);

yc = res(2);

r = res(3);

plot(P(:,1), P(:,2), '*')

viscircles([xc, yc],r);

axis equal

functionJ =Jacobian(xc, yc, P)len = size(P);

r = sqrt((xc - P(:,1)).^2 + (yc - P(:,2)).^2);

J = [(xc - P(:,1))./r, (yc - P(:,2))./r, -ones(len(1), 1)];

end

functionR =Residual(xc, yc, r, P)R = sqrt((xc - P(:,1)).^2 + (yc - P(:,2)).^2) - r;

end

效果：这次效果不错

这次虽然效果好，但是高斯牛顿迭代只会找到局部最优解，并且严重依赖于初始值，如果初始值给的不好，有可能得不到最优解甚至发散。我们来看看还有没有什么别的骚操作：

线性化的几何解法

之前我们的代价函数定位为：

所以这是一个非线性问题，只能采用非线性优化的方式来解。现在我们稍加改动，把它变成一个线性问题：定义代价函数为：

展开可得：

注意到我们要求是圆形坐标

和圆半径

,整理可得：

之后就是很tricky的一步了，我们做如下变量替换：

那么代价函数就可以写成z的线性函数的形式：

下面就可以使用最小二层来解决了，进而求得圆形坐标和圆半径：

matlab代码：

clear;

clc;

close all;

P = [1 7; 2 6; 5 8; 7 7; 9 5; 3 7]';

n= length(P);

plot(P(1,:), P(2,:), '*');

%build deisgn matrix

A = [ P(1,:); P(2,:); ones(1,n)]';

b = sum(P.*P, 1)';

ls solution

a= (A'*A)^(-1)*A'*b;

xc = 0.5*a(1);

yc = 0.5*a(2);

R = sqrt((a(1)^2+a(2)^2)/4+a(3));

R

viscircles([xc, yc],R);

axis equal

结论

这三种方法各有千秋，通常，当数据点比较少时，三种方法出来的效果差别很大，当数据点足够多时，三种方法拟合出来的圆差不多。三种方法比较

地磁3D校准模型

既然地磁场在实际应用中会经常受到硬磁和软磁干扰，那我们就要想办法校准补偿他。数学模型如下：

其实数学模型就是这个(所有的文献论文，大家都用这个模型咯)

其中 ： 校准之后的磁场(3纬矢量)

:软磁干扰矩阵

：磁传感器原始数据

:硬磁bias(3维矢量)

校准模型根据待求的参数个数也分为几种：4参数校准法：只估计硬磁干扰和磁场强度，软磁矩阵默认为单位阵。

7参数校准法：4参数校准法再加上软磁矩阵的主对角线元素(每个传感器轴的比例因子)

10参数校准法： 把软磁干扰假设为对称矩阵，求解出对称矩阵的所有元素

地磁3D校准原理

校准的数学原理其实很简单： 任何校准好的传感器坐标系地磁场的大小应该是恒定不变的。就这一条， 用数学公式说就是:

. 全部靠这条公理来实现校准参数估计：

把

展开，并且令

可得

后面就可以用最小二层法来求解矩阵A。

3D地磁校准方法

这里简单的讲下拟合球面的方法(，只是球面哦，不是椭球, 之前已经讲了如何拟合一个圆，这里只不过拓展为球，参考AN5019)

根据式子

展开可得：

写成矩阵形式：

令：

则，可以转化为一个最小二乘问题(其实和上面的圆拟合第三种方法是一样的道理)。

matlab代码

close all; % close all figures

clear; % clear all variables

clc; % clear the command terminal

%% simulate data

c = [-0.5; 0.2; 0.1]; % ellipsoid center

r = [1; 1; 1]; % semiaxis radii

[x,y,z] = ellipsoid(c(1),c(2),c(3),r(1),r(2),r(3),6);

D = [x(:),y(:),z(:)];

% add noise:

n = length(D);

noiseIntensity = 0.05; %噪声强度

D = D + randn(n,3) * noiseIntensity;

%% matlab internal fitting

[A,b,expmfs] = magcal(D, 'eye')

%fprintf( 'away from cetner %.5g\n', norm( b' - c) );

C = (D-b)*A; % calibrated data

figure(1)

plot3(D(:,1),D(:,2),D(:,3),'LineStyle','none','Marker','X','MarkerSize',2)

hold on

grid(gca,'on')

plot3(C(:,1),C(:,2),C(:,3),'LineStyle','none','Marker', ...

'o','MarkerSize',2,'MarkerFaceColor','r')

axis equal

xlabel('uT'); ylabel('uT');zlabel('uT')

legend('Uncalibrated Samples', 'Calibrated Samples','Location', 'southoutside')

title("Uncalibrated vs Calibrated" + newline + "Magnetometer Measurements")

axis equal

hold off

其中magcal是matlab2019 sensor fusion toolbox自带的函数，算法和本文章描述的一样，可以直接看里面的magcal源代码。

番外

除了采用椭球拟合方法来校准地磁场外，其实还有另外一种方法，叫做辅助向量法或者点击不变法。其基本思路就是如果除了地磁测量值外 还知道另外一个传感器坐标系下的向量(比如静止时候加速度计测得的重力场)，那么地磁场和重力场的点积应该是不变的(点积不就是两个向量的模乘以cos夹角嘛)。利用这点也可以构成校准算法。 当然还可以结合点积不变法和椭球拟合法进行更为高级的校准。这里放一个以前弄的基于MCU的直接计算硬磁软磁干扰。 利用椭球拟合 和 点积不变法 相结合的方法。只需要很少采样点，而且不需要静止采样。即可计算出 软磁干扰矩阵 和 硬磁bias。MCU计算硬磁软磁干扰https://www.zhihu.com/video/1191654170040848384

参考