* reference :
* [1] P.J.G.Teunissen, The least-square ambiguity decorrelation adjustment:
* a method for fast GPS ambiguity estimation, J.Geodesy, Vol.70, 65-82,
* 1995
* [2] X.-W.Chang, X.Yang, T.Zhou, MLAMBDA: A modified LAMBDA method for
* integer least-squares estimation, J.Geodesy, Vol.79, 552-565, 2005
lambda/mlambda 整數最小二乘估計函數：lambda
1 首先對浮點協方差陣進行LD分解：
2 lambda reduction
3 z變換，將雙差模糊度進行變換
4 mlambda search，結果存儲在E和s中（整數解）
5 逆Z變換，求解雙差整周模糊度解，存儲在F中
前言
整周模糊度的求解是GNSS高精度定位中的關鍵性問題，正確快速地固定整周模糊度能使GNSS的定位精度到達厘米甚至毫米級。在GNSS相對定位過程中，通過卡爾曼濾波可以求解得雙差整周模糊度參數的浮點解（即小數解）及其方差-協方差矩陣，而由於整周模糊度具有整數特性，那么不管是單差整周模糊度還是雙差整周模糊度都理應是整數。如何利用整周模糊度參數的浮點解及其方差-協方差矩陣，正確求得整周模糊度參數的固定解（即整數解），就是模糊度固定要解決的問題（這實際上是一個數學問題）。
本篇整理了LAMBDA算法的理論及其在RTKLIB中的代碼部分，由於LAMBDA算法在模糊度固定領域已經非常成熟，博主以往都將其黑箱處理，最近才開始深入學習，若博客中有描述有誤的地方，煩請大佬指正，感激不盡！
參考文獻：
[1] 李征航, 黃勁松. GPS測量與數據處理.第3版[M]. 武漢大學出版社, 2016.
[2] Jonge P D , Tiberius C . The LAMBDA method for integer ambiguity estimation: implementation aspects[J]. No of Lgr Series, 1998.
[3] Chang X W , Yang X , Zhou T . MLAMBDA: a modified LAMBDA method for integer least-squares estimation[J]. Journal of Geodesy, 2005, 79(9):552-565.
一、LAMBDA算法理論
最小二乘模糊度降相關平差法（Leastsquare AMBiguity Decorrelation Adjustment-LAMBDA）理論嚴密，搜索速度快，是一種被廣泛采用的模糊度固定方法。該方法主要由兩部分組成：（1）為降低模糊度參數之間相關性而進行的多維整數變換；（2）在轉換后的空間內進行模糊度搜索，然后再將結果轉換回模糊度空間中，進而求得模糊度整數解。
1、模糊度參數降相關
由於模糊度之間具有相關性，一個模糊度參數的變化會影響其他模糊度的搜索，使得搜索算法的計算量巨大。如果能夠設法降低模糊度參數之間的相關性，使得某一模糊度的變化對其他模糊度的取值的影響盡可能小，就能大大加快模糊度的搜索過程。
LAMBDA算法通過對模糊度參數及其方差-協方差陣進行整數高斯變換（也稱z變換），將他們從原空間變換到新的空間中去，實現模糊度的降相關：
1）使L矩陣中非對角線元素絕對值小於0.5
二、RTKLIB中的lambda()函數
RTKLIB中使用的LAMBDA算法，與第一節中所述的LAMBDA算法有些區別，但他們的核心思想是相同的，配合源碼學習可幫助理解LAMBDA算法（對代碼關鍵部分進行了注釋）。
需要注意的是，RTKLIB中采用lambda算法降相關，mlambda算法搜索。
/* lambda/mlambda integer least-square estimation ------------------------------ * integer least-square estimation. reduction is performed by lambda, * and search by mlambda. 用lambda算法降相關，mlambda算法搜索 * args : int n I number of float parameters * int m I number of fixed solutions * double *a I float parameters (n x 1) * double *Q I covariance matrix of float parameters (n x n) * double *F O fixed solutions (n x m) * double *s O sum of squared residulas of fixed solutions (1 x m) * return : status (0:ok,other:error) * notes : matrix stored by column-major order (fortran convension) *-----------------------------------------------------------------------------*/ 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 對雙差模糊度方差-協方差陣Q進行LDLT分解 */ if (!(info=LD(n,Q,L,D))) { /* lambda reduction lambda降相關 */ reduction(n,L,D,Z); matmul("TN",n,1,n,1.0,Z,a,0.0,z); /* z=Z'*a */ /* mlambda search mlambda搜索 */ if (!(info=search(n,m,L,D,z,E,s))) { /* 將在新空間中固定的模糊度逆變換回雙差模糊度空間中 */ info=solve("T",Z,E,n,m,F); /* F=Z'\E */ } } free(L); free(D); free(Z); free(z); free(E); return info; }
降相關子函數reduction()
/* lambda reduction (z=Z'*a, Qz=Z'*Q*Z=L'*diag(D)*L) (ref.[1]) ---------------*/ static void reduction(int n, double *L, double *D, double *Z) { int i,j,k; double del; j=n-2; k=n-2; //這里的調序變換類似插入排序的思路？ while (j>=0) { //由於第k+1，k+2，...，n-2列都進行過降相關並且沒有被上一次調序變換影響， //因此只需對第0,1，...，k-1，k列進行降相關 if (j<=k) for (i=j+1;i<n;i++) gauss(n,L,Z,i,j);//從最后一列開始，各列非對角線元素從上往下依次降相關 del=D[j]+L[j+1+j*n]*L[j+1+j*n]*D[j+1]; if (del+1E-6<D[j+1]) { /* 檢驗條件，若不滿足檢驗條件則開始進行調序變換 compared considering numerical error */ perm(n,L,D,j,del,Z);//調序變換 k=j; j=n-2;//完成調序變換后重新從最后一列開始進行降相關及排序，k記錄最后一次進行過調序變換的列序號 } else j--; } }
搜索子函數search()
搜索的目標函數為：
r的取值決定了超橢圓搜索區域的大小，若r取小了，最優解落在搜索區域外，無法搜索到最優解；若r取大了，搜索區域過大，搜索效率變低。LAMBDA算法里通過某種方式確定r的取值，后續的所有搜索都在固定半徑的超橢圓區域中進行，這種固定半徑搜索方式的效率依賴於r取值的合理性。
X.W.Chang等提出MLAMBDA算法，此方法在搜索的過程中不斷地縮小搜索區域的半徑r，大多數情況下的搜索效率都要高於LAMBDA的搜索方式。其原理如下：
由理論部分，對於任一模糊度有：
其偽代碼（參考文獻3）及代碼（RTKLIB）如下：
static int search(int n, int m, const double *L, const double *D, const double *zs, double *zn, double *s) { int i,j,k,c,nn=0,imax=0; double newdist,maxdist=1E99,y;//maxdist，當前超橢圓半徑 double *S=zeros(n,n),*dist=mat(n,1),*zb=mat(n,1),*z=mat(n,1),*step=mat(n,1); k=n-1; dist[k]=0.0; //k表示當前層，從最后一層（n-1）開始計算 zb[k]=zs[k];//即zn z[k]=ROUND(zb[k]); y=zb[k]-z[k]; step[k]=SGN(y); //四舍五入取整；取整后的數與未取整的數作差；step記錄z[k]是四舍還是五入 for (c=0;c<LOOPMAX;c++) { newdist=dist[k]+y*y/D[k]; if (newdist<maxdist) { //如果當前累積目標函數計算值小於當前超橢圓半徑 if (k!=0) { //情況1：若還未計算至第一層，繼續計算累積目標函數值 dist[--k]=newdist; //記錄下當前層的累積目標函數值，dist[k]表示了第k,k+1,...,n-1層的目標函數計算和 for (i=0;i<=k;i++) S[k+i*n]=S[k+1+i*n]+(z[k+1]-zb[k+1])*L[k+1+i*n];// zb[k]=zs[k]+S[k+k*n]; //計算Zk，即第k個整數模糊度參數的備選組的中心 z[k]=ROUND(zb[k]); y=zb[k]-z[k]; step[k]=SGN(y); //四舍五入取整；取整后的數與未取整的數作差；記錄是四舍還是五入 } else { //情況2：若已經計算至第一層，意味着所有層的累積目標函數值計算完畢 //nn為當前候選解數，m為我們需要的固定解數，這里為2，表示需要一個最優解及一個次優解 //s記錄候選解的目標函數值，imax記錄之前候選解中的最大目標函數值的坐標 if (nn<m) { //若候選解數還沒滿 if (nn==0||newdist>s[imax]) imax=nn;//若當前解的目標函數值比之前最大的目標函數值都大，那么更新imax使s[imax]指向當前解中具有的最大目標函數值 for (i=0;i<n;i++) zn[i+nn*n]=z[i];//zn存放所有候選解 s[nn++]=newdist;//s記錄當前目標函數值newdist，並加加當前候選解數nn } else { //若候選解數已滿（即當前zn中已經存了2個候選解） if (newdist<s[imax]) { //若 當前解的目標函數值 比 s中的最大目標函數值 小 for (i=0;i<n;i++) zn[i+imax*n]=z[i]; //用當前解替換zn中具有較大目標函數值的解 s[imax]=newdist; //用當前解的目標函數值替換s中的最大目標函數值 for (i=imax=0;i<m;i++) if (s[imax]<s[i]) imax=i; //更新imax保證imax始終指向s中的最大目標函數值 } maxdist=s[imax]; //用當前最大的目標函數值更新超橢圓半徑 } z[0]+=step[0]; y=zb[0]-z[0]; step[0]=-step[0]-SGN(step[0]); //在第一層，取下一個有效的整數模糊度參數進行計算（若zb為5.3，則z取值順序為5,6,4,7，...） } } else { //情況3：如果當前累積目標函數計算值大於當前超橢圓半徑 if (k==n-1) break; //如果當前層為第n-1層，意味着后續目標函數各項的計算都會超出超橢圓半徑，因此終止搜索 else { //若當前層不是第n-1層 k++; //退后一層，即從第k層退到第k+1層 z[k]+=step[k]; y=zb[k]-z[k]; step[k]=-step[k]-SGN(step[k]); //計算退后一層后，當前層的下一個有效備選解 } } } // 對s中的目標函數值及zn中的候選解進行排序（以s中目標函數值為排序標准，進行升序排序） // RTKLIB中最終可以得到一個最優解一個次優解，存在zn中，兩解對應的目標函數值，存在s中 for (i=0;i<m-1;i++) { /* sort by s */ for (j=i+1;j<m;j++) { if (s[i]<s[j]) continue; SWAP(s[i],s[j]); for (k=0;k<n;k++) SWAP(zn[k+i*n],zn[k+j*n]); } } free(S); free(dist); free(zb); free(z); free(step); if (c>=LOOPMAX) { fprintf(stderr,"%s : search loop count overflow\n",__FILE__); return -1; } return 0; }
參考鏈接
GNSS整周模糊度固定算法（LAMBDA算法） (理論推導非常詳細)