三维空间中的椭球拟合与磁力计、加速度计校订

参考资料：

https://blog.csdn.net/HJ199404182515/article/details/78018785《平面二维任意椭圆数据拟合算法推导及程序实现详解》

https://blog.csdn.net/hj199404182515/article/details/59480954 《空间二次曲面数据拟合算法推导及仿真分析》

http://www.javashuo.com/article/p-bxwiknfj-np.html 《IMU加速度、磁力计校订－－椭球拟合》

https://pdfs.semanticscholar.org/7e32/c4d82d9d1e995e560cd700ee0a0acf5f384c.pdf   1990年的论文《An algorithm for tting an ellipsoid to data》

 

最近作磁力计、加速度计的校准，用到了椭球拟合，看了很多网上的方法都是经过求偏导和极值的方法，感受较为繁琐。最终仍是本身推导的较为简便。

说是叫椭球拟合，实际上就是求椭球方程的待定系数而已，本质上是个求线性方程的最小二乘解问题。关于最小二乘能够参考个人另外一篇博客《最小二乘估计及证实》 https://blog.csdn.net/qq_31073871/article/details/81067301

博客写公式不方便，我在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个系数便可