企业官网建设流程全解析

最近在弄SINS/GPS松组合，很多人第一步就是跑PSINS工具箱的demo。我印象最深的就是这个test_SINS_GPS_153，代码量不大，信息量却非常密集。你单纯把它跑通，看到那几张误差收敛图，其实啥也没学会；真正把里面的卡尔曼滤波设置和导航解算逻辑拆开看一遍，才算入门了组合导航。

这篇文章我就以15状态为例，把这个程序从启动到出图，从状态方程到参数整定，完整过一遍。不是照着help文档念，是站在“我自己要重新写一个松组合滤波器”的角度，告诉你每一行设置背后到底在干什么。

1. 先看清test_SINS_GPS_153的整体骨架与数据流

1.1 从一行状态命令说起——程序的启动与参数初始化

打开test_SINS_GPS_153.m，第一件事是清空工作区、调用glvs初始化全局变量。这行glvs几乎出现在PSINS所有脚本里，它干的事情是把工具箱的路径、常用常量（重力、地球自转角速度、赤道半径等）全部加载进来。新手最容易忽略的是：改过工具箱目录或MATLAB工作目录后，必须先确保glvs能跑到，否则后面全报错，而且错误信息可能让人摸不着头脑。

接下来有一行非常关键的代码：

psinstypedef(153);

这行是在告诉工具箱：“我要用15状态、3个子系统（IMU、GPS、零偏）的滤波结构。”153这个数字不是随便写的——1代表一个IMU，5代表包含GPS量测，3代表状态维数的分类方式。PSINS里面还有152、188等类型，153是最经典的一组，正好覆盖位置误差、速度误差、姿态误差、陀螺零偏、加计零偏这五组状态。

然后是trjsim生成轨迹：

trj = trjsim('sinsgps_trj.mat');

这行代码很多人不理解，以为就是个加载函数。其实trjsim是PSINS里的轨迹仿真器，它用运动学约束生成一段包含姿态、速度、位置的理想轨迹，同时仿真出IMU原始角速度/加速度数据，以及GPS输出的位置/速度数据。对于学习阶段来说，这是最理想的输入——所有真值已知，你后面算出来的误差可以和真值对，验证滤波器有没有做对。

我建议第一次跑的时候，把这段轨迹的时间、IMU数据率（默认100Hz）、GPS数据率（默认1Hz）都打出来看一眼。搞清楚“原始数据是什么频率、量测数据是什么频率”，对理解后面的滤波更新节奏非常关键——卡尔曼滤波的数据率不匹配，是初学阶段最容易犯的错，但PSINS在这套demo里已经帮你处理好了。

1.2 数据流里藏着组合导航的“骨架”

整个程序的核心数据流，拆开看就三步：

1. IMU数据驱动惯性解算：把陀螺和加速度计输出积分，算出位置/速度/姿态，这一步以IMU频率（一般是100Hz）执行。
2. 量测数据驱动滤波修正：GPS输出位置/速度时，把它和惯导推算出来的位置/速度做差，作为卡尔曼滤波的“新息”，去估计误差状态。
3. 误差状态反馈修正：滤波器估计出的姿态误差、速度误差、位置误差、陀螺零偏、加计零偏，反馈回惯导解算结果中，把偏差“拉”回来。

这个数据流是松组合的通用架构。PSINS的demo写得好，好就好在把这三步压缩进了很短的代码里，但你如果真的只看不拆，很容易以为组合导航就是“调一下卡尔曼参数”的事。实际上，从IMU机械编排开始，到误差反馈的时机，每一环都会影响最终精度。

程序里控制循环的核心代码大致是：

for k=1:nn [imu, avp] = imuadderr(imu, imuerr); % 给IMU数据加常值零偏和噪声 [xx, kf] = kfupdate(kf, [pos; vn], 'M'); % 量测更新，用的是位置+速度 ... end

我插一句：这里用[pos; vn]作为量测量，其实就是松组合最经典的“位置+速度”量测方案。如果你想改成“只位置”或“位置+速度+姿态”，整个滤波器结构都要变，不是简单删掉一行的事。这一点在后面的量测矩阵部分还会细说。

2. 15状态卡尔曼滤波：一个状态方程逐项拆解

2.1 三个位置误差 + 三个速度误差 + 三个姿态角误差

PSINS里的15状态，定义上通常写成：

