MATLAB校准磁力计
初识magcal函数
语法
[A,b,expmfs] = magcal(D)
[A,b,expmfs] = magcal(D,fitkind)
描述
[A，b，expmfs] = magcal（D） 返回校正未校准磁力计数据所需的数据D。
要产生校准的磁力计数据，请使用该公式。
校准后的数据CC = (D-b)*ACexpmfs
[A，b，expmfs] = magcal（D，fitkind） 将矩阵约束为指定的类型。当只有软铁或硬铁效果时，此语法需要纠正。
输入参数
D— 原始磁力计数据
N ×3 矩阵（默认）
原始磁力计数据的输入矩阵，指定为 N x 3 矩阵。矩阵的每一列对应于 分别为第一轴、第二轴和第三轴。矩阵的每一行对应于一个单三轴测量。
数据类型：singledouble
fitkind— 矩阵输出类型
输出 A 的矩阵类型。的矩阵类型可以限制为：A
'eye'– 单位矩阵
'diag'–对角
'sym'–对称
'auto'– 前面任何一个都提供最合适的选项
输出参数
A— 软铁效应
3×3 矩阵的校正矩阵
软铁效应的校正矩阵，以 3×3 矩阵的形式返回。
b— 硬铁效应
3 x1 矢量的校正矢量
硬铁效应的校正向量，以 3×1 数组的形式返回。
expmfs— 预期磁场强度
标量
预期的磁场强度，以标量形式返回。
magcal使用实例
生成位于椭球上的未校准磁力计数据。
c = [-50; 20; 100]; % ellipsoid center
r = [30; 20; 50]; % semiaxis radii
[x,y,z] = ellipsoid(c(1),c(2),c(3),r(1),r(2),r(3),20);
D = [x(:),y(:),z(:)];校正磁力计数据，使其位于球体上。校准选项默认设置为“自动”。
[A,b,expmfs] = magcal(D); % 校准系数
expmfs % 偶极预期磁场强度，单位为uT
C = (D-b)*A; % 校准数据将未校准和校准的磁力计数据可视化。
figure(1)
plot3(x(:),y(:),z(:),'LineStyle','none','Marker','X','MarkerSize',8)
hold on
grid(gca,'on')
plot3(C(:,1),C(:,2),C(:,3),'LineStyle','none','Marker', ...
'o','MarkerSize',8,'MarkerFaceColor','r')
axis equal
xlabel('uT')
ylabel('uT')
zlabel('uT')
legend('Uncalibrated Samples', 'Calibrated Samples','Location', 'southoutside')
title("Uncalibrated vs Calibrated" + newline + "Magnetometer Measurements")
hold off其结果：
使用magcal函数实战
磁强计校准
磁强计检测沿着传感器的X、Y和Z轴的磁场强度。精确的磁场测量对于传感器融合以及航向和方位的确定至关重要。
为了对航向和方位计算有用，需要校准典型的低成本MEMS磁力计，以补偿环境噪声和制造缺陷。
理想磁强计
理想的三轴磁力计测量沿正交X、Y和Z轴的磁场强度。在没有任何磁干扰的情况下，磁力计的读数可以测量地球的磁场。如果在传感器旋转所有可能的方向时进行磁力计测量，则测量应位于球体上。球体的半径就是磁场强度。
要生成磁场样本，请使用imuSensor对象。出于这些目的，可以安全地假设每个方向的角速度和加速度为零。
N = 500;
rng(1);
acc = zeros(N,3);
av = zeros(N,3);
q = randrot(N,1); % uniformly distributed random rotations
imu = imuSensor('accel-mag');
[~,x] = imu(acc,av,q);
scatter3(x(:,1),x(:,2),x(:,3));
axis equal
title('Ideal Magnetometer Data');输出：
硬铁效应
噪声源和制造缺陷会降低磁力计的测量性能。其中最引人注目的是铁杆效应。硬铁效应是静止的干扰磁噪声源。通常，这些来自带有磁力计的电路板上的其他金属物体。硬铁效应改变了理想球体的原点。
imu.Magnetometer.ConstantBias = [2 10 40];
[~,x] = imu(acc,av,q);
figure;
scatter3(x(:,1),x(:,2),x(:,3));
axis equal
title('Magnetometer Data With a Hard Iron Offset');输出：
软铁效应
软铁效应更为微妙。它们是由传感器附近的物体引起的，这些物体会扭曲周围的磁场。这些具有拉伸和倾斜理想测量球体的效果。由此产生的测量结果位于椭球面上。
软铁磁场效应可以通过将IMU的地磁场矢量旋转到传感器框架，拉伸它，然后将它旋转回全局框架来模拟。
nedmf = imu.MagneticField;
Rsoft = [2.5 0.3 0.5; 0.3 2 .2; 0.5 0.2 3];
soft = rotateframe(conj(q),rotateframe(q,nedmf)*Rsoft);
for ii=1:numel(q)
imu.MagneticField = soft(ii,:);
[~,x(ii,:)] = imu(acc(ii,:),av(ii,:),q(ii));
end
figure;
scatter3(x(:,1),x(:,2),x(:,3));
axis equal
title('Magnetometer Data With Hard and Soft Iron Effects');输出：
校正技术
magcal函数可用于确定磁力计校准参数，这些参数既考虑了硬铁效应，也考虑了软铁效应。未校准的磁强计数据可以建模为位于椭球上，方程如下
在这个方程中，R是一个3乘3的矩阵，b是定义椭球中心的1乘3的矢量，x是未校准磁力计测量的1乘三的矢量，并且是指示磁场强度的标量。上述方程是圆锥曲线的一般形式。对于椭球体，R必须是正定的。magcal函数使用各种求解器，基于对R的不同假设。在magcal函数中，R可以被假设为单位矩阵、对角矩阵或对称矩阵。
magcal函数产生校正系数，该校正系数对位于偏移椭球上的测量进行测量，并将其转换为位于以原点为中心的理想球体上。magcal函数返回一个3乘3的实矩阵a和一个1乘3的向量b。为了校正未校准的数据，计算
这里，x是未校准磁力计测量值的1乘3阵列，m是位于球体上的校正磁力计测量的1乘三阵列。矩阵A的行列式为1，是R的矩阵平方根。此外，A的形式与R相同：单位矩阵、对角矩阵或对称矩阵。因为这些类型的矩阵不能赋予旋转，所以矩阵a在校正期间不会旋转磁力计数据。
magcal函数还返回第三个输出，即磁场强度。您可以使用磁场强度来设置ahrsfilter的ExpectedMagneticFieldStrength属性。
使用magcal函数
使用magcal功能确定校正有噪声的磁力计数据的校准参数。通过设置imuSensor中磁强计属性的NoiseDensity属性，创建有噪声的磁强计数据。在可变的soft中使用旋转和拉伸的磁场来模拟软铁效果。
imu.Magnetometer.NoiseDensity = 0.08;
for ii=1:numel(q)
imu.MagneticField = soft(ii,:);
[~,x(ii,:)] = imu(acc(ii,:),av(ii,:),q(ii));
end要找到最能校正未校准磁力计数据的A和b参数，只需将函数调用为：
[A,b,expMFS] = magcal(x);
xCorrected = (x-b)*A;绘制原始数据和校正后的数据。显示最适合原始数据的椭球体。显示更正数据应位于的球体。
de = HelperDrawEllipsoid;
de.plotCalibrated(A,b,expMFS,x,xCorrected,'Auto');输出：
magcal函数使用各种解算器来最小化残差。残差是校准数据和半径为expMFS的球体之间的距离之和。
r = sum(xCorrected.^2,2) - expMFS.^2;
E = sqrt(r.'*r./N)./(2*expMFS.^2);
fprintf('Residual error in corrected data : %.2f\n\n',E);如果只需要更正某些缺陷或实现更简单的更正计算，则可以运行单独的解算器。
仅偏移计算
许多MEMS磁力计在传感器内具有寄存器，该寄存器可用于补偿硬铁偏移。实际上，上述方程的（x-b）部分发生在传感器板上。当只需要硬铁偏移补偿时，a矩阵有效地成为单位矩阵。为了单独确定硬铁校正，magcal函数可以这样调用：
[Aeye,beye,expMFSeye] = magcal(x,'eye');
xEyeCorrected = (x-beye)*Aeye;
[ax1,ax2] = de.plotCalibrated(Aeye,beye,expMFSeye,x,xEyeCorrected,'Eye');
view(ax1,[-1 0 0]);
view(ax2,[-1 0 0]);输出：
硬铁补偿和轴缩放计算
对于许多应用，将椭球矩阵视为对角矩阵就足够了。从几何角度来看，这意味着未校准磁强计数据的椭球体近似为其半轴与坐标系轴对齐，中心偏离原点。尽管这不太可能是椭球体的实际特征，但它将校正方程简化为每轴一乘一减。
[Adiag,bdiag,expMFSdiag] = magcal(x,'diag');
xDiagCorrected = (x-bdiag)*Adiag;
[ax1,ax2] = de.plotCalibrated(Adiag,bdiag,expMFSdiag,x,xDiagCorrected,...
'Diag');输出：
全硬铁和软铁补偿
要强制magcal函数求解任意椭球体并生成稠密对称的a矩阵，请将函数调用为：
[A,b] = magcal(x,'sym');自动调整
应仔细使用“eye”、“diag”和“sym”标志，并检查输出值。在某些情况下，可能没有足够的数据进行高阶（“diag”或“sym”）拟合，并且可以使用更简单的a矩阵找到一组更好的校正参数。默认的“自动”调整选项可以处理这种情况。
考虑在高阶装配工中使用数据不足的情况。
xidx = x(:,3) > 100;
xpoor = x(xidx,:);
[Apoor,bpoor,mfspoor] = magcal(xpoor,'diag');没有足够的数据分布在椭球表面上，无法使用“diag”选项实现良好的拟合和正确的校准参数。因此，Apoor矩阵是复杂的。
disp(Apoor)输出：
0.0000 + 0.4722i 0.0000 + 0.0000i 0.0000 + 0.0000i
0.0000 + 0.0000i 0.0000 + 0.5981i 0.0000 + 0.0000i
0.0000 + 0.0000i 0.0000 + 0.0000i 3.5407 + 0.0000i
使用“自动”拟合选项可以避免这个问题，并找到一个更简单的a矩阵，它是实的、对称的和正定的。使用“auto”选项字符串调用magcal与不使用任何选项字符串调用相同。
[Abest,bbest,mfsbest] = magcal(xpoor,'auto');
disp(Abest)输出：
1 0 0
0 1 0
0 0 1
比较使用“自动”拟合器和不正确的高阶拟合器的结果表明，在校正数据之前不检查返回的A矩阵是危险的。
de.compareBest(Abest,bbest,mfsbest,Apoor,bpoor,mfspoor,xpoor);输出：
使用默认的“auto”标志调用magcal函数，将尝试“eye”、“diag”和“sym”搜索A和b的所有可能性，从而最大限度地减少残差，保持A的实数，并确保R是正定和对称的。
结论
magcal函数可以给出校准参数，以校正磁力计中的硬铁和软铁偏移。调用没有选项字符串的函数，或者等效地调用“auto”选项字符串，可以产生最佳匹配，并涵盖大多数情况。
另外，需要注意的是，magcal目前还不支持使用MATLAB Coder转为C/C++代码，实属遗憾。
附录：在实战中使用的自定义函数
定义为：HelperDrawEllipsoid.m
classdef HelperDrawEllipsoid < handle
methods (Static)
function [ax1, ax2] = plotCalibrated(A,b,Bmag, data, dataCorrected, txt)
% Plot calibrated and uncalibrated data on best fit ellipsoid and sphere.
%Open a new figure window and create two subplots
f = figure('NumberTitle', 'off', 'Name', txt);
ax1 = subplot(1,2,1);
set(ax1, 'Parent', f);
%Use a helper function to plot the ellipsoid based on the correction
%parameters. Create the ellipsoid matrix R from A that defines the
%uncalibrated data:
% (m - b).' * R * (m - b) = Bmag*Bmag
R = A.'*A;
HelperDrawEllipsoid.plotEllipsoid(R, b, Bmag);
[rdi, rdo] = HelperDrawEllipsoid.drawPartitionedPoints(ax1, R,b,Bmag, data);
title("Original Data and Best Fit Ellipsoid" + char(10) + "Using " + txt + " Fitter");
% Plot the corrected data
ax2 = subplot(1,2,2);
set(ax2, 'Parent', f);
%partition inside and outside sphere
[fdi, fdo] = HelperDrawEllipsoid.drawPartitionedPoints(ax2, eye(3), zeros(3,1).', Bmag, dataCorrected);
[xe,ye,ze] = ellipsoid(0,0,0,Bmag,Bmag,Bmag,80);
fe = surf2patch(xe,ye,ze);
HelperDrawEllipsoid.drawEllipsoid(fe);
title('Corrected Data Fit to Ideal Sphere');
if isempty(rdi) || isempty(rdo) || isempty(fdi) || isempty(fdo)
leg =legend('Collected data (uT)', 'Location', 'South');
else
leg =legend('Data inside fit (uT)', 'Data outside fit (uT)', 'Location', 'South');
end
leg.Position(1) = 0.4; %move to the middle
leg.Position(2) = 0.05;
f.Position(3) = 700;
end
function compareBest(Agood, bgood,expMFSgood, Abad, bbad, expMFSbad, data)
% Plot 'auto' fit (best) data vs data fit via 'diag'
% good - auto fit
% bad - diag fit
xgoodFixed = (data - bgood)*Agood;
f = figure('NumberTitle', 'off', 'Name', 'Correction with Auto vs Incorrect Fitter');
ax1 = subplot(1,2,1);
set(ax1, 'Parent', f);
[rdi, rdo] = HelperDrawEllipsoid.drawPartitionedPoints(ax1,eye(3),zeros(1,3),expMFSgood, xgoodFixed);
HelperDrawEllipsoid.plotEllipsoid(eye(3), zeros(1,3), expMFSgood);
title("Corrected Magnetometer Data" + char(10) + 'Using Auto Fitter');
xbadFixed = real((data - bbad)*Abad);
ax2 = subplot(1,2,2);
set(ax2, 'Parent', f);
[fdi, fdo] = HelperDrawEllipsoid.drawPartitionedPoints(ax2, eye(3),zeros(1,3),expMFSbad, xbadFixed);
HelperDrawEllipsoid.plotEllipsoid(eye(3), zeros(1,3), expMFSbad);
title("Corrected Magnetometer Data" + char(10) + 'Using Incorrect Fitter');
if isempty(rdi) || isempty(rdo) || isempty(fdi) || isempty(fdo)
leg =legend('Collected data (uT)', 'Location', 'South');
else
leg =legend('Data inside fit (uT)', 'Data outside fit (uT)', 'Location', 'South');
end
leg.Position(1) = 0.4; %move to the middle
leg.Position(2) = 0.05;
end
function [din, dout] = drawPartitionedPoints(ax, R,b, Bmag, data)
% Color data in the plot based on whether or not it is inside or outside the fitted ellipsoid/sphere
[din, dout] = HelperDrawEllipsoid.partitionEllipse(R, b, Bmag, data);
hold(ax, 'on');
plot3(ax, din(:,1),din(:,2),din(:,3),'LineStyle', 'none', 'MarkerSize', 6, 'MarkerFaceColor', 'b', 'Marker', 'o');
plot3(ax, dout(:,1),dout(:,2),dout(:,3),'LineStyle', 'none', 'MarkerSize', 6, 'MarkerFaceColor', 'r', 'Marker', 'o');
hold(ax, 'off');
axis equal
drawnow
end
function drawEllipsoid(fe)
% Draws a green ellipsoid or sphere and lights it nicely
p = patch(fe);
p.FaceColor = 'green';
p.EdgeColor = 'none';
camlight
view(3)
p.FaceAlpha = .5;
axis equal
end
function [dinside,doutside] = partitionEllipse(R, V, B, data)
%figure out which of the data is inside and which is outside of the
%computed ellipse.
res = ellipsoidResidual(data, V,R,B);
idx = res < 0;
dinside = data(idx,:);
doutside = data(~idx,:);
end
function p = plotEllipsoid(R, V, B)
% Plots an ellipsoid based on (x-V).'*R*(x-V) = B^2
N = 20;
%Singular values of R define the axes of the ellipsoid.
%Strech the values out by 50% to make a nice plot.
s = svd(R);
s = 1.5*max(s);
ax = linspace(-s*B, s*B, N)+V(1);
ay = linspace(-s*B, s*B, N)+V(2);
az = linspace(-s*B, s*B, N)+V(3);
[xi,yi,zi] = meshgrid(ax,ay,az);
d = [xi(:) yi(:) zi(:)];
res = ellipsoidResidual(d, V, R, B);
% Reshape to match xi - as if we did this element-by-element.
Uout = reshape(res, size(xi));
fv = isosurface(xi,yi,zi,Uout,0);
HelperDrawEllipsoid.drawEllipsoid(fv);
end
end
end
function res = ellipsoidResidual(data, V, R, B)
% Compute the ellipsoid equation for each data row
% res = (data - V).' * R * (data - V) - B*B
%
dOffset = data - V;
t = R*(dOffset.');
res = (sum(dOffset.* (t.'),2) - B*B);
end