GNSS-INS组合导航:KF-GINS(一）

原创

已于 2023-01-31 12:54:38 修改 · 7.4k 阅读

93 ·

CC 4.0 BY-SA版权

文章标签：

#前端 #java #javascript

于 2023-01-15 09:20:04 首次发布

文章详细介绍了KF-ginss开源程序在松组合导航中的实现，涵盖了坐标转换、卡尔曼滤波和系统噪声处理。通过设置系统状态、协方差矩阵和噪声阵，建立状态转移矩阵，并进行了EKF预测。同时，文章讨论了GNSS更新在位置观测矩阵构建中的作用，涉及杆臂补偿等关键技术。

因为毕业设计需求，最近一个月学习了开源程序KF-ginss。

这几篇文章介绍松组合导航是如何实现的，总体大概分为三部分、第一部分介绍常用的坐标系转换、大地测量学基础、角度转化，第二部分介绍组合导航、状态转移矩阵如何实现、怎样进行卡尔曼滤波更新、第三部分介绍捷联惯导部分。

Gi_engine_process.cpp文件

 Cov_.resize(RANK, RANK);
    Qc_.resize(NOISERANK, NOISERANK);
    dx_.resize(RANK, 1);
    Cov_.setZero();
    Qc_.setZero();
    dx_.setZero();

1、设置协方差矩阵，系统噪声阵和系统误差状态矩阵大小

假设有离散线性系统，k 时刻的系统状态Xk受系统噪声序列wk−1 驱动；系统状态方程及量测方
程均为系统状态量的线性方程：

wk 与vk 是互不相关的零均值白噪声序列:

Qk为状态噪声方差阵,Rk为量测噪声方差阵、

 auto imunoise                   = options_.imunoise;
    Qc_.block(ARW_ID, ARW_ID, 3, 3) = imunoise.gyr_arw.cwiseProduct(imunoise.gyr_arw).asDiagonal();
    Qc_.block(VRW_ID, VRW_ID, 3, 3) = imunoise.acc_vrw.cwiseProduct(imunoise.acc_vrw).asDiagonal();
    Qc_.block(BGSTD_ID, BGSTD_ID, 3, 3) =
        2 / imunoise.corr_time * imunoise.gyrbias_std.cwiseProduct(imunoise.gyrbias_std).asDiagonal();
    Qc_.block(BASTD_ID, BASTD_ID, 3, 3) =
        2 / imunoise.corr_time * imunoise.accbias_std.cwiseProduct(imunoise.accbias_std).asDiagonal();
    Qc_.block(SGSTD_ID, SGSTD_ID, 3, 3) =
        2 / imunoise.corr_time * imunoise.gyrscale_std.cwiseProduct(imunoise.gyrscale_std).asDiagonal();
    Qc_.block(SASTD_ID, SASTD_ID, 3, 3) =
        2 / imunoise.corr_time * imunoise.accscale_std.cwiseProduct(imunoise.accscale_std).asDiagonal();

2、初始化系统噪声阵：在噪声阵其实分为六个部分

陀螺和加速度计零偏及比例因子误差均建模为一阶高斯马尔可夫过程

 initialize(options_.initstate, options_.initstate_std);

设置系统状态(位置、速度、姿态和IMU误差)初值和初始协方差，

pvacur_.pos       = initstate.pos;
    pvacur_.vel       = initstate.vel;
    pvacur_.att.

最低0.47元/天解锁文章