先放一个画面：高架桥下，楼间距不到三十米，导航里的小箭头过每个路口就往外跳十几米，明明在主路走着，定位却经常“穿墙”到旁边楼里。这不是某个地图App的bug，而是城市峡谷里GNSS信号被遮挡、反射之后必然出现的物理现象。卫星信号在楼宇间来回弹射，测距误差奔着几十米去，任何地图软件都救不回来。但这时候如果车上有惯性导航和轮速传感器，情况完全可以不一样——用惯导做短时推演，用轮速和车体运动学约束去压住误差发散，即使卫星彻底失锁，也能把定位漂移控制在车道级以内。
这篇文章要讲的，就是用严恭敏老师开源的PSINS工具箱，从零搭一套INS/NHC/ODO组合导航仿真。内容覆盖NHC非完整约束的数学本质、ODO里程计误差建模、卡尔曼滤波器设计、PSINS核心函数调用方式，以及大量我从实际跑数据中总结出来的调参经验和坑。适合正在做车载组合导航的算法工程师、惯性导航方向的硕博研究生，以及所有被“GNSS拒止环境定位漂移”折磨过的人。
1. 城市峡谷的定位漂移困局：为什么GNSS靠不住，惯导又撑不久
先说清楚我们到底在解决什么问题。城市峡谷从来不是简单的“信号变弱”，而是多径效应和NLOS（非视距接收）叠加后的灾难现场。你收到的卫星信号，很多是从玻璃幕墙上反弹过来的“假信号”，测距误差不再是分米级，而是直接到几十米甚至上百米。这时候GNSS/INS组合导航如果还按正常权重信任GNSS位置，滤波器会被带偏得一塌糊涂。很多人以为RTK能解决，实际在城市深峡谷里，RTK的固定率经常掉到50%以下，整周模糊度一解算失败，定位直接跳回米级甚至十米级。
1.1 高楼反射下GNSS定位有多离谱
我实测过一段大约7公里的高架路，两边是连续的高层商业楼，GNSS单点定位的轨迹横跨整条路，最大偏差超过80米，过弯时定位点直接飞进旁边的小区。更麻烦的是这种误差不是单调的，它会在真值附近来回摆——一会偏左，一会偏右，滤波器如果开了GNSS位置修正，位置估计就会跟着来回抽动，姿态和航向也被污染。相比而言，惯导在这段时间内的短时递推是相当干净的，几十秒上百秒内不发散，这就是组合导航“以惯导为主体、其他传感器做校正”这一体系能成立的根本原因。
1.2 纯惯导的发散速度：数学上有多残酷
纯惯导不是不能定位，问题是它的误差随时间是发散的，而且是积分叠加式发散。陀螺零偏会导致姿态误差，姿态误差把重力加速度的一部分错误投影到水平方向，这个等效水平加速度持续作用在速度积分和位置积分上，位置误差大致按0.5·g·φ·t²的规律增长。举个例子，初始失准角如果只有0.1度，大约0.0017弧度，100秒后位置误差就已经是0.5×9.8×0.0017×10000，约83米。这还仅仅是失准角的贡献，陀螺漂移、加速度计零偏带来的误差只会让情况更糟。换句话说，车载纯惯导在低成本MEMS器件下，有效定位时间可能只有几十秒，一旦GNSS失锁超过这个时间，位置基本不能看。
1.3 为什么说单一传感器都是“瘸腿”的
GNSS怕遮挡，惯导怕发散，那是不是只用里程计就好了？也不行。里程计给出的是沿车辆前进方向的相对位移，它没有绝对的航向参照，弯一多就会累积巨大的横向误差。更关键的是轮速无法感知横向速度，车辆转弯时如果只用里程计积分，轨迹会往转弯内侧塌得非常厉害。所以这三者本质上是互补的：惯导提供高频姿态和短时高精度递推，ODO提供长期稳定的前向速度参考，NHC提供侧向和天向的运动学约束。把它们放进同一个滤波器，才能在城市峡谷这种恶劣场景下稳住定位。
2. NHC与ODO的救场逻辑：车辆运动学约束背后的数学原理
在写代码之前，一定要把NHC和ODO这两个东西的数学模型搞明白。很多人仿真失败，不是代码写错，而是坐标定义和约束方程从一开始就乱了。
2.1 NHC非完整约束：车不可以横着飞
NHC（Non-Holonomic Constraint）翻译过来叫非完整约束，本质是一个物理直觉：在正常行驶条件下，车辆不会发生侧向滑动，也不会跳离地面。把车体坐标系b系定义为右前上坐标系（x向右，y向前，z向上），那么车辆在b系下的侧向速度（x轴）和天向速度（z轴）应该近似为零，只有前向速度（y轴）是显著的。仿真时设一个很小的阈值，比如侧向速度小于0.05 m/s，天向速度小于0.05 m/s，就认为约束成立。
这个约束能发挥作用的关键在于：惯导解算出的是导航系下的速度，要使用NHC约束，必须把导航系速度投影回车体系，或者反过来把“b系下侧向/天向速度为0”这个条件等效成导航系下的速度观测。无论哪种方式，都依赖当前姿态矩阵，所以NHC对姿态误差，尤其是横滚角和俯仰角，有很强的可观性。这一点在实际仿真里体会特别明显：加了NHC之后，横滚角误差的收敛速度肉眼可见地变快。
2.2 ODO里程计：从轮速脉冲到前向速度
车上轮速传感器输出的是脉冲计数，要换算成速度需要知道每个脉冲对应的行驶距离，这就是刻度系数，单位通常是米每脉冲。问题在于这个系数不是固定不变的：轮胎气压变化、载重变化、轮胎磨损、行驶速度变化都会让它偏移。实车标定一次只能标定某个工况下的值，跑一段时间又会漂。所以组合导航滤波器里一定要把刻度系数误差设成待估状态，让它在线估计、在线修正。
仿真时，里程计输出可以这样模拟：从参考轨迹取真实前向速度，加上一个常值刻度系数误差（比如1.5%偏小），再叠加一点高斯白噪声（方差大约0.05 m/s）。这样生成的ODO数据比较接近实际轮速信号。注意一点，很多入门仿真直接把真实速度当ODO用，这相当于白送一个完美观测，滤波结果会好得不真实，完全没有工程参考价值。
2.3 组合架构选择：为什么这里用速度观测
ODO/NHC辅助惯导有两种常见架构。一种是紧组合，把轮速脉冲原始信息放进滤波器，对刻度系数、安装角做更精细的建模，抗差性强但实现复杂。另一种是松组合，先把轮速脉冲转换成前向速度，结合NHC约束构造一个三维速度观测向量，然后对惯导解算速度做校正。对于绝大多数车辆级应用，松组合速度观测已经足够，而且实现简洁、鲁棒性好，PSINS框架下也最容易改造。
在PSINS仿真里，我们实际上构造的是这样的量测：
- ODO给出b系前向速度v_od；
- NHC约束侧向和天向速度为零；
- 所以b系下的完整速度观测为v_b = [0; v_od; 0]（右前上坐标系）;
- 通过姿态矩阵把v_b投影到导航系，得到v_n_obs = C_b^n * v_b；
- 惯导解算速度v_ins和v_n_obs之差就是卡尔曼滤波的量测残差。
这个观测方程里，姿态矩阵是核心桥梁，姿态误差会直接体现在残差中，所以NHC/ODO对姿态有很强的修正能力。里程计刻度系数误差则体现在v_od与真实速度的比例偏差上，滤波器通过残差中的系统性偏差去估计它。
3. PSINS工具箱的打开姿势：环境配置与核心数据流
要把上面的数学模型跑起来，PSINS是目前最合适的选择。它不像商业软件是个黑盒子，所有算法代码都摊开在面前，每一行都可以读、可以改、可以验证。
3.1 为什么选PSINS而不是MATLAB自带工具包
MATLAB新版本的Navigation Toolbox也支持惯性导航仿真，但一是授权不便宜，二是算法细节封装太死，做算法研究的人没法方便地修改状态方程、观测矩阵。PSINS是严恭敏老师实验室开源的，十几年积累，完整实现了捷联惯导的静基座对准、动基座对准、惯导机械编排、组合导航滤波，以及大量轨迹仿真工具。最靠谱的一点是，它的更新算法和误差模型与绝大多数论文、教材一致，拿它跑出来的结果可以直接对标文献。
3.2 安装与环境配置要点
下载PSINS压缩包后，解压到任意目录，然后打开MATLAB，在主界面执行addpath(genpath(‘psins目录路径’))，把整个目录结构加入搜索路径。PSINS依赖的全局变量结构体glvs会在脚本运行时自动初始化。
我建议安装后第一时间跑一遍自带的测试脚本，比如test_SINS_GPS_153.m，确认环境没问题。这个脚本是SINS/GPS组合导航的经典示例，包含了轨迹生成、IMU误差添加、惯导解算、卡尔曼滤波、结果分析的全套流程。跑通它之后，改造成INS/NHC/ODO组合导航就顺理成章了。
3.3 理清数据流：从轨迹生成到组合解算的完整闭环
整个仿真数据流可以概括成这么几个环节：
- 用参考轨迹（位置、速度、姿态随时间变化）生成理想IMU数据；
- 给IMU数据叠加陀螺零偏、加速度计零偏、角度随机游走、速度随机游走；
- 从参考轨迹提取前向速度，叠加刻度系数误差和噪声，生成ODO数据；
- 惯导机械编排每次迭代向前推进一步，得到位置速度姿态；
- 当ODO有数据时，构造速度观测进行卡尔曼滤波修正；
- 把修正后的误差反馈给惯导解算状态，循环往复。
这个环路里的核心就是insupdate和kfupdate这两个函数。前者是捷联惯导更新，后者是卡尔曼滤波一步更新。搞清楚这两个函数怎么调用、怎么传参，整个仿真就通了。
4. 手把手搭建INS/NHC/ODO仿真：从轨迹生成到误差评估
下面进入实操环节。我直接给出一套可以在PSINS框架下运行的MATLAB脚本框架，每一步都解释为什么这样写。
4.1 第一步：生成参考轨迹与IMU原始数据
仿真首先要有一条“真值轨迹”，它决定了后续所有结果的对比基准。PSINS提供了轨迹生成工具，可以按照多段运动拼接的方式生成：直行、匀速、转弯、坡道、加减速都可以组合。以下示意代码是典型的做法：
% 参考轨迹初始化 glvs ts = 0.005; % IMU采样间隔，200Hz T = 600; % 仿真时长600秒 avp0 = [[0;0;0]*glv.deg; [0;0;0]; [34.2;108.9;400]]; % 初始姿态/速度/经纬高 % 定义多段运动并生成轨迹（示意写法） trj = trjsimu(ts, T, 'urban_route'); % 从轨迹中提取理想IMU数据 imu = trj.imu; % 添加惯导器件误差 imu_err = imuadderr(imu, eb, db, web, wdb, ts); % 提取参考真值用于后续对比 trj_avp = trj.avp;这里有几个细节值得注意。第一，初始经纬高必须设置成实际场景的位置，PSINS内部计算地球曲率半径、重力加速度等都依赖这个值。第二，ts的选取要和实际硬件一致，车载MEMS IMU常见的是100Hz到200Hz，仿真设200Hz即可。第三，imuadderr中的四个量分别是陀螺零偏、加速度计零偏、陀螺随机游走、加速度计随机游走，具体数值可以参考你所用IMU的datasheet，不要拍脑袋写。
4.2 第二步：构建NHC与ODO观测数据
从参考轨迹里可以拿到每一时刻的真实速度和真实姿态。ODO数据不能直接用真实速度，要人为注入误差。代码如下：
% 从参考轨迹提取前向速度（右前上坐标系下，前向对应速度的第二个分量） vn_true = trj.avp(:, 4:6)'; Cnb_true = q2mat(trj.avp(:, 1:4)'); % 姿态四元数转矩阵 vb_true = Cnb_true * vn_true; % 导航系速度投影到车体系 v_odo_true = abs(vb_true(2, :)); % 前向速度 % 注入刻度系数误差和噪声 k_sf = 1.015; % 刻度系数误差为1.5% v_odo = v_odo_true / k_sf + 0.05 * randn(size(v_odo_true));这一步特别容易踩坑。车体系坐标轴定义不同，前向速度的索引就不同。很多人在PSINS默认的坐标系里直接用第一个分量，结果NHC约束验证死活对不上。所以开始写代码前，先确认坐标系定义，再确认IMU安装方向，再确认轨迹行进方向，三个坐标系全部统一了再往下走。
NHC观测在代码里的体现是：当构造三维速度观测时，v_b的侧向和天向分量强制设为零，而不是用IMU解算出来的对应分量。很多人把这个细节忽略了，写到最后变成“用带噪声的真实速度替代ODO”，那NHC等于没有加。
4.3 第三步：卡尔曼滤波器设计与状态量选择
状态量的设计直接决定滤波器能估计什么。基础15维状态量包括：姿态误差3个、速度误差3个、位置误差3个、陀螺零偏3个、加速度计零偏3个。加入ODO后，还需要把里程计刻度系数误差作为一个维度，所以这里设计成16维状态量：
% 状态量定义（16维） % 1:3 姿态误差 % 4:6 速度误差 % 7:9 位置误差 % 10:12 陀螺零偏 % 13:15 加速度计零偏 % 16 里程计刻度系数误差(b系前向)状态方程用PSINS线性化后的惯导误差方程，离散化之后填充状态转移矩阵F。Q矩阵反映的是陀螺和加速度计随机游走强度，R矩阵反映的是NHC/ODO观测噪声强度。这个不是随便设的，后面专门讲怎么调。
量测矩阵H的构造是核心。对于速度观测，惯导解算速度和观测速度之差对应的H可以按下面这个框架写：
% 观测向量：3x1，惯导解算速度与NHC/ODO观测速度之差 Cbn = q2mat(qk); vn_odo_obs = Cbn * [0; v_odo(k); 0]; % 右前上坐标系 z = vn_ins - vn_odo_obs; % H矩阵分块 Hk = zeros(3, 16); Hk(1:3, 4:6) = eye(3); % 速度误差直接可见 Hk(1:3, 1:3) = -askew(vn_odo_obs); % 姿态误差通过投影影响观测 Hk(1:3, 16) = -Cbn * [0; v_odo(k) / k_sf; 0]; % 刻度系数误差这三块分别对应三种状态量是如何进入量测的。速度误差直接就是惯导速度误差本身，所以是单位阵；姿态误差是通过姿态矩阵投影到导航系时产生的误差，所以有反对称矩阵；刻度系数误差是通过改变v_od大小进入系统的。H矩阵写对了，滤波器才会正确地给各个状态量分配修正量。
4.4 第四步：主循环解算与结果存储
主循环的核心就是惯导更新加滤波更新。思路如下：
for k = 2:length(imu) % 惯导机械编排递推 ins = insupdate(ins, imu(k, 1:6)', ts); % 每步都进行ODO/NHC观测更新 if mod(k, odo_step) == 0 % 构造量测向量和量测矩阵 [z, Hk] = build_od_nhc_observation(ins, v_odo(k), k_sf_init); % 卡尔曼滤波更新 ins = kfupdate(ins, z, Hk, Rk); % 反馈校正并重置滤波状态 ins = kfupdate(ins); end % 存储结果 avp_ins(:, k) = [ins.qnb; ins.vn; ins.pos]; end注意PSINS的kfupdate有两种调用方式：带参数的是一次完整的量测更新，不带参数的是一次时间更新。组合导航里的一般节奏是每个IMU周期先做时间更新，有ODO观测时再做量测更新并把估计出的误差反馈给惯导状态。反馈之后要把滤波器状态向量清零，否则误差会重复修正，这就是kfupdate(ins)这行代码的作用。
4.5 第五步：误差评估与绘图
仿真跑完，不要只盯着轨迹图看“看起来差不多”，要有量化指标。常用做法是把解算导航结果与真值轨迹做差，统计位置误差的RMS和最大值，看二维轨迹与真值轨迹的贴合程度。PSINS里可以有现成的绘图比较脚本：
% 计算位置和姿态误差 err = avp_ins - trj_avp'; figure; subplot(2,1,1); plot(trj_avp(:,7), trj_avp(:,8), 'b'); hold on; plot(ins_pos(:,1), ins_pos(:,2), 'r'); legend('参考轨迹', 'INS/NHC/ODO解算'); xlabel('经度'); ylabel('纬度'); subplot(2,1,2); t = (0:size(err,2)-1) * ts; plot(t, err(7:9, :)*1e3); xlabel('时间/s'); ylabel('位置误差/m'); legend({'纬度误差','经度误差','高度误差'});这里的位置误差单位要注意：PSINS里位置通常用经纬度弧度表示，直接乘1e3转成米再画图才直观，更严格的做法是用初始位置的地球曲率半径换算。
5. 仿真结果解读与调参直觉：别只看轨迹漂没漂
跑通第一次仿真后，你大概率会看到两个结果：一是轨迹基本贴合，二是误差曲线有一些“锯齿”现象。这都正常，关键是学会解读这些结果背后的算法行为。
5.1 NHC带来的意外收益：横滚角可观性改善
我第一次对比“纯惯导+ODO”和“惯导+ODO+NHC”两组结果时，发现横滚角误差的收敛速度完全不是一个量级。原因也想通了：NHC把侧向速度强制设为零，这个约束对姿态矩阵的横向投影特别敏感，一旦横滚角有误差，侧向投影出非零速度，残差立刻变大，滤波器马上修正。俯仰角同理，坡道上尤其明显。航向角相对弱一些，因为航向误差主要靠转弯时的加速度变化去激励，仿真轨迹里如果全是直线，航向误差会收敛得很慢。
5.2 里程计刻度系数误差的在线估计
16维状态量里的刻度系数误差是可观性很强的状态量，只要车辆在跑，前向速度激励一直存在，滤波器就能持续估计它。我经常用这个收敛值来判断滤波器设计是否正确：如果仿真注入1.5%的刻度系数误差，估计收敛值应该非常接近这个数。如果收敛值偏差很大，先检查观测矩阵Cbn那一列的符号，再检查坐标系定义，这两处是最容易出错的。
5.3 Q/R矩阵的设参经验：别凭感觉乱填
Q矩阵是过程噪声矩阵，反映你对IMU误差模型的信任程度；R矩阵是量测噪声矩阵，反映你对NHC/ODO观测的信任程度。这两个矩阵的比值决定滤波器的带宽和噪声水平，比它们各自的绝对值重要得多。我个人调试的顺序是：先把R设成0.05²的量级（对应5cm/s的量测噪声），然后用器件datasheet和Allan方差分析结果设Q，再根据误差曲线微调。如果位置误差曲线噪声太大，往往是Q/R比值偏大、观测权重不足，调小Q或调大R即可；如果误差曲线长时间不收敛，则是Q设得过小，滤波器“太自信”地认为惯导误差模型完全准确。
5.4 NHC失效边界：急刹、滑移、原地转向
仿真时所有约束都是理想的，但实车不是。NHC在以下场景会失效：急刹车时车辆点头，前向速度突跳；路面光滑时侧滑，侧向速度不再为零；原地转向时轮速有效而GNSS大幅跨步。仿真阶段也要为这些边界情况保留处理逻辑，最简单的做法是加一个“运动状态判断”：当检测到IMU角速度或加速度突变量超过阈值时，量测噪声R放大，降低对观测的信任。这种策略在PSINS里很容易实现，改一下Rk的数值就行。
6. 代码获取与后续扩展：从仿真到实车移植的关键一步
到这里，整套INS/NHC/ODO组合导航仿真已经跑通了。完整工程代码我整理成了一个仓库，包含轨迹生成脚本、IMU误差注入模块、NHC/ODO观测生成模块、16维组合导航主循环、误差评估和绘图脚本，全部基于PSINS框架，拿下来改改IMU参数就能跑。你可以在GitHub搜索PSINS_NHC_ODO_Demo，或者到同名博客后台回复“NHC_ODO”获取下载链接。
6.1 代码里到底有什么
仓库里最核心的是三个文件：main_ins_nhc_odo.m主脚本、build_od_nhc_observation.m观测构造函数、plot_results.m结果可视化脚本。主脚本结构清晰，每个关键步骤都有注释，适合你在此基础上做各种扩展实验，比如换成不同的轨迹、注入更大的IMU误差、改成紧组合架构。
6.2 从仿真到实车移植的四个致命细节
仿真跑得再漂亮，不解决下面这四个问题，上实车必翻车：
一是时间同步。IMU和ODO数据必须统一时间戳，PSINS仿真里天然同步，但实车需要做硬件同步或者软件插值，时间不同步会导致量测残差出现系统性偏差。
二是外参标定。ODO在车体系下的位置、IMU安装角、杆臂，都会影响观测构建。安装角误差如果达到零点几度，航向误差可能直接变成米级定位偏差，这时候必须把安装角都加进滤波器估计。
三是脉冲计数到距离的转换。实车ODO输出脉冲，每个脉冲对应的距离需要精确标定。更麻烦的是倒车时脉冲方向和累计方式，很多芯片的脉冲接口正反转处理方式不一样，代码要区分清楚。
四是车轮滑移和打滑。仿真里基本不考虑，实车却逃不掉。急加速发动机会让车轮转速高于实际车速，刹车ABS时又低于实际车速，这种情况下要做滑移检测，在观测端做保护。
6.3 可以继续做的方向
这套框架的扩展空间非常大。往深了做，可以在16维状态量上加二维安装误差角，变成19维组合；可以在ODO基础上引入左右轮速差，直接约束航向角更新量；可以在GNSS信号恢复后加入GNSS位置观测，实现GNSS/INS/NHC/ODO多源融合；还可以把里程计替换成视觉里程计或激光里程计，这套NHC/ODO的观测框架同样适用。逻辑是一样的：凡是能提供运动学约束的传感器，都能挂到这个滤波器上。
我个人在实际项目里的体会是：仿真阶段把NHC/ODO这类运动学约束想透，到了实车调试阶段能省下大量时间。很多你以为的“硬件问题”，最后查出来都是坐标系定义不一致或者观测矩阵符号写反了。先用仿真把算法逻辑磨干净，再上实车做数据采集和参数标定，这是最稳妥的路径。
