satellite.m

用于GPS卫星的位置和速度解算

function gpskalman
%求解卫星位置的子程序,输入参数是卫星号number和解算的时间点T
function v=calposition(number,T)
%算卫星位置的参数
ee=sqrt(0.00669437999013);%地球的偏心率
a=6378137;   %地球赤道半径
b=6356755;   %地球极半径
GM=3986005*10^8;
omigaedot=7.2921151467e-5;
We=7.2921151467*10^(-5);%rad/s地球自转速率
    [s_indat,count]=fscanf(fidr(number),'%f',29);
    Sid=s_indat(1);
    M0=s_indat(11);%开普勒6个参数，参考时刻的平近点角2,把它转换为以弧度为单位
    A=s_indat(9);%卫星轨道长半轴，星历上给的是开方值3
    A=sqrt(A);
    omega0=s_indat(22);%参考时刻的升交点赤经减去本周开始时刻的GAST(t0)4,把它转换为以弧度为单位
    i0=s_indat(20);%参考时刻的轨道倾角5
    w=s_indat(13);%近地点角距6,把它转换为以弧度为单位
    e=s_indat(12);%卫星轨道偏心率7
    toe=s_indat(8);
    Crs=s_indat(17);                         
    deltan=s_indat(10);               
    Cuc=s_indat(14);                           
    Cus=s_indat(15);                  
    Cic=s_indat(18);                   
    Cis=s_indat(19);                       
    Crc=s_indat(16);                           
    omigadot=s_indat(23);                   
    idot=s_indat(21);
    af2=s_indat(29);
    af1=s_indat(28);
    af0=s_indat(27);
         
         t=T;
         n=sqrt(GM)/A^3;  
         n=n+deltan;
         Ms=M0+n*(t-toe);
         Es=Ms+e*sin(Ms);
         Es=Ms+e*sin(Es);
         Es=Ms+e*sin(Es);
         Es=Ms+e*sin(Es);
         Es=Ms+e*sin(Es);         
         E0=Es;
         x=(cos(E0)-e)/(1-e*cos(E0));              % x为cos(f)
         y=(sqrt(1-e*e)*sin(E0))/(1-e*cos(E0));    % y为sin(f)
         f=atan(y/x)*180/pi;
         if y>0&x>0
         f=f;
         end
        if y>0&x<0
        f=f+180;
        end
        if y<0&x>0
        f=f+360;
        end
        if y<0&x<0
        f=f+180;
        end
        f=f*pi/180;
        fs=f;
        
        u=w+fs;
        deltar=Crc*cos(2*u)+Crs*sin(2*u);
        deltai=Cic*cos(2*u)+Cis*sin(2*u);
        deltau=Cuc*cos(2*u)+Cus*sin(2*u);
        r=A*A*(1-e*cos(Es))+deltar;
        u2=u+deltau;
        i=i0+idot*(t-toe)+deltai;
        lenda=omega0+(omigadot-We)*(t-toe)-We*toe;
        
        x=r*cos(u2);
        y=r*sin(u2);
        z=0;                
        h=[x,y,z]';%卫星在轨道平面坐标系中坐标   
        v1=-omigaedot*[-sin(lenda),-cos(lenda),0;cos(lenda),-sin(lenda),0;0,0,0]*[1,0,0;0,cos(i),-sin(i);0,sin(i),cos(i)]*h;
        Hi=[cos(lenda),-sin(lenda)*cos(i),sin(lenda)*sin(i);sin(lenda),cos(lenda)*cos(i),-cos(lenda)*sin(i);0,sin(i),cos(i)]*h;%卫星在地心坐标系中空间直角坐标
        v2=n*(A^4)/sqrt(Hi(1)^2+Hi(2)^2+Hi(3)^2)*[cos(lenda),-sin(lenda),0;sin(lenda),cos(lenda),0;0,0,1]*[1,0,0;0,cos(i),-sin(i);0,sin(i),cos(i)]*[cos(w),-sin(w),0;sin(w),cos(w),0;0,0,1]*[-sin(E0);sqrt((1-e^2))*cos(E0);0];
        v=v1+v2;   %卫星速度
%------------------------------------------------------------------------------------