三维空间中的椭球拟合

参考资料：

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

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

https://blog.csdn.net/shenshikexmu/article/details/70143455 《IMU加速度、磁力计校正－－椭球拟合》

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

说是叫椭球拟合，实际上就是求椭球方程的待定系数而已，本质上是个求线性方程的最小二乘解问题。关于最小二乘可以参考我的另一篇博客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_gyh(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_gyh.m

function [VOX, VOY, VOZ, VRX, VRY, VRZ] = ellipsoidFit_gyh(x, y, z)
%椭球拟合
%形参：xyz为要拟合的三维样本数据，必须输入列向量
%返回值：Ox, Oy, Oz为拟合出的椭球中心，Rx, Ry, Rz为三个半轴的长度
K = [y.^2, z.^2, x, y, z, ones(length(x), 1)];
Y = -x.^2;
V=(K'*K)^(-1)*K'*Y;

VOX = -V(3)/2;% -C/2
VOY = -V(4)/V(1)/2;% -D/A/2
VOZ= -V(5)/V(2)/2;% -E/B/2
VRX = sqrt(VOX^2 + V(1)*VOY^2 + V(2)*VOZ^2 - V(6));% sqrt(VOX^2 + A*VOY^2 + B*VOZ^2 - F)
VRY = sqrt(VRX^2/V(1));% sqrt(VRX^2/A);
VRZ = sqrt(VRX^2/V(2));% sqrt(VRX^2/B);
end

拟合的结果如下：

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

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

ellipsoidFit_gyh函数很短，c语言移植到单片机上并不算困难。当然matlab自带m语言转c语言的功能，我没研究过，有朋友会做的话，可以留言交流一下。

三维空间中的椭球拟合

猜你喜欢