matlab编程处理大地测量学中各种坐标系之间的转换-1

自己学习编的程序，自己已全部运行，为了防止代码丢失，发个博客记录一下。以下代码比较多，每个代码块是一个函数，相互之间要调用，缺一不可。
由于代码过多，分为两个博客写，此为“matlab编程处理大地测量学中各种坐标系之间的转换-1”

%度分秒转化为弧度
function arc=AngleToArc(angle)
angleD=fix(angle);
angleFM=(angle-angleD)*100.00;
angleF=fix(angleFM);
angleM=(angleFM-angleF)/36.0;
arc=(angleD+angleF/60.00+angleM)*pi/180;

%弧度转化为度分秒
function angle=ArcToAngle(arc)
arc0=arc*180/pi;
deg1=fix(arc0);     %取整
min=fix((arc0-deg1)*60);
sec=((arc0-deg1)*60-min)*60;
angle=deg1+min/100+sec/10000;

function M= GetM(B,a,e2)
M=a*(1-e2)/sqrt(GetW(B,e2)^3);

%计算法线长N，调用GetW函数
function N=GetN(B,a,e2)
N=a/GetW(B,e2);

%计算辅助函数W
function W=GetW(B,e2)
W=sqrt(1-e2*(sin(B))^2);

function Bf= XToBf(X,a,e2)
A0=1+(3/4)*e2+(45/64)*e2^2+(175/256)*e2^3+(11025/16384)*e2^4+(43659/65536)*e2^5;
B0=(3/8)*e2+(15/32)*e2^2+(525/1024)*e2^3+(2205/4096)*e2^4+(72765/131072)*e2^5;
C0=(15/256)*e2^2+(105/1024)*e2^3+(2205/16384)*e2^4+(10395/65536)*e2^5;
D0=(35/3072)*e2^3+(105/4096)*e2^4+(10395/262114)*e2^5;
E0=(315/131072)*e2^4+(3465/524288)*e2^5;
F0=(639/1310720)*e2^5;
B1=X/[a*(1-e2)*A0];
Bi=(1/A0)*[X/(a*(1-e2))+B0*sin(2*B1)-C0*sin(4*B1)+D0*sin(6*B1)-E0*sin(8*B1)+F0*sin(10*B1)];
    while(abs(Bi-B1)>10^-20)
    B1=Bi;
    Bi=(1/A0)*[X/(a*(1-e2))+B0*sin(2*B1)-C0*sin(4*B1)+D0*sin(6*B1)-E0*sin(8*B1)+F0*sin(10*B1)];
    end
Bf=ArcToAngle(Bi);
fprintf('大地纬度为:\n Bf = %.9f\n', Bf);

%求子午线弧长
function X=BToX(B,a,e2)
B=AngleToArc(B);
A0=1+(3/4)*e2+(45/64)*e2^2+(175/256)*e2^3+(11025/16384)*e2^4+(43659/65536)*e2^5;
B0=(3/8)*e2+(15/32)*e2^2+(525/1024)*e2^3+(2205/4096)*e2^4+(72765/131072)*e2^5;
C0=(15/256)*e2^2+(105/1024)*e2^3+(2205/16384)*e2^4+(10395/65536)*e2^5;
D0=(35/3072)*e2^3+(105/4096)*e2^4+(10395/262114)*e2^5;
E0=(315/131072)*e2^4+(3465/524288)*e2^5;
F0=(639/1310720)*e2^5;
X=a*(1-e2)*[A0*B-B0*sin(2*B)+C0*sin(4*B)-D0*sin(6*B)+E0*sin(8*B)-F0*sin(10*B)];
fprintf('子午线弧长为:\n X = %.3f\n',  X);

function [x,y]= BLToxy(B,L,a,e2,typeofzone)
if typeofzone==3
    L0=ceil((L-1.5)/3)*3;
else
    L0=ceil(L/6)*6-3;
end
l=L-L0;
l=AngleToArc(l);
B0=B;
B=AngleToArc(B);
t=tan(B);
b=sqrt(a^2-e2*a^2);
e1=(sqrt(a*a-b*b))/b;
nn=e1*cos(B);
x=BToX(B0,a,e2)+GetN(B,a,e2)/2*sin(B)*cos(B)*l^2+GetN(B,a,e2)/24*sin(B)*cos(B)^3*(5-t^2+9*nn^2+4*nn^4)*l^4+GetN(B,a,e2)/720*sin(B)*cos(B)^5*(61-58*t^2+t^4)*l^6;
y=GetN(B,a,e2)*cos(B)*l+GetN(B,a,e2)/6*cos(B)^3*(1-t^2+nn^2)*l^3+GetN(B,a,e2)/120*cos(B)^5*(5-18*t^2+t^4+14*nn^2-58*nn^2*t^2)*l^5;
if typeofzone==6
    y=y+ceil(L/6)*10^6+5*10^5;
else
    y=y+ceil((L-1.5)/3)*10^6+5*10^5;
end
fprintf('结果：\nx=%.3f\ny=%.3f',x,y);

function [B,L]= xyToBL(x,y,a,e2,typeofzone)
if typeofzone==3
    L0=floor(y/(10^6))*3;
else
    L0=floor(y/(10^6))*6-3;
end
if typeofzone==6
    y=y-floor(y/(10^6))*10^6-5*10^5;
else
    y=y-floor(y/(10^6))*10^6-5*10^5;
end
X=x;
Bf= XToBf(X, a,e2);
L0=AngleToArc(L0);
Bfh=AngleToArc(Bf);
t=tan(Bfh);
b=sqrt(a^2-e2*a^2);
e1=(sqrt(a*a-b*b))/b;
n=sqrt(e1^2*cos(Bfh)^2);
Mf=GetM(Bf,a,e2);
Nf=GetN(Bf,a,e2);
B=Bfh+t/(2*Mf*Nf)*y^2-t/(24*Mf*Nf^3)*(5+3*t^2+n^2-9*n^2*t^2)*y^4+t/(720*Mf*Nf^5)*(61+90*t^2+45*t^4)*y^4;
l=1/(Nf*cos(Bfh))*y-1/(6*Nf^3*cos(Bfh))*(1+2*t^2+n^2)*y^3+1/(120*Nf^5*cos(Bfh))*(5+28*t^2+24*t^4+6*n^2+8*n^2*t^2)*y^5;
B=ArcToAngle(B);
L=ArcToAngle(l+L0);
fprintf('结果\nB=%.9f\nL=%.9f',B,L);