写这个程序的解析之前，我先说两句题外话。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是100Hz，GPS是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:k+nn-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里所有例程的数据生成、参数设置、结果绘图都是模块化的，你只要改状态定义和对应的矩阵，就能得到非常清晰的对比结果。这种工作方式比我一开始拿到代码就乱改要高效得多，也更容易写出有说服力的实验报告。
