gps_kalman_zsy.m

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

%%%%% gps kalman zishiying %%%%%
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;  %地球椭球体扁率
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 12.4664 313.6181;2 13.2561 99.1277;3 10.4480 41.9519;4 12.4727 289.5653;5 11.6077 116.5486;6 10.5432 209.4507;
       7 65.1275 73.1881;8 66.9823 101.9770;9 68.3454 286.8979;10 73.1479 200.4810;11 70.2154 77.1527;
       12 125.5712 295.1060;13 128.3618 285.5386;14 131.8240 123.7737;15 131.3074 44.5724;16 130.3997 73.6977;
       17 186.9655 97.3078;18 188.4401 98.7626;19 184.9619 319.1793;20 194.1995 55.7345;21 190.8460 172.0155;
       22 253.5063 46.1491;23 251.3094 346.3001;24 241.0610 333.6671;25 252.3879 163.3319;26 250.2394 231.0366;
       27 311.8862 334.8484;28 309.8682 287.5291;29 312.8828 146.8152;30 313.3067 94.0084];

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\trace\trace1.dat','r');
while 1
     linestring = fgets(fid);
     if linestring < 0
          break;
     end
        place=sscanf(linestring,'%*f%f%f%f%*[^\n]');
        gps_longi=place(1);
        gps_lati=place(2);
        gps_height=place(3);
        t_u = t_u+1        % 实时显示用户运行时间 
        user(1,t_u)=gps_longi;%用户经纬高信息
        user(2,t_u)=gps_lati;
        user(3,t_u)=gps_height;
  
        % 用户在大地坐标系中的经纬度数据,经度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<=30 %各个矩阵的列数表示量
                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
            u_k(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);
            
            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));
            %---计算仰角 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)=5*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;
                    if r~=q
                        i=1;
                        r=q;
                    end
                    s_n(q,i)=j;        % j :可见星标号
                    %-----------%
                    if s_n(q,i)~=0     % 将可见星提取出来
                        sd(i)=s_n(q,i);% sd ;可见星标号阵
                        % -地心坐标系下站星的几何距离 R - %                   
                        R=(x_k(q,sd(i))-xp(1,t_u))^2 + (y_k(q,sd(i))-yp(1,t_u))^2 + (z_k(q,sd(i))-zp(1,t_u))^2;
                        R=sqrt(R);
                        %------------------------------以下要产生伪距rou------------------------------%
                        %d_T(t_u)=0;  
                        %d_T(t_u)=100000+randn(1);%-仿真给出接收机在各个时刻的钟差,即折合的距离误差-% 
                        d_T(t_u)=5*randn(1);
                        %---------------------------------------%
                       
                        rou(q,i)=R+d_T(t_u)+d_star(sd(i)); % 带误差的伪距
                        %---------------------------------------------------------------------------%
                    end
                    %-----------%
                    i=i+1;
                else
                    ele(q,j)=0;
                end
                sum_s(t_u,1)=sum_s(t_u,1)+ele(q,j);
                j=j+1;
            end
     end
end
%
disp('=================以下开始定位计算,利用递推法=======================');
%-------用户总的运行时间
t_user=t_u; 
%*--------------------------matrix defined ------------------------*/ 
P =diag([25000,25000,25000,1000,1000,1000,0,0,0,90000,900]);
%P =diag([25000,25000,25000,4,4,4,4500000000000,150000000]);
        h0=9.4*1e-20;    h_1=1.8*1e-19;    h_2=3.8*1e-21;
        Qt11 = h0*T/2+2*h_1*T^2+2*pi^2*h_2*T^3/3;    Qt12 = 2*h_1*T+pi^2*h_2*T^3;    Qt22 = h0/(2*T)+2*h_1+8*pi^2*h_2*T/3;

