问题遇到的现象和发生背景
找Position, Navigation, and Timing Technologies in the 21st Century Integrated Sat书的pdf
我想要达到的结果
分享链接(百度网盘链接)
global CONST_R_E CONST_FLAT_E %defined in svmconst.m
[n,m]=size(xyz);
if (m < 3)
error('You must supply an xyz 3 vector');
end
e2 = (2- CONST_FLAT_E)* CONST_FLAT_E;
p2= xyz(:,1).^2 + xyz(:,2).^2;
p = sqrt(p2);
llh(:,2) = atan2( xyz(:,2), xyz(:,1));
% interation on Lat and Height
llh(:,1) = atan2( xyz(:,3)./p, 0.01 );
r_N = CONST_R_Eones(n,1)./ sqrt( 1- e2 sin(llh(:,1)).^2);
llh(:,3) = p./cos(llh(:,1)) - r_N;
% iteration
old_H = -1e-9;
num = xyz(:,3)./p;
while abs(llh(:,3)- old_H) > 1e-4
old_H = llh(:,3);
den = 1- e2 * r_N./(r_N+llh(:,3));
llh(:,1) = atan2(num,den);
r_N = CONST_R_E./ sqrt(1- e2* sin(llh(:,1)).^2);
llh(:,3) = p./cos(llh(:,1))- r_N;
end
llh(:,1:2) = llh(:,1:2)*180/pi;