伪距单点定位matlab程序

此代码为实现定位、定速的功能模块，得到相应数据之后调用即可。
所需数据：伪距、伪距变率、卫星权重、卫星位置、卫星速度、钟差、钟差变率、信号星上设备延迟。
此代码中，考虑到卫星钟差、接收机钟差、电离层延迟误差、对流层延迟误差，最后得到的精度大概在0.01m左右。

function [ position, velocity, sigma_0, flag ] = BDS_Positioning(Rou_B1_0,Rou_Dot_B1,Rou_B3_0,Rou_Dot_B3,Weight,  Pos_Sat_0,Vel_Sat_0,  delta_Time_Sat_B3,delta_Time_Dot_Sat_B3,TGD_B1);

    global Omega_CGCS2000;%地球自转角速度
    global Velocity_Light;%光速
    global Rad;%角度转弧度
%     B1 = 1561098000;
%     B3 = 1268520000;
%    Weight = diag(Weight);

    c = Velocity_Light;
    
    %卫星钟差
    delta_Time_Sat_B1 = delta_Time_Sat_B3 - TGD_B1;
    Rou_tj_B1 = c*delta_Time_Sat_B1;
    Rou_tj_B3 = c*delta_Time_Sat_B3;
    Rou_B1 = Rou_B1_0 + Rou_tj_B1;
    Rou_B3 = Rou_B3_0 + Rou_tj_B3;

    % 电离层误差
    deltarn1 = 1.944*(Rou_B3-Rou_B1);
    deltarn3 = 2.944*(Rou_B3-Rou_B1);
    Rou_B1 = Rou_B1 - deltarn1;%卫星钟差、电离层传播延迟修正后的伪距，量纲：米
    
    %参数初始化
    taojk = zeros(37,1);
    deltarp = zeros(37,1);
    theta = zeros(37,1);    %高度角

    xj = Pos_Sat_0;
    vj = Vel_Sat_0;
    xk = zeros(3,1);

    %循环，当位置增量小于0.001m时跳出循环
    for iteration=1:15
        
        Y = zeros(37,1);
        A = zeros(37,4);
        X = zeros(4,1);
        
        %计算地球自转修正后卫星位置、速度
        for i=1:37
            if(Weight(i)~=1)
                continue
            end
            taojk(i) = norm(Pos_Sat_0(:,i) - xk)/c;
            d = Omega_CGCS2000*taojk(i);
            xj(:,i) = Pos_Sat_0(:,i) + [0,d,0;
                                -d,0,0;
                                 0,0,0]*Pos_Sat_0(:,i);
            vj(:,i) = Vel_Sat_0(:,i) + [0,d,0;
                                -d,0,0;
                                 0,0,0]*Vel_Sat_0(:,i);
        end
        
        %对流层误差
        for i=1:37
            deltarp(i) = 2.47/((sin(theta(i)))+0.0121);
        end

        %Y A
        for i=1:37
            if(Weight(i)~=1)
                continue
            end
            r_j_k= xj(:,i)-xk;
            r0= norm(r_j_k);
            r_j_k_0= r_j_k/r0;
            Y(i) = Rou_B1(i) - r0 - deltarp(i);
            A(i,:) = [-r_j_k_0', 1];
        end
        
        %最小二乘解
        N_1= inv(A'*A);
        dX = N_1* (A'*Y);
        xk = xk+dX(1:3);
        eps = norm(dX(1:3)); %BLH NED
        if (eps<0.001)
            break;
        end

        %由经纬度计算e系至n系的旋转矩阵
        for i=1:37
            if(Weight(i)~=1)
                continue
            end
            %计算旋转矩阵
            BLH = Trans_2_XYZ_to_BLH(xk);
            B = BLH(1);
            L = BLH(2);
            H = BLH(3);
            s_az=sin(L);
            c_az=cos(L);
            ay= -B-pi/2;    %绕y 轴旋转的角度（rad）
            s_ay=sin(ay);
            c_ay=cos(ay);
            C_z=  [ c_az,   s_az,   0;
                -s_az,   c_az,   0;
                0,      0,      1];
            C_y=  [ c_ay,   0, -s_ay;
                0,      1,  0;
                s_ay,   0,  c_ay];
            C_e2n0= C_y*C_z;
            %计算方向矢量
            r_j_k= xj(:,i)-xk;
            r_j_k_n = C_e2n0*r_j_k;
            theta(i) = atan(-r_j_k_n(3)/sqrt((r_j_k_n(1)^2)+(r_j_k_n(2)^2)));
            if theta(i)<15*Rad
                Weight(i)=0;
            end
        end
    end
    
    %定速
    %Y2
    Y2 = zeros(37,1);
    A2 = zeros(37,4);
    for i=1:37
        if(Weight(i)~=1)
            continue
        end
        x_j_k_0 =(xj(:,i)-xk)/norm(xj(:,i)-xk);
        rv = dot(x_j_k_0,vj(:,i));
        Y2(i) = Rou_Dot_B3(i) - rv + c*delta_Time_Dot_Sat_B3(i);
    end
    A2 = A;
    X2 = inv(A2'*A2)*(A2'*Y2);
    
    %均方差估计
    m = length(find(Weight==1));
    E = Y-A*dX;
    sigma_0 = sqrt(E'*E/(m-4));
    
    %输出
    position = xk;
    velocity = X2(1:3,1);
    flag = 0;
end