基于 PSINS 工具箱，构建了一套完整的 IMU/GPS 组合导航仿真与后处理框架。包含加速、匀速、转弯、爬升和下降等多种机动状态的三维运动轨迹，在此基础上模拟理想 IMU 输出，并引入典型惯导器件误差模型生成含噪 IMU 测量数据。扩展卡尔曼滤波（EKF） 实现 IMU/GPS 组合导航解算，并在滤波结果的基础上进一步引入 RTS 平滑算法，对全时段状态进行后向优化，系统性对比滤波与平滑前后的定位性能差异 原创代码，非AI生成，禁止翻卖
文章目录
- 程序详解
- 运行结果
- MATLAB源代码
程序详解
在状态建模方面，系统采用经典的惯性导航误差状态模型，以位置、速度、姿态误差及惯性器件零偏为状态量。系统状态传播由惯导机械编排方程驱动，其离散形式可概括为
x
∗
k
=
Φ
∗
k
−
1
x
∗
k
−
1
+
w
∗
k
−
1
,
\\mathbf{x}*{k} = \\boldsymbol{\\Phi}*{k-1}\\mathbf{x}*{k-1} + \\mathbf{w}*{k-1},
x∗k=Φ∗k−1x∗k−1+w∗k−1, 其中
Φ
∗
k
−
1
\\boldsymbol{\\Phi}*{k-1}
Φ∗k−1为状态转移矩阵，
w
∗
k
−
1
\\mathbf{w}*{k-1}
w∗k−1表示系统噪声。GPS 提供的位置观测用于对惯导解进行周期性校正，观测更新采用 EKF 框架完成。
在此基础上，引入 RTS（Rauch–Tung–Striebel）平滑算法，利用全时间段的前向滤波结果与状态转移信息，对历史状态进行后向递推修正。RTS 平滑通过平滑增益矩阵综合相邻时刻的预测与滤波信息，有效削弱了随机噪声对状态估计的影响，显著提升了轨迹的连续性和整体精度。
程序最终从多个角度对结果进行分析与展示，包括三维轨迹对比、位置误差时序曲线、误差累积分布函数（CDF）以及误差统计指标（绝对均值、标准差和最大误差）。结果清晰表明，相比单纯 EKF 滤波，引入 RTS 平滑后的位置估计在稳态精度和整体一致性方面均得到明显改善。
运行结果
轨迹： 
误差： 
CDF图像： 
误差统计特性对比： 
MATLAB源代码
部分代码如下：
% 基于PSINS工具箱的IMU数据生成与GPS组合导航，EKF滤波，随后使用RTS平滑，对比滤波和平滑前后的轨迹
% 作者：matlabfilter
% 2026-01-22/Ver1
clear;clc;close all;
rng(0);
glvs
psinstypedef(153);
ts = 0.1; % sampling interval
avp0 = [[0;0;0]; [0;0;0]; [0;0;0]]; % 初始化avp
traj_ = [];
%% 轨迹设置
seg = trjsegment(traj_, 'init', 0);
seg = trjsegment(seg, 'uniform', 100);
seg = trjsegment(seg, 'accelerate', 10, traj_, 1);
seg = trjsegment(seg, 'uniform', 100);
seg = trjsegment(seg, 'coturnleft', 45, 2, traj_, 4);
seg = trjsegment(seg, 'climb', 10, 2, traj_, 50);
seg = trjsegment(seg, 'uniform', 100);
seg = trjsegment(seg, 'descent', 10, 2, traj_, 50);
seg = trjsegment(seg, 'uniform', 100);
seg = trjsegment(seg, 'coturnleft', 45, 2, traj_, 4);
seg = trjsegment(seg, 'uniform', 100);
seg = trjsegment(seg, 'deaccelerate', 5, traj_, 2); %2
seg = trjsegment(seg, 'uniform', 100);
% generate, save & plot
trj = trjsimu(avp0, seg.wat, ts, 1);
%% 初始化设置
[nn, ts, nts] = nnts(2, trj.ts);
imuerr = imuerrset(30, 100, 0.1, 50);
imu = imuadderr(trj.imu, imuerr);
davp0 = avperrset(1*[0.5;–0.5;20], 1, 1*[1;1;3]);
ins = insinit(avpadderr(trj.avp0,davp0), ts);
%% 滤波程序
完整代码：https://download.csdn.net/download/callmeup/92580449
或： 如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者
