Part.I Introdction
本篇博文的目的是:对RTKLIB
中LAMBDA
固定整周模糊度的算法实现做一个尽量详尽的总结。由于笔者水平有限,不当之处还望不吝赐教。
Chap.I 预备知识
LAMBDA 全称 Least-square AMBiguity Decorrelation Adjustment,最小二乘降相关平差。主要分为以下两步:(1)为降低模糊度参数之间相关性而进行的多维整数变换;(2)在转换后的空间内进行模糊度搜索,然后再将结果转换回模糊度空间中,进而求得模糊度整数解。详细的原理可以参看[2],本文主要介绍 RTKLIB 中有关 LAMBDA 搜索的实现,并附以一个示例进行验证。
Chap.II 内容概览
下面的讲的内容,后来看的时候觉得比较多,就整理了一下,做了个图,比较直观,如下。
若有错误之处,烦请告知,原图位于 GREAT.drawio/draft
Part.II 代码详解
RTKLIB 中的 LAMBDA 实现是在lambda.c
文件中的,里面主要的函数有
- lambda:外部交互接口,相当于主控制函数
- LD:LTDL 分解,注意是上三角分解
- gauss:整数高斯变换
- perm:permutation 置换排列,难道是转置?没看懂
- reduction:求出降相关矩阵 Z
- search:MLAMBDA (修正的 LAMBDA)搜索
- matmul:降相关,得到的 Z 和浮点模糊度 a ^ \hat a a^ 和方差-协方差矩阵 Q a ^ Q_{\hat a} Qa^ 相乘得到变换后的浮点模糊度 z ^ \hat z z^ 及其方差-协方差矩阵 Q z ^ Q_{\hat z} Qz^
- solve:变换回去,得到所需要的候选模糊度
下面是几个主要的函数传参
Chap.I lambda
extern int lambda(int n, int m, const double *a, const double *Q, double *F,double *s)
-
n: @param[in]
待固定的模糊度个数 -
m: @param[in]
待搜索的候选模糊度向量个数 -
a: @param[in]
实数模糊度向量n*1
-
Q: @param[in]
方差-协方差矩阵n*n
-
F: @param[out]
候选模糊度组m*n
-
s: @param[out]
整数模糊度向量与实数模糊向量的距离(二次方残差)m*1
RTKLIB 中的 lambda
函数内容如下:
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 factorization */
if (!(info=LD(n,Q,L,D))) {
/* lambda reduction */
reduction(n,L,D,Z);
matmul("TN",n,1,n,1.0,Z,a,0.0,z); /* z=Z'*a */
/* mlambda search */
if (!(info=search(n,m,L,D,z,E,s))) {
// F 搜出来的备选模糊度
info=solve("T",Z,E,n,m,F); /* F=Z'\E */
}
}
free(L); free(D); free(Z); free(z); free(E);
return info;
}
各个步骤很清晰的呈现在代码中,牛的。
Chap.II LD
static int LD(int n, const double *Q, double *L, double *D)
-
@note
Q a ^ a ^ = U D U T Q_{\hat a \hat a}=UDU^T Qa^a^=UDUT,实际上 U 在函数中是用 L 表示的 -
n: @param[in]
待固定的模糊度个数 -
Q: @param[in]
方差-协方差矩阵n*n
-
L: @param[out]
对Q
分解得到的上三角矩阵n*n
,实际上用U
表示更合适,因为是上三角阵 -
D: @param[out]
对Q
分解得到的对角矩阵,只保留了对角线元素1*n
Chap.III reduction
static void reduction(int n, double *L, double *D, double *Z)
-
@note
得到整数高斯变换阵(降相关阵)Z, Q z ^ z ^ = Z Q a ^ a ^ Z T = L z D L z T Q_{\hat z \hat z}=ZQ_{\hat a \hat a}Z^T=L_zDL_z^T Qz^z^=ZQa^a^ZT=LzDLzT -
n: @param[in]
待固定的模糊度个数 -
L: @param[in/out]
对Q
分解得到的上三角矩阵n*n
,传出前后会发生变化 -
D: @param[in/out]
对Q
分解得到的对角矩阵,只保留了对角线元素1*n
,传出前后会发生变化 -
Z: @param[out]
降相关矩阵Z
阵,n*n
,Z
的所有元素都是整数,并且其行列式为1 -
@note
:reduction
函数中调用了gauss
和perm
,对它们的理解还需进一步加强。
Chap.IV search
static int search(int n, int m, const double *L, const double *D, const double *zs, double *zn, double *s)
-
n: @param[in]
待固定的模糊度个数 -
m: @param[in]
待搜索的候选模糊度向量组个数 -
L: @param[in]
对Q
分解得到的上三角矩阵n*n
,并且经过reduction
处理 -
D: @param[in]
对Q
分解得到的对角矩阵,只保留了对角线元素1*n
,并且经过reduction
处理,目前存在如下关系: Q z ^ z ^ = Z Q a ^ a ^ Z T = L z D L z T Q_{\hat z \hat z}=ZQ_{\hat a \hat a}Z^T=L_zDL_z^T Qz^z^=ZQa^a^ZT=LzDLzT -
zs: @param[in]
降相关后的浮点模糊度 z ^ \hat z z^n*1
-
zn: @param[out]
搜索到的候选模糊度向量组m*n
-
s: @param[out]
各候选模糊度向量到浮点模糊度向量的距离m*1
s = ( z ˉ − z ^ ) T Q z ^ z ^ − 1 ( z ˉ − z ^ ) = ( a ˉ − a ^ ) T Q a ^ a ^ − 1 ( a ˉ − a ^ ) s=(\bar z-\hat z)^TQ_{\hat z\hat z}^{-1}(\bar z-\hat z)=(\bar a-\hat a)^TQ_{\hat a\hat a}^{-1}(\bar a-\hat a) s=(zˉ−z^)TQz^z^−1(zˉ−z^)=(aˉ−a^)TQa^a^−1(aˉ−a^)
Chap.V matmul & solve
这两个函数实际上是通用函数,在这里充当矩阵变换的作用。
matmul("TN", n, 1, n, 1.0, Z, a, 0.0, z);
这个函数的作用是得到降相关后的浮点模糊度向量(实际上是降相关矩阵与原浮点模糊度向量的乘积) z ^ = Z a ^ \hat z=Z\hat a z^=Za^
solve("T", Z, E, n, m, F);
这个函数将搜索得到的 z ˉ \bar z zˉ 的集合 E 转换回去得到 a ˉ \bar a aˉ 的集合 F。
F = Z − 1 E F=Z^{-1}E F=Z−1E
Part.III 一个实例
下面考虑一个三维的情况:
Chap.I 测试函数
笔者将RTKLIB
有关LAMBDA
搜索的程序移植到C++程序中,写了如下的测试代码:
void t_gtest::RLAMBDA_test() {
t_glambda2* lambda = new t_glambda2();
int n = 3, m = 7, iN = n, iMaxCan = m;
double a[3] = { 5.45,3.10,2.97 };
double Q[9] = { 6.290,5.978,0.544, 5.978,6.292,2.340, 0.544,2.340,6.288 };
int piA[3] = { 0 };
/*for (int i = 0; i < iN; i++) {
piA[i] = round(pdA[i]);
pdA[i] -= piA[i];
}*/
cout << "原始方差-协方差矩阵:" << endl;
printArr(Q, iN, iN);
cout << "浮点模糊度:" << endl;
printArr(a, 1, iN);
cout << "-----------------------------------" << endl;
double s[8] = { 0 };
double* F = new double[iN * iMaxCan]{ 0 };
int info;
double* L, * D, * Z, * z, * E;
L = lambda->zeros(n, n); D = lambda->mat(n, 1); Z = lambda->eye(n);
z = lambda->mat(n, 1); E = lambda->mat(n, m);
/* LD factorization */
if (!(info = lambda->LD(n, Q, L, D))) {
cout << "-----------------------------------" << endl;
cout << "LD 之后 L 阵:" << endl;
printArr(L, iN, iN);
cout << "LD 之后 D 阵:" << endl;
printArr(D, 1, iN);
/* lambda reduction */
lambda->reduction(n, L, D, Z);
cout << "-----------------------------------" << endl;
cout << "reduction 之后 L 阵:" << endl;
printArr(L, iN, iN);
cout << "reduction 之后 D 阵:" << endl;
printArr(D, 1, iN);
cout << "reduction 之后 Z 阵:" << endl;
printArr(Z, iN, iN);
lambda->matmul("TN", n, 1, n, 1.0, Z, a, 0.0, z); /* z=Z'*a */
cout << "-----------------------------------" << endl;
cout << "matmul 之后 z 阵:" << endl;
printArr(z, 1, iN);
cout << "matmul 之后 a 阵:" << endl;
printArr(a, 1, iN);
cout << "matmul 之后 Z 阵:" << endl;
printArr(Z, iN, iN);
/* mlambda search */
if (!(info = lambda->search(n, m, L, D, z, E, s))) {
cout << "-----------------------------------" << endl;
cout << "search 之后 E 阵:" << endl;
printArr(E, m, n);
cout << "search 之后 s 阵:" << endl;
printArr(s, 1, m);
cout << "search 之后 z 阵:" << endl;
printArr(z, 1, iN);
info = lambda->solve("T", Z, E, n, m, F); /* F=Z'\E */
cout << "-----------------------------------" << endl;
cout << "solve 之后 F 阵:" << endl;
printArr(F, m, n);
cout << "solve 之后 E 阵:" << endl;
printArr(E, m, n);
cout << "solve 之后 Z 阵:" << endl;
printArr(Z, n, n);
}
}
free(L); free(D); free(Z); free(z); free(E); free(F);
if (lambda != NULL)
{
delete lambda;
lambda = NULL;
}
}
Chap.II 结果输出
程序运行之后有如下输出:
原始方差-协方差矩阵:
6.29 5.978 0.544
5.978 6.292 2.34
0.544 2.34 6.288
浮点模糊度:
5.45 3.1 2.97
-----------------------------------
-----------------------------------
LD 之后 L 阵:
1 1.06537 0.086514
0 1 0.372137
0 0 1
LD 之后 D 阵:
0.0898576 5.4212 6.288
-----------------------------------
reduction 之后 L 阵:
1 0.267668 0.367412
0 1 0.13099
0 0 1
reduction 之后 D 阵:
4.31016 1.13526 0.626
reduction 之后 Z 阵:
-2 3 -1
3 -3 1
1 -1 0
-----------------------------------
matmul 之后 z 阵:
-4.57 10.02 2.35
matmul 之后 a 阵:
5.45 3.1 2.97
matmul 之后 Z 阵:
-2 3 -1
3 -3 1
1 -1 0
-----------------------------------
search 之后 E 阵:
-5 10 2
-4 10 2
-6 10 2
-4 10 3
-5 10 3
-3 10 2
-5 9 2
search 之后 s 阵:
0.218331 0.307273 0.59341 0.714614 0.77989 0.860234 1.03198
search 之后 z 阵:
-4.57 10.02 2.35
-----------------------------------
solve 之后 F 阵:
5 3 4
6 4 4
4 2 4
6 3 1
5 2 1
7 5 4
4 2 3
solve 之后 E 阵:
-5 10 2
-4 10 2
-6 10 2
-4 10 3
-5 10 3
-3 10 2
-5 9 2
solve 之后 Z 阵:
-2 3 -1
3 -3 1
1 -1 0
Chap.III 结果分析 & 验证
结合程序输出,对结果用 Matlab 进行了验证
验证代码如下:文章来源:https://www.toymoban.com/news/detail-419405.html
%% -------- RTKLIB 检验 --------
clc;clear
a_hat=[5.45,3.10,2.97]';
Q=[6.290 5.978 0.544;5.978 6.292 2.340;0.544 2.340 6.288];
% 原始模糊度方差与相关系数,要看的话记得打断点,后面会覆盖掉
a_11=sqrt(Q(1,1)); ro_12=1/sqrt(Q(1,1)*Q(2,2)/(Q(1,2)*Q(1,2)));
a_22=sqrt(Q(2,2)); ro_13=1/sqrt(Q(1,1)*Q(3,3)/(Q(1,3)*Q(1,3)));
a_33=sqrt(Q(3,3)); ro_23=1/sqrt(Q(2,2)*Q(3,3)/(Q(2,3)*Q(2,3)));
Z2=[-2 3 -1;3 -3 1;1 -1 0]; det(Z2); % 行列式为 1
U2=[1 1.06537 0.086514; 0 1 0.372137;0 0 1]; % 上三角
D2=diag([0.0898576 5.4212 6.288]);
Q-U2*D2*U2' % UDUT 分解正确性检验,结果为0
z_hat=Z2*a_hat; % 变换之后的浮点模糊度
Q_z=Z2*Q*Z2'; % 变换之后的协方差矩阵
% Z变换后的模糊度方差与相关系数
a_11=sqrt(Q_z(1,1)); ro_12=1/sqrt(Q_z(1,1)*Q_z(2,2)/(Q_z(1,2)*Q_z(1,2)));
a_22=sqrt(Q_z(2,2)); ro_13=1/sqrt(Q_z(1,1)*Q_z(3,3)/(Q_z(1,3)*Q_z(1,3)));
a_33=sqrt(Q_z(3,3)); ro_23=1/sqrt(Q_z(2,2)*Q_z(3,3)/(Q_z(2,3)*Q_z(2,3)));
% RTKLIB reduction 函数之后 L 和 D 变为
L_z=[1 0.267668 0.367412;0 1 0.13099;0 0 1 ];
D_z=diag([4.31016 1.13526 0.626]);
Q_z-L_z*D_z*L_z' % Z变换之后正确性检验,结果为0
% 求整数解和浮点解之间的距离
a_bar=[5 3 4]'; % 最后得到整数解
z_bar=[-5 10 2]';
(z_bar-z_hat)'*inv(Q_z)*(z_bar-z_hat) % RTKLIB 吐出的是它
(a_bar-a_hat)'*inv(Q)*(a_bar-a_hat) % 和上面相等
比较有意思的一点是变换前后搜索空间与各模糊度相关系数的变化,笔者觉得这才是 LAMBDA 的灵魂和精髓。
下面是用 Matlab 绘制的三维搜索空间的变化(注意坐标轴刻度和椭球形状)
文章来源地址https://www.toymoban.com/news/detail-419405.html
Reference
- 基于 Matlab 的方差-协方差矩阵可视化表示(椭圆、椭球)
- 【GNSS】LAMBDA 模糊度固定方法
- Teunnissen P J G. The least-square ambiguity decorrelation adjustment: a method for fast GPS integer ambiguity estimation [J]. J. Geodesy, 1995, 70(1): 65-82.
- De Jonge P, Tiberius C. The LAMBDA method for integer ambiguity estimation: implementation aspects[J]. Publications of the Delft Computing Centre, LGR-Series, 1996, 12(12): 1-47.
到了这里,关于【GNSS】RTKLIB 中 LAMBDA 搜索整周模糊度的算法实现的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!