9.6 *amb(): 模糊度固定
9.6.1 基本概念
1. 概述
利用载波相位测量值进行精密相对定位的根本问题是求解测量值,尤其是双差测量值的整周模糊度。整周模糊度的求解一般分两步完成:首先是通过一定算法求解出整周模糊度,然后再验证所解得的整周模糊度值的正确性。
2. Ratio Test
ratio-test 便是用来验证整周模糊度的正确性的。该值表示为具有第二最佳整数向量的残差与最佳整数向量的平方和的比率。demo5 中的自适应 ratio 方法可以参考 附录 A.9 自适应 Ratio。
3. 模糊度固定模式
RTKLIB 包含的模糊度固定模式主要包括:
- Continuous:持续估计并求解整周模糊度;
- Instantaneous:按历元逐个估计并求解整周模糊度;
- Fix and Hold:持续估计并解算整周模糊度。如果验证通过(满足 hold 条件),则将模糊度约束为符合条件的固定解。
这些模式主要跟是否将当前的结果对滤波器进行约束有关。该部分参考 附录 C.2中的 pos2-armode 选项。
4. LAMBDA 算法固定模糊度
LAMBDA 算法的目的是提高整周模糊度估计和捜索的效率,同时减少计算量。LAMBDA 算法流程如下:
- 建立双差方程,估计整周模糊度浮点解 及其协方差矩阵 ;
- 由 构造转换矩阵 ;
- 采用 整数转换矩阵将 和 转换成 和 ;
- 进行整周模糊度搜索求得 的整数解 及其协方差阵 ;
- 对 和 进行 逆变换得到 的整数解及其协方差矩阵。
9.6.2 manage_amb_LAMBDA():模糊度固定的工程优化(demo5新增)
manage_amb_LAMBDA()
函数是 RTKLIB 中用于通过 LAMBDA 方法解析整数模糊度(integer ambiguity resolution, AR)的静态函数。它结合了部分模糊度固定技术(partial AR)和几种随机性的策略,能优化模糊度解的可靠性和固定率。
1. 参数列表
/* args */
rtk_t *rtk IO rtk 控制结构体
double *bias IO 固定后的双差模糊度
double *xa IO 固定后的双差解
const int *sat I 卫星列表
int nf I 频段数量
int ns I 卫星数量
/* return */
int nb - 固定后的模糊度数量
2. 执行流程
a. 初始化与方差检查:
- 计算位置方差
posvar = (rtk->P[0] + rtk->P[4] + rtk->P[8]) / 3;
; - 满足以下条件则跳过 AR:
- 模式为 DGPS 或 AR 关闭(
ARMODE_OFF
)。 - AR 阈值
thresar[0] < 1.0
或posvar > thresar[1]
。利用
posvar
做判别,主要是为了避免假 Fix;thresar[1]
对应配置选项 pos2-arthres1,表示“Max Pos Var for AR”。
- 模式为 DGPS 或 AR 关闭(
b. AR 策略1:剔除单颗卫星:
- 若当前历元未 Fix 且观测数目足够(
prev_ratio2 < thres && nb_ar >= mindropsats
):- 统计上次的 AR 卫星(满足
vsat
,lock >= 0
,el >= elmin
)。 - 若
excsat < ar
,剔除第excsat
个卫星,锁定计数值设为-nb_ar
(相当于在未来nb_ar
个历元,该卫星都不参加 AR)。 - 否则将不剔除卫星,且
excsat
指向列表的开始位置(excsat = 0
)。
- 统计上次的 AR 卫星(满足
c. 初始 AR 尝试(在步骤 b 的基础上进行了首次 AR 操作,同时为可能的步骤 c 做准备):
- 设置初始系统启用标志:
gps1 = 1
(始终启用 GPS)。glo1 = 1
(若启用 GLO 且非 fix-and-hold 锁定)。sbas1
保持与 GLO 一致或者根据配置决定是否启用 SBAS。
- 调用 resamb_LAMBDA,记录比值
ratio1
和nb
。
d. AR 策略2:新卫星的处理:
- 若启用 arfilter(如果新卫星让结果变差了,则不让它进入 AR 操作):
- 若
nb >= 0
,prev_ratio2 >= thres
,且ratio < thres
或比值显著下降:- 检测新卫星(
lock = 0
),延迟使用(lock = -minlock - dly
),设置rerun = 1
。
- 检测新卫星(
- 若
- 若
rerun
,重运行 resamb_LAMBDA。
e. AR 策略3:针对 GLO 卫星的处理(要求 GLO 卫星采用 fix-and-hold 模式):
- 若启用 GLO fix-and-hold 且
ratio < thres
:- 调整
gps2, glo2, sbas2
(关闭未固定系统)。 - 重新运行 resamb_LAMBDA。
- 调整
f. 恢复剔除卫星:
- 若有剔除且
ratio < thres
且ratio < 1.5 * prev_ratio2
:如果仍没有固定,或 AR ratio 没有显著提升,则恢复之前排除的卫星。
- 恢复第
excsat
个卫星的锁定值,递增excsat
。
- 恢复第
3. 源码注释
点击查看代码
/* 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}; /* 索引、锁定计数、AR 卫星计数、剔除标志、AR 卫星列表 */
int gps1 = -1, glo1 = -1, sbas1 = -1, gps2, glo2, sbas2, nb, rerun, dly; /* 系统启用标志、返回卫星数、重试标志、延迟 */
float ratio1, posvar = 0; /* 初始比值、位置方差 */
/* 计算位置方差,跳过 AR 若过高以避免假固定 */
for (i = 0; i < 3; i++) posvar += rtk->P[i + i * rtk->nx];
posvar /= 3.0; /* 兼容性保持 */
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); /* 调试:输出上次的 AR 卫星数 */
/* 跳过 AR 若不满足条件 */
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 */
/* 如果之前的样本没有 fix 并且卫星数目足够(满足最小剔除要求), 那么就从列表中剔除下一个卫星。 */
if (rtk->sol.prev_ratio2 < rtk->sol.thres && rtk->nb_ar >= rtk->opt.mindropsats) {
/* 统计上次 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]; /* 保存锁定计数 */
rtk->ssat[i - 1].lock[f] = -rtk->nb_ar; /* 剔除卫星,延迟使用 */
}
trace(3, "AR: exclude sat %d\n", i);
excflag = 1;
} else rtk->excsat = 0; /* 重置剔除索引 */
}
/* 初始 AR 尝试,启用所有可用卫星 */
gps1 = 1; /* 始终启用 GPS */
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);
nb = resamb_LAMBDA(rtk, bias, xa, gps1, glo1, sbas1);
ratio1 = rtk->sol.ratio;
/* 剔除不良卫星 */
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;
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; /* 延迟使用 */
dly += 2; /* 交错延迟 */
rerun = 1;
}
}
}
if (rerun) {
trace(3, "rerun AR with new sats removed\n");
nb = resamb_LAMBDA(rtk, bias, xa, gps1, glo1, sbas1);
}
}
rtk->sol.prev_ratio1 = ratio1;
/* 针对 GLO 卫星的处理:针对 fix-and-hold 模式 */
if ((rtk->opt.navsys & SYS_GLO) && rtk->opt.glomodear == GLO_ARMODE_FIXHOLD && rtk->sol.ratio < rtk->sol.thres) {
glo2 = sbas2 = 0;
gps2 = (rtk->opt.gpsmodear == 0 && rtk->sol.ratio >= rtk->sol.thres) ? 0 : 1;
if (glo1 != glo2 || gps1 != gps2)
nb = resamb_LAMBDA(rtk, bias, xa, gps2, glo2, sbas2);
}
/* 恢复剔除卫星 */
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; /* 返回解析的模糊度数目 */
}
9.6.3 resamb_LAMBDA():LAMBDA 算法入口函数
本函数通过 LAMBDA/MLAMBDA 方法固定整周模糊度,简要概括:
- 使用当前历元的浮点解(移动站坐标)重新计算非差残差和后验双差残差(由于坐标更新,残差会相应变化)。
- 将卡尔曼滤波后的状态量和协方差传入 LAMBDA 算法,执行模糊度固定,输出修正后的状态和单差相位偏差。
1. 参数列表
/* args */
rtk_t *rtk I rtk_t 控制结构体,包含状态量、协方差和配置选项
1) rtk->x : 浮点解的状态量,包含所有状态(如位置、速度、加速度、单差相位偏差)
2) rtk->P : rtk->x 对应的协方差矩阵
3) rtk->xa : 使用 LAMBDA 固定模糊度后修正的状态量(整数解),不包含单差相位偏差
4) rtk->Pa : rtk->xa 对应的协方差矩阵
5) nx (rtk->nx): 卡尔曼滤波状态量的总数
6) na (rtk->na): 除单差相位偏差外的状态量个数(通常为 rtk->xa 的维度)
double *bias O 卡尔曼滤波的状态量(浮点解),包含所有状态(如位置、速度、加速度、单差相位偏差)
double *xa O 输出变量,包含两部分:
1) rtk->xa(位置等状态的整数解部分);
2) 通过固定双差整周模糊度计算得到的单差相位偏差。
int gps/glo/sbs I 控制使用哪些 GNSS 系统(GPS、GLONASS、SBAS)
/* return */
int nb - 固定的模糊度数目
2. 执行流程

