kf_gg.m

本程序设计了常用的几个坐标系的转换

        end
        %
        sum_s_2(t_u,1)=0;   % 求 q 时刻的卫星数目矩阵
        j=1;            % 卫星标号
        i=1;
        while j<=30 %各个矩阵的列数表示量
            M_k(q,j)=sate_2(j,3)+n_2*(t_k-t_0);    
            r_k(q,j)=a_2;
            %
            gui_1(q,j)=r_k(q,j)*cos(M_k(q,j));
            gui_2(q,j)=r_k(q,j)*sin(M_k(q,j));
            gui_3(q,j)=0;
            L_k_2(q,j)=sate_2(j,2)+w_ie*(t_k);
            %地坐标系坐标
            xk_2(q,j)=gui_1(q,j)*cos(L_k_2(q,j))-gui_2(q,j)*sin(L_k_2(q,j))*cos(i_0_galileo);
            yk_2(q,j)=gui_1(q,j)*sin(L_k_2(q,j))+gui_2(q,j)*cos(L_k_2(q,j))*cos(i_0_galileo);
            zk_2(q,j)=gui_2(q,j)*sin(i_0_galileo);
            %gali->gps
            D_s=1.0e-006 *[0.2378   -0.1026   -0.1235;
    0.1026    0.2378    0.0373;
    0.1235   -0.0373    0.2378];
            T=[0.3742;
   -0.6088;
    0.3613];
            X_G=[xk_2(q,j);yk_2(q,j);zk_2(q,j)]+T+D_s*[xk_2(q,j);yk_2(q,j);zk_2(q,j)];
            xk_g(q,j)=X_G(1,1);
            yk_g(q,j)=X_G(2,1);
            zk_g(q,j)=X_G(3,1);
            %---计算仰角 E=arctan(Z/sqrt(X^2+Y^2)) ,E_rad单位rad ,E_deg单位度 --%
            delta_x(q,j)=xk_2(q,j)-xp_2(1,t_u);
            delta_y(q,j)=yk_2(q,j)-yp_2(1,t_u);
            delta_z(q,j)=zk_2(q,j)-zp_2(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)=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_2(q,j)=E_deg(q,j);
                if ele_2(q,j)>=E0
                    ele_2(q,j)=1;
                    if r~=q
                        i=1;
                        r=q;
                    end
                    s_n_2(q,i)=j;        % j :可见星标号
                    %-----------%
                    if s_n_2(q,i)~=0     % 将可见星提取出来
                        sd(i)=s_n_2(q,i);% sd ;可见星标号阵
                        % -地心坐标系下站星的几何距离 rou - %                   
                        rou(q,i)=(xk_g(q,sd(i))-xp_2(1,t_u))^2 + (yk_g(q,sd(i))-yp_2(1,t_u))^2 + (zk_g(q,sd(i))-zp_2(1,t_u))^2;
                        rou(q,i)=sqrt(rou(q,i));
                        %------------------------------以下要产生伪距R------------------------------%
                        %d_T(t_u)=0;  
                        %d_T(t_u)=100000+randn(1);%-仿真给出接收机在各个时刻的钟差,即折合的距离误差-% 
                        d_T(t_u)=randn(1);
                        %---------------------------------------%
                        %R(q,i)=rou(q,i);                       % 不加任何误差时的伪距R
                        R_2(q,i)=rou(q,i)+d_T(t_u)+d_star(sd(i)); % 带误差的伪距
                        %---------------------------------------------------------------------------%
                    end
                    %-----------%
                    i=i+1;
                else
                    ele_2(q,j)=0;
                end
                sum_s_2(t_u,1)=sum_s_2(t_u,1)+ele_2(q,j);
                j=j+1;
        end
        sum_s(t_u,1)=sum_s_1(t_u,1)+sum_s_2(t_u,1);
        %
     end
end
%
disp('===============以下开始定位计算,利用递推法==============');
%-------用户总的运行时间
t_user=t_u; 
%*--------------------------matrix defined ------------------------*/ 
P1=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*kal_T/2+2*h_1*kal_T^2+2*pi^2*h_2*kal_T^3/3;   
        Qt12 = 2*h_1*kal_T+pi^2*h_2*kal_T^3;    
        Qt22 = h0/(2*kal_T)+2*h_1+8*pi^2*h_2*kal_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_kk1 =[-2.6061e+006,4.7375e+006,3.3831e+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,kal_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)*Tao;
P2=P1;   X_kk2=X_kk1;
            