X_kk =[-2.6061e+006,4.7375e+006,3.3831e+006, 2, 2, 2,1,1,1, 0, 0]';
%  set the A ,F ,G,H matrix
    A1=zeros(9,9) ;A1(1,4)=1 ; A1(2,5)=1 ;A1(3,6)=1 ; A1(4,7)=1 ;A1(5,8)=1 ; A1(6,9)=1 ;  A1(7,7)=-1 ;A1(8,8)=-1 ; A1(9,9)=-1 ;
A([1:9],[1:9])=A1;
A([10:11],[10:11])=[0,1; 0,0];
     
F([1:9],[1:9])=eye(9)+A1+A1*A1/2;
F([10:11],[10:11])=[1,T; 0,1];
Q([1:9],[1:9])=diag([0,0,0,0,0,0,4,4,4]);
Q ([10:11],[10:11])=[ Qt11,  Qt12;  
                      Qt12,  Qt22];
%Tao=zeros(11,5); Tao(4,7)=1 ;  Tao(5,8)=1 ;   Tao(6,9)=1 ;   Tao(10,10)=1 ;  Tao(11,11)=1 ;
%G=(eye(11)+A/2+A*A/6)*Tao;
            
%----------------------------------------------------------------------------------------%
for t_u=1:t_user  %用户运行的总点数
    Xkk_1=F*X_kk; % kalman equation
    % get_Q_kk( );         %// get robust Q_kk value
            Nd_tmp(1,1) = T*T*T*T*T/20.0;  Nd_tmp(1,2) = T*T*T*T/8.0;   Nd_tmp(1,3) = T*T*T/6.0;
            Nd_tmp(2,1) = Nd_tmp(1,2);     Nd_tmp(2,2) = T*T*T/3.0;     Nd_tmp(2,3) = T*T/2.0;
            Nd_tmp(3,1) = Nd_tmp(1,3);     Nd_tmp(3,2) = Nd_tmp(2,3);   Nd_tmp(3,3) = T;
            if X_kk(7,1) >= 0.0 
                qax= (4.0-pi)/pi*(20.0-X_kk(7,1))*(20.0-X_kk(7,1));
            else
                qax= (4.0-pi)/pi*(20.0+X_kk(7,1))*(20.0+X_kk(7,1));
            end
    
            if X_kk(8,1) >= 0.0 
                qay= (4.0-pi)/pi*(20.0-X_kk(8,1))*(20.0-X_kk(8,1));
            else 
                qay= (4.0-pi)/pi*(20.0+X_kk(8,1))*(20.0+X_kk(8,1));
            end
            
            if X_kk(9,1) >= 0.0 
                qaz= (4.0-pi)/pi*(20.0-X_kk(9,1))*(20.0-X_kk(9,1));
            else  
                qaz= (4.0-pi)/pi*(20.0+X_kk(9,1))*(20.0+X_kk(9,1));
            end
          
            for kc1=1:3
                for kc2=1:3
                    Q(kc1,kc2)     = 2.0* qax*Nd_tmp(kc1,kc2);
                    Q(kc1+3,kc2+3) = 2.0* qay*Nd_tmp(kc1,kc2);
                    Q(kc1+6,kc2+6) = 2.0* qaz*Nd_tmp(kc1,kc2);
                end
            end  
            Q ([10:11],[10:11])=[ Qt11,  Qt12;  
                                  Qt12,  Qt22];
    %end get_Q_kk()
  

    q=t_uu0+t_u;
    
    kk=sum_s(t_u,1);%可见星个数
    for i=1:kk
        sd(i)=s_n(q,i);% sd ;可见星标号阵
        % -地心坐标系下站星的几何距离 R - %                   
        R=sqrt((x_k(q,sd(i))-Xkk_1(1,1))^2 + (y_k(q,sd(i))-Xkk_1(2,1))^2 + (z_k(q,sd(i))-Xkk_1(3,1))^2);
        %-----求解用户位置变化量 ---%
        %  求A,V=AX+L %
        a_11=(x_k(q,sd(i))-Xkk_1(1,1))/R;        a_12=(y_k(q,sd(i))-Xkk_1(2,1))/R;
        a_13=(z_k(q,sd(i))-Xkk_1(3,1))/R;        a_14=1;
                             
        H(i,[1:11])=[-a_11,-a_12,-a_13,0,0,0,0,0,0,a_14,0];
        Measure_kk(i,1) = rou(q,i)-R-Xkk_1(10,1);
        R_k(i,i)=100;
    end 
    %   do_kalman( );        
    %Pkk_1 = F*P*F' +G*Q*G';                 % kalman equation
    Pkk_1 = F*P*F' +Q; 
    K = Pkk_1* H'*inv(H*Pkk_1*H'+R_k);      % 
    X_kk = Xkk_1 +K*Measure_kk;  % 
    P =( eye(11)- K*H )*Pkk_1;               % kalman equation
    %   end do_kalman()
    GPS_Result(t_u,1)=X_kk(1,1);
    GPS_Result(t_u,2)=X_kk(2,1);
    GPS_Result(t_u,3)=X_kk(3,1);
    %
    covP(1,t_u)=sqrt(P(1,1));covP(2,t_u)=sqrt(P(2,2));covP(3,t_u)=sqrt(P(3,3));covP(4,t_u)=sqrt(P(4,4));
    covP(5,t_u)=sqrt(P(5,5));covP(6,t_u)=sqrt(P(6,6));covP(7,t_u)=sqrt(P(7,7));covP(8,t_u)=sqrt(P(8,8));
    %-----对比定位前后的坐标，做差----%
    delta1_p_end(1,t_u)=abs(xp(1,t_u)-X_kk(1,1));
    delta2_p_end(1,t_u)=abs(yp(1,t_u)-X_kk(2,1));
    delta3_p_end(1,t_u)=abs(zp(1,t_u)-X_kk(3,1));
    delta_p_end(1,t_u)=sqrt(delta1_p_end(1,t_u)^2+delta2_p_end(1,t_u)^2+delta3_p_end(1,t_u)^2);