图9.6-1 resamb_LAMBDA() 执行流程(流程图参考该博客)
a. 初始化:
内部变量的含义:
nb :双差模糊度的个数;
nx :所有状态量的个数;
na :除了单差模糊度之外的所有状态量个数(位置、速度和加速度);
DP :中间矩阵 D*P,D为状态转移矩阵,P为状态协方差矩阵;
y :双差模糊度;
b :存储模糊度最优解与次优解的矩阵;
db :中间矩阵 Qb^-1*(b0-b),模糊度改正数(Qab*db)的一部分;
Qb :双差模糊度的协方差矩阵;
Qab:双差模糊度与其他状态量的协方差矩阵;
QQ :中间矩阵 Qab*Qb^-1,模糊度协方差改正数(Qab*Qb^-1*Qab')的一部分;
s : s[0]为最优双差整周模糊度残差,s[1]为次优双差整周模糊度残差。
- 重置
rtk->sol.ratio
和rtk->nb_ar
。 - 调用
ddidx
生成转换矩阵D
和索引ix
:- 若有效卫星对
nb < minfixsats-1
,返回 -1。
- 若有效卫星对
- 内存分配:
- 分配中间矩阵:
y
,DP
,b
,db
,Qb
,Qab
,QQ
。
- 分配中间矩阵:
b. 双差计算:
y=D*xc, Qb=D*Qc*D', Qab=Qac*D',其中部分字母可以按下面方式理解:
- a:PVA(位置、速度和加速度)状态量
- b:双差相位模糊度
- c:单差相位模糊度
- ':N'=N^T,这里的'表示转置
- 计算双差模糊度 y:
y[i] = x[ix[i*2]] - x[ix[i*2+1]]
。y=D*xc,对应手册式(E.7.15)中的局部运算:N'=D*xc(这里 xc 指的是状态量 x 中的的单差载波相位模糊度)
- 计算双差模糊度协方差 Qb:
Qb=D*Qc*D',对应手册式(E.7.16)中的局部运算: Q_N=D*Qc*D'。
- 计算中间矩阵 DP=D*Qc:
DP[i+j*nb] = P[ix[i*2]+(na+j)*nx] - P[ix[i*2+1]+(na+j)*nx]
。 - 计算 Qb=DP*D':
Qb[i+j*nb] = DP[i+(ix[j*2]-na)*nb] - DP[i+(ix[j*2+1]-na)*nb]
。
- 计算中间矩阵 DP=D*Qc:
- 计算 Qab:
Qab[i+j*na] = P[i+ix[j*2]*nx] - P[i+ix[j*2+1]*nx]
。Qab=Qac*D',对应手册式(E.7.16)中的局部运算:Q_RN=Qac*D'(协方差阵通常包括方差项和协方差项,协方差项做变换与方差项不同,只需要一边乘D或D')。
c. LAMBDA 固定:
- 调用
lambda(nb, 2, y, Qb, b, s)
:- 输出
b[:,0]
(最优解),b[:,1]
(次优解),s[0]
(最优残差),s[1]
(次优残差)。
- 输出
- 计算比率
ratio = s[1]/s[0]
,限制< 999.9
。
d. 比率阈值调整:
该部分可以参考附录 A.9 自适应Ratio。
- 若
thresar[5] ≠ thresar[6]
:- 用多项式拟合调整 thres(基于卫星对数 nb1 ≤ 50 和 thresar[0])。
- 否则
thres = thresar[0]
。
e. 验证与更新:
- 若
s[0] ≤ 0
或s[1]/s[0] ≥ thres
:- 初始化
xa
,Pa
为浮点解。 - 则:
- bias[i] = b[i],y[i] -= b[i](残差)。
- 若
Qb
可逆:- (解残差)。
- (状态更新,式 E.7.19)。
- (协方差更新)。
- 调用
restamb()
将双差转为单差,存入xa
。
- 初始化
3. 注意事项
- “单差转双差以消除接收机初始相位项”的含义:
- 载波相位测量值不仅包含几何距离、模糊度和钟差,还包括接收机硬件引入的初始相位偏差(Initial Phase Bias)。这是由于接收机的本地振荡器(Local Oscillator)在测量载波相位时有一个未知的起始相位。它应该可以被吸收到接收机钟差项中。
- 单差能消除卫星钟差和短基线情形下的大部分大气延时误差,而单差转双差我们更关心的是它能进一步消除掉接收机钟差。
4. 源码注释
点击查看代码
static int resamb_LAMBDA(rtk_t *rtk, double *bias, double *xa, int gps, int glo, int sbs)
{
/*
nb :双差模糊度的个数;
nx :所有状态量的个数;
na :除了单差模糊度之外的所有状态量个数(位置、速度和加速度);
DP :中间变量D*P矩阵,D为状态转移矩阵,P为状态协方差矩阵;
y :双差模糊度;
b :存储模糊度最优解与次优解的矩阵;
db :中间矩阵 Qb^-1*(b0-b),模糊度固定改正数的一部分;
Qb :双差模糊度的协方差矩阵;
Qab:双差模糊度与其他状态量的协方差矩阵;
QQ :中间矩阵 Qab*Qb^-1,模糊度协方差改正数(Qab*Qb^-1*Qab')的一部分;
s : s[0]为最优双差整周模糊度残差,s[1]为次优双差整周模糊度残差。
*/
prcopt_t *opt=&rtk->opt;
int i,j,nb,nb1,info,nx=rtk->nx,na=rtk->na;
double *DP,*y,*b,*db,*Qb,*Qab,*QQ,s[2];
int *ix;
double coeff[3];
trace(3,"resamb_LAMBDA : nx=%d\n",nx);
rtk->sol.ratio=0.0;
rtk->nb_ar=0;
/*
ddidx 创建将 EKF 状态量从单差转到双差的转换矩阵 D
此处存在版本更改,最新版本不再使用 ddmat,而是 ddidx 函数。
*/
ix=imat(nx,2);
if ((nb=ddidx(rtk,ix,gps,glo,sbs))<(rtk->opt.minfixsats-1)) { /* nb is sat pairs */
errmsg(rtk,"not enough valid double-differences\n");
free(ix);
return -1; /* flag abort */
}
rtk->nb_ar=nb;
/* nx=# of float states, na=# of fixed states, nb=# of double-diff phase biases */
y=mat(nb,1); DP=mat(nb,nx-na); b=mat(nb,2); db=mat(nb,1); Qb=mat(nb,nb);
Qab=mat(na,nb); QQ=mat(na,nb);
/* phase-bias covariance (Qb) and real-parameters to bias covariance (Qab) */
/* y=D*xc, Qb=D*Qc*D', Qab=Qac*D' */
/*
a:PVA(位置、速度和加速度)状态量
b:双差相位模糊度
c:单差相位模糊度
':N'=N^T,这里的'表示转置
*/
for (i=0;i<nb;i++) {
// y=D*xc,对应手册式(E.7.15)中的局部运算:N'=D*xc(这里xc指的是状态量的单差载波相位模糊度)
y[i]=rtk->x[ix[i*2]]-rtk->x[ix[i*2+1]];
}
for (j=0;j<nx-na;j++) for (i=0;i<nb;i++) {
// DP=D*Qc,对应手册式(E.7.16)中的局部运算:D*Qc
DP[i+j*nb]=rtk->P[ix[i*2]+(na+j)*nx]-rtk->P[ix[i*2+1]+(na+j)*nx];
}
for (j=0;j<nb;j++) for (i=0;i<nb;i++) {
// Qb=DP*D',对应手册式(E.7.16)中的局部运算:QN=D*Qc*D'
Qb[i+j*nb]=DP[i+(ix[j*2]-na)*nb]-DP[i+(ix[j*2+1]-na)*nb];
}
for (j=0;j<nb;j++) for (i=0;i<na;i++) {
// Qab=Qac*D',对应手册式(E.7.16)中的局部运算:Q_RN=Qac*D'(协方差做变换只需要一边乘D或D')
Qab[i+j*na]=rtk->P[i+ix[j*2]*nx]-rtk->P[i+ix[j*2+1]*nx];
}
#ifdef TRACE
double QQb[MAXSAT];
for (i=0;i<nb;i++) QQb[i]=1000*Qb[i+i*nb];
trace(3,"N(0)= "); tracemat(3,y,1,nb,7,2);
trace(3,"Qb*1000= "); tracemat(3,QQb,1,nb,7,4);
#endif
/* lambda/mlambda integer least-square estimation */
/* return best integer solutions */
/* b are best integer solutions, s are residuals */
/*
调用 lambda 进行模糊度固定,将协方差矩阵进行 LD 分解,
nb 为双差模糊度部分,b 为存储模糊度最优解与次优解的矩阵,
s[0]为最优双差模糊度固定解残差,s[1]为次优双差模糊度固定解残差。
*/
if (!(info=lambda(nb,2,y,Qb,b,s))) {
trace(3,"N(1)= "); tracemat(3,b ,1,nb,7,2);
trace(3,"N(2)= "); tracemat(3,b+nb,1,nb,7,2);
rtk->sol.ratio=s[0]>0?(float)(s[1]/s[0]):0.0f;
if (rtk->sol.ratio>999.9) rtk->sol.ratio=999.9f;
/* adjust AR ratio based on # of sats, unless minAR==maxAR */
if (opt->thresar[5]!=opt->thresar[6]) {
nb1=nb<50?nb:50; /* poly only fitted for upto 50 sat pairs */
/* generate poly coeffs based on nominal AR ratio */
for ((i=0);i<3;i++) {
coeff[i] = ar_poly_coeffs[i][0];
for ((j=1);j<5;j++)
// 这里通过遍历查找表(3*5),以固定Ratio(thresar[0])为基础生成了三个参数
coeff[i] = coeff[i]*opt->thresar[0]+ar_poly_coeffs[i][j];
}
// 通过多项式拟合,以卫星模糊度的数目生成调整后的AR ratio
rtk->sol.thres = coeff[0];
for (i=1;i<3;i++) {
rtk->sol.thres = rtk->sol.thres*1/(nb1+1)+coeff[i];
}
// 确保ratio在最大值和最小值之间
rtk->sol.thres = MIN(MAX(rtk->sol.thres,opt->thresar[5]),opt->thresar[6]);
} else {
// 当 minAR==maxAR 的时候,直接使用固定 Ratio(thresar[0])
rtk->sol.thres=(float)opt->thresar[0];
}
/* validation by popular ratio-test of residuals*/
// 如果 Ratio 值大于阈值,求解固定解 rtk->xa 以及固定解的协方差 rtk->Pa
if (s[0]<=0.0||s[1]/s[0]>=rtk->sol.thres) {
/* init non phase-bias states and covariances with float solution values */
/* transform float to fixed solution (xa=x-Qab*Qb\(b0-b)) */
for (i=0;i<na;i++) {
rtk->xa[i]=rtk->x[i];
for (j=0;j<na;j++) rtk->Pa[i+j*na]=rtk->P[i+j*nx];
}
/* y = differences between float and fixed dd phase-biases
bias = fixed dd phase-biases */
for (i=0;i<nb;i++) {
bias[i]=b[i];
y[i]-=b[i];
}
/* adjust non phase-bias states and covariances using fixed solution values */
if (!matinv(Qb,nb)) { /* returns 0 if inverse successful */
/* rtk->xa = rtk->x-Qab*Qb^-1*(b0-b) */
matmul("NN",nb,1,nb,Qb ,y,db); /* db = Qb^-1*(b0-b) */
matmulm("NN",na,1,nb,Qab,db,rtk->xa); /* rtk->xa = rtk->x-Qab*db */
/* rtk->Pa=rtk->P-Qab*Qb^-1*Qab') */
/* covariance of fixed solution (Qa=Qa-Qab*Qb^-1*Qab') */
matmul("NN",na,nb,nb,Qab,Qb ,QQ); /* QQ = Qab*Qb^-1 */
matmulm("NT",na,na,nb,QQ ,Qab,rtk->Pa); /* rtk->Pa = rtk->P-QQ*Qab' */
trace(3,"resamb : validation ok (nb=%d ratio=%.2f thresh=%.2f s=%.2f/%.2f)\n",
nb,s[0]==0.0?0.0:s[1]/s[0],rtk->sol.thres,s[0],s[1]);
/* translate double diff fixed phase-bias values to single diff
fix phase-bias values, result in xa */
// 调用restamb(),重新存储单差的模糊度
restamb(rtk,bias,nb,xa);
}
else nb=0;
}
else { /* validation failed */
errmsg(rtk,"ambiguity validation failed (nb=%d ratio=%.2f thresh=%.2f s=%.2f/%.2f)\n",
nb,s[1]/s[0],rtk->sol.thres,s[0],s[1]);
nb=0;
}
}
else {
errmsg(rtk,"lambda error (info=%d)\n",info);
nb=0;
}
free(ix);
free(y); free(DP); free(b); free(db); free(Qb); free(Qab); free(QQ);
return nb; /* number of ambiguities */
}
9.6.4 holdamb():Fix and hold 模式下模糊度保持
holdamb()
是 RTKLIB 中用于保持整数模糊度(hold integer ambiguity)的函数,适用于 fix-and-hold 策略。它通过生成伪信息(pseudo-innovations)更新卡尔曼滤波状态。
1. 参数列表
/* args */
rtk_t *rtk IO rtk 控制结构体
const double *xa I 固定解
/* return */
none
2. 执行流程
a. 卫星遍历与约束设置:
- 遍历星座
m
,频段f
和卫星i
:- 条件:系统匹配、
fix[f] = 2
(已固定)、azel[1] ≥ elmaskhold
。 - 记录索引
index[n] = IB(i+1, f, &rtk->opt)
。 - 设置 hold 状态:
rtk->ssat[i].fix[f] = 3
。
- 条件:系统匹配、
- 双差约束:
- 双差残差:
v[nv] = (xa[index[0]] - xa[index[i]]) - (rtk->x[index[0]] - rtk->x[index[i]])
。 - 构建雅可比矩阵 H:
H[index[0] + nv * rtk->nx] = 1.0
:参考卫星。H[index[i] + nv * rtk->nx] = -1.0
:其他卫星。
- 双差残差:
b. 量测更新:
- 若
nv > 0
:- 分配
R
(nv * nv),对角线设为VAR_HOLDAMB
(hold 模糊度方差)。 - 调用
filter()
更新rtk->x, rtk->P
:
- 分配
// demo5 中的代码涉及到了 GLO/SBS 卫星的处理,这里的代码是 RTKLIB 原本的代码
static void holdamb(rtk_t *rtk, const double *xa)
{
double *v,*H,*R;
int i,n,m,f,info,index[MAXSAT],nb=rtk->nx-rtk->na,nv=0,nf=NF(&rtk->opt);
trace(3,"holdamb :\n");
v=mat(nb,1); H=zeros(nb,rtk->nx);
// 循环遍历各个卫星,查找满足条件的卫星,并设置相应标志位 rtk->ssat[i].fix=3
for (m=0;m<5;m++) for (f=0;f<nf;f++) {
for (n=i=0;i<MAXSAT;i++) {
if (!test_sys(rtk->ssat[i].sys,m)||rtk->ssat[i].fix[f]!=2||
rtk->ssat[i].azel[1]<rtk->opt.elmaskhold) {
continue;
}
index[n++]=IB(i+1,f,&rtk->opt);
rtk->ssat[i].fix[f]=3; /* hold */
}
// 计算固定解双差和浮点解双差的差值,形成量测信息,并更新 H 阵
/* constraint to fixed ambiguity */
for (i=1;i<n;i++) {
// rtk->xa 为固定解、rtk->x 为浮点解
v[nv]=(xa[index[0]]-xa[index[i]])-(rtk->x[index[0]]-rtk->x[index[i]]);
H[index[0]+nv*rtk->nx]= 1.0;
H[index[i]+nv*rtk->nx]=-1.0;
nv++;
}
}
// 若观测量数量有效,设置 R 阵,并调用 filter() 量测更新
if (nv>0) {
R=zeros(nv,nv);
for (i=0;i<nv;i++) R[i+i*nv]=VAR_HOLDAMB;
/* update states with constraints */
if ((info=filter(rtk->x,rtk->P,H,v,R,rtk->nx,nv))) {
errmsg(rtk,"filter error (info=%d)\n",info);
}
free(R);
}
free(v); free(H);
}
9.6.5 lambda():lambda 整数最小二乘估计
lambda()
函数是 RTKLIB 中实现整数最小二乘估计(integer least-square estimation)的外部函数,结合 LAMBDA(Least-squares AMBiguity Decorrelation Adjustment)和 MLAMBDA 方法,用于 GNSS 模糊度固定。它通过矩阵分解和搜索优化整数解的可靠性和精度。
LAMBDA 内部包含了很多纯数学函数,如 LD 分解、降相关等,工程上通常只需要知道这些操作的作用即可,因此后文将不会分析这些函数。有关这些算法的更多内容,请参考 附录A.10 模糊度固定原理。
1. 参数列表:
/* args */
int n I 浮点参数数(双差相位偏差数)
int m I 固定解数(通常为 2,表示最优和次优解)
double *a I 浮点参数向量 (n x 1,双差相位偏差)
double *Q I 浮点参数协方差矩阵 (n x n)
double *F O 输出固定解矩阵 (n x m)
double *s O 输出固定解的平方残差和 (1 x m)
/* return */
int status - 0 表示成功,其他表示失败
2. 执行流程:
a. LD 分解:
- 调用
LD()
函数分解 为 :- 若成功 (),继续;否则返回 。
b. LAMBDA 降相关性:
- 调用
reduction()
对 进行降相关处理(采用整数高斯变换,也即 z 变换):- (约简参数)。
- (约简协方差)。
c. MLAMBDA 搜索:
- 调用
search()
在 空间搜索整数解:- 输出 (n x m) 和 (1 x m)。
- 若成功 (),继续;否则返回 。
d. 反变换:
- 调用
solve()
计算 (将解转换回原空间)。
3. 源码注释
点击查看代码
extern int lambda(int n, int m, const double *a, const double *Q, double *F,
double *s)
{
int info;
double *L,*D,*Z,*z,*E;
if (n<=0||m<=0) return -1;
L=zeros(n,n); D=mat(n,1); Z=eye(n); z=mat(n,1); E=mat(n,m);
// 调用LD(),首先对浮点协方差阵进行 LD 分解
/* LD factorization */
if (!(info=LD(n,Q,L,D))) {
// 调用 reduction(),lambda 降相关性
/* lambda reduction */
reduction(n,L,D,Z);
// z 变换,将双差模糊度进行变换
matmul("TN",n,1,n,1.0,Z,a,0.0,z); /* z=Z'*a */
// 调用 search(),mlambda 搜索,结果存储在 E 和 s 中(整数解)
/* mlambda search */
if (!(info=search(n,m,L,D,z,E,s))) {
//逆Z变换,将在新空间中固定的模糊度逆变换回双差模糊度空间中,存储在F中
info=solve("T",Z,E,n,m,F); /* F=Z'\E */
}
}
free(L); free(D); free(Z); free(z); free(E);
return info;
}