1. 从代码入手理解GNSS-INS松组合导航的骨架大家好我是老张在自动驾驶和机器人定位领域摸爬滚打了十来年跟各种惯性导航单元IMU和全球卫星导航系统GNSS打交道的日子数都数不过来。今天想和大家聊一个非常经典且实用的技术GNSS-INS松组合导航。如果你正在做无人机、自动驾驶汽车或者任何需要高精度定位的项目这套组合拳你肯定绕不开。理论公式看多了容易懵尤其是卡尔曼滤波那一堆矩阵推导是不是感觉每个字母都认识连起来就不知道在说什么了我当年也是这么过来的。后来我发现最好的学习方法不是死磕论文而是直接看一个优秀的开源实现把公式和代码一行行对应起来。这就好比学做菜看再多菜谱不如站在灶台边看师傅操作一遍。今天我们要拆解的这道“硬菜”就是武汉大学牛小骥教授团队开源的KF-GINS。这个程序在业内口碑很好结构清晰非常适合用来学习松组合导航的卡尔曼滤波到底是怎么在代码里“跑”起来的。简单来说GNSS-INS松组合就是让“短跑冠军”和“长跑健将”搭档。GNSS比如GPS、北斗像是一个每隔一秒或更久才报告一次绝对位置的“考官”它的位置信息长期来看很准没有累积误差但更新慢在楼宇间、隧道里还容易“失联”。而INS惯性导航系统则是一个“蒙眼狂奔者”它通过IMU陀螺仪和加速度计测量角速度和比力自己积分推算位置、速度和姿态。它的优点是数据输出频率极高通常几百赫兹瞬间响应快但缺点是误差会随着时间飞速累积跑一会儿就不知道自己在哪儿了。松组合导航就是让INS自己先跑预测然后每隔一段时间用GNSS给出的绝对位置信息量测去纠正INS跑偏的状态更新。这个“预测-更新”的核心大脑就是卡尔曼滤波。KF-GINS这个程序几乎就是一个标准卡尔曼滤波在GNSS-INS松组合场景下的教科书式实现。接下来我们就化身代码侦探一起钻进KF-GINS的源码里看看卡尔曼滤波是如何从冰冷的公式变成一行行热气腾腾的C代码的。2. 初始化给滤波器一个“起点”万事开头难卡尔曼滤波也不例外。在KF-GINS里一切的开始都在GIEngineProcess::initialization这个函数里。初始化说白了就是告诉滤波器“我们的系统从哪儿开始我们大概有多相信这个开始。”2.1 状态向量与协方差矩阵你的“家底”和“自信度”首先滤波器要管理一个“状态向量”这里面装着所有它要估计的变量。在松组合导航里状态向量通常包括位置3维经度、纬度、高度速度3维北向、东向、天向姿态3维横滚、俯仰、航向IMU误差包括陀螺零偏、加速度计零偏、比例因子误差等通常也是3维一组在KF-GINS的代码里你会在头文件中找到类似P_IDV_IDPHI_ID这样的枚举常量它们就是状态向量中各个分量的索引。初始化时我们需要给这些状态赋一个初值。比如程序可能从GNSS接收到的第一个有效数据中获取初始位置和速度姿态则可能假设初始水平横滚俯仰为0航向由GNSS轨迹粗略估计或直接设为0。但光有初值还不够我们还得告诉滤波器我们对这个初值“有多自信”。这就是初始协方差矩阵P0。它是一个方阵对角线上的元素代表各个状态分量的初始方差不确定度。比如如果你对初始位置的精度很有信心比如来自差分GNSS就可以把位置对应的方差设得很小例如0.01平方米如果你完全不知道初始姿态就把姿态的方差设得很大例如10平方弧度。在代码里你会看到类似Cov_.setIdentity() * 某个值或者更精细地对每个分量单独赋值的操作。这个初始协方差矩阵是滤波器不确定度的起点它会随着滤波过程不断演变。2.2 噪声矩阵配置理解系统的“脾气”卡尔曼滤波要处理两种噪声过程噪声和量测噪声。过程噪声描述了系统模型本身的不精确程度比如IMU的误差模型不完美量测噪声描述了传感器观测的不确定性比如GNSS的定位误差。在KF-GINS中配置这些噪声矩阵是初始化阶段的重头戏。我们重点看过程噪声协方差矩阵Qc_的初始化。IMU的误差比如陀螺仪的零偏并不是一个固定不变的常数它会随着温度、时间缓慢漂移。在KF-GINS里这类误差被建模为一阶高斯-马尔可夫过程。这个模型很实用它用一个相关时间参数来描述误差变化的“惯性”相关时间越长说明误差变化越慢越像常数相关时间越短说明误差变化越快越像白噪声。看看源码中这段关键配置auto imunoise options_.imunoise; Qc_.block(BGSTD_ID, BGSTD_ID, 3, 3) 2 / imunoise.corr_time * imunoise.gyrbias_std.cwiseProduct(imunoise.gyrbias_std).asDiagonal();我来翻译一下gyrbias_std是陀螺零偏的标准差它描述了零偏的波动幅度。corr_time就是相关时间。这个公式2 / corr_time * std^2正是一阶高斯-马尔可夫过程对应的连续时间系统噪声谱密度。代码通过.block操作把这个3x3的噪声矩阵块填充到大的Qc_矩阵中陀螺零偏BG对应的位置。同理对于陀螺仪和加速度计的比例因子误差SG,SA以及加速度计零偏BA也是类似的配置方式。而对于陀螺和加速度计的白噪声角度随机游走ARW和速度随机游走VRW则直接将其方差标准差的平方放在对角线上。这些参数gyr_arw,acc_vrw,gyrbias_std,corr_time等通常来自IMU的数据手册或实验室标定它们是连接物理传感器和滤波模型的桥梁。把这些噪声参数配准是滤波器能否稳定工作的关键一步配大了滤波器反应迟钝配小了容易发散。3. 核心预测步INS如何“蒙眼狂奔”初始化完成后滤波器就进入循环。每当一个新的IMU数据到来就触发一次预测步时间更新。这是INS的主场也是卡尔曼滤波中理论最密集的部分。预测步要做两件事1. 推算系统状态位置、速度、姿态2. 更新状态估计的不确定度协方差矩阵。3.1 状态递推从角速度和比力到位置变化这部分在KF-GINS里属于捷联惯导SINS机械编排虽然不是卡尔曼滤波的直接内容但却是状态预测的基础。简单说就是拿到IMU测量的载体坐标系下的角增量delta_theta和速度增量delta_v通过一系列坐标转换和积分得到位置、速度、姿态的变化。代码里会涉及到大量的坐标系转换从载体坐标系b系到导航坐标系n系通常是当地东北天。姿态更新通常用四元数或旋转矩阵避免欧拉角的奇异性问题。速度更新要补偿哥氏加速度、重力加速度等。位置更新则是速度的积分。这些计算在update或mechanization之类的函数里完成它们为卡尔曼滤波提供了状态的“名义值”或“预测值”。3.2 误差状态与状态转移矩阵F矩阵描述误差如何传播卡尔曼滤波在松组合中通常采用误差状态卡尔曼滤波Error-State KF。我们不直接滤波估计庞大的全状态位置、速度、姿态而是估计它们的误差位置误差、速度误差、姿态误差角。这样做的好处是误差量通常很小线性化更有效而且姿态误差可以用三维向量表示避免了四元数的约束问题。预测步的核心就是推导误差状态是如何随时间变化的即误差状态方程δx_dot F * δx G * w。其中F就是状态转移矩阵或系统矩阵G是噪声驱动矩阵w是系统噪声。F矩阵的推导是组合导航的难点也是KF-GINS源码中最“吓人”的部分。我们来看一段代码比如速度误差关于位置误差的部分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); ... F.block(V_ID, P_ID, 3, 3) temp;这行代码计算的是速度误差索引V_ID对纬度位置误差P_ID的第一个元素的偏导数。它包含了地球自转WGS84_WIE、载体运动速度pvapre_.vel以及地球曲率rmh,rnh分别是子午圈和卯酉圈曲率半径加高度共同影响的复杂结果。这些项都是从导航力学方程线性化得来的。虽然公式复杂但在代码里它就是按照教科书上的推导老老实实地把每一项算出来填到F矩阵的对应位置。F矩阵的其他部分还描述了速度误差如何受速度自身、姿态误差、加速度计零偏等影响姿态误差如何受位置、速度、地球自转、陀螺零偏等影响。IMU的误差零偏、比例因子则按照一阶高斯-马尔可夫模型其导数与自身成负相关-1/corr_time。3.3 协方差预测与离散化不确定度的“发酵”有了连续的误差状态方程我们还需要将其离散化因为计算机是离散处理的。KF-GINS里采用了简单的欧拉法进行离散化Phi.setIdentity(); Phi Phi F * imucur.dt; // Phi 即离散状态转移矩阵 Φ ≈ I F*Δt这里Phi就是离散时间状态转移矩阵Φ。接下来是预测协方差矩阵PP_k|k-1 Φ * P_k-1|k-1 * Φ^T Q_d。其中Q_d是离散时间过程噪声协方差矩阵。它的计算需要考虑噪声驱动矩阵G和连续噪声强度Qc_并进行离散化。KF-GINS使用了一种精度更高的方法Qd G * Qc_ * G.transpose() * imucur.dt; Qd (Phi * Qd * Phi.transpose() Qd) / 2; // 近似计算离散噪声协方差 Q_d最后执行协方差预测Cov_ Phi * Cov_ * Phi.transpose() Qd; // Cov_ 就是 P 矩阵 dx_.setZero(); // 误差状态预测值通常设为零这一步之后系统状态由SINS机械编排更新和其不确定度由协方差矩阵Cov_表示都完成了向前推进一步。滤波器现在有了一个带不确定度的预测状态只等GNSS观测数据来“校正”了。4. 量测更新步GNSS如何“拨乱反正”当GNSS接收机输出一个新的位置信息时量测更新步量测更新就被触发。这是卡尔曼滤波的“修正”环节用外部观测来纠正系统的内部推算。4.1 杆臂补偿与观测新息统一“测量尺”第一个关键点是杆臂补偿。GNSS天线和IMU通常不在同一个物理点上它们之间有一个固定的空间偏移称为杆臂。在比较INS推算的位置和GNSS观测的位置之前必须把INS推算的位置从IMU中心转换到GNSS天线相位中心。antenna_pos pvacur_.pos Dr_inv * pvacur_.att.cbn * options_.antlever;这行代码做了三件事pvacur_.att.cbn * options_.antlever将杆臂矢量在载体坐标系b系下通过姿态旋转矩阵cbn转换到导航坐标系n系。Dr_inv * ...由于位置是大地坐标经纬高而杆臂在n系是东北天米制坐标需要乘以一个从米到弧度的转换矩阵Dr_inv与子午圈、卯酉圈半径有关。最后加到IMU的大地坐标位置上得到天线相位中心的大地坐标。然后计算观测新息Innovationdz也就是GNSS观测值与INS预测值之间的差异dz Dr * (antenna_pos - gnssdata.blh);这里Dr是Dr_inv的逆将大地坐标差转换回米制的东北天坐标。这个dz就是卡尔曼滤波更新的“驱动力”它包含了GNSS观测带来的新信息。4.2 观测矩阵H矩阵建立观测与状态的桥梁我们需要告诉滤波器观测值dz位置差和我们要估计的误差状态δx之间是什么关系。这就是观测方程dz H * δx v其中H是观测矩阵v是观测噪声。对于松组合GNSS直接观测位置所以H矩阵相对简单。位置误差自然直接对应状态向量中的位置误差项H_gnsspos.block(0, P_ID, 3, 3) Eigen::Matrix3d::Identity();但别忘了杆臂由于杆臂的存在天线的位置误差不仅受IMU位置误差影响还受姿态误差影响。因为如果载体姿态有误差那么将杆臂从b系转到n系时就会出错从而导致推算的天线位置出错。这个影响可以通过杆臂矢量与姿态误差角的叉乘来表示H_gnsspos.block(0, PHI_ID, 3, 3) Rotation::skewSymmetric(pvacur_.att.cbn * options_.antlever);这里skewSymmetric函数生成一个反对称矩阵实现的就是叉乘运算。这一项是松组合中非常重要的细节忽略了它在载体有姿态变化特别是转动时滤波器的校正效果会变差。4.3 卡尔曼增益与状态更新完成修正万事俱备只欠更新。这是卡尔曼滤波的标准五步曲中的后三步计算卡尔曼增益KK P * H^T * (H * P * H^T R)^-1。其中R是GNSS位置观测的噪声协方差矩阵通常是一个对角阵对角线元素是东、北、天方向的位置方差。增益K决定了我们是更相信预测INS还是更相信观测GNSS。如果GNSS噪声R很小K就大更新力度强反之则弱。更新误差状态估计δx δx K * (dz - H * δx)。由于预测步中误差状态通常设为零dx_.setZero()所以这里就是δx K * dz。计算出的δx包含了位置、速度、姿态等所有误差的估计值。更新协方差矩阵P (I - K * H) * P。这次更新后系统的不确定度协方差P会减小因为引入了新的观测信息。在KF-GINS的gnssUpdate函数最后你会看到对dx_和Cov_的更新操作。紧接着还有一个反馈校正步骤将估计出的误差状态δx反馈到INS的“名义状态”中修正位置、速度、姿态以及IMU的误差估计如零偏并将误差状态dx_重新置零为下一个滤波周期做准备。这一步确保了滤波器始终围绕一个误差较小的名义状态进行线性化提高了滤波的精度和稳定性。5. 实战经验阅读与调试KF-GINS源码的几点心得看了这么多理论对应代码最后分享几点我通过阅读和实际使用KF-GINS源码积累的经验希望能帮你少走弯路。第一理解程序的数据流是关键。KF-GINS的主循环通常由IMU数据驱动。每来一个IMU数据就做一次预测INS机械编排 误差协方差预测。IMU数据会缓存在一个缓冲区里。当GNSS数据到来时程序会找到与GNSS时间戳对齐的IMU数据然后进行量测更新。理解这个imu_callback和gnss_callback如何协同工作数据如何对齐和时间同步比死磕某一个矩阵公式更重要。第二善用调试工具观察矩阵数值。刚开始看的时候面对F矩阵里一大堆复杂的计算很容易晕。我的方法是在VS Code或CLion里设断点运行程序处理一段真实或仿真数据然后直接查看关键变量如FPhiCov_Kdx_在每一步的数值。看看协方差矩阵Cov_的对角线元素方差是如何随着时间增长预测步和突然减小更新步的。看看卡尔曼增益K的大小感受滤波器对不同状态分量的信任程度。这种直观的感受是看书得不到的。第三重点关注噪声参数配置。前面提到options_.imunoise里的那些参数ARW VRW bias_std corr_time以及GNSS的观测噪声R是滤波器的“调谐旋钮”。如果你发现滤波器结果发散或者过于平滑反应迟钝首先应该检查这些参数是否与你的IMU和GNSS设备匹配。一个实用的方法是用一段静止数据初始化然后让设备静止一段时间观察速度、位置等状态的估计误差是否合理以此来反推和调整噪声参数。第四杆臂和安装偏差角不容忽视。options_.antlever这个杆臂参数一定要测量准确。如果载体安装存在非正交性即IMU的b系与载体的实际坐标系不重合还需要考虑安装偏差角并在姿态转换时进行补偿。KF-GINS的代码框架清晰这些补偿都可以在相应的位置加入。在实际项目中这些物理参数的标定误差往往是限制组合导航精度的主要因素之一而不是算法本身。阅读KF-GINS这样的优质源码就像是在跟一位经验丰富的工程师对话。它把教科书上抽象的公式变成了解决实际工程问题的具体工具。希望这次从代码视角的梳理能帮你打通GNSS-INS松组合导航卡尔曼滤波实现的任督二脉。下次当你自己需要实现或调试一个组合导航滤波器时不妨再打开KF-GINS对照着看看相信你会有新的收获。