初识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 < handlemethods (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 subplotsf = 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*BmagR = 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 dataax2 = 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');elseleg =legend('Data inside fit (uT)', 'Data outside fit (uT)', 'Location', 'South');endleg.Position(1) = 0.4; %move to the middleleg.Position(2) = 0.05;f.Position(3) = 700;endfunction compareBest(Agood, bgood,expMFSgood, Abad, bbad, expMFSbad, data)% Plot 'auto' fit (best) data vs data fit via 'diag'%   good - auto fit%   bad - diag fitxgoodFixed = (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');elseleg =legend('Data inside fit (uT)', 'Data outside fit (uT)', 'Location', 'South');endleg.Position(1) = 0.4; %move to the middleleg.Position(2) = 0.05;endfunction [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 equaldrawnowendfunction drawEllipsoid(fe)% Draws a green ellipsoid or sphere and lights it nicelyp = patch(fe);p.FaceColor = 'green';p.EdgeColor = 'none';camlightview(3)p.FaceAlpha = .5;axis equalendfunction [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,:);endfunction 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);endend
endfunction 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