Chapter.2 导航定位算法软件学习——Ginav（二）SPP算法
作者：齐花Guyc(CAUC)
标准单点定位SPP
进行标准单点定位SPP的解算，需要利用观测数据（o文件）、导航文件广播星历（n文件）。
流程如下：
- 检查可见卫星数量是否大于等于4。
- 利用导航文件广播星历计算卫星位置钟差、速度钟漂。
速度钟差是利用前后两个历元的卫星位置和钟差的差除时间间隔得到的。 - 解算接收机位置和钟差，这是SPP中最核心的部分。
- 筛选数据后，更新数据。
sppos.m
├── satposs.m 计算卫星位置
├── estpos.m 解算接收机位置钟差
├── rescode.m 计算残差、观测模型和权重矩阵
├── least_square.m 最小二乘法解算状态更新量
└── 验证收敛
├── raim_FDE.m 接收机自主完整性监测与故障检测排除
├── estvel.m 解算接收机的速度和钟漂
├── 检查位置和速度的一致性
├── 检查接收机钟差估计的合理性
└── 更新状态
下面主要分析SPP核心部分——PVT解算：
rescode 计算残差、观测模型和权重矩阵
rescode 函数根据观测数据、导航电文、卫星信息和当前状态估计，计算：
- 伪距残差向量
- 观测矩阵
- 权重矩阵
- 输出：卫星可用性标志、方位角/仰角、残差和观测计数等
- 计算几何距离和视线向量 geodist.m
几何距离
ρ i = ( x s − x r ) 2 + ( y s − y r ) 2 + ( z s − z r ) 2 \rho_i = \sqrt{(x_s - x_r)^2 + (y_s - y_r)^2 + (z_s - z_r)^2} ρi=(xs−xr)2+(ys−yr)2+(zs−zr)2
地球自转校正
ρ i = ρ i + ω e c ( x s y r − y s x r ) \rho_i = \rho_i + \frac{\omega_e}{c} (x_s y_r - y_s x_r) ρi=ρi+cωe(xsyr−ysxr) - 伪距修正 prange.m 包含DCB差分码修正伪距和TGD群延迟修正伪距
DCB指 GNSS 系统中由于接收机或卫星硬件引起的伪距观测偏差，特别是在不同频率或不同码类型之间。未校正 DCB 会引入系统性偏差，导致定位误差，尤其在多频或多系统 SPP 中。DCB 校正通过从伪距中减去估计的偏差值，使观测值更接近真实几何距离，从而提高定位精度。
DCB与TGD群延迟都是码偏差校正的参数，TGD是广播星历中提供的卫星端码偏差，TGD 精度较低，适用于基本校正；DCB是更广义的偏差，包括卫星和接收机的偏差，通常由外部数据（IGS产品）提供，适用于多频率和多系统，精度更高。
DCB文件中，VALUE表示卫星和接收器之间的码偏差值，正值表示接收机的码比卫星的码提前，负值表示接收机的码比卫星的码滞后；RMS是码偏差值的均方根误差，表示测量的精度。
DCB校正参考博文
以GPS系统为例，DCB分为频内偏差（相同频率L1、不同码P码和C码之间 如P1C1 P2C2）、频间偏差（不同频率L1和L2、相同码P码 如P1P2 C1C2）。
在此引出GPS和BDS的数据标识
GPS数据标识
C：伪距观测值，L：载波相位观测值，D：导航电文数据，S：信噪比
1：L1，2：L2
C：民码，P：军码
BDS数据标识
C：伪距观测值，L：载波相位观测值，D：多普勒观测值，S：信噪比
1：B1，2：B2，7：B3
getdcb函数
getdcb 函数从导航电文或外部数据中提取 DCB 值，为每个频率/码类型的伪距提供校正值，单位为米。
无电离层组合修正伪距
利用电离层一阶项与频率的平方成反比的性质，进而消除电离层一阶项。
γ = ( f i / f j ) 2 \gamma=(f_i/f_j)^2 γ=(fi/fj)2
电离层、对流层延迟校正
电离层模型：klobuchar model
对流层模型：saastamoninen model加权最小二乘计算估计接收机的位置和钟差。原理参考：【GNSS原理】【最小二乘法】Chapter.5 GNSS定位算法——LS和WLS方法
其中观测伪距直接来自 GNSS 接收机的原始测量数据，预测伪距 P ^ i \hat{P}_i P^i 是基于当前估计的接收机状态（位置和钟差）以及卫星位置计算的理论值。
基于状态向量 x 0 = [ x r , y r , z r , c ⋅ δ t r ] T \mathbf{x}_0 = [x_r, y_r, z_r, c \cdot \delta t_r]^T x0=[xr,yr,zr,c⋅δtr]T，通过观测模型： P ^ i = r i + c ⋅ δ t r − c ⋅ δ t s i + I i + T i \hat{P}_i = r_i + c \cdot \delta t_r - c \cdot \delta t_s^i + I_i + T_i P^i=ri+c⋅δtr−c⋅δtsi+Ii+Ti其中 r i r_i ri 为几何距离， I i I_i Ii 和 T i T_i Ti 为延迟修正。
伪距残差 v i = P i − P ^ i v_i = P_i - \hat{P}_i vi=Pi−P^i 反映观测与预测的差异，用于最小二乘优化。如果位置解算失败，且卫星数量 >= 6 且启用了 RAIM FDE，则进行 RAIM（Receiver Autonomous Integrity Monitoring,接收机自主完整性监测） FDE（Failure Detection and Exclusion,故障检测与排除）原理参考：【GNSS软件接收机】【理论简介】Chapter.3 RAIM 和 FDE
同样利用加权最小二乘估计接收机的速度和钟漂。原理参考：【GNSS原理】【最小二乘法】Chapter.5 GNSS定位算法——LS和WLS方法
其中观测多普勒来自GNSS 接收机的多普勒频移测量，预测多普勒是基于当前估计的接收机速度和钟漂，以及卫星速度计算的理论值。
基于状态向量 x = [ v x , v y , v z , c ⋅ d ( δ t r ) d t ] T \mathbf{x} = [v_x, v_y, v_z, c \cdot \frac{d(\delta t_r)}{dt}]^T x=[vx,vy,vz,c⋅dtd(δtr)]T，通过观测模型： D ^ i = − 1 λ ⋅ ( d r d t + c ⋅ d ( δ t r ) d t − c ⋅ d ( δ t s ) d t ) \hat{D}_i = -\frac{1}{\lambda} \cdot \left( \frac{dr}{dt} + c \cdot \frac{d(\delta t_r)}{dt} - c \cdot \frac{d(\delta t_s)}{dt} \right) D^i=−λ1⋅(dtdr+c⋅dtd(δtr)−c⋅dtd(δts))
其中 d r d t \frac{dr}{dt} dtdr 为几何距离变化率。
多普勒残差 v = − λ D − D ^ i v = -\lambda D -\hat{D}_i v=−λD−D^i 反映观测与预测的差异，用于最小二乘优化速度和钟漂。检查位置和速度的一致性
根据速度和时间差预测位置变化，计算实际位置变化，计算预测位置变化与实际变化的差值。如果差值过大，认为解算异常。检查接收机钟差估计的合理性
比较当前钟差与基于历史钟差和钟漂预测的钟差，如果预测钟差与实际钟差的偏差过大，认为解算异常。
附 SPP 解算主函数
function [rtk,stat0]=sppos(rtk,obs,navs)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%% 标准单点定位 (Standard Point Positioning, SPP) %%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 输入参数：
% rtk - RTK 控制结构体，包含定位选项、状态和解算结果
% obs - 观测数据结构体，包含接收机的伪距、载波相位等观测值
% navs - 导航电文数据，包含广播星历信息
%
% 输出参数：
% rtk - 更新后的 RTK 控制结构体，包含解算后的位置、速度、钟差等
% stat0 - 状态标志 (0: 错误; 1: 成功)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
global glc
opt=rtk.opt; % 获取 RTK 控制结构体中的定位选项
nobs=size(obs,1); % 计算观测数据条数（即观测到的卫星数量）
% 检查卫星数量是否足够，至少需要 4 颗卫星进行定位
if nobs<4, stat0=0; return; end % 卫星不足，设置状态为失败
% 初始化解算状态和时间
rtk.sol.stat=glc.SOLQ_NONE; % 初始解算状态为 无解
rtk.sol.time=obs(1).time; % 设置解算时间为观测数据的第一个历元时间
% defulat options for spp 设置默认 SPP 定位选项
if rtk.opt.mode~=glc.PMODE_SPP
% 如果当前模式不是 SPP，强制使用 SPP 默认选项
opt.sateph =glc.EPHOPT_BRDC; % 卫星星历选项：使用广播星历
opt.tropopt=glc.IONOOPT_BRDC; % 电离层校正选项：使用广播模型（Klobuchar 模型）
opt.ionoopt=glc.TROPOPT_SAAS; % 对流层校正选项：使用 Saastamoinen 模型
end
% cumpute satellite(space vehicle) position,clock bias,velocity,clock drift
% 计算卫星位置、钟差、速度和钟漂
% sv 为卫星信息结构体，包含每颗卫星的位置、速度、钟差等
sv=satposs(obs,navs,opt.sateph);
% compute reciever position,clock bias
% 估计接收机位置和钟差
% 调用 estpos 函数，计算接收机的位置 (rtk.sol.pos) 和钟差 (rtk.sol.dtr)
% 返回更新后的 rtk 结构体、卫星信息 (sat_) 和状态 (stat0)
[rtk,sat_,stat0]=estpos(rtk,obs,navs,sv,opt);
% raim failure detection and exclution(FDE)
% 如果位置解算失败，且卫星数量 >= 6 且启用了 RAIM FDE，则进行 RAIM FDE
% RAIM FDE：接收机自主完整性监测与故障检测排除
if stat0==0&&nobs>=6&&rtk.opt.posopt(5)==1
% 获取当前 GPS 周和秒
[wn,sow]=time2gpst(rtk.sol.time);
% 打印 RAIM FDE 信息
fprintf('Info:GPS week = %d sow = %.3f,RAIM failure detection and exclution\n',wn,sow);
% 调用 raim_FDE 函数，重新检测和剔除异常卫星，更新解算结果
[rtk,sat_,stat0]=raim_FDE(rtk,obs,navs,sv,opt,sat_);
end
% compute reciever velocity,clock drift
% 如果位置解算成功，进一步估计接收机的速度和钟漂
if stat0~=0
% 调用 estvel 函数，计算接收机速度 (rtk.sol.vel) 和钟漂 (rtk.sol.dtrd)
rtk=estvel(rtk,obs,navs,sv,opt,sat_);
end
% check the consistency of position and velocity
% 检查位置和速度的一致性
% 比较当前历元和上一历元的位置、速度，确保解算结果合理
tt=timediff(obs(1).time,rtk.rcv.time);% 计算当前历元与上一历元的时间差
if norm(rtk.rcv.oldpos)~=0&&norm(rtk.sol.pos)~=0&&...
norm(rtk.rcv.oldvel)~=0&&norm(rtk.sol.vel)~=0&&abs(tt)<=1
% 根据速度和时间差预测位置变化
dpos0=(rtk.sol.vel+rtk.rcv.oldvel)/2*abs(tt);
% 计算实际位置变化
dpos1=rtk.sol.pos-rtk.rcv.oldpos;
% 计算预测位置变化与实际变化的差值
diff=abs(dpos0-dpos1);
% 如果差值过大（最大分量 > 30 米 或 向量模 > 50 米），认为解算异常
if max(diff)>30||norm(diff)>50
stat0=0;
rtk.sol.stat=glc.SOLQ_NONE;
end
end
% check the correctness of the receiver clock bias estimation
% 检查接收机钟差估计的合理性
% 比较当前钟差与基于历史钟差和钟漂预测的钟差
tt=timediff(obs(1).time,rtk.rcv.time);
if abs(rtk.sol.dtrd)>5&&abs(tt)<=10 % 钟漂 > 5 m/s 且时间差 <= 10 秒
if rtk.rcv.clkbias~=0&&rtk.rcv.clkdrift~=0&&rtk.sol.dtr(1)~=0&&...
rtk.sol.dtrd~=0
% 根据历史钟差、钟漂和当前钟漂预测钟差
clkbias0=rtk.rcv.clkbias+(rtk.rcv.clkdrift+rtk.sol.dtrd)/2*abs(tt);
% 当前估计的钟差（单位：米）
clkbias1=rtk.sol.dtr(1)*glc.CLIGHT;
% 如果预测钟差与实际钟差的偏差过大，认为解算异常
if abs(clkbias0-clkbias1)>0.3*abs(rtk.rcv.clkdrift+rtk.sol.dtrd)/2
stat0=0;
rtk.sol.stat=glc.SOLQ_NONE;
end
end
end
% 如果解算状态有效，更新接收机状态
if rtk.sol.stat~=glc.SOLQ_NONE
rtk.rcv.time=obs(1).time;
rtk.rcv.oldpos=rtk.sol.pos;
rtk.rcv.oldvel=rtk.sol.vel;
rtk.rcv.clkbias=rtk.sol.dtr(1)*glc.CLIGHT;
rtk.rcv.clkdrift=rtk.sol.dtrd;
end
% 初始化所有卫星的状态
for i=1:glc.MAXSAT
rtk.sat(i).vs=0;
rtk.sat(i).azel=[0,0];
rtk.sat(i).snr(1)=0;
rtk.sat(i).resp(1)=0;
rtk.sat(i).resc(1)=0;
end
% 更新观测卫星的状态
for i=1:nobs
rtk.sat(obs(i).sat).azel=sat_.azel(i,:);
rtk.sat(obs(i).sat).snr(1)=obs(i).S(1);
if sat_.vsat(i)==0,continue;end
rtk.sat(obs(i).sat).vs=1;
rtk.sat(obs(i).sat).resp(1)=sat_.resp(i);
end
return