Chapter.12 GNSS定位算法——模糊度固定LAMBDA算法

作者：齊花Guyc(CAUC)

---

一.整周模糊度理論

整周模糊度是全球導航衛星系統高精度定位中的核心概念，指的是載波相位觀測值中未知的整周周期數。GNSS載波相位測量以波長為單位，但由于初始相位未知，觀測值包含一個整數倍的波長和一個小數相位。準確解算整周模糊度并將其固定為整數是實現厘米級定位精度的關鍵。
在LAMBDA算法中，整周模糊度的解算基于雙差載波相位數據，通過最小二乘方法和整數約束來求解。

1.LAMBDA算法干了一件什么事情？

高精度定位需要用到GNSS的載波相位數據，但這些數據里有個未知數——整周模糊度。這個模糊度是載波信號的整周周期數，類似于信號跑了幾圈才到你這里，但它一開始是未知的。
LAMBDA算法就像一個“解謎大師”，專門用來破解這個模糊度，讓它從一個模糊的“小數”（浮點解）變成一個確定的“整數”（整數解）。一旦模糊度被正確固定，定位精度就能從幾米提升到厘米級。

2.LAMBDA算法步驟

LAMBDA算法主要分三個步驟：

1. 去相關
2. 尋找整數解
3. 驗證結果

（1）去相關（Z變換）

GNSS的模糊度數據（浮點解）就像一堆纏在一起的毛線，彼此高度相關，很難直接處理。Z變換把這些毛線解開，變成一堆獨立的線（去相關后的模糊度）。這就像把一個復雜的迷宮變成規則的方格，搜索起來更省力。

setp1：獲取原始數據
假設浮點解為 $[1.23,4.56,?2.78{]}^T$，協方差矩陣為
$$Q_{\hat{a}\hat{a}}=\left[\begin{array}{ccc}1.0& 0.8& 0.3\\ 0.8& 1.5& 0.4\\ 0.3& 0.4& 1.2\end{array}\right]$$
此時手上拿到一堆纏在一起的毛線和一張說明書，說明每根毛線有多粗（誤差大小）和怎么纏在一起（相關性）。

setp2：分析誤差表格
為了造出Z矩陣，先要通過 $L^{T}DL$ 分解把誤差表格拆開，弄清楚模糊度之間的纏法。
$$Q_{\hat{a}\hat{a}}=L^{T}DL$$
比如分解后為：

$$D=\left[\begin{array}{ccc}0.5& 0& 0\\ 0& 0.7& 0\\ 0& 0& 0.6\end{array}\right],L=\left[\begin{array}{ccc}1& 0& 0\\ l_{21}& 1& 0\\ l_{31}& l_{32}& 1\end{array}\right]$$
此時相當于把毛線團拆開，記錄下每根線的長度（D）和怎么交叉纏繞（L），為打造整理機（Z矩陣）做準備。

setp3：構造Z矩陣
使用整數高斯變換構造Z矩陣，通過一系列整數運算，逐步構造Z矩陣，目標是讓 $Q_{\hat{z}\hat{z}}$ 的非對角元素盡量小。
Z變換把高相關的模糊度變成低相關的，搜索空間從細長橢圓變成接近圓形。

---

整數高斯變換步驟

1. 初始化Z矩陣
   一開始，Z矩陣設為單位矩陣。$$Z=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]$$
2. 逐對降低相關性
   檢查矩陣$Q_{\hat{a}\hat{a}}$的非對角元素，找到相關性最大的元素（$Q_{\hat{a}\hat{a}}(1,2)=0.8$）。用整數高斯變換調整Z矩陣，減少這對模糊度的相關性。方法是構造一個整數變換矩陣，比如：
   $$T=\left[\begin{array}{ccc}1& ?k& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]$$
   其中 $k$ 是整數（可以通過四舍五入 $Q_{\hat{a}\hat{a}}(1,2)/Q_{\hat{a}\hat{a}}(2,2)$ 得到）。
   更新Z矩陣：$Z=Z?T$，同時更新逆矩陣：$Z^{?1}=T^{?1}?Z^{?1}$。
