MATLAB Psins工具箱实战：从insinit到insupdate的SINS核心函数深度解析
刚接触惯性导航系统（SINS）开发的工程师常会遇到这样的困境：面对Psins工具箱里密密麻麻的MATLAB函数，既不知道从何处切入，也不清楚各个子函数之间的调用关系。本文将带您从最基础的insinit初始化函数开始，逐步拆解到核心算法insupdate的实现细节，通过实际代码演示和IMU数据测试，构建完整的惯性导航解算能力。
1. 环境准备与工具箱配置
在开始代码解析前，需要确保MATLAB环境已正确配置Psins工具箱。推荐使用MATLAB R2018b或更高版本，以获得最佳兼容性。工具箱安装步骤如下：
- 从官方渠道获取Psins工具箱压缩包
- 解压至MATLAB工作目录下的
toolbox文件夹 - 在MATLAB命令行执行：
addpath(genpath('toolbox/psins')); savepath; - 验证安装：
which insinit
注意：若出现路径冲突，建议清理MATLAB路径中其他导航工具箱，避免函数命名冲突。
工具箱包含的主要目录结构如下：
| 目录 | 内容描述 |
|---|---|
/algo | 核心算法实现 |
/demo | 示例脚本 |
/init | 初始化函数 |
/misc | 辅助工具函数 |
2. 系统初始化：insinit函数详解
insinit是SINS解算的起点，负责创建并初始化惯性导航系统结构体。其典型调用方式有三种：
% 方式1：使用AVP数组初始化 avp0 = [att0; vn0; pos0]; % 姿态/速度/位置初始值 ins = insinit(avp0, ts); % ts为采样周期 % 方式2：带误差设置的初始化 avperr = [1;1;10]*1e-3; % 姿态误差(deg)、速度误差(m/s)、位置误差(m) ins = insinit(avp0, ts, avperr); % 方式3：分量形式初始化 ins = insinit(qnb0, vn0, pos0, ts); % 四元数/速度/位置分开输入初始化过程中会调用两个关键子函数：
ethinit.m：计算地球相关参数
eth = ethinit(pos, vn); % 输入位置和速度主要输出参数包括：
Re：地球半径e2：地球偏心率平方wie：地球自转角速度g0：重力加速度
inslever.m：处理杆臂效应补偿
ins = inslever(ins, lever); % lever为杆臂参数矩阵
初始化完成后，ins结构体将包含以下核心字段：
| 字段名 | 描述 | 数据类型 |
|---|---|---|
att | 姿态角(roll/pitch/yaw) | 3×1 double |
vn | 速度(n系) | 3×1 double |
pos | 位置(lat/lon/hgt) | 3×1 double |
ts | 采样周期 | scalar |
eth | 地球参数结构体 | struct |
3. 核心算法：insupdate的实现原理
insupdate函数完成SINS的捷联解算核心过程，采用双子样算法进行姿态、速度和位置更新。其标准调用格式为：
ins = insupdate(ins, imu); % imu为[gyro,acc]增量采样3.1 算法流程分解
圆锥/划船效应补偿：
[phim, dvbm] = cnscl(imu,0); % 圆锥/划船补偿 phim = ins.Kg*phim-ins.eb*nts; % 陀螺校准 dvbm = ins.Ka*dvbm-ins.db*nts; % 加速度计校准外推计算（t1/2时刻）：
vn01 = ins.vn+ins.an*nts2; % 速度外推 pos01 = ins.pos+ins.Mpv*vn01*nts2; % 位置外推 ins.eth = ethupdate(ins.eth, pos01, vn01); % 更新地球参数速度更新：
ins.fn = qmulv(ins.qnb, ins.fb); % 比力坐标变换 ins.an = rotv(-ins.eth.wnin*nts2, ins.fn) + ins.eth.gcc; vn1 = ins.vn + ins.an*nts; % 速度更新结果位置更新：
ins.Mpvvn = ins.Mpv*(ins.vn+vn1)/2; % 梯形积分 ins.pos = ins.pos + ins.Mpvvn*nts; % 位置更新姿态更新：
ins.qnb = qupdt2(ins.qnb, phim, ins.eth.wnin*nts); [ins.qnb, ins.att, ins.Cnb] = attsyn(ins.qnb); % 同步四元数/欧拉角/DCM
3.2 关键参数说明
在insupdate中需要特别关注的几个参数：
双子样处理：
nn = size(imu,1); % 子样数 nts = nn*ins.ts; % 总时间间隔 nts2 = nts/2; % 半周期地球参数更新：
ins.eth = ethupdate(ins.eth, pos, vn);主要更新：
- 子午圈半径RMh
- 卯酉圈半径RNh
- 导航系角速度wnie/wnen
机械编排矩阵：
ins.Mpv = [0, 1/ins.eth.RMh, 0; 1/ins.eth.clRNh, 0, 0; 0, 0, 1]; % 位置更新矩阵
4. 实战演练：完整SINS解算流程
下面通过一个完整的示例演示如何从初始化到连续更新：
%% 初始化参数 att0 = [0; 0; 90]*glv.deg; % 初始姿态(deg) vn0 = [0; 0; 0]; % 初始速度 pos0 = [34; 108; 380]; % 初始位置[lat;lon;hgt](deg/m) ts = 0.01; % 采样周期10ms %% 初始化SINS ins = insinit([att0; vn0; pos0], ts); %% 生成模拟IMU数据 T = 60; % 时长60秒 imu = imustatic(att0, pos0, ts, T); %% 解算循环 avp = zeros(fix(T/ts), 10); % 预分配内存 for k=1:size(imu,1) ins = insupdate(ins, imu(k,:)); avp(k,:) = [ins.att; ins.vn; ins.pos; ins.tk]'; end %% 结果可视化 t = (1:size(avp,1))'*ts; figure; subplot(311), plot(t, avp(:,1:3)/glv.deg); title('姿态'); subplot(312), plot(t, avp(:,4:6)); title('速度'); subplot(313), plot(t, avp(:,7:8)); title('位置(经纬)');提示：实际工程中建议使用
prealloc函数进行内存预分配，避免循环中动态扩容影响性能。
5. 常见问题与调试技巧
5.1 初始化参数选择
- 姿态初始化误差：超过5°可能引起发散
- 位置初始化精度：
- 纬度/经度：优于0.01°
- 高度：优于10米
- IMU参数校准：
ins.Kg = eye(3)-diag([0.01;0.01;0.02]); % 陀螺比例因子 ins.Ka = eye(3)-diag([0.005;0.005;0.01]); % 加表比例因子
5.2 数值稳定性处理
- 四元数归一化：
ins.qnb = ins.qnb/norm(ins.qnb); - 高度通道阻尼：
if abs(ins.pos(3))>1000 % 高度超过1km时启用阻尼 ins.an(3) = ins.an(3) - 0.1*ins.vn(3); end
5.3 性能优化建议
- 减少动态内存分配：预先计算
Mpv等矩阵 - 向量化操作：避免循环中的逐元素计算
- 使用mex函数：对
cnscl等耗时函数进行C++改写
在最近的一个车载导航项目中，通过将insupdate中的四元数更新替换为mex实现，解算速度提升了约40%。但需要注意，mex编译需要配置MATLAB的mex编译器，且跨平台兼容性需要额外测试。
