文章目录
- 一、改进的模糊度固定算法
-
- [1. 简述模糊度固定算法思路](#1. 简述模糊度固定算法思路)
- [2. 详解 manage_amb_LAMBDA()函数](#2. 详解 manage_amb_LAMBDA()函数)
- [3. 我的思考和疑问](#3. 我的思考和疑问)
一、改进的模糊度固定算法
早些时候在阅读RTKLIB源码时,就对其模糊度固定算法感觉非常恐惧,首先是觉得整数最小二乘降相关是一个非常难的算法,在数学和物理意义上都理解不了。其次就是在RTKLIB中给了太多的选项,各种判断条件、重置条件进一步的增加了阅读和理解的难度。直到慢慢静下心来,结合陈凯凯哥的GINAV 、MATLAB中的LAMBDA、PARLAMBDA 代码包、回归论文,对其有一个感性和理性的认识之后,回头再来看看RTKLIB源码,也发现了一些比较有意思的东西,也产生了一些新的疑问。很久没有更新自己的CSDN了,找个机会记录一下。
给出一些我认为不错的能够帮助理解的资料:
1. 简述模糊度固定算法思路
整周模糊度的固定思路较为简单,在进行玩相对定位的浮点模糊度计算之后,我们可以得到一组双差模糊度的浮点解和他们的方差-协方差矩阵。模糊度固定就是根据该浮点解和对应的方差协方差矩阵进行二次加工,企图通过其协方差矩阵对浮点解进行整数的求解。最简单的模糊度求解方法有四舍五入、直接取整等方法。但这样的方法过于暴力,并没有从全局进行最优的考虑,使用这样方法求解出来的最优解和次优解比较难通过比率校验,且最后表现在位置坐标上的精度并不高,目前一般业界都采用整数最小二乘降相关(LAMBDA)的方式进行求解。
在上面其实简单的提了一下,如何进行模糊度固定,我们是通过其双差模糊度的浮点解和方差-协方差矩阵 进行固定的。我们是否需要固定每个浮点双差模糊度呢?如果全部模糊度固定不成功,我们应该怎么尽可能的获取一个比较好的解呢?这就涉及了第二个问题:部分模糊度固定 。在本篇文章中暂时不介绍这个概念,后序文章会介绍几种部分模糊度固定的方法。
下面我们介绍一下RTKLIB实现的固定方法。
2. 详解 manage_amb_LAMBDA()函数
我们首先来看下函数体,他是怎么实现整个函数的:
c
/* resolve integer ambiguity by LAMBDA using partial fix techniques and multiple attempts -----------------------*/
static int manage_amb_LAMBDA(rtk_t *rtk, double *bias, double *xa, const int *sat, int nf, int ns)
{
int i, f, lockc[NFREQ], ar = 0, excflag = 0, arsats[MAXOBS] = {0};
int gps1 = -1, glo1 = -1, sbas1 = -1, gps2, glo2, sbas2, nb, rerun, dly;
float ratio1, posvar = 0;
/* calc position variance, will skip AR if too high to avoid false fix */
for (i = 0; i < 3; i++)
posvar += rtk->P[i + i * rtk->nx];
posvar /= 3.0; /* maintain compatibility with previous code */
trace(3, "posvar=%.6f\n", posvar);
trace(3, "prevRatios= %.3f %.3f\n", rtk->sol.prev_ratio1, rtk->sol.prev_ratio2);
trace(3, "num ambiguities used last AR: %d\n", rtk->nb_ar);
/* skip AR if don't meet criteria */
if (rtk->opt.mode <= PMODE_DGPS || rtk->opt.modear == ARMODE_OFF ||
rtk->opt.thresar[0] < 1.0 || posvar > rtk->opt.thresar[1])
{
trace(3, "Skip AR\n");
rtk->sol.ratio = 0.0;
rtk->sol.prev_ratio1 = rtk->sol.prev_ratio2 = 0.0;
rtk->nb_ar = 0;
return 0;
}
/* if no fix on previous sample and enough sats, exclude next sat in list */
if (rtk->sol.prev_ratio2 < rtk->sol.thres && rtk->nb_ar >= rtk->opt.mindropsats)
{
/* find and count sats used last time for AR */
for (f = 0; f < nf; f++)
for (i = 0; i < ns; i++)
if (rtk->ssat[sat[i] - 1].vsat[f] && rtk->ssat[sat[i] - 1].lock[f] >= 0 && rtk->ssat[sat[i] - 1].azel[1] >= rtk->opt.elmin)
{
arsats[ar++] = i;
}
if (rtk->excsat < ar)
{
i = sat[arsats[rtk->excsat]];
for (f = 0; f < nf; f++)
{
lockc[f] = rtk->ssat[i - 1].lock[f]; /* save lock count */
/* remove sat from AR long enough to enable hold if stays fixed */
rtk->ssat[i - 1].lock[f] = -rtk->nb_ar;
}
trace(3, "AR: exclude sat %d\n", i);
excflag = 1;
}
else
rtk->excsat = 0; /* exclude none and reset to beginning of list */
}
/* for inital ambiguity resolution attempt, include all enabled sats */
gps1 = 1; /* always enable gps for initial pass */
glo1 = (rtk->opt.navsys & SYS_GLO) ? (((rtk->opt.glomodear == GLO_ARMODE_FIXHOLD) && !rtk->holdamb) ? 0 : 1) : 0;
sbas1 = (rtk->opt.navsys & SYS_GLO) ? glo1 : ((rtk->opt.navsys & SYS_SBS) ? 1 : 0);
/* first attempt to resolve ambiguities */
nb = resamb_LAMBDA(rtk, bias, xa, gps1, glo1, sbas1);
ratio1 = rtk->sol.ratio;
/* reject bad satellites if AR filtering enabled */
if (rtk->opt.arfilter)
{
rerun = 0;
/* if results are much poorer than previous epoch or dropped below ar ratio thresh, remove new sats */
if (nb >= 0 && rtk->sol.prev_ratio2 >= rtk->sol.thres && ((rtk->sol.ratio < rtk->sol.thres) || (rtk->sol.ratio < rtk->opt.thresar[0] * 1.1 && rtk->sol.ratio < rtk->sol.prev_ratio1 / 2.0)))
{
trace(3, "low ratio: check for new sat\n");
dly = 2;
for (i = 0; i < ns; i++)
for (f = 0; f < nf; f++)
{
if (rtk->ssat[sat[i] - 1].fix[f] != 2)
continue;
/* check for new sats */
if (rtk->ssat[sat[i] - 1].lock[f] == 0)
{
trace(3, "remove sat %d:%d lock=%d\n", sat[i], f, rtk->ssat[sat[i] - 1].lock[f]);
rtk->ssat[sat[i] - 1].lock[f] = -rtk->opt.minlock - dly; /* delay use of this sat with stagger */
dly += 2; /* stagger next try of new sats */
rerun = 1;
}
}
}
/* rerun if filter removed any sats */
if (rerun)
{
trace(3, "rerun AR with new sat removed\n");
/* try again with new sats removed */
nb = resamb_LAMBDA(rtk, bias, xa, gps1, glo1, sbas1);
}
}
rtk->sol.prev_ratio1 = ratio1;
/* if fix-and-hold gloarmode enabled, re-run AR with final gps/glo settings if differ from above */
if ((rtk->opt.navsys & SYS_GLO) && rtk->opt.glomodear == GLO_ARMODE_FIXHOLD && rtk->sol.ratio < rtk->sol.thres)
{
glo2 = sbas2 = 0;
/* turn off gpsmode if not enabled and got good fix (used for debug and eval only) */
gps2 = rtk->opt.gpsmodear == 0 && rtk->sol.ratio >= rtk->sol.thres ? 0 : 1;
/* if modes changed since initial AR run or haven't run yet,re-run with new modes */
if (glo1 != glo2 || gps1 != gps2)
nb = resamb_LAMBDA(rtk, bias, xa, gps2, glo2, sbas2);
}
/* restore excluded sat if still no fix or significant increase in ar ratio */
if (excflag && (rtk->sol.ratio < rtk->sol.thres) && (rtk->sol.ratio < (1.5 * rtk->sol.prev_ratio2)))
{
i = sat[arsats[rtk->excsat++]];
for (f = 0; f < nf; f++)
rtk->ssat[i - 1].lock[f] = lockc[f];
trace(3, "AR: restore sat %d\n", i);
}
rtk->sol.prev_ratio1 = ratio1 > 0 ? ratio1 : rtk->sol.ratio;
rtk->sol.prev_ratio2 = rtk->sol.ratio;
return nb;
}
在这一部分,我简单的概括了一下,这个函数主要执行了使用部分模糊度修复和多次尝试固定的方法求解整数模糊度
,我们简单来看一下这个函数的具体操作。
- 计算滤波器的位置误差,从方差矩阵中提取了位置状态的估计误差,通过求解其估计误差判断是否进行模糊度的固定。
误差太大的时候估计的模糊度真的有意义吗? 请读者思考
- 模糊度解算算法准入条件判断。
- 进行排障算法,排障第一步:在上一次不是固定解,且还有多余的双差模糊度可供挑选的时候,进行排障;排障算法主要执行了卫星失能,按照他内部的自增顺序进行卫星的失能,若失能后能够进入固定解,则证明是因为该卫星的双差模糊度导致无法通过比率校验,排除了故障。
- 选择参加模糊度固定的卫星系统,调用LAMBDA算法进行固定。
- AR滤波模式,首先我们需要判断结果是否真的很差,在这里是
rtklibexplorer
给出的判断准则,各位读者可以换成自己的判断准则。如果结果真的很差,则对新的卫星lock=0
的卫星进行延迟参与计算,若此时有多颗新上的卫星,则根据循环顺序对其延迟周期进行自增,避免同一时间多颗新上卫星参与计算。最重要的是,该模式中会重新调用LAMBDA算法进行固定。 - 若对GLO选择了
fixandhold
模式,则进行格洛纳斯系统的计算和固定。 - 进行排障算法,排障第二步:如果排障算法并没有带来ratio的改善
改善准则也是自己定义的
,证明选择错误了卫星,需要对齐进行复原。等待下一个历元对新的卫星进行排障。 - 记录上一历元和本历元的ratio值,以便后续历元进行调用。
3. 我的思考和疑问
搞清楚了整个流程之后,我仍存在部分疑问。
e.g:
- 函数相当于实现了部分模糊度的功能,按照顺序排除了某一颗卫星让系统进入了固定解。在静态场景,卫星周跳、高度角、仰角变化不大的时候是一个好方法。能够排除有固定偏置的卫星让求出的模糊度通过ratio校验。但在动态场景下,受到多径、遮挡、周跳影响,滤波器频繁重置模糊度的情况下,每次能够成功进入模糊度固定的卫星都不同,这样的方式是否起到了反作用呢?本该固定的历元因为排除了某些卫星导致固定不了?或者换句话说,我们既然确定了影响了模糊度固定的卫星,却没有对卫星进行应有的操作,如重置浮点模糊度。
- 在AR滤波模式中,对刚锁定的卫星进行了延期操作,虽然能解决当前历元的模糊度固定,但实质上并没有解决存在浮点偏置的卫星,延迟了
dly
个周期之后该卫星还是会尝试进行固定。在这里是否需要重置模糊度呢? 还是仅需要考虑是因为刚锁定的时候浮点模糊度上尚未收敛呢?
- 代码中的返回值都是
nb
,含义是双差模糊度的个数,通过双差模糊度来判断是否存在不妥呢,特别是多频进行计算的时候,双差模糊度个数远大于参与计算的卫星,在部分场景下是否会引起故障呢?