3. 迭代優化
   重復檢查非對角元素，找下一個相關性最大的元素，繼續重復步驟2。
   每次調整后，計算新的協方差矩陣。
   $$Q_{\hat{z}\hat{z}}=Z^{?1}{Q}_{\hat{a}\hat{a}}{Z}^{?T}$$
   直到非對角元素足夠小（接近對角矩陣）為止。

---

setp4：變換數據
用Z矩陣變換模糊度和協方差矩陣
用Z矩陣的逆把原始模糊度變成新模糊度：$$\hat{z}=Z^{?1}\hat{a}$$
用Z矩陣把原始協方差變成新協方差：$$Q_{\hat{z}\hat{z}}=Z^{?1}{Q}_{\hat{a}\hat{a}}{Z}^{?T}$$
$$Q_{\hat{z}\hat{z}}=\left[\begin{array}{ccc}0.5& 0.1& 0.05\\ 0.1& 0.7& 0.03\\ 0.05& 0.03& 0.6\end{array}\right]$$
非對角元素的值變小，說明相關性降低。

（2）尋找整數解（整數估計）

在變換后的模糊度中，尋找最優整數解，用Z反變換回到原始模糊度的整數解。

整數估計的方法有三種：

1. 四舍五入
2. 整數自舉法
3. 整數最小二乘（ILS）

四舍五入
沒考慮模糊度間的關系，成功率低；隨便猜，簡單但容易錯。

整數自舉法
從最精確的模糊度開始取整，根據相關性調整其他模糊度，再取整；考慮部分相關性，成功率比四舍五入高。跟著線索一步步找，比瞎猜好，但可能不是最佳位置。

整數最小二乘（ILS）
尋找讓誤差最小的整數解，數學上：
$$\hat{a}=\arg \underset{a\in {\mathbb{Z}}^n}{\min }(\hat{a}?a{)}^T{Q}_{\hat{a}\hat{a}}^{?1}(\hat{a}?a)$$
$\hat{a}$ 是原始模糊度(浮點解)；$a$ 是可能的整數解;$Q_{\hat{a}\hat{a}}$ 是協方差矩陣，記錄模糊度的可靠性和相關性。$(\hat{a}?a{)}^T{Q}_{\hat{a}\hat{a}}^{?1}(\hat{a}?a)$：衡量整數解離浮點解有多遠，考慮了數據的可靠性和相關性。

步驟：

1. 定義搜索空間
   在去相關空間中，找到誤差最小的整數解，目標函數是：
   $$F(z)=(\hat{z}?z{)}^T{Q}_{\hat{z}\hat{z}}^{?1}(\hat{z}?z)\le {\chi }^2$$
   定義了一個超橢球（搜索空間），包含所有足夠接近 $\hat{z}$ 的整數點 $z$；${\chi }^2$ 是閾值，控制橢球大小。
