下面的matlab运行有错误吗

问题遇到的现象和发生背景

找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;