新手也能懂：用严恭敏PSINS工具箱跑通SINS/GPS松组合仿真（附完整代码解读）
从零开始掌握PSINS工具箱：SINS/GPS松组合仿真实战指南
刚接触惯性导航领域的研究者，面对严恭敏教授的PSINS工具箱时，常会被其中复杂的算法和代码结构所困扰。本文将以工具箱中的test_SINS_GPS_153示例为切入点，用生活化的类比和逐行解析，带您理解松组合导航的核心原理与实现方法。
1. 环境准备与基础概念
在开始代码实践前，我们需要先搭建好MATLAB环境并理解几个关键概念。PSINS工具箱支持从R2016b到最新版本的MATLAB，建议安装2020b或更高版本以获得最佳兼容性。
基础工具准备清单：
- MATLAB安装（建议2020b+）
- PSINS工具箱下载（GitHub或严教授主页获取）
- 基础导航知识（无需精通，了解基本术语即可）
松组合导航可以类比为我们日常使用手机导航的场景：手机内置的加速度计和陀螺仪（相当于SINS系统）会持续估算位置，但存在累积误差；而GPS信号（相当于量测更新）则定期提供绝对位置参考，两者相互校正。这种"自主推算+定期校准"的模式，正是卡尔曼滤波在导航领域的经典应用。
2. 仿真数据加载与初始化
打开test_SINS_GPS_153.m文件，我们首先看到的是轨迹数据加载和参数初始化部分。这里用到的trjfile函数会载入预存的理想轨迹数据，相当于为我们提供了一个"标准答案"。
trj = trjfile('trj10ms.mat'); % 加载10ms采样间隔的参考轨迹 [nn, ts, nts] = nnts(2, trj.ts); % 设置子样数和采样时间关键参数说明：
| 参数 | 说明 | 典型值 |
|---|---|---|
| nn | 子样数 | 2 |
| ts | 采样间隔(s) | 0.01 |
| nts | 导航周期(s) | 0.02 |
提示：
nn=2表示使用双子样算法，能有效减小圆锥误差和划桨误差的影响，这是高精度惯性导航的常用技术。
传感器误差设置是仿真真实性的关键。imuerrset函数定义了IMU的各类误差参数：
imuerr = imuerrset(0.03, 100, 0.001, 5); % 设置IMU误差参数 imu = imuadderr(trj.imu, imuerr); % 给理想数据添加误差这相当于给"完美"的传感器数据加入了现实世界中存在的噪声和偏差，使得我们的仿真更接近实际应用场景。
3. 卡尔曼滤波器实现解析
卡尔曼滤波是松组合的核心算法，其实现主要包含初始化、预测更新和量测更新三个阶段。在PSINS工具箱中，这些功能被封装为简洁的函数调用。
滤波器初始化：
davp0 = avperrset([0.5;-0.5;20], 0.1, [1;1;3]); % 设置初始误差 ins = insinit(avpadderr(trj.avp0,davp0), ts); % 惯导初始化 rk = poserrset([1;1;3]); % 位置量测噪声 kf = kfinit(ins, davp0, imuerr, rk); % 卡尔曼滤波初始化这个过程中，工具箱完成了：
- 状态向量和协方差矩阵的维度确定
- 过程噪声和量测噪声的设置
- 初始状态不确定性的定义
预测更新流程：
for k=1:nn:len-nn+1 wvm = imu(k:k1,1:6); % 获取IMU增量 ins = insupdate(ins, wvm); % 惯导机械编排 kf.Phikk_1 = kffk(ins); % 计算状态转移矩阵 kf = kfupdate(kf); % 执行预测更新 ... end这段代码实现了惯性导航的核心——机械编排算法，通过积分角速度和加速度数据，不断更新姿态、速度和位置信息。
4. GPS量测更新与结果分析
GPS量测更新的触发条件通常设置为固定时间间隔，在示例中采用了每秒更新一次的频率：
if mod(t,1)==0 % 每秒触发一次GPS更新 posGPS = trj.avp(k1,7:9)' + davp0(7:9).*randn(3,1); % 模拟GPS量测 kf = kfupdate(kf, ins.pos-posGPS, 'M'); % 量测更新 [kf, ins] = kffeedback(kf, ins, 1, 'avp'); % 状态反馈 ... end量测更新关键点：
- GPS位置信息加入了随机噪声模拟真实环境
- 量测残差计算（INS估算位置与GPS量测之差）
- 卡尔曼增益计算和状态更新
- 误差状态反馈校正惯导输出
仿真结束后，工具箱提供了丰富的绘图函数来可视化结果：
insplot(avp); % 绘制导航参数 avperr = avpcmpplot(trj.avp, avp); % 绘制误差曲线 kfplot(xkpk, avperr, imuerr); % 绘制滤波器状态这些图表能直观展示：
- 姿态、速度、位置的估计精度
- 各状态量的收敛情况
- IMU误差参数的可观测性
5. 常见问题排查与调试技巧
初次运行仿真时，可能会遇到各种问题。以下是几个典型场景的解决方法：
问题1：运行时报维度错误
- 检查
psinstypedef是否正确定义了状态维度 - 确认
kfinit中各参数的维度一致性
问题2：导航误差持续发散
- 检查IMU误差参数设置是否合理
- 验证量测更新是否正常触发
- 调整过程噪声和量测噪声参数
问题3：仿真结果与预期不符
- 使用
disp或plot中间变量 - 分步执行排查问题环节
- 参考工具箱中的其他示例对比
调试时可以充分利用MATLAB的调试工具：
- 设置断点逐步执行
- 查看工作区变量
- 使用
tic/toc计时定位性能瓶颈
6. 进阶应用与扩展思路
掌握基础仿真后，可以尝试以下扩展实践：
多源信息融合：
- 增加速度量测（如里程计）
- 加入姿态量测（如视觉辅助）
- 融合高度信息（气压计/雷达）
算法改进方向：
- 实现自适应卡尔曼滤波
- 尝试UKF等非线性滤波算法
- 加入故障检测与容错机制
工程实践技巧：
- 将常用配置封装为函数
- 建立参数配置文件
- 开发自动化测试脚本
在实际项目中，我通常会先运行这个基础仿真作为基准，然后逐步添加实际系统中的各种影响因素，如杆臂补偿、时间同步误差等，这样能系统性地验证算法鲁棒性。