2. 搜索整數解
   在超橢球內，找到讓 $F(z)$ 最小的整數解（最優解），并記錄次優解。
   有兩種搜索方式：枚舉搜索和收縮搜索
   枚舉搜索檢查橢球內所有整數點，計算每個點的 $F(z)$，找出最小和次小值。全面不會漏解，但耗時長。
   收縮搜索動態縮小橢球，優先檢查最可能的整數點。
   初始化搜索空間后，分解$Q_{\hat{z}\hat{z}}$，用Cholesky分解將其分解為$Q_{\hat{z}\hat{z}}=L{L}^T$，令$y=L^{?1}(\hat{z}?z)$，則變換目標函數為$$F(z)=(\hat{z}?z{)}^T{Q}_{\hat{z}\hat{z}}^{?1}(\hat{z}?z)=y^{T}y=\sum _{i=1}^n{y}_i^2$$
   橢球變成：${\sum }_{i=1}^n{y}_i^2\le {\chi }^2$ 更簡單的球形約束。
   假設前 $i?1$ 個整數 $z_1,z_2,\dots ,z_{i?1}$ 已確定，計算第 $i$ 個整數 $z_i$ 的條件范圍：
   $$z_i\in \left[\lfloor {\hat{z}}_{i|i?1}?\frac{\sqrt{{\chi }^2?{\sum }_{j=1}^{i?1}{y}_j^2}}{l_{ii}}\rceil ,\lceil {\hat{z}}_{i|i?1}+\frac{\sqrt{{\chi }^2?{\sum }_{j=1}^{i?1}{y}_j^2}}{l_{ii}}\rceil \right]$$
   ${\hat{z}}_{i|i?1}$：基于前 $i?1$ 個整數的條件估計；$l_{ii}$：$L$ 的對角元素。
   從初始橢球開始，逐維度搜索整數點，找到更小誤差的解，更新 ${\chi }^2$，縮小橢球。

---

復習一下卡方分布${\chi }^2$的概念：
想象在玩投飛鏢游戲，每次投的飛鏢偏離靶心的距離是一個正態分布（隨機但圍繞中心）。如果你投了很多次，把每次偏離距離的平方加起來，這個總和的表現規律就是卡方分布。告訴你，誤差平方的總和大概會落在什么范圍內。
如果有 $k$ 個獨立的標準正態分布變量（均值為0，方差為1），記為 $Z_1,Z_2,\dots ,Z_k$，那么它們的平方和：
$$X=Z_1^2+Z_2^2+?+Z_k^2$$
服從卡方分布，自由度為 $k$，記為 $X～{\chi }^2(k)$，自由度 $k$ 表示有多少個獨立變量，決定了卡方分布的形狀。

在此參考：張鎮虎的博客

自由度 $k$：表示獨立正態變量的個數；顯著性水平 $\alpha$：表示誤差平方和超過 ${\chi }^2$ 的概率，置信水平 $1?\alpha$ 表示包含在 ${\chi }^2$ 內的概率。
例子：自由度 $k=3$，置信水平95%（$\alpha =0.05$），查表得 ${\chi }^2=7.815$，表示95%的概率誤差平方和小于7.815。

---

（3）驗證結果（比率驗證）

用比率檢驗來檢查ILS找到的整數模糊度解是否可靠，確保最佳整數解是正確的，避免選錯導致定位誤差。ILS會輸出一個最優解和一個次優解，比率檢驗通過比較這兩者的誤差大小，判斷最佳解是否明顯優于其他解，從而決定是否接受。

比較這兩個整數解離浮點解有多遠。如果最優解明顯比次優解近很多，就放心取；如果兩者差不多近，就需要保持懷疑，可能選擇保留浮點解。
$$\text{比率}=\frac{F(\hat{a})}{F({\hat{a}}^{\prime })}\le \mu$$
$F(\hat{a})=(\hat{a}?\hat{a}{)}^T{Q}_{\hat{a}\hat{a}}^{?1}(\hat{a}?\hat{a})$: 最優解的加權平方誤差；$F({\hat{a}}^{\prime })=(\hat{a}?{\hat{a}}^{\prime }{)}^T{Q}_{\hat{a}\hat{a}}^{?1}(\hat{a}?{\hat{a}}^{\prime })$: 次優解的加權平方誤差。
$Q_{\hat{a}\hat{a}}$: 原始模糊度的協方差矩陣；
$\mu$: 閾值，決定接受最佳解的嚴格程度，$\mu$的取值可以根據設置可接受的失敗率 $P_f$，比如0.001（0.1%）計算，也可以固定閾值（經驗值）。
如果比率 $\le \mu$，接受最佳解 $\hat{a}$; 否則拒絕，保留浮點解 $\hat{a}$。