end
%求位置误差的平均值
    mm1=mean(delta1_p_end(1,:));
    mm2=mean(delta2_p_end(1,:));
    mm3=mean(delta3_p_end(1,:));
    mm=mean(delta_p_end(1,:)); 
%-----------------------------------各个时刻递推完毕-------------------------------------% 
disp('============Now ready to plot==============');

fh_trace=figure('Name','Flight Trace Kalman ZSY','NumberTitle','off');
    plot3(xp(1,:),yp(1,:),zp(1,:),'r',GPS_Result(:,1),GPS_Result(:,2),GPS_Result(:,3),'b');
    title('定位跟踪仿真');
    xlabel('经度（米）');ylabel('纬度（米）');zlabel('高度（米）');
    grid;    
fh_error1=figure('Name','Position Error1 Kalman ZSY','NumberTitle','off');
    subplot(3,1,1);plot(t,delta1_p_end(1,:),'b',t,mm1,'r.');title('X、Y、Z方向位置误差仿真');ylabel('X方向误差值(m)');grid;
    subplot(3,1,2);plot(t,delta2_p_end(1,:),'b',t,mm2,'r.');ylabel('Y方向误差值(m)');grid;
    subplot(3,1,3);plot(t,delta3_p_end(1,:),'b',t,mm3,'r.');xlabel('t(sec)');ylabel('Z方向误差值(m)');grid;
fh_error2=figure('Name','Position Error2 Kalman ZSY','NumberTitle','off');
    plot(t,delta_p_end(1,:),'b',t,mm,'r.');
    title('位置误差仿真');
    xlabel('t(sec)');ylabel('误差值(m)');
    grid;
fh_covP=figure('Name','covP Kalman ZSY','NumberTitle','off');
    plot(t,covP(1,:),'b',t,covP(2,:),'r',t,covP(3,:),'y',t,covP(4,:),'m',...
        t,covP(5,:),'c',t,covP(6,:),'g',t,covP(7,:),'k',t,covP(8,:),'m.');
    title('均方差仿真');
    xlabel('t(sec)');ylabel('均方差值(m)');
    grid;