clc;
clear all;
%……%%……%
A=load('yuan.txt');
[N,M]=size(A);
Bsx=A(:,1);%x y z
Bsy=A(:,2);
Bsz=A(:,3);
B=3700;
%B=494*15;%
Bs=[Bsx Bsy Bsz];%BsN×3，
C0=eye(3,3);%，
b0=[+00;-00;0];
C=C0;%
b=b0;
Q=zeros(3,3);%Q()LE
L=zeros(3,1);
E=zeros(9,1);
H=zeros(9,9);
N=floor(N/9)*9;
tic
%……%%……%
for k=0:N/9-1
for i=0:8%，，
Bs1=(Bs(k+(N/9)*i+1,:))';%Bs，Bs1()，
B1=inv(C)*(Bs1-b);%B1，^
E(i+1)=B1'*B1-B*B;
H(i+1,:)=[(B1(1))^2 (B1(2))^2 (B1(3))^2 -B1(1)*B1(2) -B1(1)*B1(3) -B1(2)*B1(3) 2*B1(1) 2*B1(2) 2*B1(3)];
end
r=rank(H);
if r==9% ，
X=inv(H)*E;
Q(1,1)=1-X(1);Q(1,2)=0.5*X(4);Q(1,3)=0.5*X(5);%Q
Q(2,1)=Q(1,2);Q(2,2)=1-X(2);Q(2,3)=0.5*X(6);
Q(3,1)=Q(1,3);Q(3,2)=Q(2,3);Q(3,3)=1-X(3);
[V,D]=eig(Q);%，A，D，AV。
C1=V*sqrtm(D)*inv(V);%C1，C
C1=inv(C1);
L(1)=X(7);L(2)=X(8);L(3)=X(9);
b1=C*inv(Q)*L;%b1
C=C*C1
b=b+b1
end
k=k+1;
% if (abs(b1)==0)(C1==eye(3,3))%b1，（M1）
% break
% end
end
toc
%……%%……%
C1=eye(3,3);
% C1=[0.912 -0.154 0.1021;
% -0.154 1.1372 0.017
% 0.1021 0.017 -0.5624];
for j=1:N
Bs2=(Bs(j,:))';
Bs3(j,:)=(inv(C1)*(Bs2))';
end
scatter3((Bs3(:,1))/15,Bs3(:,2)/15,Bs3(:,3)/15,'+','b');%
%axis equal;
view(90,90)
B1=zeros(N,3);%
for j=1:N
Bs1=(Bs(j,:))';
B1(j,:)=(inv(C)*(Bs1-b))';
end
hold on
% figure
scatter3((B1(:,1)-50)/15,B1(:,2)/15,B1(:,3)/15,'.','g');
%axis equal;
xlabel('By/mG');
ylabel('Bx/mG');
zlabel('Bz/mG');
view(90,90)
sita=0:pi/20:2*pi;
plot((3700/15)*cos(sita),(3700/15)*sin(sita),'k');
%axis square;
hold off
legend('','','')