X = [phi; dvn; dpos; eb; db]

也就是三部分核心导航参数误差加两部分器件误差。第一组phi是姿态角误差（平台失准角），方向通常在各坐标系（姿态阵对应的坐标轴）上分解；第二组dvn是速度误差，通常定义在导航坐标系（东北天）三轴上；第三组dpos是位置误差，包含纬度误差、经度误差、高度误差，单位是弧度（米），注意PSINS内部在处理位置量测时会把经纬度误差转换成米，这里有个隐含的单位换算逻辑，后面参数设置还会提到。

为什么用“误差”作为状态，而不是直接用位置、速度、姿态作为状态？这是惯导组合滤波和常规卡尔曼滤波最大的区别。惯导解算本身是一个递推积分过程，它的“绝对状态”一直在变，直接估计绝对状态很难建立线性模型；而惯导误差的传播规律是线性化的，可以用一组线性微分方程近似描述。所以PSINS里线性卡尔曼滤波器的状态，本质上都是误差状态，这一点理解透了，看代码就不会晕。

这三组状态的物理含义，可以这么记：

• 姿态误差，决定了惯导系统“指的方向歪了多少”，它误差大，速度/位置误差很快就会被“带偏”。
• 速度误差，可以直观认为是东北天三轴的速度偏差。
• 位置误差，就是纬度、经度、高度偏差。

2.2 陀螺零偏与加计零偏：误差模型里的“隐藏变量”

如果只做纯惯性解算，陀螺零偏和加计零偏是没法直接观测的，它们只是缓慢影响姿态/速度/位置的“罪魁祸首”。但组合导航最大的优势就是可以用GPS的位置/速度量测，间接把这些零偏“估计”出来。

15状态里的后6个状态，分别是3个陀螺零偏（eb）和3个加速度计零偏（db）。在PSINS的建模中，陀螺零偏和加计零偏通常被建模为随机常数（或一阶马尔可夫过程）。随机常数意味着它在滤波器看来是一个“不随时间变化但未知的量”，滤波器的任务就是通过量测新息，一点点逼近它的真值。

初学的时候容易纠结一个问题：IMU出厂标定不就给了零偏吗？为什么还要滤波去估？因为出厂标定给的是常温下的常值零偏，实际使用中温度变化、安装应力、上电随机性都会让零偏发生变化。组合导航系统把零偏当状态去实时估计，好处非常多：

• 姿态解算时可以对陀螺输出做在线补偿；
• 从滤波估计出的零偏值，还能反过来评估IMU器件质量；
• 在GPS信号中断的短暂时间段内，估计好的零偏可以让纯惯性解算维持更高的精度。

所以千万别小看这几个“隐藏变量”，组合导航能不能扛得住卫星信号短时中断，很大程度就看零偏估得准不准。

2.3 系统方程与状态转移矩阵是怎么填出来的

PSINS的kfinit里面，状态转移矩阵Ft不是预先算好的常数矩阵，而是根据当前的姿态阵Cnb和比力值实时计算出来的。15状态模型下，姿态误差方程、速度误差方程、位置误差方程对状态量的偏导数，共同构成Ft。

举个最直观的例子，姿态误差方程里有一项叫“陀螺零偏耦合项”——陀螺零偏会直接通过姿态误差方程作用在姿态误差变化率上。而速度误差方程里同时含姿态误差项（姿态歪了，比力投影也歪了）和加计零偏项。所以Ft矩阵的右上角、中间区域都是非零的，滤波器的协方差才能在不同状态组之间“传递信息”。这就解释了为什么陀螺零偏能通过速度/位置量测被估计出来——因为状态转移矩阵建立了“零偏影响位置误差”的链路。

在这个demo里，F矩阵和系统噪声矩阵Q的离散化，由kfupdate自动完成。你需要做的只是把初始协方差P0、系统噪声方差Q、量测噪声方差R设置好。这三个设置，就是整篇文章的精髓，咱接下来展开讲。

3. 卡尔曼滤波参数设置：P0、Q、R怎么调才合理

3.1 初始协方差阵，不是越大越好

之前看很多新手把初始协方差设得特别大，理由是“我对初值不确定，所以让滤波器自己收敛”。这在某些仿真里行得通，在组合导航里却容易出问题。原因很简单：惯导解算的递推性质决定了，如果姿态误差初始协方差设太大，滤波器前期可能给自己“极大权限”去修正，反而导致位置/速度估计剧烈跳动。更麻烦的是，PSINS里P0的设定单位很讲究：

