因为毕业设计需求，最近一个月学习了开源程序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.euler = initstate.euler;pvacur_.att.cbn   = Rotation::euler2matrix(pvacur_.att.euler);pvacur_.att.qbn   = Rotation::euler2quaternion(pvacur_.att.euler);

初始化位置、速度、姿态

imuerror_ = initstate.imuerror;

3、初始化IMU误差（IMU时间、补偿零偏误差、补偿比力因子误差）

因为设计到一些细节，最近没有太多时间整理，后续会进行详细的补充。

4、确定H矩阵（通过扰动误差，确定H阵；由于e系和n系误差扰动不同，对应的H阵也会有相应的变化,此程序采用N系）

位置误差

temp.setZero();temp(0, 0)                = -pvapre_.vel[2] / rmh;temp(0, 2)                = pvapre_.vel[0] / rmh;temp(1, 0)                = pvapre_.vel[1] * tan(pvapre_.pos[0]) / rnh;temp(1, 1)                = -(pvapre_.vel[2] + pvapre_.vel[0] * tan(pvapre_.pos[0])) / rnh;temp(1, 2)                = pvapre_.vel[1] / rnh;F.block(P_ID, P_ID, 3, 3) = temp;F.block(P_ID, V_ID, 3, 3) = Eigen::Matrix3d::Identity();

程序中：

速度误差

 temp.setZero();temp(0, 0) = -2 * pvapre_.vel[1] * WGS84_WIE * cos(pvapre_.pos[0]) / rmh -pow(pvapre_.vel[1], 2) / rmh / rnh / pow(cos(pvapre_.pos[0]), 2);temp(0, 2) = pvapre_.vel[0] * pvapre_.vel[2] / rmh / rmh - pow(pvapre_.vel[1], 2) * tan(pvapre_.pos[0]) / rnh / rnh;temp(1, 0) = 2 * WGS84_WIE * (pvapre_.vel[0] * cos(pvapre_.pos[0]) - pvapre_.vel[2] * sin(pvapre_.pos[0])) / rmh +pvapre_.vel[0] * pvapre_.vel[1] / rmh / rnh / pow(cos(pvapre_.pos[0]), 2);temp(1, 2) = (pvapre_.vel[1] * pvapre_.vel[2] + pvapre_.vel[0] * pvapre_.vel[1] * tan(pvapre_.pos[0])) / rnh / rnh;temp(2, 0) = 2 * WGS84_WIE * pvapre_.vel[1] * sin(pvapre_.pos[0]) / rmh;temp(2, 2) = -pow(pvapre_.vel[1], 2) / rnh / rnh - pow(pvapre_.vel[0], 2) / rmh / rmh +2 * gravity / (sqrt(rmrn[0] * rmrn[1]) + pvapre_.pos[2]);F.block(V_ID, P_ID, 3, 3) = temp;temp.setZero();temp(0, 0)                  = pvapre_.vel[2] / rmh;temp(0, 1)                  = -2 * (WGS84_WIE * sin(pvapre_.pos[0]) + pvapre_.vel[1] * tan(pvapre_.pos[0]) / rnh);temp(0, 2)                  = pvapre_.vel[0] / rmh;temp(1, 0)                  = 2 * WGS84_WIE * sin(pvapre_.pos[0]) + pvapre_.vel[1] * tan(pvapre_.pos[0]) / rnh;temp(1, 1)                  = (pvapre_.vel[2] + pvapre_.vel[0] * tan(pvapre_.pos[0])) / rnh;temp(1, 2)                  = 2 * WGS84_WIE * cos(pvapre_.pos[0]) + pvapre_.vel[1] / rnh;temp(2, 0)                  = -2 * pvapre_.vel[0] / rmh;temp(2, 1)                  = -2 * (WGS84_WIE * cos(pvapre_.pos(0)) + pvapre_.vel[1] / rnh);F.block(V_ID, V_ID, 3, 3)   = temp;F.block(V_ID, PHI_ID, 3, 3) = Rotation::skewSymmetric(pvapre_.att.cbn * accel);F.block(V_ID, BA_ID, 3, 3)  = pvapre_.att.cbn;F.block(V_ID, SA_ID, 3, 3)  = pvapre_.att.cbn * (accel.asDiagonal());

姿态误差

 temp.setZero();temp(0, 0) = -WGS84_WIE * sin(pvapre_.pos[0]) / rmh;temp(0, 2) = pvapre_.vel[1] / rnh / rnh;temp(1, 2) = -pvapre_.vel[0] / rmh / rmh;temp(2, 0) = -WGS84_WIE * cos(pvapre_.pos[0]) / rmh - pvapre_.vel[1] / rmh / rnh / pow(cos(pvapre_.pos[0]), 2);temp(2, 2) = -pvapre_.vel[1] * tan(pvapre_.pos[0]) / rnh / rnh;F.block(PHI_ID, P_ID, 3, 3) = temp;temp.setZero();temp(0, 1)                    = 1 / rnh;temp(1, 0)                    = -1 / rmh;temp(2, 1)                    = -tan(pvapre_.pos[0]) / rnh;F.block(PHI_ID, V_ID, 3, 3)   = temp;F.block(PHI_ID, PHI_ID, 3, 3) = -Rotation::skewSymmetric(wie_n + wen_n);F.block(PHI_ID, BG_ID, 3, 3)  = -pvapre_.att.cbn;F.block(PHI_ID, SG_ID, 3, 3)  = -pvapre_.att.cbn * (omega.asDiagonal());

 F.block(BG_ID, BG_ID, 3, 3) = -1 / options_.imunoise.corr_time * Eigen::Matrix3d::Identity();F.block(BA_ID, BA_ID, 3, 3) = -1 / options_.imunoise.corr_time * Eigen::Matrix3d::Identity();F.block(SG_ID, SG_ID, 3, 3) = -1 / options_.imunoise.corr_time * Eigen::Matrix3d::Identity();F.block(SA_ID, SA_ID, 3, 3) = -1 / options_.imunoise.corr_time * Eigen::Matrix3d::Identity();

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

