写这个程序的解析之前我先说两句题外话。PSINS工具箱在惯导圈子里基本算标配了严恭敏老师把整套捷联惯导算法和组合导航框架用MATLAB写得清清楚楚尤其是test_SINS_GPS开头的这一系列例程几乎每个做组合导航的学生都要跑一遍。test_SINS_GPS_153这个程序我前前后后读过好几遍也在这个基础上改过不知道多少版工程代码。这篇文章不打算逐行翻译源码而是把里面的卡尔曼滤波设置和导航解算主流程掰开揉碎讲清楚——15个状态到底是怎么来的、滤波器参数为什么要这么给、主循环里每一步在做什么、以及你照着改的时候最容易踩到什么坑。不管你是刚接触PSINS的初学者还是已经跑通过例程想改成自己数据的从业者这篇应该都能帮上忙。1. 程序定位与15状态的整体设计思路1.1 文件名里的“153”到底是什么先把这个编号说清楚。test_SINS_GPS_153里的“15”指的就是15维滤波状态“3”一般是程序内部对例程版本的编号——有的版本里你还会见到test_SINS_GPS_9、test_SINS_GPS_12、test_SINS_GPS_18这类程序区别主要在于状态维数和是否估计杆臂、时间同步误差等参数。153这个例程在整个PSINS里属于“松组合标准配置”姿态、速度、位置全反馈器件零偏在线估计量测用的是GPS的位置和速度。它的定位非常明确演示SINS/GPS松组合导航从初始化到滤波解算再到结果评估的完整闭环。你在它的基础上改数据格式、改量测类型、加状态都比从零手写一个组合导航系统要快得多。1.2 15个状态是怎么分配出来的15状态组合导航的模型可以这样理解把惯导系统的误差当成一个线性系统的状态传感器的常值零偏也当成状态去在线估计。具体分配如下第1~3维姿态误差失准角单位弧度记为phi第4~6维速度误差单位m/s记为dv第7~9维位置误差单位m记为dpos第10~12维陀螺零偏三个轴单位rad/s记为eb第13~15维加速度计零偏单位m/s²记为db所以整个滤波器要干的事就是根据惯导解算结果和GPS量测的差值把上面这15个状态估计出来然后把前9个状态反馈回去修正惯导解算把后6个零偏估计出来供后续补偿使用。1.3 为什么要用15状态而不是更少如果只做纯惯导解算加上简单修正9状态只估计姿态、速度、位置误差就够了。但实际工程中陀螺和加速度计必然存在零偏而且零偏会随温度、时间缓慢变化。如果不把它放进状态里去估计零偏的误差会一直激励位置误差滤波结果就会带着“隐形偏差”。把零偏扩展进状态是对“为什么这6个状态非加不可”最直接的回答。代价就是状态维数从9涨到15状态转移矩阵从9×9变成15×15计算量增大一些但对于现代计算机来说完全不是问题。扩展之后的好处非常明显滤波器可以区分哪些误差是导航参数引起的、哪些是器件零偏引起的反馈也更精准。2. 卡尔曼滤波器的初始化与参数设置2.1 状态转移矩阵惯导误差方程的离散化PSINS里卡尔曼滤波器的核心参数都封装在kf这个结构体里。设置状态转移矩阵的时候不是手写一个15维常量矩阵而是基于惯导误差方程实时计算的。这里要理解一个关键点SINS的误差方程是时变的因为姿态、比力、位置一直在变所以F矩阵每一拍都要重新算。在test_SINS_GPS_153里初始化阶段会执行类似这样的代码kf.Phikk_1 eye(15) Ft*ts;这里的Ft是连续时间状态转移矩阵ts是滤波周期通常等于IMU采样周期的整数倍。为什么用一阶近似而不是精确矩阵指数因为IMU周期一般取0.01s或0.005s矩阵范数乘以ts远小于1一阶泰勒展开的精度已经完全够用。这一点很多初学者会纠结其实没必要——你以为要精确计算矩阵指数实际上在10ms量级的步长下一阶近似的误差比传感器噪声低好几个数量级。Ft的具体形式在PSINS里是通过kffk这个函数生成的15×15的矩阵里左上角9×9块是捷联惯导的误差方程右上角和左下角分别是对应零偏到导航误差的耦合项。整个矩阵的物理含义是状态量之间如何互相影响。2.2 量测方程与量测噪声阵Rk松组合的量测非常直接就是GPS给出的位置、速度与惯导解算的位置、速度之差。在15状态模型里量测矩阵Hk要完成一件比较绕的事把15个状态映射到量测量上。假设状态排列是phi(1:3)、dv(4:6)、dpos(7:9)、eb(10:12)、db(13:15)那么位置量测对应第7~9维速度量测对应第4~6维。Hk在程序中通常是分块拼接出来的kf.Hk [zeros(3,3), eye(3), zeros(3,3), zeros(3,6); ... zeros(3,3), zeros(3,3), eye(3), zeros(3,6)];这里第一行对应速度量测第二行对应位置量测。量测噪声阵Rk的设置直接影响滤波器的收敛速度和稳态精度。实际中GPS的水平定位噪声一般在米级速度噪声在0.01~0.1m/s量级所以Rk可以设置为kf.Rk diag([0.1, 0.1, 0.1, 1, 1, 3].^2);速度项取0.1位置项取1~3。如果你用的是RTK或者差分GPS位置噪声可以收到厘米级这里就应该相应调小。很多人在仿真里会把Rk设得非常小觉得越小越好——这是不对的Rk太小会导致滤波器过于相信量测把量测噪声当成真实运动结果就是位置曲线抖动明显。2.3 系统噪声阵Qk的构成逻辑Qk在PSINS里通常以等效噪声方差的形式给出表示状态激励的不确定性。对于惯导GPS组合导航Qk主要来源于陀螺和加速度计的随机游走以及零偏的不稳定性。在test_SINS_GPS_153里Qk的典型设置是通过imuerr里的参数换算过来的。比如陀螺的角度随机游走是0.01°/sqrt(h)换算成噪声方差就要转成rad²/s加速度计的速度随机游走类似。PSINS里有一个常用的套路用imuerrset函数设置器件误差参数然后在滤波初始化里把这些参数转成连续时间噪声数组kf.Qk diag([zeros(1,6), ... imuerr.web(1)^2, imuerr.web(2)^2, imuerr.web(3)^2, ... imuerr.wdb(1)^2, imuerr.wdb(2)^2, imuerr.wdb(3)^2]) * ts;这里前6个零对应导航状态本身没有过程噪声——位置、速度、姿态误差的变化是由器件误差驱动的不是自己随机游走的。但实际中由于未建模误差的存在也有人会把前6维设一个极小量来增强滤波器的适应性。注意Qk要乘以ts因为离散化的噪声方差要和状态转移矩阵的时间步长对齐。2.4 初始方差阵Pk的设置思路Pk反映初始时刻对状态估计不确定度的认识。设置得过大滤波初期会出现较大的超调甚至振荡设置得过小滤波器收敛慢对真实误差的跟踪能力下降。比较合理的做法是参考初始对准和初始定位的精度来给。姿态误差角在初始对准后一般在角分级换算成弧度就是1e-3量级速度误差取决于初始速度给得准不准一般0.1~1m/s位置误差取决于GPS单点定位精度几米到十几米。零偏的不确定度则由器件标称零偏稳定性决定。PSINS里典型写法kf.Pk diag([1e-3, 1e-3, 1e-3, ... % 姿态误差 0.1, 0.1, 0.1, ... % 速度误差 10, 10, 10, ... % 位置误差 1e-5, 1e-5, 1e-5, ... % 陀螺零偏 rad/s 1e-3, 1e-3, 1e-3].^2); % 加计零偏 m/s²这个Pk如果设得太小滤波增益会偏低GPS信息用不充分设得太大一开始的纯惯导误差会放大甚至导致前几步状态跳变剧烈。我的经验是Pk宁大勿小特别是零偏状态给大一些能让滤波器更快“咬住”真实的零偏值。3. 主循环里的导航解算与滤波更新流程3.1 时间同步与数据读取方式打开test_SINS_GPS_153你看到的第一部分通常是加载仿真数据、初始化全局变量和设置IMU采样周期。PSINS的数据都是按行存储每行依次是时间、三轴陀螺角增量或角速度、三轴加速度增量或比力。这里最容易踩的坑是时间同步。GPS数据的更新频率通常比IMU低IMU是100HzGPS是1Hz甚至10Hz。主循环一般写成for k 1:nn:length(imu) ... end其中nn代表一个滤波周期内IMU的帧数。当GPS时间点和IMU时间点不严格对齐时直接拿GPS的位置速度去和惯导解算值做差会产生一个“时间不同步误差”。PSINS的仿真数据里GPS时间通常是严格对齐的但换到你自己的实采数据时一定要先做时间插值或者把GPS时间戳对齐到IMU时间戳上否则滤波结果会出现周期性波动。3.2 惯导机械编排每一拍在解算什么在组合导航主循环里IMU数据要先经过纯惯导的机械编排姿态、速度、位置更新然后才能和GPS量测做差。PSINS里这一步通常调用sins函数或者insupdate函数ins insupdate(ins, imu(k:knn-1, :));姿态更新用的是等效旋转矢量基于双子样或者圆锥补偿算法速度更新要考虑比力积分和重力/哥氏力补偿位置更新是速度的积分。这三步看上去简单但里面涉及坐标系转换、地球自转补偿等细节。PSINS把这些封装得非常好你不需要每行都懂但要清楚一个事实滤波器的预测步其实是在机械编排基础上进行的机械编排错了后面量测更新再准也救不回来。机械编排输出的是姿态、速度、位置三组导航参数这些值包含真实运动信息也包含传感器误差导致的漂移。量测更新要做的事情就是利用GPS信息把这个漂移“拉回来”。3.3 滤波更新与状态反馈的配合当GPS数据到来时程序会构造量测向量zk——通常是惯导解算的速度、位置减去GPS的速度、位置。然后调用kfupdate函数做卡尔曼滤波更新kf kfupdate(kf, zk);在PSINS的153例程里量测更新完成后会把估计出的姿态误差、速度误差、位置误差反馈回惯导解算值把陀螺零偏、加计零偏记录到imuerr里供后续补偿。这个反馈操作是组合导航最关键的环节却也是很多初学者容易搞混的地方。为什么要反馈因为卡尔曼滤波的误差状态模型是线性近似状态估计的误差如果一直累积线性化假设就会失效。反馈的及时性决定了滤波器能否始终工作在小误差范围内。这也是“误差状态卡尔曼滤波”和“全状态卡尔曼滤波”的区别我们不是直接估计位置速度本身而是估计它们和真实值的差。反馈有两种策略一种是每个滤波周期都反馈闭环一种是只在初始阶段反馈开环。PSINS的153例程默认是全程反馈这也是工程上更常用的方案。3.4 结果绘图与精度评估程序跑完之后PSINS会弹出好几张图常见的有insplot画的纯惯导解算结果与参考轨迹对比kfplot画的状态估计曲线以及avpcmpplot画的组合导航结果与参考值之差。这些图不能只看个热闹。我拿到一个仿真结果第一眼先看位置误差曲线是不是收敛在一个常数附近而不是持续发散然后看零偏估计曲线是否稳定、是否收敛到仿真真实值附近。零偏曲线是最能暴露模型错误的地方——如果你把加计零偏的量级搞错了或者把单位搞错了零偏估计曲线要么一直往下飘要么直接发散。4. 常见问题与排查技巧实录4.1 滤波发散先查Qk和Rk的比例组合导航滤波器发散九成以上和Qk、Rk的比例失调有关。Rk给得太大滤波器不信任量测误差消不掉结果就是组合导航输出和纯惯导差不多一直在漂Rk给得太小滤波器过于信任量测位置输出高频抖动姿态角上会出现和GPS噪声同频的毛刺。排查的时候有个很实用的办法把Pk的对角元素打出来看每个状态最后是否收敛到合理范围。如果姿态误差状态的方差明显偏大说明量测对姿态的约束不够这时候优先检查Hk里姿态误差对应的列是否为零。松组合里位置和速度量测对姿态误差的观测性本来就弱姿态误差主要通过速度和位置的耦合间接估计收敛慢是正常的但如果完全不收敛就要看是不是量测更新根本没生效。4.2 初始对准不准后面全白搭153例程里有一个初始对准的过程通常用alignsb或aligni0实现。初始对准的姿态误差直接进入Pk的姿态项初始值也给后续滤波器的收敛带来了负担。如果你发现组合导航开始阶段位置误差曲线出现一个明显的“拱起”再回落多半是初始姿态误差偏大。解决办法是把仿真前几秒的数据先用来做静基座对准或者把Pk的姿态项初始方差调大一些让滤波器有足够的自由度把初始误差拉回来。还有一种情况你把初始位置设置错了。初始位置误差几十米Pk的位置项初始方差却只给了10滤波器会觉得量测和预测的差值是“不可能事件”反而把位置误差状态压得很小结果就是很长时间都拉不回来。所以初始位置一定要给准Pk一定要能覆盖初始误差的范围。4.3 单位错了结果半死不活PSINS里姿态角单位几乎全是弧度陀螺零偏是rad/s加速度计零偏是m/s²。仿真中角度相关参数很容易写成度。比如初始失准角如果是角分级写成1e-3是弧度如果写了1e-3度就会小57倍姿态误差估计出来几乎为零看起来像滤波器失效。遇到这种情况不要急着调参数先把所有输入单位检查一遍。我见过一个同学调了一周滤波发散最后发现是轨迹生成时把经纬度当成了弧度输入。这种问题在仿真里尤其隐蔽因为结果是“看起来合理但精度不对”而不是“明显发散”。4.4 反馈策略选择全程反馈还是分段反馈PSINS默认全程反馈在大多数场景下工作得很好但有一个例外当量测长时间中断时比如GPS信号被遮挡全程反馈的误差状态在量测断档期间会持续累积恢复量测的一瞬间会产生很大的冲击。工程上常见的处理是“分段开环闭环”策略量测正常时闭环反馈量测中断时切换成纯惯导开环解算恢复量测后再重新闭合。实现方式也不算复杂在主循环里判断GPS是否有效无效时只做insupdate而跳过kfupdate等量测恢复后再把误差反馈打开。这个改动在PSINS框架下只需要十几行代码但效果非常明显。5. 把153例程改造成你自己的工程5.1 从仿真数据切换到实采数据跑通153例程只是第一步真正上手项目时你会面对实采数据。实采数据和仿真数据的差别主要在两方面一是时间戳不整齐二是传感器噪声特性不一致。时间戳的问题上面说过解决思路是先把GPS和IMU各自插值到公共时间轴上。PSINS里有imbat、gpsinterp这类工具函数可以用但你要注意插值方式对量测噪声统计特性的影响——插值后的GPS点不是独立的相邻点的噪声会相关这会轻微影响卡尔曼滤波的最优性。传感器噪声特性方面仿真里imuerr是生成数据时给定的滤波器的Qk也按这个真值设置所以滤波器性能近乎最优。实采数据里你是不知道真实噪声特性的Qk只能从器件手册或者Allan方差分析里估计而且实际噪声往往有相关性不是纯白噪声。所以实采数据的滤波结果通常比仿真差一截这是正常现象。5.2 从15状态扩展加杆臂、加时间同步误差153例程里的15状态假设GPS天线和IMU中心重合且GPS时间同步理想。实际工程里这两个假设基本都不成立。杆臂误差几厘米到几十厘米对姿态误差估计和速度量测都有影响时间同步误差几十毫秒在城市驾驶场景下等效于几米的量测误差。扩展做法是在状态向量里增加杆臂3维和时间同步误差1维变成19状态或者在量测构造时补偿掉杆臂带来的速度/位置差异性。PSINS里有对应的inslever和相关例程可以参考。扩展之后状态转移矩阵和量测矩阵都要相应修改Hk里杆臂的分量不是简单的0/1组合需要根据姿态矩阵和杆臂向量推导这一步建议在本子上推清楚再写代码。5.3 基于这个框架做算法验证最后多说一句关于怎么用好这个例程。很多人跑通153之后就开始闷头改自己的算法我建议反过来先基于153的框架把你的改动做成可对比的对照实验。比如你想验证“加上失准角估计对定位精度有多大提升”就在15状态基础上改成18状态或21状态跑同一组数据对比位置误差曲线和最终的零偏估计曲线。PSINS里所有例程的数据生成、参数设置、结果绘图都是模块化的你只要改状态定义和对应的矩阵就能得到非常清晰的对比结果。这种工作方式比我一开始拿到代码就乱改要高效得多也更容易写出有说服力的实验报告。