%----------------------------------------------------------------------------------------%
for t_u=1:t_user  %用户运行的总点数
    
    Xkk_1=F*X_kk1; % kalman equation

    q=t_uu0+t_u;
    kk=sum_s_1(t_u,1);%gps可见星个数
    for i=1:kk
        sd(i)=s_n_1(q,i);% sd ;可见星标号阵
        % -地心坐标系下站星的几何距离 rou - %                   
        rou(t_u,i)=sqrt((xk_1(q,sd(i))-Xkk_1(1,1))^2 + (yk_1(q,sd(i))-Xkk_1(2,1))^2 + (zk_1(q,sd(i))-Xkk_1(3,1))^2);
        %-----求解用户位置变化量 ---%
        %  求A,V=AX+L %
        a_11(i,t_u)=(xk_1(q,sd(i))-Xkk_1(1,1))/rou(t_u,i);
        a_12(i,t_u)=(yk_1(q,sd(i))-Xkk_1(2,1))/rou(t_u,i);
        a_13(i,t_u)=(zk_1(q,sd(i))-Xkk_1(3,1))/rou(t_u,i);
        a_14(i,t_u)=1;
                             
        H1(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_kk1(i,1) = R_1(q,i)-rou(t_u,i)-Xkk_1(7,1);
        R_k1(i,i)=0.05;
    end 
    %   do_kalman( );        
    Pkk_1 = F*P1*F' +G*Q*G';                 % kalman equation
    K1 = Pkk_1* H1'*inv(H1*Pkk_1*H1'+R_k1);      % 
    X_kk1 = Xkk_1 +K1*Measure_kk1;  % 
    P1 =( eye(8)- K1*H1 )*Pkk_1;               % kalman equation
    %   end do_kalman()
 
    GPS_Result(t_u,1)=X_kk1(1,1);
    GPS_Result(t_u,2)=X_kk1(2,1);
    GPS_Result(t_u,3)=X_kk1(3,1);
    %
    %%%%%%%%%%%%%%%%
    Xkk_1=F*X_kk2; % kalman equation
    kk=sum_s_2(t_u,1);%galileo可见星个数
    for i=1:kk
        sd(i)=s_n_2(q,i);% sd ;可见星标号阵
        % -地心坐标系下站星的几何距离 rou - %  
        rou(t_u,i)=sqrt((xk_g(q,sd(i))-Xkk_1(1,1))^2 + (yk_g(q,sd(i))-Xkk_1(2,1))^2 + (zk_g(q,sd(i))-Xkk_1(3,1))^2);
        %-----求解用户位置变化量 ---%
        %  求A,V=AX+L %
        a_21(i,t_u)=(xk_g(q,sd(i))-Xkk_1(1,1))/rou(t_u,i);
        a_22(i,t_u)=(yk_g(q,sd(i))-Xkk_1(2,1))/rou(t_u,i);
        a_23(i,t_u)=(zk_g(q,sd(i))-Xkk_1(3,1))/rou(t_u,i);
        a_24(i,t_u)=1;
                             
        H2(i,[1:8])=[-a_21(i,t_u),-a_22(i,t_u),-a_23(i,t_u),0,0,0,a_24(i,t_u),0];
        Measure_kk2(i,1) = R_2(q,i)-rou(t_u,i)-Xkk_1(7,1);
        R_k2(i,i)=0.05;
    end 
    %   do_kalman( );        
    Pkk_1 = F*P2*F' +G*Q*G';                 % kalman equation
    K2 = Pkk_1* H2'*inv(H2*Pkk_1*H2'+R_k2);      % 
    X_kk2 = Xkk_1 +K2*Measure_kk2;  % 
    P2 =( eye(8)- K2*H2 )*Pkk_1;               % kalman equation
    %   end do_kalman()

    Gali_Result(t_u,1)=X_kk2(1,1);
    Gali_Result(t_u,2)=X_kk2(2,1);
    Gali_Result(t_u,3)=X_kk2(3,1);
    delta1_p_end(1,t_u)=abs(xp(1,t_u)-X_kk1(1,1));    delta2_p_end(1,t_u)=abs(yp(1,t_u)-X_kk1(2,1));    delta3_p_end(1,t_u)=abs(zp(1,t_u)-X_kk1(3,1));
    delta_p_end1(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);
    delta1_p_end(1,t_u)=abs(xp(1,t_u)-X_kk2(1,1));    delta2_p_end(1,t_u)=abs(yp(1,t_u)-X_kk2(2,1));    delta3_p_end(1,t_u)=abs(zp(1,t_u)-X_kk2(3,1));
    delta_p_end2(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);
    
    X_kk=delta_p_end2(1,t_u)/(delta_p_end2(1,t_u)+delta_p_end1(1,t_u))*X_kk1+...
         delta_p_end1(1,t_u)/(delta_p_end2(1,t_u)+delta_p_end1(1,t_u))*X_kk2;%加权平均
    
    %-----对比定位前后的坐标，做差----%
    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
    
mm=mean(delta_p_end(1,:));%求误差的平均值
vv=sqrt(delta_p_end(1,:)*delta_p_end(1,:)'/t_user);
%-----------------------------------各个时刻递推完毕-------------------------------------% 
disp('============Now ready to plot==============');
figure(1) % 递推定位前后的用户位置坐标差
    plot(GPS_Result(:,1),GPS_Result(:,2),'b',xp(1,:),yp(1,:),'r',xp(1,1),yp(1,1),'bO',xp(1,t_user),yp(1,t_user),'b*');
    grid;
%figure(2)
    %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.')
figure(3) % 递推定位前后的用户位置坐标差
    plot(t,delta_p_end(1,:),'b',t,mm,'r.');
    xlabel('t(sec)');
    ylabel('误差值(m)');
    grid;