Part.I Introdction
本篇博文的目的是：對RTKLIB中LAMBDA固定整周模糊度的算法實現(xiàn)做一個盡量詳盡的總結(jié)。由于筆者水平有限，不當之處還望不吝賜教。
Chap.I 預(yù)備知識
LAMBDA 全稱 Least-square AMBiguity Decorrelation Adjustment，最小二乘降相關(guān)平差。主要分為以下兩步：（1）為降低模糊度參數(shù)之間相關(guān)性而進行的多維整數(shù)變換；（2）在轉(zhuǎn)換后的空間內(nèi)進行模糊度搜索，然后再將結(jié)果轉(zhuǎn)換回模糊度空間中，進而求得模糊度整數(shù)解。詳細的原理可以參看[2]，本文主要介紹 RTKLIB 中有關(guān) LAMBDA 搜索的實現(xiàn)，并附以一個示例進行驗證。
Chap.II 內(nèi)容概覽
下面的講的內(nèi)容，后來看的時候覺得比較多，就整理了一下，做了個圖，比較直觀，如下。
若有錯誤之處，煩請告知，原圖位于 GREAT.drawio/draft
Part.II 代碼詳解
RTKLIB 中的 LAMBDA 實現(xiàn)是在lambda.c文件中的，里面主要的函數(shù)有
- lambda：外部交互接口，相當于主控制函數(shù)
- LD：LTDL 分解，注意是上三角分解
- gauss：整數(shù)高斯變換
- perm：permutation 置換排列，難道是轉(zhuǎn)置？沒看懂
- reduction：求出降相關(guān)矩陣 Z
- search：MLAMBDA （修正的 LAMBDA）搜索
- matmul：降相關(guān)，得到的 Z 和浮點模糊度 a ^ \hat a a^ 和方差-協(xié)方差矩陣 Q a ^ Q_{\hat a} Qa^? 相乘得到變換后的浮點模糊度 z ^ \hat z z^ 及其方差-協(xié)方差矩陣 Q z ^ Q_{\hat z} Qz^?
- solve：變換回去，得到所需要的候選模糊度
下面是幾個主要的函數(shù)傳參
Chap.I lambda
extern int lambda(int n, int m, const double *a, const double *Q, double *F,double *s)
-
n: @param[in]待固定的模糊度個數(shù) -
m: @param[in]待搜索的候選模糊度向量個數(shù) -
a: @param[in]實數(shù)模糊度向量n*1 -
Q: @param[in]方差-協(xié)方差矩陣n*n -
F: @param[out]候選模糊度組m*n -
s: @param[out]整數(shù)模糊度向量與實數(shù)模糊向量的距離（二次方殘差）m*1
RTKLIB 中的 lambda 函數(shù)內(nèi)容如下：
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;
}
各個步驟很清晰的呈現(xiàn)在代碼中，牛的。
Chap.II LD
static int LD(int n, const double *Q, double *L, double *D)
-
@noteQ a ^ a ^ = U D U T Q_{\hat a \hat a}=UDU^T Qa^a^?=UDUT，實際上 U 在函數(shù)中是用 L 表示的 -
n: @param[in]待固定的模糊度個數(shù) -
Q: @param[in]方差-協(xié)方差矩陣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得到整數(shù)高斯變換陣（降相關(guān)陣）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=Lz?DLzT? -
n: @param[in]待固定的模糊度個數(shù) -
L: @param[in/out]對Q分解得到的上三角矩陣n*n，傳出前后會發(fā)生變化 -
D: @param[in/out]對Q分解得到的對角矩陣，只保留了對角線元素1*n，傳出前后會發(fā)生變化 -
Z: @param[out]降相關(guān)矩陣Z陣，n*n，Z的所有元素都是整數(shù)，并且其行列式為1 -
@note：reduction函數(shù)中調(diào)用了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]待固定的模糊度個數(shù) -
m: @param[in]待搜索的候選模糊度向量組個數(shù) -
L: @param[in]對Q分解得到的上三角矩陣n*n，并且經(jīng)過reduction處理 -
D: @param[in]對Q分解得到的對角矩陣，只保留了對角線元素1*n，并且經(jīng)過reduction處理，目前存在如下關(guān)系： 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=Lz?DLzT? -
zs: @param[in]降相關(guān)后的浮點模糊度 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
這兩個函數(shù)實際上是通用函數(shù)，在這里充當矩陣變換的作用。
matmul("TN", n, 1, n, 1.0, Z, a, 0.0, z);
這個函數(shù)的作用是得到降相關(guān)后的浮點模糊度向量（實際上是降相關(guān)矩陣與原浮點模糊度向量的乘積） z ^ = Z a ^ \hat z=Z\hat a z^=Za^
solve("T", Z, E, n, m, F);
這個函數(shù)將搜索得到的 z ˉ \bar z zˉ 的集合 E 轉(zhuǎn)換回去得到 a ˉ \bar a aˉ 的集合 F。
F = Z ? 1 E F=Z^{-1}E F=Z?1E
Part.III 一個實例
下面考慮一個三維的情況：
Chap.I 測試函數(shù)
筆者將RTKLIB有關(guān)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 << "原始方差-協(xié)方差矩陣：" << 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 結(jié)果輸出
程序運行之后有如下輸出：
原始方差-協(xié)方差矩陣：
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 結(jié)果分析 & 驗證
結(jié)合程序輸出，對結(jié)果用 Matlab 進行了驗證
驗證代碼如下：文章來源：http://www.zghlxwxcb.cn/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];
% 原始模糊度方差與相關(guān)系數(shù)，要看的話記得打斷點，后面會覆蓋掉
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 分解正確性檢驗，結(jié)果為0
z_hat=Z2*a_hat; % 變換之后的浮點模糊度
Q_z=Z2*Q*Z2'; % 變換之后的協(xié)方差矩陣
% Z變換后的模糊度方差與相關(guān)系數(shù)
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 函數(shù)之后 L 和 D 變?yōu)?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變換之后正確性檢驗，結(jié)果為0
% 求整數(shù)解和浮點解之間的距離
a_bar=[5 3 4]'; % 最后得到整數(shù)解
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) % 和上面相等
比較有意思的一點是變換前后搜索空間與各模糊度相關(guān)系數(shù)的變化，筆者覺得這才是 LAMBDA 的靈魂和精髓。
下面是用 Matlab 繪制的三維搜索空間的變化（注意坐標軸刻度和橢球形狀）
文章來源地址http://www.zghlxwxcb.cn/news/detail-419405.html
Reference
- 基于 Matlab 的方差-協(xié)方差矩陣可視化表示（橢圓、橢球）
- 【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.
到了這里，關(guān)于【GNSS】RTKLIB 中 LAMBDA 搜索整周模糊度的算法實現(xiàn)的文章就介紹完了。如果您還想了解更多內(nèi)容，請在右上角搜索TOY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！
