%% 最小二乘法球面拟合程序
% 球面拟合算法
% H=load('X0.rpt');我在运行程序的时候用的是此行，但这样并不便于各位程序猿学习;
% 为了便于各位程序猿学习，我将X0.RPT的内容列于下方，该数据文件来源于有限元分析软件PATRAN
H= [97 39.657715 5.221072 0.199951 0.000000 -0.000627 -0.000011
98 79.315674 10.442080 0.799805 0.000000 -0.000626 -0.000017
99 73.910400 30.614687 0.799805 0.000000 -0.000626 -0.000042
100 36.955078 15.307363 0.199951 0.000000 -0.000627 -0.000023
101 118.973389 15.663170 1.800293 0.000000 -0.000625 -0.000023
102 110.865234 45.922016 1.800293 0.000000 -0.000625 -0.000060
103 158.631104 20.884161 3.201172 0.000000 -0.000623 -0.000029
104 147.820557 61.229370 3.201172 0.000000 -0.000623 -0.000079
105 63.468506 48.700882 0.799805 0.000000 -0.000626 -0.000064
106 31.734375 24.350483 0.199951 0.000000 -0.000627 -0.000034
107 95.202393 73.051376 1.800293 -0.000000 -0.000625 -0.000094
108 126.936523 97.401825 3.201172 -0.000000 -0.000623 -0.000124
109 48.700928 63.468285 0.799805 0.000000 -0.000626 -0.000082
110 24.350342 31.734135 0.199951 0.000000 -0.000627 -0.000043
111 73.051025 95.202423 1.800293 -0.000000 -0.000625 -0.000121
112 97.401855 126.936501 3.201172 -0.000000 -0.000623 -0.000160
113 30.614990 73.910347 0.799805 -0.000000 -0.000626 -0.000095
114 15.307617 36.955200 0.199951 0.000000 -0.000627 -0.000050
115 45.922119 110.865555 1.800293 -0.000000 -0.000625 -0.000141
116 61.229492 147.820694 3.201172 -0.000000 -0.000623 -0.000186
117 10.442383 79.315575 0.799805 -0.000000 -0.000626 -0.000102
118 5.220947 39.657818 0.199951 0.000000 -0.000627 -0.000053
119 15.663330 118.973366 1.800293 -0.000000 -0.000625 -0.000151
120 20.884277 158.631134 3.201172 -0.000000 -0.000623 -0.000200
121 -10.441895 79.315575 0.799805 -0.000000 -0.000627 -0.000102
122 -5.220703 39.657818 0.199951 0.000000 -0.000627 -0.000053
123 -15.663330 118.973366 1.800293 -0.000000 -0.000625 -0.000151
124 -20.884033 158.631134 3.201172 -0.000000 -0.000624 -0.000200
125 -30.614990 73.910347 0.799805 -0.000000 -0.000627 -0.000096
126 -15.307617 36.955196 0.199951 0.000000 -0.000627 -0.000050
127 -45.921875 110.865547 1.800293 -0.000000 -0.000625 -0.000142
128 -61.229492 147.820755 3.201172 -0.000000 -0.000624 -0.000187
129 -48.701172 63.468281 0.799805 0.000000 -0.000627 -0.000083
130 -24.350342 31.734133 0.199951 0.000000 -0.000627 -0.000044
131 -73.051025 95.202415 1.800293 -0.000000 -0.000626 -0.000122
132 -97.401855 126.936493 3.201172 -0.000000 -0.000624 -0.000162
133 -63.468018 48.700939 0.799805 0.000000 -0.000627 -0.000065
134 -31.733887 24.350481 0.199951 0.000000 -0.000627 -0.000035
135 -95.202637 73.051369 1.800293 -0.000000 -0.000626 -0.000095
136 -126.936523 97.401817 3.201172 -0.000000 -0.000624 -0.000126
137 -73.910645 30.614679 0.799805 0.000000 -0.000627 -0.000043
138 -36.955078 15.307360 0.199951 0.000000 -0.000627 -0.000024
139 -110.865234 45.922005 1.800293 0.000000 -0.000626 -0.000062
140 -147.820801 61.229355 3.201172 0.000000 -0.000624 -0.000081
141 -79.315430 10.442073 0.799805 0.000000 -0.000627 -0.000018
142 -39.657471 5.220757 0.199707 0.000000 -0.000627 -0.000011
143 -118.973145 15.663159 1.800293 0.000000 -0.000626 -0.000025
144 -158.630859 20.884209 3.201172 0.000000 -0.000624 -0.000032
145 -79.315430 -10.442053 0.799805 0.000000 -0.000627 0.000008
146 -39.657471 -5.220733 0.199707 0.000000 -0.000627 0.000002
147 -118.973145 -15.663143 1.800293 0.000000 -0.000626 0.000014
148 -158.630859 -20.884195 3.201172 0.000000 -0.000624 0.000020
149 -73.910645 -30.614658 0.799805 0.000000 -0.000627 0.000033
150 -36.955078 -15.307398 0.199707 0.000000 -0.000627 0.000014
151 -110.865234 -45.921989 1.800293 0.000000 -0.000626 0.000051
152 -147.820801 -61.229343 3.201172 0.000000 -0.000624 0.000070
153 -63.468018 -48.700916 0.799805 0.000000 -0.000627 0.000055
154 -31.733887 -24.350393 0.199707 0.000000 -0.000627 0.000025
155 -95.202637 -73.051414 1.800293 0.000001 -0.000626 0.000085
156 -126.936523 -97.401863 3.201172 0.000001 -0.000624 0.000114
157 -48.701172 -63.468319 0.799805 0.000000 -0.000627 0.000073
158 -24.350342 -31.734171 0.199707 0.000000 -0.000627 0.000034
159 -73.051025 -95.202400 1.800293 0.000001 -0.000626 0.000112
160 -97.401855 -126.936478 3.201172 0.000001 -0.000624 0.000151
161 -30.614990 -73.910324 0.799805 0.000001 -0.000627 0.000086
162 -15.307617 -36.955173 0.199951 0.000000 -0.000627 0.000041
163 -45.921875 -110.865524 1.800293 0.000001 -0.000625 0.000132
164 -61.229492 -147.820724 3.201172 0.000001 -0.000624 0.000177
165 -10.441895 -79.315613 0.799805 0.000001 -0.000627 0.000093
166 -5.220703 -39.657852 0.199707 0.000000 -0.000627 0.000044
167 -15.663330 -118.973404 1.800293 0.000001 -0.000625 0.000142
168 -20.884766 -158.631104 3.201172 0.000001 -0.000624 0.000191
169 10.442383 -79.315613 0.799805 0.000001 -0.000626 0.000093
170 5.220947 -39.657852 0.199707 0.000000 -0.000627 0.000044
171 15.663086 -118.973404 1.800293 0.000001 -0.000625 0.000142
172 20.884277 -158.631104 3.201172 0.000001 -0.000623 0.000191
173 30.614990 -73.910324 0.799805 0.000001 -0.000626 0.000087
174 15.307617 -36.955170 0.199951 0.000000 -0.000627 0.000041
175 45.922119 -110.865517 1.800293 0.000001 -0.000625 0.000133
176 61.229492 -147.820724 3.201172 0.000001 -0.000623 0.000178
177 48.700928 -63.468315 0.799805 0.000000 -0.000626 0.000074
178 24.350342 -31.734169 0.199707 0.000000 -0.000627 0.000035
179 73.051025 -95.202393 1.800293 0.000001 -0.000625 0.000114
180 97.401855 -126.936592 3.201172 0.000001 -0.000623 0.000153
181 63.468506 -48.700912 0.799805 0.000000 -0.000626 0.000056
182 31.734375 -24.350515 0.199951 0.000000 -0.000627 0.000026
183 95.202393 -73.051407 1.800293 0.000001 -0.000625 0.000087
184 126.936523 -97.401848 3.201172 0.000001 -0.000623 0.000117
185 73.910400 -30.614653 0.799805 0.000000 -0.000626 0.000034
186 36.955078 -15.307395 0.199707 0.000000 -0.000627 0.000015
187 110.865234 -45.921978 1.800293 0.000000 -0.000625 0.000053
188 147.820557 -61.229328 3.201172 0.000000 -0.000623 0.000072
189 79.315674 -10.442045 0.799805 0.000000 -0.000626 0.000009
190 39.657715 -5.221104 0.199951 0.000000 -0.000627 0.000002
191 118.973389 -15.663133 1.800293 0.000000 -0.000625 0.000016
192 158.631104 -20.884182 3.201172 0.000000 -0.000623 0.000023
386 0.000000 0.000000 0.000000 0.000000 -0.000627 -0.000005];
% 节点编号 X坐标 Y坐标 Z坐标 X向位移分量 Y向位移分量 Z向位移分量
x=H(:,2)+H(:,5);
y=H(:,3)+H(:,6);
z=H(:,4)+H(:,7);
num_points = length(x);
x_avr = sum(x)/num_points;
y_avr = sum(y)/num_points;
z_avr = sum(z)/num_points;
xx_avr = sum(x.*x)/num_points;
yy_avr = sum(y.*y)/num_points;
zz_avr = sum(z.*z)/num_points;
xy_avr = sum(x.*y)/num_points;
xz_avr = sum(x.*z)/num_points;
yz_avr = sum(y.*z)/num_points;
xxx_avr = sum(x.*x.*x)/num_points;
xxy_avr = sum(x.*x.*y)/num_points;
xxz_avr = sum(x.*x.*z)/num_points;
xyy_avr = sum(x.*y.*y)/num_points;
xzz_avr = sum(x.*z.*z)/num_points;
yyy_avr = sum(y.*y.*y)/num_points;
yyz_avr = sum(y.*y.*z)/num_points;
yzz_avr = sum(y.*z.*z)/num_points;
zzz_avr = sum(z.*z.*z)/num_points;
%计算法方程组的系数矩阵
A = [xx_avr - x_avr*x_avr,xy_avr - x_avr*y_avr,xz_avr - x_avr*z_avr;
xy_avr - x_avr*y_avr,yy_avr - y_avr*y_avr,yz_avr - y_avr*z_avr;
xz_avr - x_avr*z_avr,yz_avr - y_avr*z_avr,zz_avr - z_avr*z_avr];
b = [xxx_avr - x_avr*xx_avr + xyy_avr - x_avr*yy_avr + xzz_avr - x_avr*zz_avr;
xxy_avr - y_avr*xx_avr + yyy_avr - y_avr*yy_avr + yzz_avr - y_avr*zz_avr;
xxz_avr - z_avr*xx_avr + yyz_avr - z_avr*yy_avr + zzz_avr - z_avr*zz_avr];
b = b/2;
resoult = inv(A)*b;
x00 = resoult(1); %拟合后的球心x坐标
y00 = resoult(2); %拟合后的球心y坐标
z00 = resoult(3); %拟合后的球心z坐标
r = sqrt(xx_avr-2*x00*x_avr+x00*x00 + yy_avr-2*y00*y_avr+y00*y00 + zz_avr-2*z00*z_avr+z00*z00); %拟合后的球面半径
zz=zeros(length(x),1);
for i=1:length(x)
zz(i,1)=sqrt((r^2-(x(i,1)-x00)^2+(y(i,1)-y00)^2))-z00; % 拟合后的Z
end
D=zz-z;
PV=max(H(:,7))-min(H(:,7));
RMS=std(H(:,7));
scatter3(x,y,D);
fprintf('拟合结果\n');
fprintf('x0 = %f', x00);
fprintf('y0 = %f', y00);
fprintf('z0 = %f', z00);
fprintf('r = %f', r);
