Student_5_BDS_Positioning.m

% 功能：由伪距、伪距变化率，进行单点定位、定速计算。
% 输入：-------------------------------------------------------------------
% Rou： 伪距观测值(m)，32*1维
% Rou_Dot： 伪距变化率观测值(m/s)，32*1维
% Weight： 权矩阵，32*32维，第 i行 i列元素为 1表示第 i个伪距观测方程有效，为 0表示无效。
%
% Pos_Sat_0： 卫星位置(m)，3*32维，WGS84坐标系
% Vel_Sat_0： 卫星速度(m/s)，3*32维，WGS84坐标系
%

function [ position velocity sigma_0 flag ]=Student_5_BDS_Positioning（RouRou_DotWeight  Pos_Sat_0Vel_Sat_0  Delta_Time_SatDelta_Time_Dot_Sat）;
% 功能：由伪距、伪距变化率，进行单点定位、定速计算。
% 输入：-------------------------------------------------------------------
% Rou：              伪距观测值（m），32*1维
% Rou_Dot：          伪距变化率观测值（m/s），32*1维
% Weight：           权矩阵，32*32维，第 i行 i列元素为 1表示第 i个伪距观测方程有效，为 0表示无效。
% 
% Pos_Sat_0：        卫星位置（m），3*32维，WGS84坐标系
% Vel_Sat_0：        卫星速度（m/s），3*32维，WGS84坐标系
% 
% Delta_Time_Sat：   卫星钟差（s），32*1维
% Delta_Time_Dot_Sat:卫星钟差变率（无量纲），32*1维
% 
% 输出：-------------------------------------------------------------------
% position：         位置（m），3*1维，WGS84坐标系
% velocity：         速度（m/s），3*1维，WGS84坐标系
% sigma_0            伪距测量均方差估计值（m）
% flag               标志，0表示计算成功，非 0表示失败。
%%
global Omega_CGCS2000              %地球自转角速度（rad/s）
global Velocity_Light              %真空中的光速（m/s）
n=length（Rou）;
Xk0=[0;0;0];
k=0;
while 1
    j=1;
    for i=1:n
        if Weight（ii）~=0
        tau_