You are an expert in writing code in many different languages. Your goal is to perform code completion for the following code, keeping in mind the rest of the code and the file meta data.
    Metadata details:
    filename: togeod.m
    path of file: ./repos/nav_matlab/example/gps_spp_test
    
    the code of the file until where you have to start completion: function [dphi,dlambda,h] = togeod(a,finv,X,Y,Z)
%TOGEOD   Subroutine to calculate geodetic coordinates
%         latitude, longitude, height given Cartesian
%         coordinates X,Y,Z, and reference ellipsoid

    
function [dphi,dlambda,h] = togeod(a,finv,X,Y,Z)
%TOGEOD   Subroutine to calculate geodetic coordinates
%         latitude, longitude, height given Cartesian
%         coordinates X,Y,Z, and reference ellipsoid
%         values semi-major axis (a) and the inverse
%         of flattening (finv).

%  The units of linear parameters X,Y,Z,a must all agree (m,km,mi,ft,..etc)
%  The output units of angular quantities will be in decimal degrees
%  (15.5 degrees not 15 deg 30 min).  The output units of h will be the
%  same as the units of X,Y,Z,a.

%  Copyright (C) 1987 C. Goad, Columbus, Ohio
%  Reprinted with permission of author, 1996
%  Fortran code translated into MATLAB
%  Kai Borre 03-30-96
% Changed according to Matlab ver. 6.5.1, 9 February 2004

h = 0;
tolsq = 1.e-10;
maxit = 1000;
% compute radians-to-degree factor
rtd = 180/pi;
% compute square of eccentricity
if finv < 1.e-20
   esq = 0;
else  
   esq = (2-1/finv)/finv; 
end