• 姿态误差的P0一般以弧度（rad）为单位；
• 速度误差是m/s，注意平方后是(m/s)^2；
• 位置误差如果用经纬度弧度，数值非常小，如果用米，数值就比较大，务必搞清楚你手上这份代码里坐标单位是什么。

test_SINS_GPS_153里给的初始协方差是一组经过验证的合理值：姿态误差在角分级，速度误差在0.1m/s级，位置误差在几米到十几米级，零偏在IMU标称值附近放开。我给一个通用的设置思路，你可以按这个原则去调试自己手里的数据：

• 姿态：按初始对准精度给，一般1分到10分（对应0.0003~0.003rad），别拍脑袋给个0.1rad。
• 速度：按GPS/外部辅助的初速度误差给，0.1~0.5m/s很常见。
• 位置：按GNSS单点定位精度给，3~10m。
• 陀螺零偏：按IMU说明书零偏重复性给，一般给10倍于重复性的值，让前面一段时间充分收敛。
• 加计零偏：同理。

这组参数一旦给错了，不是“精度差一点”的问题，而是整个滤波器的收敛性都会被破坏，表现就是前几分钟曲线出现大幅振荡。

3.2 系统噪声阵，代表“你对模型有多信任”

系统噪声Q体现的是IMU器件噪声（角度随机游走、速度随机游走）以及那些没有被状态模型描述的高频误差。Q给大了，滤波器会认为状态在快速变化，增益不敢太小，量测修正比较激进；Q给小了，滤波器觉得状态很稳，更信任预测值，GPS量测的新息被“压制”。

在PSINS的kfinit里，Q是按连续系统噪声谱密度来设置的。程序里常会看到Q(4:6,4:6) = imuerr.web^2这样的写法。很多人不理解为什么陀螺角度随机游走成了姿态误差的噪声项，其实是因为角度随机游走==姿态误差方程里陀螺噪声的积分，速度随机游走==速度误差方程里加计噪声的积分。

我调滤波器的习惯是：先按IMU手册的参数填Q，然后固定P0和R，去跑一段中低动态的仿真，看速度误差曲线有没有“锯齿”状。如果有明显锯齿，说明Q偏大、量测修正过度；如果误差曲线缓慢偏离0但量测来了也不怎么拉回，说明Q偏小、滤波器太信任惯导预测了。这个调参手感，比单纯背公式来得快得多。

3.3 量测噪声方差阵，决定GPS信息以多大权重进来

test_SINS_GPS_153里GPS量测的位置/速度噪声，通常设置成：

R = diag([pos_std(1)^2; pos_std(2)^2; pos_std(3)^2; vel_std(1)^2; vel_std(2)^2; vel_std(3)^2]);

其中位置标准差的单位要和量测量一致。这个demo里，量测量是GPS的纬度、经度、高度和东北天速度。这里有一个非常容易踩的坑：如果GPS的位置量测用的是经纬度（弧度），而滤波器的位置状态用的是经纬度，那R的位置方差就要非常非常小，小到10的负十几次方量级，因为弧度数值本身就小。PSINS的设计里，对位置量测往往做了“位置量测转换”，把经纬度误差乘以地球半径转成米，这个时候R才能用米做单位。你去看kfupdate调用之前那行代码，体会一下量测到底是“角度”还是“米”，这一步直接影响R量级。

量测噪声的直观理解是：GPS的位置给出一个位置读数，你认为这个读数有多可靠？如果R给得比实际噪声小，滤波器会把GPS噪声当成真实位置变化去跟，结果就是速度误差曲线高频抖动，位置误差虽然小但“不干不净”，有毛刺；如果R给得偏大，GPS修正力度不足，组合导航精度更偏向纯惯导，长时间漂移就压不住。好的R设置应该是你的速度误差曲线平滑下压收敛，位置误差维持在分米到米级，而不是剧烈振荡。

4. 导航解算环节：惯导更新与滤波更新的配合节奏

4.1 机械编排在循环里的位置

程序主循环里，惯导解算用的是insupdate，它本质上把陀螺角增量、加速度增量代入惯导机械编排方程，完成姿态、速度、位置的递推。PSINS里这个引擎成熟度很高，你要关注的是两件事：

