gps_kalman.m

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

                    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(q,i)=(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(q,i)=sqrt(R(q,i));
                        
                        %------------------------------以下要产生伪距rou------------------------------%
                        %d_T(t_u)=0;  
                        %d_T(t_u)=100000+randn(1);%-仿真给出接收机在各个时刻的钟差,即折合的距离误差-% 
                        d_T(t_u)=10*randn(1);
                        %---------------------------------------%
                        rou(q,i)=R(q,i)+d_T(t_u)+d_star(sd(i)); % 带误差的伪距
                        rou_v(q,i)=(rou(q,i)-rou(q-1,i))/T;
                        %---------------------------------------------------------------------------%
                    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,90000,900]);
        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;
Q =[100,  0,  0,  0,  0;
      0,100,  0,  0,  0;
      0,  0,100,  0,  0;
      0,  0,  0,  Qt11,  Qt12;  
      0   0,  0,  Qt12,  Qt22];
X_kk =[-2.5419e+006,4.7806e+006,3.3606e+006, 2, 2, 2, 0, 0]';
%  set the A ,F ,G,H matrix
A1=zeros(6,6) ;A1(1,4)=1 ; A1(2,5)=1 ;A1(3,6)=1 ;   
A([1:6],[1:6])=A1;
A([7:8],[7:8])=[0,1; 0,0];
     
F([1:6],[1:6])=eye(6)+A1+A1*A1/2;
F([7:8],[7:8])=[1,T; 0,1];

Tao=zeros(8,5); Tao(4,1)=1 ;  Tao(5,2)=1 ;   Tao(6,3)=1 ;   Tao(7,4)=1 ;  Tao(8,5)=1 ;
G=(eye(8)+A/2+A*A/6)*T*Tao;
            
%----------------------------------------------------------------------------------------%
for t_u=1:t_user  %用户运行的总点数
    Xkk_1=F*X_kk; % kalman equation

    q=t_uu0+t_u;
    
    kk=sum_s(t_u,1);%可见星个数
    for i=1:kk
        sd(i)=s_n(q,i);% sd ;可见星标号阵
        % -地心坐标系下站星的几何距离 R - %                   
        R(t_u,i)=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(i,t_u)=(x_k(q,sd(i))-Xkk_1(1,1))/R(t_u,i);
        a_12(i,t_u)=(y_k(q,sd(i))-Xkk_1(2,1))/R(t_u,i);
        a_13(i,t_u)=(z_k(q,sd(i))-Xkk_1(3,1))/R(t_u,i);
        a_14(i,t_u)=1;
                             
        H(i,[1:8])=[-a_11(i,t_u),-a_12(i,t_u),-a_13(i,t_u),0,0,0,a_14(i,t_u),0];
        Measure_kk(i,1) = R(q,i)-rou(t_u,i)-Xkk_1(7,1);
        %R_k(i,i)=0.05;
        R_k(i,i)=4000;
    end
 
    %   do_kalman;        
    Pkk_1 = F*P*F' +G*Q*G';                
    K = Pkk_1* H'*inv(H*Pkk_1*H'+R_k);      
    X_kk = Xkk_1 +K*Measure_kk;  
    P =( eye(8)- K*H )*Pkk_1;               
    %   end do_kalman
    X(t_u,1)=X_kk(1,1);
    Y(t_u,1)=X_kk(2,1);
    Z(t_u,1)=X_kk(3,1);
    %%%%%%%%空间直角坐标——>大地坐标%%%%%%%%%%%
    lamda=atan(Y(t_u,1)/X(t_u,1))*180/pi;
       if lamda<0
          lamda=180+lamda;
       end
    N(1)=a_e;
    H(1)=sqrt(X(t_u,1)^2+Y(t_u,1)^2+Z(t_u,1)^2)-sqrt(a_e*b_e);
    phi(1)=atan(Z(t_u,1)/sqrt(X(t_u,1)^2+Y(t_u,1)^2)*(1-e_2*N(1)/(N(1)+H(1)))^(-1))*180/pi;
    N(2)=a_e/sqrt(1-e_2*(sin(phi(1)*pi/180))^2);
    H(2)=sqrt(X(t_u,1)^2+Y(t_u,1)^2)/cos(phi(1)*pi/180)-N(2);
    phi(2)=atan(Z(t_u,1)/sqrt(X(t_u,1)^2+Y(t_u,1)^2)*(1-e_2*N(2)/(N(2)+H(2)))^(-1))*180/pi;
    i=2;
       while((abs(phi(i)-phi(i-1))>(0.00001/(60*60)))|(abs(H(i)-H(i-1))>0.001))
             i=i+1;
             N(i)=a_e/sqrt(1-e_2*(sin(phi(i-1)*pi/180))^2);
             H(i)=sqrt(X(t_u,1)^2+Y(t_u,1)^2)/cos(phi(i-1)*pi/180)-N(i);
             phi(i)=atan(Z(t_u,1)/sqrt(X(t_u,1)^2+Y(t_u,1)^2)*(1-e_2*N(i)/(N(i)+H(i)))^(-1))*180/pi;
       end
    L_end(t_u,1)=lamda;
    B_end(t_u,1)=phi(i);
    H_end(t_u,1)=H(i);
    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 ','NumberTitle','off');
    plot3(user(1,:),user(2,:),user(3,:),'r',L_end(:,1),B_end(:,1),H_end(:,1),'b');
    title('航迹跟踪');
    xlabel('经度（米）');ylabel('纬度（米）');zlabel('高度（米）');
    grid; 
% fh_numstar=figure('Name','visible satellite','NumberTitle','off');
%     plot(t,sum_s(:,1),'m.-');
%     title('可见星数目仿真');
%     xlabel('t(sec)');ylabel('可见星数目n');
%     ylim([3,8]);
%     grid;
% fh_trace=figure('Name','Flight Trace Kalman','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','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_covP=figure('Name','covP Kalman','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;