系统噪声驱动阵

 G.block(V_ID, VRW_ID, 3, 3)    = pvapre_.att.cbn;G.block(PHI_ID, ARW_ID, 3, 3)  = pvapre_.att.cbn;G.block(BG_ID, BGSTD_ID, 3, 3) = Eigen::Matrix3d::Identity();G.block(BA_ID, BASTD_ID, 3, 3) = Eigen::Matrix3d::Identity();G.block(SG_ID, SGSTD_ID, 3, 3) = Eigen::Matrix3d::Identity();G.block(SA_ID, SASTD_ID, 3, 3) = Eigen::Matrix3d::Identity();

状态转移矩阵

 Phi.setIdentity();Phi = Phi + F * imucur.dt;

计算系统传播噪声

 Qd = G * Qc_ * G.transpose() * imucur.dt;Qd = (Phi * Qd * Phi.transpose() + Qd) / 2;

5、EKF预测协方差和系统误差状态

 EKFPredict(Phi, Qd);
}

void GIEngineProcess::gnssUpdate(GNSS &gnssdata) {// IMU位置转到GNSS天线相位中心位置// convert IMU position to GNSS antenna phase center positionEigen::Vector3d antenna_pos;Eigen::Matrix3d Dr, Dr_inv;Dr_inv      = Earth::DRi(pvacur_.pos);Dr          = Earth::DR(pvacur_.pos);antenna_pos = pvacur_.pos + Dr_inv * pvacur_.att.cbn * options_.antlever;

关键技术：杆臂补偿

Eigen::MatrixXd dz;dz = Dr * (antenna_pos - gnssdata.blh);

确定新息，但是在紧张组合中新息将与惯导推算位置、电离层、对流层等有关，相比此时松组合更复杂

构造位置观测矩阵

 Eigen::MatrixXd H_gnsspos;H_gnsspos.resize(3, Cov_.rows());H_gnsspos.setZero();H_gnsspos.block(0, P_ID, 3, 3)   = Eigen::Matrix3d::Identity();H_gnsspos.block(0, PHI_ID, 3, 3) = Rotation::skewSymmetric(pvacur_.att.cbn * options_.antlever);

感谢武汉大学卫星导航定位技术研究中心多源智能导航实验室(i2Nav)牛小骥教授团队开源的KF-GINS软件平台

GNSS-INS组合导航:KF-GINS(一）相关推荐

关于GNSS/INS组合导航开源软件GINav
GINav 理论资料参考<GNSS/INS组合导航软件开发> <组合导航从入门到精通>--1 绪论 <组合导航从入门到精通>--2 高精度GNSS定位模型 < ...
GNSS/INS组合导航实习面试
GNSS/INS组合导航面试美团无人机.云创智行.阿里达摩院.图森蔚来组合导航.来牟创新. 腾讯地图出行事业部.百度地图持续更新文章目录 GNSS/INS组合导航面试 1.GNSS方面的问题模 ...
GNSS/INS 组合导航（一）：定位技术分类与介绍
一.文档学习连接 https://download.csdn.net/download/yongjinfeiba/10761520 一. 定位技术分类 1.1 基于相对测量的定位(航位推算) (1)轮 ...
GNSS/INS组合导航（八）：INS/GPS组合导航
INS/GPS组合导航对比INS与GPS导航方法,二者都有其各自的优缺点. 惯性导航系统INS是一种全自主的导航系统,可以输出超过200Hz的高频信号,并且具有较高的短期测量精度.除了提供位置与速度 ...
GNSS/INS组合导航笔记
文章目录问题1:不可交换误差问题2:划桨运动问题3:关于卡尔曼滤波器效果判断(INS/GPS) 问题4:失准角理解问题5:关于feedback反馈矫正问题6:组合导航精度结果主要依赖于GNS ...
GNSS/INS组合导航（七）：卡尔曼滤波
Kalman滤波导航系统的精度受到惯性传感器初始化以及算法的误差影响.低成本MEMS传感器由于严重的随机误差,INS输出可能迅速漂移.因此低精度的IMU基本上不能作为导航独立传感器进行使用. 在之 ...
GNSS/INS组合导航学习-GINAV（一）
从今天开始整理一下,最近半年学习的组合导航算法目前开源程序 1.严老师PSINS工具箱 (MATLAB--卡尔曼滤波算法) 2.GINAV (MATLAB--卡尔曼滤波算法 ) 3.KF-GINS ...
GNSS/INS组合导航（1）-- 姿态矩阵
对于开始接触惯性导航的人来说,姿态矩阵是必经之路.我在开始学习惯导的过程中,只是用姿态矩阵,但没具体去研究其对应欧拉角旋转方式,最近把自己绕晕了,所以推导完后记录一下自己对欧拉角与旋转矩阵理解,重点针 ...
GNSS/INS组合导航（四）：惯性导航系统
可以结合下面连接看: https://blog.csdn.net/hltt3838/category_10500565.html 惯性导航理论依据: 牛顿第一定律(在不受外力作用下,物体将保持静止或匀 ...
GNSS/INS组合导航（三）：GPS全球定位系统
全球定位系统(GPS)由美国国防部在20世纪70年代开发.GPS的定位基础是24颗卫星组成的网络.每颗卫星发送一个包含伪随机噪声(PRN)码与导航信息的无线电信号.接收机通过PRN码获得无线电信号的传 ...

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