一是解算频率：IMU数据率是100Hz，主循环通常按IMU周期处理，GPS量测只在整数秒时刻到达，所以在100Hz的循环里只有1Hz的采样点会触发量测更新。不要试图在一个50Hz或200Hz的数据上去直接套这个demo的参数，滤波器更新时间不一致，状态转移矩阵离散化结果就变了，整套参数都要跟着调整。

二是数据量纲：PSINS里IMU数据一般是增量形式（角增量、速度增量），如果你手里的数据是角速率/加速度形式，需要先做积分转换，否则惯导解算结果会差好几个数量级。这也是很多人跑自己数据跑不通的元凶之一——demo里的IMU前置处理函数imuadderr已经把标准增量形式准备好了，自己的数据可不一定。

4.2 量测更新的时机与顺序

卡尔曼滤波在一个滤波周期内有两个阶段：时间更新（预测）和量测更新（修正）。

test_SINS_GPS_153里，IMU每个周期都做惯导解算；当GPS数据到来时，就把当前惯导解算出来的位置/速度和GPS量测的位置/速度之差，作为新息喂给kfupdate的'M'分支（量测更新模式）。量测更新的本质是：用新息乘以卡尔曼增益，得到全部15个状态的修正量，再把修正量反馈到位置、速度、姿态和零偏估计中去。

这里有个经典细节：量测更新之后，姿态阵、位置、速度都要做相应修正，而且修正完的姿态阵要重新归一化正交化，PSINS里用q2att或类似函数处理。有时候你看到卡尔曼增益矩阵K某些行数值异常大，要立刻检查是不是量测矩阵H的坐标定义不对——比如GPS速度是NED系的，结果你的滤波状态里存的却是东北天系，那误差就大了去了。这类问题归到底是个“坐标系一致性”问题，我自己的教训是：但凡组合结果发散，先查坐标系/单位，再查参数。

4.3 惯导解算和滤波更新的数据流转

整个流程可以压缩成一句话：IMU每拍一帧，惯导往前走一步；GPS每来一次，滤波器把惯导的漂移拉回来一把。

把这个节奏看成文字流程是这样的：

• IMU数据输入，insupdate更新位置/速度/姿态；
• 判断当前时刻是否有GPS量测；
• 有量测，构建量测向量Z = [GPS位置 - 惯导位置; GPS速度 - 惯导速度]；没有量测，只做时间更新；
• 调用kfupdate，拿到误差状态估计值；
• 把误差反馈给当前位置、速度、姿态，并清掉或保留滤波器的状态量（PSINS里常见策略是反馈后自动把状态量清零，这样下一周期滤波器的状态是围绕0附近的小误差，线性度更好）；
• 循环。

这个“反馈后清零”的策略特别重要。很多教材讲卡尔曼滤波时，会说“状态量就是误差真值”，但在组合导航实现里，如果你不把估计出来的误差及时反馈回去，滤波器的状态量会越来越大，直接违背线性化假设。PSINS里的kffeedback就干这件事——反馈完的剩余量，才是下一轮滤波真正要“低头看”的量。

5. 跑完程序后怎么判断收敛与精度，以及实测数据要改什么

5.1 看哪些曲线，能判断滤波是否正常

程序跑完会出几组图：姿态误差、速度误差、位置误差，还可能包含陀螺零偏/加计零偏的估计曲线。第一眼应该看位置误差，尤其是水平位置误差，它最直观地反映组合效果：

• 正常情况：位置误差在开始时较大，但很快就收敛到1米以内，然后保持平稳，不会持续发散。速度误差也一样，几百秒后稳定在cm/s~dm/s级。
• 异常情况一：误差曲线呈发散状，越走越大。先查坐标系、单位、量测构造，然后查P0/R是否合理。
• 异常情况二：误差曲线从0开始就很平，看起来“完美”。这种情况要警惕，八成是仿真数据里没加IMU误差，或者量测噪声设得太过理想，滤波器毫无压力，不代表真实场景的性能。

陀螺零偏收敛曲线特别值得看。如果程序里IMU仿真参数设置的陀螺零偏是10度每小时，但滤波估计出来的零偏收敛到15度每小时，说明有别的误差源在“抢占”这部分信息。这时候不是急着调Q，而是要回头检查IMU仿真数据里是不是加了安装误差或刻度因子误差——这些误差没有被15状态建模，会残留在状态估计里，导致零偏估不准。

