gps_kalman.m

本程序设计了GPS卫星仿真器

%%%%% gps kalman %%%%%
clear;
%------------- 参数定义 -----------%
pi=3.1415926;
C=3.0e8;              %光速
a=26609e3;            %轨道长半轴长，单位已经换算为 m
e=0.006;              %轨道的偏心率
i_0=55*pi/180;        %基准时间t_0的轨道倾角
a_e=6378137;          %地球椭球的长半径
f_e=1/298.257223563;  %地球椭球体扁率
b_e=(1-f_e)*a_e;      %地球椭球短半轴
e_2=2*f_e-f_e^2;      %GPS参考椭球第一偏心率的平方
E0=10;                %定义的高度角比较值
mu=3.986008e14;       %开普勒常数，单位为m3/s2
w_ie=7.292115147e-5;  %地球自转平均角速率，单位rad/s
% 卫星轨道参数矩阵epoch:2007-04-01 14:21:46，第一列卫星标号1~30，第二列升交点赤经W_0，第三列平近点角距M_0 %
sate=[
      1  325.73 190.96;2  325.73 220.48;3  325.73 330.17;4  325.73 83.58;                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                  
      5  25.73  249.90;6  25.73  352.12;7  25.73  25.25; 8  25.73  124.10;
      9  85.73  286.20;10 85.73  48.94; 11 85.73  155.08;12 85.73  183.71;
      13 145.73 312.30;14 145.73 340.93;15 145.73 87.06; 16 145.73 209.81;
      17 205.73 11.90; 18 205.73 110.76;19 205.73 143.88;20 205.73 246.11;
      21 265.73 52.42; 22 265.73 165.83;23 265.73 275.52;24 265.73 305.04];
for j=1:24
    sate(j,2)=sate(j,2)*pi/180;%升交点赤经
    sate(j,3)=sate(j,3)*pi/180;%平近点角
end
t_0=0;         %星历的参考历元
a3=a^3;
n=sqrt(mu/a3)  % n=(2*pi)/T=sqrt(mu/a3),应用了开普勒第三定律
k=1;
i=1;
A_i=1;
r=1;
T=1;
%
t_u=0;
t_uu0=500;     % 用户运行起始时间 
%---------------------- 初始位置 ---------------%

