GINav 学习笔记
环境：win10, matlab 2020a
文章目录
- GINav 学习笔记
- **数据处理部分：**
- 数据文件类型：
- 标准单点定位（SPP）用到的文件:
- SPP/INS松组合用到的文件：
- SPP/INS紧组合用到的文件：
- **初始化程序：**
- GINavEXE.m
- exepos.m
- **GNSS部分：**
- gnss_processor.m
- exclude_sat.m （设置观测时间段、卫星类型，剔除不符合要求的卫星）
- gnss_solver.m
- sppos.m
- satposs.m（卫星位置计算）
- estpos.m（伪距单点定位位置计算）
- least_square.m（最小二乘法）
- estvel.m（接收机速度计算）
- **GNSS/INS部分：**
- gi_processor.m（GNSS/INS组合导航算法）
- gi_Loose.m（GNSS/INS 松组合模式）
- gnss_ins_lc.m（GNSS/INS 松组合计算）
- gi_Tight.m（GNSS/INS紧组合模式）
- sppins.m（SPP/INS紧组合解算）
- rescode_sppins.m
- resdop_sppins.m
- udsate_sppins.m（紧组合导航更新状态估计量）
- sppins_filter.m（紧组合卡尔曼滤波）
- udpos_sppins.m（INS的数据更新）
- **数据流：**
- `rtk` 结构体
- 代码改进
数据处理部分：
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-lSFt4I8R-1682761720723)(C:\Users\1\AppData\Roaming\Typora\typora-user-images\image-20230414143632980.png)]
数据文件类型：
o文件和n文件和p文件区别
RINEX (Receiver Independent Exchange Format) 文件是一种标准的 GNSS (Global Navigation Satellite System) 数据格式，用于存储 GPS、GLONASS、Galileo 等卫星导航系统接收机的观测数据和导航数据。
在 RINEX 文件中，“o” 文件和 “n” 文件都是观测文件，但它们包含的数据类型和格式略有不同。
“o” 文件包含接收机观测到的原始伪距和载波相位观测值，以及与观测相关的元数据，例如接收机和天线的标识、观测时间和卫星编号等。这些观测数据通常用于精密定位和导航应用。
“n” 文件包含卫星导航系统的星历和时钟偏差数据，用于计算接收机观测值的绝对位置。这些数据通常由卫星导航系统广播，并通过卫星导航系统的控制中心定期更新。
总之，“o” 文件包含接收机观测数据，而 “n” 文件包含卫星导航系统的星历和时钟偏差数据，两者都是精密定位和导航应用所必需的数据。
对于一般的 GNSS 应用，接收机只需要观测数据文件（通常是 “o” 和 “n” 文件）就可以实现基本的位置解算和导航功能。只有当需要进行更高级的数据处理和精度控制时，才需要使用 P 文件进行数据处理和校正。
“P” 文件包含了接收机在某个观测时段内的位置信息、钟差信息、接收机的天线相位中心以及其他一些额外的数据信息。这些数据通常用于精密定位和导航应用中的数据处理和校正。
DCB (Differential Code Bias) file
DCB (Differential Code Bias) 文件是一种 GNSS 数据文件，用于存储卫星导航系统中不同频段的码差信息。
GNSS 接收机测量卫星信号时，不同频段的信号传播速度可能会有微小的差异，这会导致不同频段的码距（pseudo-range）和码相（carrier-phase）之间存在固定的差异，称为差分码偏差（DCB）。差分码偏差对 GNSS 数据处理和精密定位应用有重要影响，需要进行精确的校正。
DCB 文件通常由 GNSS 数据处理中心生成，包含了不同卫星和频段之间的差分码偏差信息。DCB 文件的格式通常是文本格式，其中包含了卫星编号、频段编号以及相应的码差偏差值等信息。
在进行 GNSS 数据处理时，需要使用 DCB 文件进行数据校正，以消除不同频段之间的差分码偏差对测量结果的影响，提高精度和可靠性。
总之，DCB 文件是一种存储卫星导航系统中不同频段的码差信息的文件，用于进行 GNSS 数据处理中的数据校正，以提高精度和可靠性。
BSX(MGEX DCB) file
BSX 文件是一种包含多系统、多频段差分码偏差（DCB）信息的 GNSS 数据文件。它通常是由多个数据处理中心协作生成的，是针对 Multi-GNSS Experiment (MGEX) 项目而设计的。
MGEX 项目是一个国际性的 GNSS 实验计划，旨在推进多系统、多频段 GNSS 数据的开发和应用。BSX 文件是 MGEX 项目的一个重要组成部分，其中包含了全球范围内的多个卫星导航系统（如 GPS、GLONASS、Galileo、BeiDou 等）和多个频段之间的差分码偏差信息。
BSX 文件的格式是文本格式，其中包含了卫星编号、频段编号以及相应的差分码偏差值等信息。BSX 文件的命名方式通常为 “MGEXYYDDD.BSX”，其中 YY 表示年份，DDD 表示一年中的第几天。
在进行 GNSS 数据处理时，需要使用 BSX 文件进行数据校正，以消除不同卫星和频段之间的差分码偏差对测量结果的影响，提高精度和可靠性。
总之，BSX 文件是一种包含多系统、多频段差分码偏差（DCB）信息的 GNSS 数据文件，通常是由多个数据处理中心协作生成，用于进行 GNSS 数据处理中的数据校正，以提高精度和可靠性。
EOP file
EOP (Earth Orientation Parameters) 文件是一种存储地球自转参数的文件，包含了地球自转角速度、极移以及其他相关的地球自转参数信息。这些参数信息对于 GNSS 数据处理和精密定位应用有重要影响，需要进行精确的校正。
地球自转参数是描述地球自转状态的物理量，包括了地球自转角速度、极移、地球自转角度等。这些参数的变化会影响 GNSS 接收机接收卫星信号的频率和时间，从而影响到定位和导航精度。
EOP 文件通常由国际地球自转参考系统 (IERS) 提供，并按照一定的格式存储。EOP 文件中包含了地球自转参数的历史数据，通常以天为单位，每天的数据包含了一组地球自转参数信息。
标准单点定位（SPP）用到的文件:
configuration file
E:\graduation_design\program\GINav-main\conf\SPP\GINav_SPP_CPT.ini
rover observation file
E:\graduation_design\program\GINav-main\data\data_cpt\cpt0870.19o
broadcast ephemeris file
E:\graduation_design\program\GINav-main\data\data_cpt\brdm0870.19p
DCB file
E:\graduation_design\program\GINav-main\data\data_cpt*.DCB
BSX(MGEX DCB) file
E:\graduation_design\program\GINav-main\data\data_cpt\CAS0MGXRAP_20190870000_01D_01D_DCB.BSX
EOP file
E:\graduation_design\program\GINav-main\data\data_cpt\COD20464.ERP
SPP/INS松组合用到的文件：
configuration file
E:\graduation_design\program\GINav-main\conf\LC\GINav_SPP_LC_CPT.ini
rover observation file
E:\graduation_design\program\GINav-main\data\data_cpt\cpt0870.19o
broadcast ephemeris file
E:\graduation_design\program\GINav-main\data\data_cpt\brdm0870.19p
DCB file
E:\graduation_design\program\GINav-main\data\data_cpt*.DCB
BSX(MGEX DCB) file
E:\graduation_design\program\GINav-main\data\data_cpt\CAS0MGXRAP_20190870000_01D_01D_DCB.BSX
EOP file
E:\graduation_design\program\GINav-main\data\data_cpt\COD20464.ERP
IMU file
E:\graduation_design\program\GINav-main\data\data_cpt\cpt_imu.csv
SPP/INS紧组合用到的文件：
configuration file
E:\graduation_design\program\GINav-main\conf\LC\GINav_SPP_LC_CPT.ini
rover observation file
E:\graduation_design\program\GINav-main\data\data_cpt\cpt0870.19o
broadcast ephemeris file
E:\graduation_design\program\GINav-main\data\data_cpt\brdm0870.19p
DCB file
E:\graduation_design\program\GINav-main\data\data_cpt*.DCB
BSX(MGEX DCB) file
E:\graduation_design\program\GINav-main\data\data_cpt\CAS0MGXRAP_20190870000_01D_01D_DCB.BSX
EOP file
E:\graduation_design\program\GINav-main\data\data_cpt\COD20464.ERP
IMU file
E:\graduation_design\program\GINav-main\data\data_cpt\cpt_imu.csv
初始化程序：
GINavEXE.m
···
% execute positioning
exepos(opt,file);
···
总的执行文件执行exepos(opt, file)函数。
exepos.m
···
% process all data
if opt.ins.mode==glc.GIMODE_OFF
% gnss
gnss_processor(rtk,opt,obsr,obsb,nav);
elseif opt.ins.mode==glc.GIMODE_LC||opt.ins.mode==glc.GIMODE_TC
% gnss/ins integration
gi_processor(rtk,opt,obsr,obsb,nav,imu);
end
toc
return
···
exepos.m 提供了两个选择：exepos.m:3选择GNSS模式，调用gnss_processor函数；exepos.m:6选择组合导航模式，调用gi_processor函数。
GNSS部分：
gnss_processor.m
function gnss_processor(rtk,opt,obsr,obsb,nav)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% start gnss processor to generate navigation solutions
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Copyright(c) 2020-2025, by Kai Chen, All rights reserved.
%8/12/2020
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
global glc
ti=0;
hbar=waitbar(0,'Processing...','Name','GINav', 'CreateCancelBtn', 'delete(gcbf);');
H=get(0,'ScreenSize'); w=600; h=450; x=H(3)/2-w/2; y=H(4)/2-h/2;
hfig=figure;set(gcf,'Position',[x y w h]);
% initialize rtk sturct
rtk=initrtk(rtk,opt);
% set time span
tspan=timespan(rtk,obsr);
if tspan<=0,error('Time span is zero!!!');end
while 1
if ti>tspan,break;end
% search rover obs
[obsr_,nobs,obsr]=searchobsr(obsr);
if nobs<0
str=sprintf('Processing... %.1f%%',100);
waitbar(ti/tspan,hbar,str);
break;
end
% exclude rover obs
[obsr_,nobs]=exclude_sat(obsr_,rtk);
if nobs==0,continue;end
if opt.mode>=glc.PMODE_DGNSS&&opt.mode<=glc.PMODE_STATIC
% search base obs
[obsb_,nobs]=searchobsb(obsb,obsr_(1).time);
% exclude base obs
if nobs~=0,[obsb_,~]=exclude_sat(obsb_,rtk);end
else
obsb_=NaN;
end
% solve an epoch
[rtk,~]=gnss_solver(rtk,obsr_,obsb_,nav);
if rtk.sol.stat~=0
% write solution to output file
outsol(rtk);
% kinematic plot
plot_trajectory_kine(hfig,rtk);
else
[week,sow]=time2gpst(obsr_(1).time);
fprintf('Warning:GPS week = %d sow = %.3f,GNSS unavailable!\n',week,sow);
end
% update progress bar
ti=ti+1;
str=sprintf('Processing... %.1f%%',100*ti/tspan);
waitbar(ti/tspan,hbar,str);
end
close(hbar);
return
这是一段用于生成导航解的GNSS处理代码。下面逐行解释：
第2-6行：版权声明和版本信息。
第8行：定义全局变量glc。
第9行：初始化变量ti为0。
第11-15行：创建等待条并定义窗口大小。
第17行：调用初始化rtk的函数。
第20-22行：计算时间跨度，并检查其是否小于等于0。
第24-45行：循环处理观测数据。如果处理的时间已经大于观测数据的时间跨度，则跳出循环。
第27-31行：搜索正在处理的卫星观测数据，并返回可用卫星数目。如果卫星数目小于0，更新等待条并跳出循环。
第33-35行：排除rtk结构中的卫星数据。
第37-43行：如果rtk的处理模式处于差分GNSS到静态定位之间，搜索基站的卫星观测数据并排除。否则，设置基站观测数据为NaN。
第45行：求解一个时刻的导航解。
第47-54行：如果求解成功，则将导航解输出到输出文件中，并在图形界面上绘制轨迹。否则，打印警告信息。
第56-60行：更新进度条和提示信息。
第62行：关闭等待条。
返回。
gnss_processor.m:46 中的对于单个epoch开始进行解算，调用gnss_solver(rtk,obsr_,obsb_,nav)函数。
exclude_sat.m （设置观测时间段、卫星类型，剔除不符合要求的卫星）
function gnss_processor(rtk,opt,obsr,obsb,nav)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% start gnss processor to generate navigation solutions
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Copyright(c) 2020-2025, by Kai Chen, All rights reserved.
%8/12/2020
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
global glc
ti=0;
hbar=waitbar(0,'Processing...','Name','GINav', 'CreateCancelBtn', 'delete(gcbf);');
H=get(0,'ScreenSize'); w=600; h=450; x=H(3)/2-w/2; y=H(4)/2-h/2;
hfig=figure;set(gcf,'Position',[x y w h]);
% initialize rtk sturct
rtk=initrtk(rtk,opt);
% set time span
tspan=timespan(rtk,obsr);
if tspan<=0,error('Time span is zero!!!');end
while 1
if ti>tspan,break;end
% search rover obs
[obsr_,nobs,obsr]=searchobsr(obsr);
if nobs<0
str=sprintf('Processing... %.1f%%',100);
waitbar(ti/tspan,hbar,str);
break;
end
% exclude rover obs
[obsr_,nobs]=exclude_sat(obsr_,rtk);
if nobs==0,continue;end
if opt.mode>=glc.PMODE_DGNSS&&opt.mode<=glc.PMODE_STATIC
% search base obs
[obsb_,nobs]=searchobsb(obsb,obsr_(1).time);
% exclude base obs
if nobs~=0,[obsb_,~]=exclude_sat(obsb_,rtk);end
else
obsb_=NaN;
end
% solve an epoch
[rtk,~]=gnss_solver(rtk,obsr_,obsb_,nav);
if rtk.sol.stat~=0
% write solution to output file
outsol(rtk);
% kinematic plot
plot_trajectory_kine(hfig,rtk);
else
[week,sow]=time2gpst(obsr_(1).time);
fprintf('Warning:GPS week = %d sow = %.3f,GNSS unavailable!\n',week,sow);
end
% update progress bar
ti=ti+1;
str=sprintf('Processing... %.1f%%',100*ti/tspan);
waitbar(ti/tspan,hbar,str);
end
close(hbar);
return
其中，
nobs0=size(obs0,1); nobs=0; obs=repmat(gls.obs_tmp,nobs0,1);这行代码先获取输入参数
obs0的行数，即观测值的数目，并初始化nobs和obs，nobs表示筛选后的观测值数目，obs是存储筛选后的观测值的结构体数组，其大小和类型与obs0相同。
ts=rtk.opt.ts; te=rtk.opt.te;这行代码获取接收机设置的时间段，
ts表示起始时间，te表示结束时间。
mask=set_sysmask(rtk.opt.navsys);这行代码获取接收机设置的卫星系统掩码，即确定可用的卫星系统类型。
for i=1:nobs0
time=obs0(i).time;
if ts.time~=0
dt=timediff(time,ts);
if dt<0,continue;end
end
if te.time~=0
dt=timediff(time,te);
if dt>0,continue;end
end
%if obs0(i).sat==4;continue;end
[sys,~]=satsys(obs0(i).sat);
if mask(sys)==0,continue;end
obs(nobs+1)=obs0(i);
nobs=nobs+1;
end
这是一个
for循环，用于遍历所有的观测值，根据时间段和卫星系统掩码筛选符合条件的观测值，并将它们存储到obs中。具体来说：
- 对于每个观测值，首先获取它的时间戳
time。- 如果接收机设置了起始时间
ts，那么计算观测值时间和起始时间之间的时间差dt，如果dt小于零，则说明观测值在起始时间之前，应该被忽略。- 如果接收机设置了结束时间
te，那么计算观测值时间和结束时间之间的时间差dt，如果dt大于零，则说明观测值在结束时间之后，应该被忽略。- 如果卫星系统掩码中没有包含该观测值所属的卫星系统，则说明该卫星系统不可用，应该忽略
gnss_solver.m
function [rtk,stat0]=gnss_solver(rtk,obsr,obsb,nav)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve an epoch for gnss
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Copyright(c) 2020-2025, by Kai Chen, All rights reserved.
%8/12/2020
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
global glc
time=rtk.sol.time; opt=rtk.opt;
rtk.sol.stat=glc.SOLQ_NONE;
if opt.mode==glc.PMODE_SPP||opt.mode>=glc.PMODE_PPP_KINEMA
% scan obs for spp
[obsr,nobs]=scan_obs_spp(obsr);
if nobs==0,stat0=0;return;end
% correct BDS2 multipath error
if ~isempty(strfind(opt.navsys,'C'))
obsr=bds2mp_corr(rtk,obsr);
end
end
% standard point positioning
[rtk,stat0]=sppos(rtk,obsr,nav);
if stat0==0,return;end
if time.time~=0,rtk.tt=timediff(rtk.sol.time,time);end
if opt.mode==glc.PMODE_SPP,return;end
% suppress output of single solution
rtk.sol.stat=glc.SOLQ_NONE;
if opt.mode>=glc.PMODE_PPP_KINEMA
% scan obs for ppp
[obsr,nobs]=scan_obs_ppp(obsr);
if nobs==0,stat0=0;return;end
% repair receiver clock jump (only for GPS)
[rtk,obsr]=clkrepair(rtk,obsr,nav);
% precise point psitioning
[rtk,stat0]=pppos(rtk,obsr,nav);
return;
end
if ~isstruct(obsb),stat0=0;return;end
rtk.sol.age=timediff(obsr(1).time,obsb(1).time);
% relative psotioning
[rtk,stat0]=relpos(rtk,obsr,obsb,nav);
return
该代码是用于GNSS解算中的一个时刻，对观测数据进行处理并解算出该时刻的定位结果。
输入变量：
- rtk：rtk结构体，存储了当前GNSS解算的各项参数和变量。
- obsr：观测数据，包括接收机接收到的卫星信号以及对应的时间和频率等信息。
- obsb：基准站观测数据（仅在差分定位时有用），与obsr结构相同。
- nav：导航电文数据，包括卫星轨道、钟差等信息。
输出变量：
- rtk：更新后的rtk结构体，包括定位结果、历元状态等。
- stat0：当前定位结果的状态，0表示未解算成功，1表示解算成功。
函数的具体步骤如下：
- 获取当前GNSS解算的模式，如果是单点定位或者动态单点定位，则直接调用sppos()函数进行标准点定位，并返回状态。
- 如果是动态精密定位，则调用pppos()函数进行精密点定位，并返回状态。
- 如果是差分定位，则调用relpos()函数进行相对定位，并返回状态。
- 在进行标准点定位或者相对定位之前，先扫描观测数据并进行处理，去除低质量数据或者进行修正，例如在BDS2的情况下，使用bds2mp_corr()函数对多路径误差进行修正。
- 在进行精密点定位之前，还需进行接收机钟差跳变的修正，使用clkrepair()函数。
- 最后，计算定位结果，并更新rtk结构体中的相关参数。如果定位成功，则更新rtk.sol.stat为解算成功状态，否则为无解状态。
需要注意的是，该函数只是解算一个历元的数据，如果需要连续解算多个历元，需要在循环中多次调用该函数。
sppos.m
function [rtk,stat0]=sppos(rtk,obs,navs)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%% standard point positioning %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%input: rtk - rtk control struct
% obs - observations
% nav - navigation message
%output: rtk - rtk control struct
% stat0 - state (0:error 1:ok)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
global glc
opt=rtk.opt; nobs=size(obs,1);
if nobs<4, stat0=0; return; end
rtk.sol.stat=glc.SOLQ_NONE;
rtk.sol.time=obs(1).time;
% defulat options for spp
if rtk.opt.mode~=glc.PMODE_SPP
opt.sateph =glc.EPHOPT_BRDC;
opt.tropopt=glc.IONOOPT_BRDC;
opt.ionoopt=glc.TROPOPT_SAAS;
end
% cumpute satellite(space vehicle) position,clock bias,velocity,clock drift
sv=satposs(obs,navs,opt.sateph);
% compute reciever position,clock bias
[rtk,sat_,stat0]=estpos(rtk,obs,navs,sv,opt);
% raim failure detection and exclution(FDE)
if stat0==0&&nobs>=6&&rtk.opt.posopt(5)==1
[wn,sow]=time2gpst(rtk.sol.time);
fprintf('Info:GPS week = %d sow = %.3f,RAIM failure detection and exclution\n',wn,sow);
[rtk,sat_,stat0]=raim_FDE(rtk,obs,navs,sv,opt,sat_);
end
% compute reciever velocity,clock drift
if stat0~=0
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);
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
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
这段代码实现了标准点位解算（sppos），输入是rtk控制结构体、观测数据obs和导航数据navs，输出是更新后的rtk结构体和状态stat0（0表示错误，1表示成功）。
首先，代码检查观测数据是否充足（大于等于4个），否则直接返回错误状态。
然后根据rtk.opt.mode的值判断定位模式，如果不是单点定位（glc.PMODE_SPP），则使用默认选项（opt.sateph =glc.EPHOPT_BRDC; opt.tropopt=glc.IONOOPT_BRDC; opt.ionoopt=glc.TROPOPT_SAAS;）。
接下来计算卫星的位置、钟差、速度和钟漂（satposs函数）。
然后根据观测数据和计算出的卫星信息，估算接收机位置和钟差（estpos函数）。
如果状态stat0为0，且观测数据数量大于等于6且rtk.opt.posopt(5)==1（即开启了RAIM FDE），则进行RAIM故障检测和排除（raim_FDE函数）。
接着计算接收机的速度和钟漂（estvel函数）。
然后检查位置和速度的一致性。如果上一时刻的接收机位置、本次解算的接收机位置、上一时刻的接收机速度和本次解算的接收机速度都不为0且时间差小于等于1秒，则计算位置变化量，并检查变化量是否超出阈值。如果超出，则认为解算失败。
接着检查接收机钟差估计的正确性。如果接收机钟差估计值的绝对值超过5秒，并且时间差小于等于10秒，则计算当前接收机钟差（包括漂）估计值与上一时刻的估计值的变化量，并检查变化量是否超出阈值。如果超出，则认为解算失败。
如果当前解算状态为成功（rtk.sol.stat~=glc.SOLQ_NONE），则更新rtk结构体中的时间、接收机位置、接收机速度、接收机钟差和接收机钟漂等信息。
最后，将每颗卫星的可见性和观测数据存储在rtk结构体中的sat数组中，用于后续计算。
satposs.m（卫星位置计算）
function sv=satposs(obs,nav,ephopt)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%compute satellite position,clock bias,velocity,clock drift
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%input：obs - observation
% nav - navigation message
% ephopt- ephemeric option(0:using broadcast eph;1:using precise eph)
%output：sv - space vehicle struct(record satellite position,clock bias,
% velocity and clock drift)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%1.satellite position and clock are values at signal transmission time
%2.satellite position is referenced to antenna phase center
%3.satellite clock does not include code bias correction (tgd or bgd)
%4.any pseudorange and broadcast ephemeris are always needed to get signal
% transmission time
%5.only surport broadcast/precise ephemeris,not RTCM-SSR
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
global glc gls
STD_BRDCCLK =30.0; time0=obs(1).time;
nobs=size(obs,1); % number of observation
sv=repmat(gls.sv,nobs,1);
for i=1:nobs
for j=1:glc.NFREQ
pr=obs(i).P(j);
if pr~=0,break;end
end
if pr==0,continue;end
time=timeadd(time0,-pr/glc.CLIGHT); %raw single transition time
[dts,stat1]=ephclk(time,obs(i),nav);
if stat1==0,continue;end
time=timeadd(time,-dts); %signal transition time
[sv(i),stat2]=satpos(time,obs(i),nav,ephopt,sv(i));
if stat2==0,continue;end
if sv(i).dts==0
[dts,stat1]=ephclk(time,obs(i),nav);
if stat1==0,continue;end
sv(i).dtsd=dts; sv(i).dtsd=0;
sv(i).vars=STD_BRDCCLK^2;
end
end
return
estpos.m（伪距单点定位位置计算）
function [rtk,sat_,stat]=estpos(rtk,obs,nav,sv,opt)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%estimate reciever position and clock bias
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
global glc
NX=3+glc.NSYS; MAXITER=10; iter=1;
time=obs(1).time; xr0=[rtk.sol.pos';zeros(glc.NSYS,1)];
while iter<=MAXITER
% compute residual,measurement model,weight matrix
[v,H,P,vsat,azel,resp,nv,ns]=rescode(iter,obs,nav,sv,xr0,opt);
sat_.vsat=vsat;
sat_.azel=azel;
sat_.resp=resp;
if nv<NX, stat=0;return;end
% least square algrithm
[dx,Q]=least_square(v,H,P);
xr0=xr0+dx;
iter=iter+1;
if dot(dx,dx)<1e-4
rtk.sol.time=timeadd(obs(1).time,-xr0(4)/glc.CLIGHT);
rtk.sol.ns =ns;
rtk.sol.ratio=0;
rtk.sol.pos =xr0(1:3)';
rtk.sol.vel =zeros(1,3);
rtk.sol.posP(1)=Q(1,1);rtk.sol.posP(2)=Q(2,2);rtk.sol.posP(3)=Q(3,3);
rtk.sol.posP(4)=Q(1,2);rtk.sol.posP(5)=Q(2,3);rtk.sol.posP(6)=Q(1,3);
rtk.sol.dtr(1) =xr0(4)/glc.CLIGHT;
rtk.sol.dtr(2) =xr0(5)/glc.CLIGHT;
rtk.sol.dtr(3) =xr0(6)/glc.CLIGHT;
rtk.sol.dtr(4) =xr0(7)/glc.CLIGHT;
rtk.sol.dtr(5) =xr0(8)/glc.CLIGHT;
% validate solution
stat=valsol(time,v,P,vsat,azel,opt);
if stat==1
rtk.sol.stat=glc.SOLQ_SPP; return;
end
return;
end
end
if iter>MAXITER
stat=0;
[week,sow]=time2gpst(time);
fprintf('Warning:GPS week = %d sow = %.3f,SPP iteration divergent!\n',week,sow);
return;
end
return
这段代码实现了单点定位的主要功能，包括观测数据的残差计算、测量模型计算、加权矩阵计算、最小二乘解法等。
首先，在函数参数中，输入了rtk结构体、观测数据obs、导航电文数据nav、卫星信息sv以及定位选项opt。其中，rtk结构体包含了定位结果，obs包含了一帧观测数据，nav包含了导航电文数据，sv包含了卫星位置、速度等信息，opt包含了定位选项。
接着，初始化了一些常量和变量。常量包括状态向量的维度、最大迭代次数，而变量包括当前迭代次数、接收机位置和钟差等。
然后，在循环中，计算出测量残差v、测量模型H、加权矩阵P。这里的计算涉及到了误差方程，即将接收机位置和钟差等未知量表示成观测量的线性组合，从而构造误差方程。通过求解误差方程，可以得到测量残差、测量模型以及加权矩阵。
接下来，利用最小二乘法求解状态向量的增量dx，即使得残差平方和最小的参数改变量。通过将dx加到状态向量xr0中，更新接收机位置和钟差等未知量。然后再进行下一轮迭代，直到满足停止条件，即增量dx足够小。
最后，通过验证解的合理性，判断定位结果的有效性，并返回rtk结构体。如果迭代次数超过最大迭代次数，则返回失败信息。
least_square.m（最小二乘法）
function [x,VAR]=least_square(v,H,P)
%normal vector
N = (H'*P*H);
%cumpute unknown parameter
x = (N^-1)*H'*P*v;
%convariance of parameter
VAR = N^-1;
return
这段代码实现了最小二乘法的计算过程，用于求解位置和钟差的最优解。
最小二乘法是一种优化方法，它的目标是通过最小化测量残差的平方和来估计参数的值。在本程序中，最小二乘法被用于计算位置和钟差的最优解。
输入参数：
- v：观测值的残差向量
- H：设计矩阵
- P：加权矩阵
输出参数：
- x：未知参数向量（即位置和钟差）
- VAR：参数向量的协方差矩阵
代码解释：
首先，我们需要计算设计矩阵H和加权矩阵P的乘积H’PH，这个矩阵在最小二乘法中称为法方程矩阵。然后，我们需要计算观测残差向量v与法方程矩阵的乘积，即H’Pv，这个向量在最小二乘法中称为正规方程向量。接着，我们通过解线性方程组 (H’PH)*x = H’Pv 来计算未知参数向量x（即位置和钟差）。最后，我们计算参数向量的协方差矩阵VAR，它等于法方程矩阵的逆。
estvel.m（接收机速度计算）
function rtk=estvel(rtk,obs,nav,sv,opt,sat_) %#ok
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%estimate reciever velocity and clock drift
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
MAXITER=10; iter=1; x=zeros(4,1); rtk.sol.dtrd=0;
while iter<=MAXITER
% residual,measurement model,weight matrix
[v,H,P,nv]=resdop(rtk,obs,nav,sv,sat_,x);
if nv<4,break;end
% least square
[dx,~]=least_square(v,H,P);
x=x+dx;
if dot(dx,dx)<10^-4
rtk.sol.vel=x(1:3)';
rtk.sol.dtrd=x(4);
break;
end
iter=iter+1;
end
return
这段代码是一个用于估计接收机速度和钟漂的函数。函数输入包括：rtk（接收机状态和历元解）、obs（观测值）、nav（导航电文数据）、sv（卫星状态）、opt（设置选项）和sat_（指定卫星）。函数输出包括更新的rtk解，包括接收机速度和钟漂。
该函数使用迭代方法求解。每个迭代步骤包括以下步骤：
1.计算残差向量v，以及相应的Jacobi矩阵H和权重矩阵P。
2.使用最小二乘法计算未知参数的增量dx。
3.使用增量更新参数x。
4.如果dx的范数小于一个给定的阈值，则退出迭代，并将更新的解赋值给rtk。
如果在迭代过程中发生任何错误，或者残差向量v的数量小于4，则函数将提前退出迭代。
在每个迭代步骤中，函数都会调用一个名为resdop的子函数来计算残差向量v，Jacobi矩阵H和权重矩阵P。resdop函数的输入和输出与estvel函数相同，除了它还接受参数x，该参数是估计的接收机速度和钟漂。resdop函数的作用是计算残差向量v和Jacobi矩阵H，用于最小二乘法的求解。
GNSS/INS部分：
gi_processor.m（GNSS/INS组合导航算法）
function gi_processor(rtk,opt,obsr,obsb,nav,imu)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% start gnss/ins processor to generate navigation solutions
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Copyright(c) 2020-2025, by Kai Chen, All rights reserved.
%8/12/2020
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
global glc gls
ins_align_flag=0; ins_realign_flag=0; ti=0;
rtk_align_falg=0; MAX_GNSS_OUTAGE=30;
oldobstime=gls.gtime;
hbar=waitbar(0,'Preparation...','Name','GINav', 'CreateCancelBtn', 'delete(gcbf);');
H=get(0,'ScreenSize'); w=600; h=450; x=H(3)/2-w/2; y=H(4)/2-h/2;
hfig=figure;set(gcf,'Position',[x y w h]);
% initialize rtk_align sturct
rtk_align=initrtk(rtk,opt);
% set time span
tspan=timespan(rtk_align,obsr);
if tspan<=0,error('Time span is zero!!!');end
while 1
if ti+1>tspan,break;end
% search imu data
[imud,imu,stat]=searchimu(imu);
if stat==0
str=sprintf('Processing... %.1f%%',100*tspan/tspan);
waitbar(tspan/tspan,hbar,str);
break;
end
% match rover obs
[obsr_,nobsr]=matchobs(rtk_align,imud,obsr);
% match base obs
if (opt.mode==glc.PMODE_DGNSS||opt.mode==glc.PMODE_KINEMA)&&nobsr~=0
[obsb_,nobsb]=searchobsb(obsb,obsr_(1).time);
if nobsb~=0,[obsb_,~]=exclude_sat(obsb_,rtk_align);end
else
obsb_=NaN;
end
if nobsr~=0
ti=ti+1;
str=sprintf('Processing... %.1f%%',100*ti/tspan);
waitbar(ti/tspan,hbar,str);
% aviod duplicate observations
if (oldobstime.time~=0&&timediff(oldobstime,obsr_(1).time)==0)...
||obsr_(1).time.sec~=0
if ins_align_flag~=0
ins=ins_mech(ins,imud);
ins=ins_time_updata(ins);
end
oldobstime=obsr_(1).time;
continue;
end
oldobstime=obsr_(1).time;
if ins_align_flag==0
% INS initial alignment
[rtk_align,ins_align_flag]=ins_align(rtk_align,obsr_,obsb_,nav);
if ins_align_flag==1
ins=rtk_align.ins;
rtk_gi=gi_initrtk(rtk,opt,rtk_align);
if opt.ins.mode==glc.GIMODE_LC
rtk_gnss=rtk_align;
end
% write solution to output file
ins.time=rtk_gi.sol.time;
rtk_gi=ins2sol(rtk_gi,ins);
outsol(rtk_gi);
% kinematic plot
plot_trajectory_kine(hfig,rtk_gi);
fprintf('Info:INS initial alignment ok\n');
% record previous information
ins.oldpos=ins.pos;
ins.oldobsr=obsr_;
ins.oldobsb=obsb_;
else
% kinematic plot
plot_trajectory_kine(hfig,rtk_align);
end
else
% INS re-alignment
gi_time=rtk_gi.gi_time;
if gi_time.time~=0&&abs(timediff(ins.time,gi_time))>MAX_GNSS_OUTAGE
if rtk_align_falg==0
rtk_align=initrtk(rtk,opt);
rtk_align_falg=1;
end
[rtk_align,ins_realign_flag]=ins_align(rtk_align,obsr_,obsb_,nav);
if ins_realign_flag==1
% bg and ba are not reset
bg=ins.bg; ba=ins.ba;
ins=rtk_align.ins;
ins.bg=bg; ins.ba=ba;
rtk_gi=gi_initrtk(rtk,opt,rtk_align);
if opt.ins.mode==glc.GIMODE_LC
rtk_gnss=rtk_align;
end
rtk_align_falg=0;
% write solution to output file
ins.time=rtk_gi.sol.time;
rtk_gi=ins2sol(rtk_gi,ins);
outsol(rtk_gi);
% kinematic plot
plot_trajectory_kine(hfig,rtk_gi);
fprintf('Info:INS re-alignment ok\n');
% record previous information
ins.oldpos=ins.pos;
ins.oldobsr=obsr_;
ins.oldobsb=obsb_;
continue;
end
% use INS solutions before re-alignment
fprintf('Warning:GPS week = %d sow = %.3f,GNSS outage!\n',week,sow);
ins=ins_mech(ins,imud);
ins=ins_time_updata(ins);
rtk_gi.ngnsslock=0;
% write solution to output file
rtk_gi=ins2sol(rtk_gi,ins);
outsol(rtk_gi);
% kinematic plot
plot_trajectory_kine(hfig,rtk_gi);
% record previous information
ins.oldpos=ins.pos;
ins.oldobsr=obsr_;
ins.oldobsb=obsb_;
continue;
end
% INS mechanization and time update
ins=ins_mech(ins,imud);
ins=ins_time_updata(ins);
rtk_gi=ins2sol(rtk_gi,ins);
% GNSS measurement update
rtk_gi.ins=ins;
if opt.ins.mode==glc.GIMODE_LC
% GNSS/INS loosely couple
[rtk_gi,rtk_gnss,stat_tmp]=gi_Loose(rtk_gi,rtk_gnss,obsr_,obsb_,nav);
elseif opt.ins.mode==glc.GIMODE_TC
% GNSS/INS tightly couple
[rtk_gi,stat_tmp]=gi_Tight(rtk_gi,obsr_,obsb_,nav);
end
ins=rtk_gi.ins;
if stat_tmp==0
[week,sow]=time2gpst(obsr_(1).time);
fprintf('Warning:GPS week = %d sow = %.3f,GNSS unavailable!\n',week,sow);
end
% write solution to output file
outsol(rtk_gi);
% kinematic plot
plot_trajectory_kine(hfig,rtk_gi);
% record previous information
ins.oldpos=ins.pos;
ins.oldobsr=obsr_;
ins.oldobsb=obsb_;
end
else
if ins_align_flag==0,continue;end
if rtk_align_falg==1&&ins_realign_flag==0,continue;end
% INS mechanization and time update
ins=ins_mech(ins,imud);
ins=ins_time_updata(ins);
% If GNSS is not available, use the INS solutions
time1=ins.time.time+ins.time.sec;
time2=round(ins.time.time+ins.time.sec);
if nobsr<=0&&abs(time1-time2)<(0.501/rtk_gi.opt.ins.sample_rate)
ti=ti+1;
str=sprintf('Processing... %.1f%%',100*ti/tspan);
waitbar(ti/tspan,hbar,str);
fprintf('Warning:GPS week = %d sow = %.3f,GNSS outage!\n',week,sow);
rtk_gi.ngnsslock=0;
rtk_gi=ins2sol(rtk_gi,ins);
% write solution to output file
outsol(rtk_gi);
% kinematic plot
plot_trajectory_kine(hfig,rtk_gi);
end
end
end
close(hbar);
return
变量：
- rtk: 结构体，包含了 RTK 定位所需的各种参数和数据。
- opt: 结构体，包含了程序运行时的各种选项和参数。
- obsr: 结构体数组，存储接收机接收到的卫星观测量，包括码伪距和载波相位等。
- obsb: 结构体数组，存储基站接收到的卫星观测量。
- nav: 结构体数组，存储星历和钟差等导航电文数据。
- imu: 结构体数组，存储 IMU 数据。
函数：
- initrtk: 初始化 RTK 定位参数，返回包含了 RTK 定位所需的各种参数和数据的结构体。
- timespan: 计算观测数据的时间跨度。
- searchimu: 搜索与指定时刻最接近的 IMU 数据。
- matchobs: 匹配卫星观测量和 IMU 数据。
- searchobsb: 搜索与指定时刻最接近的基站卫星观测量。
- exclude_sat: 排除指定卫星。
- ins_align: INS 初始对准和重新对准。
- gi_initrtk: 初始化 GiNav 定位参数，返回包含了 GiNav 定位所需的各种参数和数据的结构体。
- ins2sol: 将 INS 数据转化为定位解数据。
- outsol: 将定位解数据输出到文件。
- plot_trajectory_kine: 绘制运动轨迹图。
该函数实现了GNSS/INS组合导航算法，包含以下步骤：
- 初始化RTK（Real Time Kinematic）参数结构体rtk_align，并计算时间跨度tspan；
- 进入主循环，从IMU（惯性测量单元）数据中搜索与rtk_align时间匹配的IMU数据；
- 对搜到的IMU数据进行INS（Inertial Navigation System）解算；
- 搜索与rtk_align时间匹配的GPS观测数据，如果当前没有可用的GPS观测数据，则终止程序；
- 如果没有进行初始化对齐，执行步骤6；否则，进行步骤7；
- 对初始位置进行INS对齐，计算出初始的INS状态，并构建GI-RTK（GNSS/INS RTK）参数结构体rtk_gi，将INS解算结果写入输出文件，绘制轨迹图；
- 对当前时间的位置进行INS重对齐或GI-GNSS滤波，将INS解算结果写入输出文件，绘制轨迹图；
此外，该函数还定义了全局变量和一些常数，包括ins_align_flag、ins_realign_flag、ti、rtk_align_falg、MAX_GNSS_OUTAGE等。同时，该函数调用了一些其他自定义的函数，如initrtk、timespan、searchimu、matchobs、ins_align、gi_initrtk、exclude_sat、ins2sol、outsol、plot_trajectory_kine、ins_mech和gi_gnss_filter等。
gi_Loose.m（GNSS/INS 松组合模式）
function [rtk_gi,rtk_gnss,stat0]=gi_Loose(rtk_gi,rtk_gnss,obsr,obsb,nav)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% gnss/ins loosely coupled mode
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Copyright(c) 2020-2025, by Kai Chen, All rights reserved.
%8/12/2020
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
global glc
[rtk_gnss,~]=gnss_solver(rtk_gnss,obsr,obsb,nav);
if rtk_gnss.sol.stat~=glc.SOLQ_NONE
[rtk_gi,rtk_gnss,stat0]=gnss_ins_lc(rtk_gi,rtk_gnss);
else
stat0=0;
end
return
这段 MATLAB 代码实现了 GNSS/INS 松耦合模式下的解算过程。函数接受四个输入参数：
rtk_gi、rtk_gnss、obsr和nav，以及三个输出参数：rtk_gi、rtk_gnss和stat0。首先，函数调用
gnss_solver函数对 GNSS 测量数据进行解算，得到rtk_gnss结构体，其中包含了解算出的位置、速度和钟差等信息。接下来，通过判断
rtk_gnss.sol.stat的值是否等于glc.SOLQ_NONE来判断 GNSS 解算是否成功。若解算成功，则调用gnss_ins_lc函数进行 GNSS/INS 松耦合解算，并将结果更新到rtk_gi和rtk_gnss结构体中。若解算失败，则将stat0的值设为 0。最后，函数返回更新后的
rtk_gi、rtk_gnss和stat0结构体。
gnss_ins_lc.m（GNSS/INS 松组合计算）
function [rtk_gi,rtk_gnss,stat]=gnss_ins_lc(rtk_gi,rtk_gnss)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%% GNSS/INS Loosely Coupled %%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%inputs: rtk_gi - rtk_gi control struct
% rtk_gnss - rtk_gnss control struct
%output: rtk_gi - rtk_gi control struct
% rtk_gnss - rtk_gnss control struct
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Copyright(c) 2020-2025, by Kai Chen, All rights reserved.
%8/12/2020
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%GNSS/INS LC includes SPP/INS LC,PPD/INS LC,PPK/INS LC and PPP/INS LC
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
global glc
ins=rtk_gi.ins; opt=rtk_gnss.opt; stat=1;
VAR_POS=10^2; VAR_VEL=0.15^2; MAX_DPOS=10; MAX_DVEL=5;
% XYZ to BLH
rr=rtk_gnss.sol.pos';
[pos_GNSS,Cne]=xyz2blh(rr);
% VE to VN
ve=rtk_gnss.sol.vel;
vel_GNSS=Cne*ve';
% XYZ variance to BLH variance
posP=rtk_gnss.sol.posP; T=Dblh2Dxyz(pos_GNSS);
P=[posP(1) posP(4) posP(6);
posP(4) posP(2) posP(5);
posP(6) posP(5) posP(3)];
if max(diag(P))>VAR_POS
rr=zeros(3,1);
else
P=T^-1*P*(T')^-1;
VAR1=[P(1,1);P(2,2);P(3,3)];
end
% VE variance to VN variance
velP=rtk_gnss.sol.velP;
P=[velP(1) velP(4) velP(6);
velP(4) velP(2) velP(5);
velP(6) velP(5) velP(3)];
if max(diag(P))>VAR_VEL
ve=zeros(3,1);
else
P=Cne*P*Cne';
VAR2=[P(1,1);P(2,2);P(3,3)];
if norm(VAR2)==0
VAR2=[VAR_VEL;VAR_VEL;VAR_VEL];
end
end
% lever arm correction for position and velocity
pos_INS=ins.pos+ins.Mpv*ins.Cnb*ins.lever;
vel_INS=ins.vel+ins.Cnb*askew(ins.web)*ins.lever;
% exclude large gross errors
if rtk_gi.ngnsslock>10&&norm(rr)~=0
rr_INS=blh2xyz(pos_INS); dpos=abs(rr_INS-rr);
if max(dpos)>MAX_DPOS
rr=zeros(3,1);
end
end
if rtk_gi.ngnsslock>10&&norm(ve)~=0
dvel=abs(vel_INS-vel_GNSS);
if max(dvel)>MAX_DVEL
ve=zeros(3,1);
end
end
% calculate v,H and R
if norm(rr)~=0&&norm(ve)~=0
v1=pos_INS-pos_GNSS'; v2=vel_INS-vel_GNSS;
if opt.mode==glc.PMODE_SPP||(opt.mode==glc.PMODE_DGNSS&&opt.dynamics==1)||...
(opt.mode==glc.PMODE_KINEMA&&opt.dynamics==1)||...
(opt.mode==glc.PMODE_PPP_KINEMA&&opt.dynamics==1)
v=[v1;v2];
R1=diag(VAR1); R2=diag(VAR2); R=blkdiag(R1,R2);
H=zeros(6,15);
H(1,7)=1;H(2,8)=1;H(3,9)=1;
H(4,4)=1;H(5,5)=1;H(6,6)=1;
else
v=pos_INS-pos_GNSS';
R=diag(VAR1);
H=zeros(3,15);
H(1,7)=1;H(2,8)=1;H(3,9)=1;
end
elseif norm(rr)~=0&&norm(ve)==0
v=pos_INS-pos_GNSS';
R=diag(VAR1);
H=zeros(3,15);
H(1,7)=1;H(2,8)=1;H(3,9)=1;
elseif norm(rr)==0&&norm(ve)~=0
if opt.mode==glc.PMODE_SPP||(opt.mode==glc.PMODE_DGNSS&&opt.dynamics==1)||...
(opt.mode==glc.PMODE_KINEMA&&opt.dynamics==1)||...
(opt.mode==glc.PMODE_PPP_KINEMA&&opt.dynamics==1)
v=vel_INS-vel_GNSS;
R=diag(VAR2);
H=zeros(3,15);
H(1,4)=1;H(2,5)=1;H(3,6)=1;
else
stat=0;
end
elseif norm(rr)==0&&norm(ve)==0
stat=0;
end
% IGG-3 robust model, only for SPP/INS mode
if opt.ins.aid(2)==1&&opt.mode==glc.PMODE_SPP&&stat==1&&rtk_gi.ngnsslock>10
Q=H*ins.P*H'+R;
c0 = 2; c1 = 5;
nv = size(v,1); std_res=zeros(nv,1); rfact=zeros(nv,1);
for i=1:nv
std_res(i)=abs(v(i))/sqrt(Q(i,i));
end
% robust factor
for i=1:nv
if std_res(i) <= c0
rfact(i) = 1;
elseif (std_res(i) >= c0) && (std_res(i) <= c1)
rfact(i)= abs(std_res(i))/c0 * ((c1-c0)/(c1-abs(std_res(i))))^2;
else
rfact(i) = 10^6;
end
end
for i=1:nv
for j=1:nv
if j~=i,continue;end
R(i,j)=R(i,j)*sqrt(rfact(i)*rfact(j));
end
end
idx=(rfact==10^6);
v(idx,:)=[]; H(idx,:)=[]; R(idx,:)=[]; R(:,idx)=[];
if isempty(v),stat=0;end
end
if stat==1
% measurement update
[ins,stat]=lc_filter(v,H,R,ins.x,ins.P,ins);
if stat==0
[week,sow]=time2gpst(rtk.sol.time);
fprintf('Warning:GPS week = %d sow = %.3f,filter failed!\n',week,sow);
return;
end
end
if stat==1
rtk_gi.ins=ins;
rtk_gi.ngnsslock=rtk_gi.ngnsslock+1;
rtk_gi.gi_time=rtk_gnss.sol.time;
% update solution status
rtk_gi=udsol_lc(rtk_gi,rtk_gnss);
end
return
这是一个 GNSS/INS 松耦合算法的 MATLAB 实现。 GNSS 是全球卫星定位系统，INS 是惯性导航系统，两者通过松耦合实现数据融合。此算法将 GNSS 数据和 INS 数据进行组合，以提高导航系统的性能。这个函数将 GNSS 和 INS 的控制结构作为输入，计算和输出融合后的控制结构。
这个函数首先将 GNSS 接收机的位置和速度转换为大地坐标系下的经纬度高程（BLH）以及大地系下的速度（VN）。如果定位精度或速度精度过大，则使用零值代替，如果接收机到卫星的距离过大，则忽略。然后使用位置和速度的信息计算协方差矩阵，并进行杆臂校正。最后，使用协方差矩阵计算变量的方差、残差和协方差矩阵。
此算法包括单点定位/惯性导航松耦合（SPP/INS LC）、伪距差分/惯性导航松耦合（PPD/INS LC）、精密定位/惯性导航松耦合（PPK/INS LC）和精密点位置/惯性导航松耦合（PPP/INS LC）。
gnss_ins_lc.m:95 是选择SPP与INS的松组合模式
gnss_ins_lc.m:72 计算v、H、R
在这段Matlab代码中，v、H和R分别是以下内容：
v: 表示向量差，是指通过惯性导航系统（INS）和全球定位系统（GNSS）的解算结果之间的误差。
H: 表示观测矩阵，它描述了状态转移矩阵和观测值之间的关系。
R: 表示协方差矩阵，它描述了测量误差的方差或协方差。在这段代码中，它是一个对角矩阵，其中对角线元素是误差的方差。
gi_Tight.m（GNSS/INS紧组合模式）
function [rtk,stat]=gi_Tight(rtk,obsr,obsb,nav)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% gnss/ins tightly coupled mode
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Copyright(c) 2020-2025, by Kai Chen, All rights reserved.
%8/12/2020
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
global glc
opt=rtk.opt; time=rtk.gi_time;
if time.time~=0,rtk.tt=timediff(obsr(1).time,time);end
if opt.mode==glc.PMODE_SPP||opt.mode>=glc.PMODE_PPP_KINEMA
% scan obs for spp
[obsr,nobs]=scan_obs_spp(obsr);
if nobs==0,stat=0;return;end
% correct BDS2 multipath error
if ~isempty(strfind(opt.navsys,'C'))
obsr=bds2mp_corr(rtk,obsr);
end
end
if opt.mode>=glc.PMODE_PPP_KINEMA
% scan obs for ppp
[obsr,nobs]=scan_obs_ppp(obsr);
if nobs==0,stat=0;return;end
% repair receiver clock jump (only for GPS)
[rtk,obsr]=clkrepair(rtk,obsr,nav);
end
if opt.mode==glc.PMODE_SPP
% SPP/INS
[rtk,stat]=sppins(rtk,obsr,nav);
elseif opt.mode==glc.PMODE_DGNSS||opt.mode==glc.PMODE_KINEMA
% PPD/INS or PPK/INS
if ~isstruct(obsb),stat=0;return;end
rtk.sol.age=timediff(obsr(1).time,obsb(1).time);
[rtk,stat]=rtkins(rtk,obsr,obsb,nav);
else
% PPP/INS
[rtk,stat]=pppins(rtk,obsr,nav);
end
return
这是一个实现了GNSS/INS紧耦合集成算法的MATLAB函数，名称为
gi_Tight。函数的输入包括：
rtk：一个结构体，包含有关接收机运动状态和先前解算结果（如果有的话）的信息。obsr：接收机观测到的原始观测值，包括伪距和相位观测值。obsb：基站观测到的原始观测值，如果是差分模式或者PPK模式下，则包含了基线解算结果。nav：卫星导航电文数据，包含有关卫星的轨道参数和时钟偏差等信息。该函数的实现主要包括以下步骤：
- 根据接收机工作模式，对观测值进行预处理，如扫描观测值以过滤出需要的卫星、进行BDS2多路径误差修正等。
- 根据接收机工作模式，对观测值进行时钟跳变修复等处理，以提高解算精度。
- 根据接收机工作模式，选择不同的解算方法，如单点定位、差分定位、PPP等，进行解算。
- 返回解算结果和状态信息。
sppins.m（SPP/INS紧组合解算）
function [rtk,stat]=sppins(rtk,obs,navs)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%% SPP/INS Tightly Coupled %%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%inputs: rtk - rtk control struct
% obs - observation
% navs - all navigation message
%output: rtk - rtk control struct
% stat0- state
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Copyright (C) 2020-2025, by Kai Chen, All rights reserved.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
global glc
stat=1; opt=rtk.opt; nobs=size(obs,1); niter=1;
% default configuration
opt.tropopt=glc.IONOOPT_BRDC;
opt.ionoopt=glc.TROPOPT_SAAS;
opt.sateph =glc.EPHOPT_BRDC;
% update state
rtk=udstate_sppins(rtk);
% cumpute satellite position,clock bias,velocity,clock drift
sv=satposs(obs,navs,opt.sateph);
xp=rtk.x; Pp=rtk.P; x_fb=zeros(rtk.nx,1);
for i=1:niter
% pseudorange residual,measurement matrix,measurement noise matrix
[v1,H1,R1,nv1,sat_]=rescode_sppins(i,obs,navs,sv,xp,Pp,opt,rtk);
if nv1<3,stat=0;break;end
% dopller residual,measurement matrix,measurement noise matrix
[v2,H2,R2,nv2]=resdop_sppins(obs,navs,sv,sat_,xp,Pp,rtk);
% % exclude abnormal pseudorange and doppler
% if nv1>0
% [v1,H1,R1,nv1]=robust_sppins(v1,H1,R1,0);
% end
% if nv2>0
% [v2,H2,R2,nv2]=robust_sppins(v2,H2,R2,1);
% end
if nv1>0&&nv2>0
v=[v1;v2]; H=[H1;H2]; R=blkdiag(R1,R2);
elseif nv1>0&&nv2==0
v=v1; H=H1; R=R1;
elseif nv1==0&&nv2>0
v=v2; H=H2; R=R2;
else
stat=0; break;
end
% measurement update
[xp,Pp,x_fb0,stat]=sppins_filter(v,H,R,xp,Pp);
if stat==0
[week,sow]=time2gpst(obs(1).time);
fprintf('Warning:GPS week = %d sow = %.3f,SPP/INS filter failed!\n',week,sow);
break;
end
x_fb=x_fb+x_fb0;
end
if stat==1
rtk.x=xp; rtk.P=Pp;
rtk.gi_time=obs(1).time;
rtk.ngnsslock=rtk.ngnsslock+1;
% update INS parameters
rtk=sppins_feedback(rtk,x_fb);
% update solution status
rtk.sol.time=obs(1).time;
rtk.sol.stat=glc.SOLQ_SPP;
rtk.sol.ns=sat_.ns;
rtk=udsol_sppins(rtk);
rtk.oldsol=rtk.sol;
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);
rtk.sat(obs(i).sat).oldobs=obs(i);
end
return
这段代码实现了一个基于SPP/INS紧耦合的定位算法，通过观测数据和导航数据来计算接收机的位置、速度等参数。下面是代码的一些解释：
- 首先定义了一些变量和常量，如rtk（接收机控制结构体）、obs（观测数据）、navs（导航数据）、opt（选项结构体）、nobs（观测数据数量）、niter（迭代次数）等。
- 接下来通过调用函数
udstate_sppins来更新状态，即将先前的状态和协方差矩阵作为当前的先验状态和先验协方差矩阵。- 然后计算卫星的位置、钟差、速度和钟漂，通过调用
satposs函数实现。- 接下来进行迭代，每次迭代先计算伪距残差（包括测量矩阵和测量噪声矩阵），然后计算多普勒残差（同样包括测量矩阵和测量噪声矩阵）。
- 在进行迭代之前，还可以通过调用
robust_sppins函数去除异常的伪距或多普勒观测值。- 接下来根据伪距和多普勒观测值的数量来确定测量矩阵和测量噪声矩阵。
- 然后进行测量更新，通过调用
sppins_filter函数实现。- 如果测量更新失败，则更新状态为0，否则更新状态为1，并更新接收机的位置、速度等参数。
- 最后，将接收机的状态存储到rtk结构体中，并更新rtk中每个卫星的信息，如可见性、卫星高度角、信噪比等。
总体来说，这段代码实现了一个基于SPP/INS紧耦合的定位算法，通过迭代计算观测值的伪距残差和多普勒残差来实现定位，同时也更新了接收机的姿态和速度等信息。
sppins.m:24计算接收机位置、速度、钟差等。在此部分代码中，函数 satposs 用于计算所有卫星的位置和速度，其中 obs 是接收机接收到的观测数据，navs 是所有的导航电文数据，opt.sateph 是卫星轨道模型的选择。接下来，将卫星位置和速度、接收机状态量（即位置和速度）和误差协方差矩阵存储在 sv、xp 和 Pp 中。
在这段代码中，INS与SPP进行了紧组合的部分主要是在循环迭代中进行的，即在计算残差、雅克比矩阵以及测量噪声矩阵的过程中。在计算完残差后，将所有的残差向量、雅克比矩阵和测量噪声矩阵合并在一起，然后使用SPP/INS紧组合滤波器更新状态向量和协方差矩阵。同时，在更新状态向量和协方差矩阵的过程中，还需要将INS的反馈量加入到状态向量中，以实现INS与SPP的紧组合。
具体来说，这段代码实现了基于紧组合的惯性导航系统（Inertial Navigation System，INS）和单点定位（Single Point Positioning，SPP）的卡尔曼滤波联合解算。在每一次迭代中，代码首先计算卫星的位置、钟差、速度和钟漂，并根据当前状态和观测值计算伪距和多普勒观测值的残差、观测矩阵和观测噪声协方差矩阵。然后，代码使用卡尔曼滤波算法，将伪距和多普勒观测值残差作为测量更新当前的状态向量和状态协方差矩阵。最后，代码更新了惯性导航系统的状态向量和状态协方差矩阵，并返回卡尔曼滤波的状态和解算结果。
卡尔曼滤波的核心在于状态的预测和更新，这段代码中使用的卡尔曼滤波方法在以下几行中体现出来了：
- 在函数
sppins的开头，调用了函数udstate_sppins来更新状态。这个函数的作用是根据当前的状态和时间，通过惯性导航算法对状态进行预测。 - 在函数
sppins中，根据预测值计算出伪距残差和多普勒残差，并将它们合并成一个残差向量v，同时计算出观测矩阵H和测量噪声协方差矩阵R。 - 在函数
sppins_filter中，根据卡尔曼滤波的公式，利用预测的状态值、预测的协方差矩阵、观测矩阵H、残差向量v和测量噪声协方差矩阵R，计算出新的状态值和协方差矩阵。
因此，这段代码使用了卡尔曼滤波方法对INS与SPP进行了紧组合。
rescode_sppins.m
function [v,H,R,nv,sat_]=rescode_sppins(iter,obs,nav,sv,x,P,opt,rtk)
global glc
nobs=size(obs,1); NSYS=glc.NSYS;
v=zeros(nobs+NSYS,1); H=zeros(nobs+NSYS,rtk.nx); R=zeros(nobs+NSYS,nobs+NSYS);
vsat=zeros(nobs,1); azel=zeros(nobs,2); resp=zeros(nobs,1);
nv=0; ns=0; mask=zeros(5,1);
lam_carr0=glc.CLIGHT/glc.FREQ_GPS_L1;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0508
% num_obsat = nobs - 3;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
pos=x(7:9)'; rr=blh2xyz(pos); T=Dblh2Dxyz(pos);
for i=1:nobs
%%%%%%%%%%%%%%%%%%%%%%%%%%% 0508
% if i >= num_obsat, break; end
%%%%%%%%%%%%%%%%%%%%%%%%%%%
vsat(i)=0; azel(i,:)=[0,0]; resp(i)=0;
[sys,~]=satsys(obs(i).sat);
if sys==0,continue;end
if i<nobs&&obs(i).sat==obs(i+1).sat,continue;end
rs = sv(i).pos; dts = sv(i).dts; Vars=sv(i).vars;
[r,LOS]=geodist(rs,rr); azel(i,:)=satazel(pos,LOS);
if r<=0||azel(i,2)<opt.elmin,continue;end
% pesudorange with code bias correction
[pr,Vmea]=prange(obs(i),nav,opt,azel(i,:),iter); %#ok
if pr==0,continue;end
if abs(pr)>5e7,continue;end
% excluded satellite
stat=satexclude(obs(i).sat,Vars,sv(i).svh,opt);
if stat==0,continue;end
% ionospheric delay
[ionoerr,Viono]=iono_cor(glc.IONOOPT_BRDC,obs(i).time,nav,obs(i).sat,pos,azel(i,:)); %#ok
lam_L1=nav.lam(obs(i).sat,1);
if lam_L1>0,ionoerr=ionoerr*(lam_L1/lam_carr0)^2;end
% tropospheric delay
[troperr,Vtrop]=trop_cor(glc.TROPOPT_SAAS,obs(i).time,nav,pos,azel(i,:)); %#ok
% design measurement matrix
H(nv+1,:)=0;
H(nv+1,7:9)=LOS*T;
if sys==glc.SYS_GPS
dtr=x(rtk.ic+1); mask(1)=1;
H(nv+1,rtk.ic+1)=1;
elseif sys==glc.SYS_GLO
dtr=x(rtk.ic+1)+x(rtk.ic+2); mask(2)=1;
H(nv+1,rtk.ic+1)=1; H(nv+1,rtk.ic+2)=1;
elseif sys==glc.SYS_GAL
dtr=x(rtk.ic+1)+x(rtk.ic+3); mask(3)=1;
H(nv+1,rtk.ic+1)=1; H(nv+1,rtk.ic+3)=1;
elseif sys==glc.SYS_BDS
dtr=x(rtk.ic+1)+x(rtk.ic+4); mask(4)=1;
H(nv+1,rtk.ic+1)=1; H(nv+1,rtk.ic+4)=1;
elseif sys==glc.SYS_QZS
dtr=x(rtk.ic+1)+x(rtk.ic+5); mask(5)=1;
H(nv+1,rtk.ic+1)=1; H(nv+1,rtk.ic+5)=1;
end
% pseudorange residuals
v(nv+1)=pr-(r+dtr-glc.CLIGHT*dts+ionoerr+troperr);
% variance matrix
VARr=varerr_spp(opt,azel(i,2),sys);
R(nv+1,nv+1)=VARr;
% record validate satellite,rasidual
vsat(i)=1;
resp(i)=v(nv+1,1);
nv=nv+1;
ns=ns+1;
end
sat_.vsat=vsat; sat_.azel=azel; sat_.resp=resp; sat_.ns=ns;
v(nv+1:end,:)=[]; H(nv+1:end,:)=[]; R(nv+1:end,:)=[];R(:,nv+1:end)=[];
% IGG-3 model proposed by YuanXi Yang
if opt.ins.aid(2)==1&&rtk.ngnsslock>10
Q=H*P*H'+R;
c0 = 2; c1 = 5;
nv = size(v,1); std_res=zeros(nv,1); rfact=zeros(nv,1);
for i=1:nv
std_res(i)=abs(v(i))/sqrt(Q(i,i));
end
% robust factor
for i=1:nv
if std_res(i) <= c0
rfact(i) = 1;
elseif (std_res(i) >= c0) && (std_res(i) <= c1)
rfact(i)= abs(std_res(i))/c0 * ((c1-c0)/(c1-abs(std_res(i))))^2;
else
rfact(i) = 10^6;
end
end
idx=find(rfact==10^6);
ratio=size(idx,1)/nv;
if ratio<0.7
for i=1:nv
for j=1:nv
if j~=i,continue;end
R(i,j)=R(i,j)*sqrt(rfact(i)*rfact(j));
end
end
end
end
return
这段代码是用于实现单点定位（Single Point Positioning，SPP）的功能。SPP是一种简单的定位方法，通过接收单一接收机接收到的卫星信号，使用伪距（Pseudorange）测量值进行定位计算。
以下是代码的详细解释：
代码首先定义了一些变量和矩阵，包括观测值的数量（nobs），系统的数量（NSYS），存储结果的向量（v），设计矩阵（H），观测值的协方差矩阵（R）等。
然后，代码进入一个循环，处理每个观测值。
在循环的开始，代码对当前观测值的一些变量进行初始化。
然后，代码判断当前观测值所属的卫星系统（GPS、GLONASS、Galileo等）以及卫星号。
如果系统为0，表示无法确定该观测值所属的卫星系统，代码将跳过该观测值的处理。
如果当前观测值与下一个观测值属于同一个卫星，代码将跳过该观测值的处理。
对于有效的观测值，代码首先计算卫星的位置、接收机到卫星的距离、卫星的方位角和仰角等信息。
如果卫星到接收机的距离小于等于0或卫星的仰角小于设定的最小可见仰角（opt.elmin），代码将跳过该观测值的处理。
然后，代码计算带有码偏差校正的伪距（Pseudorange）测量值。如果伪距值为0或超过了一个阈值（5e7），代码将跳过该观测值的处理。
代码使用一些条件和规则对卫星进行排除，如卫星信号质量较差、卫星处于排除列表中等。
接下来，代码计算电离层延迟（ionospheric delay）和对流层延迟（tropospheric delay），并进行修正。
代码根据卫星所属的系统，构建观测方程的设计矩阵（H）和伪距残差（v）。
代码计算观测方程的协方差矩阵（R）。
代码记录有效的卫星、残差等信息。
代码更新总观测量的数量（nv）和有效卫星的数量（ns）。
循环结束后，代码删除多余的部分观测值，并调整矩阵的大小。
如果开启了惯性导航辅助（opt.ins.aid(2)为1）并且NGNSS（GNSS定位的可用卫星数量）大于10（rtk.ngnsslock>10），则代码进入IGG-3模型的处理。
代码计算观测方程的协方差矩阵Q，然后定义一些参数c0和c1。
代码通过计算标准化残差（std_res）来评估观测值的拟合度。
根据标准化残差，代码计算鲁棒因子（rfact）。
如果鲁棒因子小于10^6的观测值占总观测值数量的比例超过0.7，代码不进行任何修正。
否则，代码根据鲁棒因子对观测方程的协方差矩阵进行修正。
最后，代码返回处理后的结果，包括有效卫星（vsat）、卫星的方位角和仰角（azel）、伪距残差（resp）以及有效卫星数量（ns）。
这段代码实现了SPP定位过程中的观测值处理、伪距计算、残差计算、协方差矩阵计算等关键步骤。通过该代码，可以获取到定位所需的观测值残差和相关的卫星信息，用于后续的定位计算和精度评估。
resdop_sppins.m
function [v,H,R,nv]=resdop_sppins(obs,nav,sv,sat_,x,P,rtk)
global glc
nobs=size(obs,1);v=zeros(nobs,1); H=zeros(nobs,rtk.nx); R=zeros(nobs,nobs);
var=zeros(nobs,nobs); nv=0; azel=zeros(nobs,2);
pos=x(7:9); [rr,Cen]=blh2xyz(pos); vr=Cen*x(4:6);
% ins=rtk.ins;
% lever=Cen*ins.Cnb*ins.lever;
% wnin=ins.eth.wnin; wbib=ins.wbib;
% K_phi=Cen*(ins.Cnb*askew(askew(ins.lever)*wbib)+askew(wnin)*ins.Cnb*ins.lever);
% vr=Cen*rtk.x(4:6)-Cen*ins.Cnb*askew(ins.lever)*wbib-Cen*askew(wnin)*ins.Cnb*ins.lever;
% dtr_drift=rtk.x(rtk.ic+2);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0508
% num_obsat = nobs - 3;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for i=1:nobs
%%%%%%%%%%%%%%%%%%%%%%%%%%% 0508
% if i >= num_obsat, break; end
%%%%%%%%%%%%%%%%%%%%%%%%%%%
sat=obs(i).sat; lam=nav.lam(sat,:); dop=obs(i).D(1);
if lam(1)==0||dop==0,continue;end
rs=sv(i).pos; vs=sv(i).vel;
dts_drift=sv(i).dtsd; vsat=sat_.vsat(i);
[r,LOS]=geodist(rs,rr); azel(i,:)=satazel(pos,LOS);
if r<=0||azel(i,2)<rtk.opt.elmin,continue;end
[sys,~]=satsys(sat);
if sys==0||lam(1)==0||dop==0||norm(vs)<=0||vsat==0
continue;
end
% doppler residuals
del_r=rs-rr; del_v=vs-vr; dtr_drift=x(rtk.ic+6);
rate=dot(del_v,LOS)+glc.OMGE/glc.CLIGHT*(vs(2)*rr(1)+rs(2)*vr(1)-vs(1)*rr(2)-rs(1)*vr(2));
v(nv+1,1) = -lam(1)*dop-(rate+dtr_drift-glc.CLIGHT*dts_drift);
% measurement matrix
T=Dblh2Dxyz(pos);
m=(del_v/r-del_r*(del_r'*del_v)/r^3)';
n=LOS*Cen;
H(nv+1,:)=0;
%H(nv+1,1:3)=m*askew(lever)-n*K_phi;
H(nv+1,4:6)=n;
H(nv+1,7:9)=m*T;
H(nv+1,rtk.ic+6)=1;
% variance
var(nv+1,1)=(0.03/sin(azel(i,2)))^2;
nv=nv+1;
end
if nv==0
v=NaN;H=NaN;R=NaN;return;
end
% measurement noise matrix
for i=1:nv
for j=1:nv
if i==j
R(i,j)=var(i);
else
R(i,j)=0;
end
end
end
if nv<nobs
v(nv+1:end,:)=[]; H(nv+1:end,:)=[]; R(nv+1:end,:)=[]; R(:,nv+1:end)=[];
end
% IGG-3 model proposed by YuanXi Yang
if rtk.opt.ins.aid(2)==1&&rtk.ngnsslock>10
Q=H*P*H'+R;
c0 = 1.5; c1 = 3;
nv = size(v,1); std_res=zeros(nv,1); rfact=zeros(nv,1);
for i=1:nv
std_res(i)=abs(v(i))/sqrt(Q(i,i));
end
% robust factor
for i=1:nv
if std_res(i) <= c0
rfact(i) = 1;
elseif (std_res(i) >= c0) && (std_res(i) <= c1)
rfact(i)= abs(std_res(i))/c0 * ((c1-c0)/(c1-abs(std_res(i))))^2;
else
rfact(i) = 10^6;
end
end
idx=find(rfact==10^6);
ratio=size(idx,1)/nv;
if ratio<0.6
for i=1:nv
for j=1:nv
if j~=i,continue;end
R(i,j)=R(i,j)*sqrt(rfact(i)*rfact(j));
end
end
end
end
return
代码初始化一些变量，包括观测值残差（v）、设计矩阵（H）、观测噪声方差矩阵（R）、有效观测值数量（nv）和卫星的方位角和仰角（azel）。
代码根据输入参数计算接收机的位置（pos）和速度（vr），并将其转换为地心坐标系下的位置（rr）和速度（Cen）。
代码进入一个循环，遍历每个观测值。
代码根据卫星的频率（lam）和多普勒观测值（dop）判断该观测值是否有效。
代码计算卫星的位置（rs）和速度（vs），并获取卫星钟差漂移（dts_drift）以及该观测值对应的有效卫星标识（vsat）。
代码计算接收机与卫星的几何距离（r）和方向向量（LOS）以及接收机与卫星的方位角和仰角（azel）。
根据卫星系统（sys）判断是否为有效的卫星，若无效则继续下一次循环。
代码计算多普勒残差（rate）。
代码计算观测值残差（v），其中包括了多普勒残差以及接收机钟差漂移（dtr_drift）。
代码计算设计矩阵（H），其中包括了对速度和位置的设计矩阵。
代码计算观测噪声方差（var）。
更新有效观测值数量（nv）。
循环结束后，根据有效观测值数量（nv）进行进一步处理。
代码根据观测噪声方差（var）构建观测噪声方差矩阵（R）。
如果有效观测值数量（nv）小于总观测值数量（nobs），则将多余的观测值相关的变量截断。
如果开启了惯性导航辅助（opt.ins.aid(2)为1）并且NGNSS（GNSS定位的可用卫星数量）大于10（rtk.ngnsslock>10），则代码进入IGG-3模型的处理。
代码计算观测方程的协方差矩阵Q，然后定义一些参数c0和c1。
代码通过计算标准化残差（std_res）来评估观测值的拟合
代码根据标准化残差（std_res）计算鲁棒因子（rfact）。当标准化残差小于等于c0时，鲁棒因子为1；当标准化残差在c0和c1之间时，鲁棒因子采用一个二次函数的形式计算；当标准化残差大于c1时，鲁棒因子设定为一个较大的值10^6。
代码统计鲁棒因子为10^6的数量，并计算其占总观测值数量的比例。
如果鲁棒因子为10^6的比例小于0.6，则对观测噪声方差矩阵（R）进行修正。修正方式是将R的对角线元素乘以相应的鲁棒因子的平方根。
函数执行完毕，返回计算得到的观测值残差（v）、设计矩阵（H）、观测噪声方差矩阵（R）和有效观测值数量（nv）。
总体来说，这段代码根据给定的观测数据、导航数据和卫星信息，计算了SPP定位中的DOP残差。它通过计算多普勒残差和观测值残差，并构建设计矩阵和观测噪声方差矩阵，为后续的定位解算提供了必要的输入。此外，代码还实现了IGG-3模型中的鲁棒性处理，以提高定位的鲁棒性和可靠性。
udsate_sppins.m（紧组合导航更新状态估计量）
function rtk=udstate_sppins(rtk)
% update pos vel att
rtk=udpos_sppins(rtk);
% update clk
rtk=udclk_sppins(rtk);
return
这段代码是一个函数，用于在紧组合导航中更新状态估计量（包括位置、速度、姿态和钟差等信息）的函数。具体来说：
- 函数调用了
udpos_sppins和udclk_sppins两个函数来分别更新位置速度姿态和钟差的估计值。- 最后将更新后的状态估计量保存在
rtk结构体中，返回给调用函数。这个函数是整个紧组合导航滤波器的核心部分之一，用于实现状态估计的更新和滤波过程。
sppins_filter.m（紧组合卡尔曼滤波）
function [X,P,x_fb,stat]=sppins_filter(v,H,R,x_pre,P_pre)
nx=size(x_pre,1);
X=zeros(nx,1); P=zeros(nx,nx); x_fb=zeros(nx,1); stat=1;
Q=H*P_pre*H'+R;
if det(Q)==0
stat=0; return;
end
K=P_pre*H'*Q^-1;
x=K*v;
P=(eye(nx)-K*H)*P_pre;
if ~isreal(x)||~isreal(P)
stat=0;
end
Cnb = att2Cnb(x_pre(1:3));
Cnb = (eye(3)+askew(x(1:3)))*Cnb;
if ~isreal(Cnb)
stat=0; return;
end
att = Cnb2att(Cnb);
vel = x_pre(4:6)-x(4:6);
pos = x_pre(7:9)-x(7:9);
bg = x_pre(10:12)+x(10:12);
ba = x_pre(13:15)+x(13:15);
X(1:15,1) = [att;vel;pos;bg;ba];
X(16:nx,1) = x_pre(16:end)+x(16:end);
x_fb=x;
return
这段代码实现了SPP（单点定位）和INS（惯性导航系统）的紧组合，使用卡尔曼滤波方法将两个系统的测量结果进行融合，得到一个更精确的位置和速度估计。
具体来说，这段代码输入了以下参数：
- v: 包含SPP和INS测量结果的残差向量
- H: 观测矩阵，描述SPP和INS的测量值如何与状态向量相关联
- R: 包含SPP和INS测量误差的协方差矩阵
- x_pre: 上一次滤波的状态向量估计值
- P_pre: 上一次滤波的状态向量估计值的协方差矩阵
代码首先计算出Q矩阵，然后根据卡尔曼滤波的原理计算出增益矩阵K，将v向量与K相乘得到状态向量的增量x，最后计算出状态向量的估计值X和协方差矩阵的估计值P。
最后，代码将增量x中的角度变化和速度变化分别转化为姿态和速度的变化，计算出新的姿态、速度、位置、陀螺仪偏差和加速度计偏差，并返回更新后的状态向量估计值X、卡尔曼增益矩阵K、状态向量增量x以及状态变量是否为实数的状态码stat。
udpos_sppins.m（INS的数据更新）
function rtk=udpos_sppins(rtk)
ins=rtk.ins;
rtk.x(1:15)=0;
rtk.P(1:15,1:15)=0;
% lever arm correction
pos=ins.pos+ins.Mpv*ins.Cnb*ins.lever;
vel=ins.vel+ins.Cnb*askew(ins.web)*ins.lever;
rtk.x(1:15)=[ins.att;vel;pos;ins.bg;ins.ba];
rtk.P(1:15,1:15)=ins.P;
return
这段代码是针对位置、速度和姿态的更新。首先将状态向量的前15个元素清零，将前15x15的协方差矩阵清零。然后进行杠杆臂校正，得到经过杠杆臂校正后的位置和速度信息，并将其以及姿态、陀螺仪偏差和加速度计偏差放入状态向量的前15个元素中，同时将INS的协方差矩阵赋值给RTK的协方差矩阵。最后返回更新后的RTK结构体。
注意，这段代码仅仅是在更新惯性导航系统(INS)的状态和状态协方差矩阵，没有全球卫星定位系统(GNSS)的数据参与。该函数的输入参数
rtk中包含了INS的数据信息，包括INS的位置、速度、姿态、陀螺仪和加速度计的bias等信息，通过该函数，将INS的状态信息存储在rtk中，以供后续使用。
数据流：
rtk 结构体
代码改进
sppins_filter.m
将原代码（标准卡尔曼滤波）改成如下代码（扩展卡尔曼滤波）
function [X,P,x_fb,stat]=sppins_filter(v,H,R,x_pre,P_pre)
nx=size(x_pre,1);
X=zeros(nx,1); P=zeros(nx,nx); x_fb=zeros(nx,1); stat=1;
Q=H*P_pre*H'+R;
if det(Q)==0
stat=0; return;
end
K=P_pre*H'*inv(Q);
x=K*v;
P=(eye(nx)-K*H)*P_pre;
if ~isreal(x)||~isreal(P)
stat=0;
end
% state error transition matrix
F = zeros(nx,nx);
F(1:3,1:3) = -askew(x(1:3));
F(4:6,1:3) = -eye(3);
F(7:9,4:6) = -eye(3);
F(10:12,10:12) = -eye(3);
F(13:15,13:15) = -eye(3);
% state transition matrix
phi = eye(nx) + F;
% propagate state and covariance
X = phi * x_pre + x;
P = phi * P_pre * phi' + F * Q * F';
Cnb = att2Cnb(X(1:3));
Cnb = (eye(3)+askew(X(1:3)))*Cnb;
if ~isreal(Cnb)
stat=0; return;
end
att = Cnb2att(Cnb);
vel = X(4:6);
pos = X(7:9);
bg = X(10:12);
ba = X(13:15);
X(1:15,1) = [att;vel;pos;bg;ba];
X(16:nx,1) = X(16:end);
x_fb=X;
end
543
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