5.2 我踩过的“P0给太小导致发散”的坑

有一阵子我为了追求“滤波器快速收敛”，把初始协方差压到非常小，结果恰恰相反——前面几百秒姿态误差一直起不来，滤波器对量测的响应很迟钝，后面靠GPS每1秒修正一点，才慢慢磨回来。原因其实很好理解：P0代表你对初始误差范围的认识。你把它压到极小，相当于告诉滤波器“我的初始姿态非常准”，卡尔曼增益的前期会被严重压制，量测修正不起作用。

所以我的经验是：P0宁可给宽，也不要给窄。给宽了，前几十秒会有一次比较明显的误差修正过程，但滤波器会很快收敛到稳态；给窄了，容易让滤波器“过于自信”，整个收敛过程拉长，甚至在某些误差建模不准的场景下直接滤波发散。PSINS demo里的P0参数其实是一个比较“宽松”的默认值，你可以在此基础上往外扩1~2个数量级试，但不要往小了压。

5.3 从仿真到实测数据，至少要改五处

仿真跑通了，紧接着要用实测数据，这往往是大部分人栽跟头的地方。基于我自己的经验，从demo到实测，至少这五步要改：

1. 初始姿态：仿真数据可以从精确初始姿态开始，实测数据必须做初始对准。拿不准姿态时，要么用静止对准先解算一组初始姿态，要么把姿态初始协方差再放宽一个量级。

2. IMU参数：把仿真里人为设置的陀螺零偏/加计零偏/角度随机游走/速度随机游走，替换成IMU标定报告里的真实值。注意单位换算，实测数据里的角度增量可能是弧度，零偏可能是度每小时，量纲要统一。

3. GPS量测噪声：仿真数据里设置的R是个常量，实测量测噪声其实随时在变——GDOP大的时候噪声大，卫星数变了噪声也变。PSINS支持时变量测噪声，你在构造R时根据卫星数实时更新，比固定R效果好很多。

4. 时间系统对齐：IMU和GPS时间戳必须严格对齐。时序稍微错个几百毫秒，组合出来的速度精度立竿见影下降。先做时间同步，再谈滤波调参。

5. 杆臂补偿：GPS天线相位中心和IMU测量中心不重合，这个位置差异叫杆臂，在速度/位置量测方程里必须考虑。很多入门demo不写这一步，但实测数据里杆臂误差可以达到几厘米到几十厘米，必须做补偿。PSINS里有对应的量测构造方式，别漏了。

6. 从demo到自己的工程：把15状态这套逻辑吃透

说实话，test_SINS_GPS_153这套demo的价值不在于“跑出好结果”，而在于它把松组合导航的骨架展示得足够干净。你把姿态误差、速度误差、位置误差、陀螺零偏、加计零偏这15个状态逐项理解透，后面不管是加气压高度计辅助、加磁力计辅助，还是切到紧组合，思路都是一样的：加状态，改量测，调噪声。

PSINS的好用之处在于，它的机械编排、滤波框架都已经优化得足够好，你用不着从零推导每一步；但不好的地方也在这里——如果只看皮毛，函数封装一层套一层，容易让人迷失，最后变成只会跑demo、不会做工程的人。我的建议是，哪天你能把kfupdate里时间更新和量测更新的矩阵维度掰扯得明明白白，能说出状态转移矩阵里哪一块是姿态误差对速度误差的耦合，你的组合导航水平就已经超过大多数只调参的人了。

我在实际使用中还有一个小习惯：改参数前，先对同一组仿真数据跑一个“纯惯导”的结果，再跑组合导航的结果，两张图叠加对比看。这样你一眼就能看出滤波到底“拉回了多少误差”，比只看组合结果更有价值。遇到滤波发散的疑难杂症时，我也会把这套对比图翻出来，确认误差是“本来惯导就发散”还是“滤波把状态带偏了”——这两种情况的处理思路完全不一样。

这套15状态组合导航，说到底就是跟误差作斗争。你理解误差从哪来、往哪去、谁影响谁，剩下的都是一些工程细节。希望这篇拆解能帮你在PSINS的学习路径上少走点弯路。