Open Source Inertial Navigation Toolkit

%Computes the curvature matrix and the gravity.
%Type 1: Non-singular implementation for wander azimuth mechanization
%Type 2: Singular implementation for local geodetic frame mechanization

%Inputs
%Type=1 or 2
%Cen3: 3rd column Earth to nav transformation matrix.
%       [cos(ALPHA)cos(LAT) -sin(ALPHA)cos(LAT) -sin(LAT)]'
%height: Height above elipsoid
%Vn: Velocity in navigation frame

function [Fc, wen_n, wie_n , grav]=geoparam(type, Cen3, height, Vn)

SM_AXIS=6378137;
E_SQR=0.00669437999014;

%Local radii
sr_e=sqrt(1.0-E_SQR*Cen3(3)^2);
Re_h=(SM_AXIS/sr_e)+height;
Rn_h=(SM_AXIS*(1-E_SQR)/(sr_e^3))+height;

if (type==1)
    %Wander azimuth frame mechanization. Note Fc(3,:) will be zero in this implementation
    %Curvature matrix in navigation frame
    sr_a=SM_AXIS*E_SQR/(sr_e^3)/(Re_h)/(Rn_h);
    Fc=zeros(3);
    Fc(1,1)=sr_a*(Cen3(2)^2)+(1/(Re_h));
    Fc(1,2)=-sr_a*Cen3(1)*Cen3(2);
    Fc(2,1)=Fc(1,2);
    Fc(2,2)=sr_a*(Cen3(1)^2)+(1/(Re_h));
    
elseif (type==2)
    %local geodetic frame implementation. Note that, this implementation
    %explicitly uses 1/cosPHI. Therefore, it is singular at the pole.
    
    %Curvature matrix
    Fc=zeros(3);
    Fc(1,1)=1/Re_h;
    Fc(2,2)=1/Rn_h;
    tanLAT=-Cen3(3)/Cen3(1);
    Fc(3,1)=-tanLAT/Re_h;
end


if (nargout>1)
    wen_n=Fc*[Vn(2);-Vn(1);0];  %Note: In wander-azimuth mechanization, wen_n(3)=0
    wie_n=Cen3*7292115e-11;
end

if (nargout==4)
    %Plump bob gravity
    %(Just as a different flavour, I am going to use here a very old gravity model instead of WGS84)
    %See:Heiskanen and Moritz (1967)
    sL2=Cen3(3)^2;
    sL4=Cen3(3)^4;

    a1=9.7803267715;
    a2=0.0052790414;
    a3=0.0000232718;
    a4=-0.0000030876910891;
    a5=0.0000000043977311;
    a6=0.0000000000007211;
    grav=a1*(1+a2*sL2+a3*sL4)+(a4+a5*sL2)*height+a6*height^2;
    
%     %WGS84 approximation for plumb bob gravity.
%     NORMAL_GRV=9.7803253359;
%     GRV_CONS=0.00193185265241;
%     FLATTENING=0.00335281066475;
%     M_FAKTOR=0.00344978650684;
% 
%     sLat=-Cen3(3);
%     g1=NORMAL_GRV*(1+GRV_CONS*sLat*sLat)/sqrt(1.0-E_SQR*sLat^2);
%     grav=g1*(1.0-(2.0/SM_AXIS)*(1.0+FLATTENING+M_FAKTOR-2.0*FLATTENING*sLat*sLat)*height+3.0*height*height/SM_AXIS/SM_AXIS);
end
geoparam_v001
