GINav 学习笔记
环境:win10, matlab 2020a
文章目录
- GINav 学习笔记
- **数据处理部分:**
- 数据文件类型:
- o文件和n文件和p文件区别
- DCB (Differential Code Bias) file
- BSX(MGEX DCB) file
- EOP file
- 标准单点定位(SPP)用到的文件:
- configuration file
- rover observation file
- broadcast ephemeris file
- DCB file
- BSX(MGEX DCB) file
- EOP file
- SPP/INS松组合用到的文件:
- configuration file
- rover observation file
- broadcast ephemeris file
- DCB file
- BSX(MGEX DCB) file
- EOP file
- IMU file
- SPP/INS紧组合用到的文件:
- configuration file
- rover observation file
- broadcast ephemeris file
- DCB file
- BSX(MGEX DCB) file
- EOP file
- IMU file
- **初始化程序:**
- 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紧组合解算)
- 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
···
exepos(opt,file);
···
总的执行文件执行exepos(opt, file)
函数。
exepos.m
···
if opt.ins.mode==glc.GIMODE_OFF
gnss_processor(rtk,opt,obsr,obsb,nav);
elseif opt.ins.mode==glc.GIMODE_LC||opt.ins.mode==glc.GIMODE_TC
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)
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]);
rtk=initrtk(rtk,opt);
tspan=timespan(rtk,obsr);
if tspan<=0,error('Time span is zero!!!');end
while 1
if ti>tspan,break;end
[obsr_,nobs,obsr]=searchobsr(obsr);
if nobs<0
str=sprintf('Processing... %.1f%%',100);
waitbar(ti/tspan,hbar,str);
break;
end
[obsr_,nobs]=exclude_sat(obsr_,rtk);
if nobs==0,continue;end
if opt.mode>=glc.PMODE_DGNSS&&opt.mode<=glc.PMODE_STATIC
[obsb_,nobs]=searchobsb(obsb,obsr_(1).time);
if nobs~=0,[obsb_,~]=exclude_sat(obsb_,rtk);end
else
obsb_=NaN;
end
[rtk,~]=gnss_solver(rtk,obsr_,obsb_,nav);
if rtk.sol.stat~=0
outsol(rtk);
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
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)
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]);
rtk=initrtk(rtk,opt);
tspan=timespan(rtk,obsr);
if tspan<=0,error('Time span is zero!!!');end
while 1
if ti>tspan,break;end
[obsr_,nobs,obsr]=searchobsr(obsr);
if nobs<0
str=sprintf('Processing... %.1f%%',100);
waitbar(ti/tspan,hbar,str);
break;
end
[obsr_,nobs]=exclude_sat(obsr_,rtk);
if nobs==0,continue;end
if opt.mode>=glc.PMODE_DGNSS&&opt.mode<=glc.PMODE_STATIC
[obsb_,nobs]=searchobsb(obsb,obsr_(1).time);
if nobs~=0,[obsb_,~]=exclude_sat(obsb_,rtk);end
else
obsb_=NaN;
end
[rtk,~]=gnss_solver(rtk,obsr_,obsb_,nav);
if rtk.sol.stat~=0
outsol(rtk);
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
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
[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)
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
[obsr,nobs]=scan_obs_spp(obsr);
if nobs==0,stat0=0;return;end
if ~isempty(strfind(opt.navsys,'C'))
obsr=bds2mp_corr(rtk,obsr);
end
end
[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
rtk.sol.stat=glc.SOLQ_NONE;
if opt.mode>=glc.PMODE_PPP_KINEMA
[obsr,nobs]=scan_obs_ppp(obsr);
if nobs==0,stat0=0;return;end
[rtk,obsr]=clkrepair(rtk,obsr,nav);
[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);
[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)
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;
if rtk.opt.mode~=glc.PMODE_SPP
opt.sateph =glc.EPHOPT_BRDC;
opt.tropopt=glc.IONOOPT_BRDC;
opt.ionoopt=glc.TROPOPT_SAAS;
end
sv=satposs(obs,navs,opt.sateph);
[rtk,sat_,stat0]=estpos(rtk,obs,navs,sv,opt);
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
if stat0~=0
rtk=estvel(rtk,obs,navs,sv,opt,sat_);
end
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
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)
global glc gls
STD_BRDCCLK =30.0; time0=obs(1).time;
nobs=size(obs,1);
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);
[dts,stat1]=ephclk(time,obs(i),nav);
if stat1==0,continue;end
time=timeadd(time,-dts);
[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)
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
[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
[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;
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)
N = (H'*P*H);
x = (N^-1)*H'*P*v;
VAR = N^-1;
return
这段代码实现了最小二乘法的计算过程,用于求解位置和钟差的最优解。
最小二乘法是一种优化方法,它的目标是通过最小化测量残差的平方和来估计参数的值。在本程序中,最小二乘法被用于计算位置和钟差的最优解。
输入参数:
输出参数:
- 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_)
MAXITER=10; iter=1; x=zeros(4,1); rtk.sol.dtrd=0;
while iter<=MAXITER
[v,H,P,nv]=resdop(rtk,obs,nav,sv,sat_,x);
if nv<4,break;end
[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)
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]);
rtk_align=initrtk(rtk,opt);
tspan=timespan(rtk_align,obsr);
if tspan<=0,error('Time span is zero!!!');end
while 1
if ti+1>tspan,break;end
[imud,imu,stat]=searchimu(imu);
if stat==0
str=sprintf('Processing... %.1f%%',100*tspan/tspan);
waitbar(tspan/tspan,hbar,str);
break;
end
[obsr_,nobsr]=matchobs(rtk_align,imud,obsr);
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);
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
[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
ins.time=rtk_gi.sol.time;
rtk_gi=ins2sol(rtk_gi,ins);
outsol(rtk_gi);
plot_trajectory_kine(hfig,rtk_gi);
fprintf('Info:INS initial alignment ok\n');
ins.oldpos=ins.pos;
ins.oldobsr=obsr_;
ins.oldobsb=obsb_;
else
plot_trajectory_kine(hfig,rtk_align);
end
else
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=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;
ins.time=rtk_gi.sol.time;
rtk_gi=ins2sol(rtk_gi,ins);
outsol(rtk_gi);
plot_trajectory_kine(hfig,rtk_gi);
fprintf('Info:INS re-alignment ok\n');
ins.oldpos=ins.pos;
ins.oldobsr=obsr_;
ins.oldobsb=obsb_;
continue;
end
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;
rtk_gi=ins2sol(rtk_gi,ins);
outsol(rtk_gi);
plot_trajectory_kine(hfig,rtk_gi);
ins.oldpos=ins.pos;
ins.oldobsr=obsr_;
ins.oldobsb=obsb_;
continue;
end
ins=ins_mech(ins,imud);
ins=ins_time_updata(ins);
rtk_gi=ins2sol(rtk_gi,ins);
rtk_gi.ins=ins;
if opt.ins.mode==glc.GIMODE_LC
[rtk_gi,rtk_gnss,stat_tmp]=gi_Loose(rtk_gi,rtk_gnss,obsr_,obsb_,nav);
elseif opt.ins.mode==glc.GIMODE_TC
[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
outsol(rtk_gi);
plot_trajectory_kine(hfig,rtk_gi);
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=ins_mech(ins,imud);
ins=ins_time_updata(ins);
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);
outsol(rtk_gi);
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)
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)
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;
rr=rtk_gnss.sol.pos';
[pos_GNSS,Cne]=xyz2blh(rr);
ve=rtk_gnss.sol.vel;
vel_GNSS=Cne*ve';
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
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
pos_INS=ins.pos+ins.Mpv*ins.Cnb*ins.lever;
vel_INS=ins.vel+ins.Cnb*askew(ins.web)*ins.lever;
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
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
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
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
[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;
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)
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
[obsr,nobs]=scan_obs_spp(obsr);
if nobs==0,stat=0;return;end
if ~isempty(strfind(opt.navsys,'C'))
obsr=bds2mp_corr(rtk,obsr);
end
end
if opt.mode>=glc.PMODE_PPP_KINEMA
[obsr,nobs]=scan_obs_ppp(obsr);
if nobs==0,stat=0;return;end
[rtk,obsr]=clkrepair(rtk,obsr,nav);
end
if opt.mode==glc.PMODE_SPP
[rtk,stat]=sppins(rtk,obsr,nav);
elseif opt.mode==glc.PMODE_DGNSS||opt.mode==glc.PMODE_KINEMA
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
[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)
global glc
stat=1; opt=rtk.opt; nobs=size(obs,1); niter=1;
opt.tropopt=glc.IONOOPT_BRDC;
opt.ionoopt=glc.TROPOPT_SAAS;
opt.sateph =glc.EPHOPT_BRDC;
rtk=udstate_sppins(rtk);
sv=satposs(obs,navs,opt.sateph);
xp=rtk.x; Pp=rtk.P; x_fb=zeros(rtk.nx,1);
for i=1:niter
[v1,H1,R1,nv1,sat_]=rescode_sppins(i,obs,navs,sv,xp,Pp,opt,rtk);
if nv1<3,stat=0;break;end
[v2,H2,R2,nv2]=resdop_sppins(obs,navs,sv,sat_,xp,Pp,rtk);
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
[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;
rtk=sppins_feedback(rtk,x_fb);
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进行了紧组合。
udsate_sppins.m(紧组合导航更新状态估计量)
function rtk=udstate_sppins(rtk)
rtk=udpos_sppins(rtk);
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;
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
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);
phi = eye(nx) + F;
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
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)