%----------------------------------------------------------------------------------------%
fid = fopen('E:\work\program\New\Programs\trace.dat','r');
while 1
     linestring = fgets(fid);
     if linestring < 0
          break;
     end
        place=sscanf(linestring,'%*f%f%f%f%f%*[^\n]');
        gps_longi=place(1);
        gps_lati=place(2);
        gps_height=place(3);
        gps_velo=place(4);
        t_u = t_u+1        % 实时显示用户运行时间 
        user(1,t_u)=gps_longi;%用户经纬高信息
        user(2,t_u)=gps_lati;
        user(3,t_u)=gps_height;
        user(4,t_u)=gps_velo;
        V(1,t_u)=user(4,t_u);
  
        % 用户在大地坐标系中的经纬度数据,经度L,纬度B,高度H %
        user1(1,t_u)=user(1,t_u)*pi/180;%完成弧度转换
        user1(2,t_u)=user(2,t_u)*pi/180;
        L=user1(1,t_u);
        B=user1(2,t_u);
        H=user(3,t_u);
        % 计算椭球的卯酉圈曲率半径N
        W=sqrt(1-e_2*sin(B)^2);
        N=a_e/W;
        % 将用户在大地坐标系下的坐标转换为地球坐标系的空间直角坐标[xp,yp,zp]
        xp(1,t_u)=(N+H)*cos(B)*cos(L);
        yp(1,t_u)=(N+H)*cos(B)*sin(L);
        zp(1,t_u)=(N*(1-e_2)+H)*sin(B);
        % 求系数阵h
        h(1,1)=-sin(B)*cos(L);h(1,2)=-sin(B)*sin(L);h(1,3)=cos(B);
        h(2,1)=-sin(L);       h(2,2)=cos(L);        h(2,3)=0;
        h(3,1)=cos(B)*cos(L); h(3,2)=cos(B)*sin(L); h(3,3)=sin(B);
        
        t_k=t_uu0+t_u;  % 找到对应于用户运行的时刻的卫星所在的位置，用户打第t_u个点时，时间为t_uu0+t_u
        q=t_k;          % 各个矩阵的行数表示量
        t(t_u)=t_u;
        j=1;            % 卫星标号
        sum_s(t_u,1)=0;   % 求 q 时刻的卫星数目矩阵
            while j<=24 %各个矩阵的列数表示量
                M_k(q,j)=sate(j,3)+n*(t_k-t_0);    
                Et_1(q,j)=M_k(q,j);
                t_end=1;  
                while(t_end)
                    Et(q,j)=M_k(q,j)+e*sin(Et_1(q,j));        
                    delta_E(q,j)=Et(q,j)-Et_1(q,j);
                    Et_1(q,j)=Et(q,j);
                    if abs(delta_E(q,j))<=1.0e-6
                        E_k(q,j)=Et(q,j);  
                        t_end=0;
                    end 
                end 
            %-------------- 求真近点角 f 的值，并进行象限判断 -----------%
                A=cos(E_k(q,j))-e;               %分母一定是是大于0的数,所以只取分子来做判断
                B=sqrt(1-e^2)*sin(E_k(q,j));
                if (A==0)
                    f(q,j)=pi/2;            
                elseif (B==0)
                    f(q,j)=pi;
                else
                    f(q,j)=atan(abs(B/A));
                    if ((B>0)&(A<0))
                        f(q,j)=pi-f(q,j);
                    elseif ((B<0)&(A<0))
                        f(q,j)=pi+f(q,j);
                    elseif ((B<0)&(A>0))
                        f(q,j)=2*pi-f(q,j);
                    end
                end
            w(q,j)=0;    
            u_k(q,j)= w(q,j)+f(q,j);         
            r_k(q,j)=a*(1-e*cos(E_k(q,j)));    
            i_k(q,j)=i_0;    
            L_k(q,j)=sate(j,2)-w_ie*(t_k);
            P=[cos(w(q,j))*cos(L_k(q,j))-sin(w(q,j))*sin(L_k(q,j))*cos(i_k(q,j));
                    cos(w(q,j))*sin(L_k(q,j))+sin(w(q,j))*cos(L_k(q,j))*cos(i_k(q,j));
                                    sin(w(q,j))*sin(i_k(q,j))];
            Q=[-sin(w(q,j))*cos(L_k(q,j))-cos(w(q,j))*sin(L_k(q,j))*cos(i_k(q,j));
                    -sin(w(q,j))*sin(L_k(q,j))+cos(w(q,j))*cos(L_k(q,j))*cos(i_k(q,j));
                                    cos(w(q,j))*sin(i_k(q,j))];
            %计算卫星位置 %
            x_k(q,j)=r_k(q,j)*cos(u_k(q,j))*cos(L_k(q,j))-r_k(q,j)*sin(u_k(q,j))*sin(L_k(q,j))*cos(i_k(q,j));    
            y_k(q,j)=r_k(q,j)*cos(u_k(q,j))*sin(L_k(q,j))+r_k(q,j)*sin(u_k(q,j))*cos(L_k(q,j))*cos(i_k(q,j));
            z_k(q,j)=r_k(q,j)*sin(u_k(q,j))*sin(i_k(q,j));
            % 计算卫星速度 %
            Vs=-n*a*sin(E_k(q,j))/(1-e*cos(E_k(q,j)))*P+n*a*sqrt(1-e_2)*cos(E_k(q,j))/(1-e*cos(E_k(q,j)))*Q;
            Vsx_k(q,j)=Vs(1,1);
            Vsy_k(q,j)=Vs(2,1);
            Vsz_k(q,j)=Vs(3,1);
            %---计算仰角 E=arctan(Z/sqrt(X^2+Y^2)) ,E_rad单位rad ,E_deg单位度 -----%
            delta_x(q,j)=x_k(q,j)-xp(1,t_u);
            delta_y(q,j)=y_k(q,j)-yp(1,t_u);
            delta_z(q,j)=z_k(q,j)-zp(1,t_u);
            %求卫星在 %  站心坐标系下 % 的坐标
            X_sta(q,j)=h(1,1)*delta_x(q,j)+h(1,2)*delta_y(q,j)+h(1,3)*delta_z(q,j);
            Y_sta(q,j)=h(2,1)*delta_x(q,j)+h(2,2)*delta_y(q,j)+h(2,3)*delta_z(q,j);
            Z_sta(q,j)=h(3,1)*delta_x(q,j)+h(3,2)*delta_y(q,j)+h(3,3)*delta_z(q,j);
            %---------------------给出对应各颗卫星的星历误差---------------------%
            %d_star(j)=0;
            %d_star(j)=50+randn(1);
            d_star(j)=10*randn(1);
            %--------------------------------------------------------------------%
            E_deno(q,j)=X_sta(q,j)^2+Y_sta(q,j)^2;
            E_deno(q,j)=sqrt(E_deno(q,j));
                if E_deno(q,j)==0
                    E_rad(q,j)=pi/2;
                    E_deg(q,j)=90;
                else
                    E_rad(q,j)=atan(Z_sta(q,j)/E_deno(q,j));
                    E_deg(q,j)=E_rad(q,j)*180/pi;
                end
                %-- 开始高度角比较 ,E0 为给定的高度角,判断可见星 --%         
                ele(q,j)=E_deg(q,j);
                if ele(q,j)>=E0
                    ele(q,j)=1;