1. Toy模板网首页
  2. > Toy博客

【GNSS】RTKLIB 中 LAMBDA 搜索整周模糊度的算法实现

10月前 作者：流浪猪头拯救地球 分类：Toy博客 阅读(53) 违法举报

这篇具有很好参考价值的文章主要介绍了【GNSS】RTKLIB 中 LAMBDA 搜索整周模糊度的算法实现。希望对大家有所帮助。如果存在错误或未考虑完全的地方，请大家不吝赐教，您也可以点击"举报违法"按钮提交疑问。

Part.I Introdction

本篇博文的目的是：对RTKLIB中LAMBDA固定整周模糊度的算法实现做一个尽量详尽的总结。由于笔者水平有限，不当之处还望不吝赐教。

Chap.I 预备知识

LAMBDA 全称 Least-square AMBiguity Decorrelation Adjustment，最小二乘降相关平差。主要分为以下两步：（1）为降低模糊度参数之间相关性而进行的多维整数变换；（2）在转换后的空间内进行模糊度搜索，然后再将结果转换回模糊度空间中，进而求得模糊度整数解。详细的原理可以参看[2]，本文主要介绍 RTKLIB 中有关 LAMBDA 搜索的实现，并附以一个示例进行验证。

Chap.II 内容概览

下面的讲的内容，后来看的时候觉得比较多，就整理了一下，做了个图，比较直观，如下。

【GNSS】RTKLIB 中 LAMBDA 搜索整周模糊度的算法实现
若有错误之处，烦请告知，原图位于 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=Lz​DLzT​
  • 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=Lz​DLzT​
  • 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 一个实例

下面考虑一个三维的情况：
【GNSS】RTKLIB 中 LAMBDA 搜索整周模糊度的算法实现

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 进行了验证

验证代码如下：

%% -------- 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 的灵魂和精髓。
【GNSS】RTKLIB 中 LAMBDA 搜索整周模糊度的算法实现
【GNSS】RTKLIB 中 LAMBDA 搜索整周模糊度的算法实现
下面是用 Matlab 绘制的三维搜索空间的变化（注意坐标轴刻度和椭球形状）
【GNSS】RTKLIB 中 LAMBDA 搜索整周模糊度的算法实现文章来源地址https://www.toymoban.com/news/detail-419405.html

Reference

  1. 基于 Matlab 的方差-协方差矩阵可视化表示（椭圆、椭球）
  2. 【GNSS】LAMBDA 模糊度固定方法
  3. 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.
  4. 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模板网！

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处： 如若内容造成侵权/违法违规/事实不符，请点击违法举报进行投诉反馈，一经查实，立即删除！

分享到：
领支付宝红包 赞助服务器费用

相关文章

  • SimSearch：一个轻量级的springboot项目索引构建工具，实现快速模糊搜索

    大部分项目都会涉及模糊搜索功能，而实现模糊搜索一般分为两个派系： like简约派系 搜索引擎派系 对于较为大型的项目来说，使用Solr、ES或者Milvus之类的引擎是比较流行的选择了（效果只能说优秀），而对于中小型项目，如果考虑这些较为重型的引擎，就意味着开发成本和

    2024年02月02日
    浏览(93)

  • 模糊聚类算法——模糊C均值聚类及matlab实现

    模糊C均值聚类算法（Fuzzy C-Means, FCM）。 1. 算法概述 模糊C均值聚类算法是一种经典的模糊聚类算法，用于无监督学习中的数据聚类问题。它通过为每个数据点分配模糊隶属度，将数据点划分到不同的聚类中心。与传统的硬聚类算法不同，模糊C均值聚类允许数据点同时属于多

    2024年01月24日
    浏览(46)

  • Python实现高斯模糊算法（含完整源码）

    Python实现高斯模糊算法（含完整源码） 在图像处理中，高斯模糊是一种常用的模糊滤镜算法，其主要原理是通过对图像进行卷积操作来减少噪点和细节，从而得到更加平滑的图像效果。在Python语言中，我们可以通过NumPy和OpenCV等第三方库来实现高斯模糊算法。 下面是基于Nu

    2024年02月14日
    浏览(49)

  • 在C ++ OpenCV 和 FFTW 中 实现快速去模糊算法

    在图像处理中，模糊是一个常见的问题，它可能由于各种原因（如运动模糊，焦点模糊等）而产生。幸运的是，有一种称为去模糊的技术，可以帮助我们恢复原始的、清晰的图像。在本文中，我们将介绍如何在C++中使用OpenCV和FFTW库实现快速去模糊算法。 去模糊算法的基本思

    2024年02月13日
    浏览(38)

  • ruby 搜索功能-精准搜索-模糊搜索

    实现搜索功能首先需要在controller 文件中增加 如下的语句，可以精准搜索和模糊搜索，具体看自己需要的情况 假设user项目中存在省份和城市这两列 那么，进行设置省份和城市的精准搜索 同样的。也可以进行模糊搜索，假设存在invited_users 和 tel 这两列 那么进行模糊搜索 然后

    2024年02月12日
    浏览(40)

  • 一起学Elasticsearch系列-模糊搜索

    本文已收录至Github，推荐阅读 👉 Java随想录 微信公众号：Java随想录 在 Elasticsearch 中，模糊搜索是一种近似匹配的搜索方式。它允许找到与搜索词项相似但不完全相等的文档。 前缀匹配通过指定一个前缀值，搜索并匹配索引中指定字段的文档，找出那些以该前缀开头的结果

    2024年02月01日
    浏览(46)
  • vue uniapp 微信小程序 搜索下拉框 模糊搜索

    vue uniapp 微信小程序 搜索下拉框 模糊搜索

    话不多说 直接贴代码 MVVM 就是 Model-View-ViewModel 的缩写，MVVM 将视图和业务逻辑分开。 View：视图层，Model 数据模型，而 ViewModel 是把两者建立通信的桥梁。 在 MVVM 框架下，View 和 Model 之间没有直接的联系，而是通过 ViewModel 进行交互。View 和 ViewModel 之间以及 Model 和 ViewModel 之

    2024年02月09日
    浏览(94)

  • mysql 国密加密字段排序和模糊搜索

    双写 加密字段和明文分别存到两个字段中 , 查询只对明文进行操作 .  (备注: 这种只是应对检查或者设计的方式 , 对于程序没有实际意义) 使用函数 利用mysql已有加解密的函数 , 在排序和模糊搜索之前解密数据 , 再进行排序或者模糊搜索 . (备注: 查询速度受到很大影响 , 不能

    2024年02月05日
    浏览(43)
  • 用ES搜索关键字并且返回模糊字段高亮

    用ES搜索关键字并且返回模糊字段高亮

       一般来说，各个网站，首页的搜索，都会有进行全文搜索的示例，并且把模糊匹配的多个数据进行标记(高亮)，这样便于全局检索关键的数据，便于客户进行浏览。基于此，本文简单介绍这种功能基本java 的 实现    由于公司页面此功能隐藏了，本文就以接口调用返回看具

    2024年02月14日
    浏览(56)
  • C语言实现哈希搜索算法

    C语言实现哈希搜索算法

    哈希搜索，也叫散列查找，是一种通过哈希表（散列表）实现快速查找目标元素的算法。哈希搜索算法通常适用于需要快速查找一组数据中是否存在某个元素的场景，其时间复杂度最高为 O(1)，而平均情况下的时间复杂度通常相当接近 O(1)，因此在实际应用中具有很高的效率和

    2024年02月12日
    浏览(31)

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包，优惠每天领

二维码1

领取红包

二维码2

领红包