三维空间中的椭球拟合+磁力计校准算法+加速度计校准算法

本文由【暴躁的野生猿】发表于CSDN，如有非法转载请帮忙举报谢谢。

参考资料：

平面二维任意椭圆数据拟合算法推导及程序实现详解_Inspire-CSDN博客《平面二维任意椭圆数据拟合算法推导及程序实现详解》

空间二次曲面数据拟合算法推导及仿真分析_Inspire-CSDN博客 《空间二次曲面数据拟合算法推导及仿真分析》

IMU加速度、磁力计校正－－椭球拟合_shenshikexmu的博客-CSDN博客_椭球拟合 《IMU加速度、磁力计校正－－椭球拟合》

An Algorithm for Tting an Ellipsoid to Data | Semantic Scholar   1990年的论文《An algorithm for tting an ellipsoid to data》

最近做磁力计、加速度计的校准，用到了椭球拟合，看了不少网上的方法都是通过求偏导和极值的方法，感觉较为繁琐。最终还是自己推导的较为简便。

说是叫椭球拟合，实际上就是求椭球方程的待定系数而已，本质上是个求线性方程的最小二乘解问题。关于最小二乘可以参考我的另一篇博客《最小二乘估计及证明》 最小二乘估计及证明_野生猿-群号1025127672-CSDN博客_最小二乘估计

博客写公式不方便，我在word里写好，直接贴图上来了

理论已经讲完了，下面用matlab看看效果如何：

主文件：test.m

%生成模拟数据
OX = 100;
OY = 123;
OZ = 345;
Rx = 100;
Ry = 200;
Rz = 150;
x = [];
y = [];
z = [];
for theta = 0: 0.1: 2*pi%经度
    for phi = 0: 0.1: pi%纬度
        x = [x, Rx * cos(theta) * sin(phi) + OX];
        y = [y, Ry * sin(theta) * sin(phi) + OY];
        z = [z, Rz * cos(phi) + OZ];
    end
end

% %unifrnd(a,b,m,n)%产生m行n列[a,b]之间的随机数
error = 10;
x = x + unifrnd(-error, error, 1, length(x));
y = y + unifrnd(-error, error, 1, length(y));
z = z + unifrnd(-error, error, 1, length(z));
x = x';
y = y';
z = z';

%显示模拟数据
hold on;
grid on;
axis equal;
xlabel('X轴');
ylabel('Y轴');
zlabel('Z轴');
plot3(x,y,z, '.');

[VOX, VOY, VOZ, VRX, VRY, VRZ] = ellipsoidFit_my(x, y, z);%求解椭球参数

%拟合结果的可视化显示
ellipsoid(VOX, VOY, VOZ, VRX, VRY, VRZ, 50);% ellipsoid(OX, OY, OZ, ra, rb, rc, 50);
alpha(0.001)
plot3([VOX - VRX, VOX + VRX], [VOY, VOY], [VOZ, VOZ], 'LineWidth',5);
plot3([VOX, VOX], [VOY - VRY, VOY + VRY], [VOZ, VOZ],  'LineWidth',5);
plot3([VOX, VOX], [VOY, VOY], [VOZ - VRZ, VOZ + VRZ],  'LineWidth', 5);
fprintf('真实值  : XYZ中心[%0.2f, %0.2f, %0.2f], 半轴长[%1.2f, %1.2f, %1.2f]\n', OX, OY, OZ, Rx, Ry, Rz);
fprintf('拟合结果: XYZ中心[%0.2f, %0.2f, %0.2f], 半轴长[%1.2f, %1.2f, %1.2f]\n', VOX, VOY, VOZ, VRX, VRY, VRZ);

椭球拟合函数：ellipsoidFit_my.m

function [VOX, VOY, VOZ, VRX, VRY, VRZ] = ellipsoidFit_my(x, y, z)
%椭球拟合
%形参：xyz为要拟合的三维样本数据，维数为Nx3，N为样本数目
%返回值：Ox, Oy, Oz为拟合出的椭球中心，Rx, Ry, Rz为三个半轴的长度
%本来把全部代码都贴在这里了，还是有伸手党文章都不读就知道在评论里要代码，
根本就不是真心想学习的人，烦得很，
最终决定把这个函数删掉了。
仔细阅读了博文的朋友们肯定能自己写出来
end

拟合的结果如下：

发现拟合的结果相当不错。

然后用我自己写的函数[VOX, VOY, VOZ, VRX, VRY, VRZ] = ellipsoidFit(x, y, z)，拟合了一下实际采集的磁力计数据，拟合的结果如下：

ellipsoidFit_my函数很短，c语言移植到单片机上并不算困难，有需求的朋友可以这个网页上搜索【联系方式】四个字找到我的联系方式加群拿源码，或者留言。

-----------------

前文-2019-12-12新增：

讲了轴对齐的椭球的拟合方法，实际上对于磁力计、加速度计校准，大多也都是采用这种方法。对于轴不对齐的椭球，求解过程是类似的，需要的采样点至少为9个，因为轴不对齐的椭球通用方程为：

求解a~i共9个系数即可。然而，上式9个系数不是随便给的，这9个系数必须满足一定的约束条件，上式才能成为一个椭球，否则就是一个任意的三维曲面。这个约束条件就是“要满足二次型”，二次型这个名词，我记得本科在同济版的《线性代数》最后一章才讲到，本质上讲，这个二次型矩阵实现了对一个标准圆球的三轴拉伸与旋转，这样可以形成任意姿态的椭球，直观理解可参见知乎马同学的文章（www.zhihu.com/question/38902714），他的文章是讲二维平面椭圆的，其原理与三维椭球完全一致。

那么下一篇进阶的文章就是：《轴不对齐的椭球校准算法》

还有终极版的椭球校准：《轴不正交的椭球校准》

从实用角度来说，本文可以满足大多数的便宜(5000元以内)传感器校准需求了，一般的无人机、无人驾驶汽车等等用的器件不可能更贵了。

如果有更精密的传感器（豪车、军车、军用飞机等应用场景）需要校准，那就必须得用进阶版的校准算法。