【PSINS工具箱】MATLAB例程，二维平面上的组合导航，EKF融合速度、位置和IMU数据，4维观测量

关注

原创最新推荐文章于 2026-11-07 18:14:40 发布 1.6k 阅读 AI 写同款· GEO 优化 ›

基于无迹卡尔曼滤波的惯性导航+DVL组合导航算法其中imu_data和dvl_data为已存储的IMU和DVL数据，imu_error和dvl_error为误差校正矩阵，noise_cov为系统噪声协方差矩阵，obs_noise_cov为测量噪声协方差矩阵。惯性导航和DVL是两种常见的水下机器人定位方法，惯性导航主要利用IMU测量姿态和加速度信息来推算机器人位置，而DVL则利用声纳测量水流速度和推算机器人相对于水流的运动量，通过两者的融合可以提高定位精度。将IMU和DVL数据进行组合，通过UKF进行状态估计，得到机器人当前的位置和速度信息。阅读详情

在这里插入图片描述

关于工具箱

本文所述的代码需要基于PSINS工具箱，工具箱的讲解：

PSINS初学指导：https://blog.youkuaiyun.com/callmeup/article/details/137087932

本文为二维平面上的定位，另有三维的：

【PSINS】以速度和位置作为观测量（即6维观测量）的组合导航滤波，EKF实现，提供可直接运行的MATLAB代码：https://blog.youkuaiyun.com/callmeup/article/details/144404734?spm=1011.2415.3001.5331

程序简介

代码概述

本代码实现了一个在二维平面上，采用扩展卡尔曼滤波（EKF） 对捷联惯性导航系统（SINS） 与外部观测（模拟GPS位置和DVL速度计） 进行组合导航的完整仿真流程。它使用专业的惯性导航算法工具箱 PSINS，模拟了载体的运动轨迹、IMU传感器误差，并通过EKF算法融合惯性解算结果与外部观测信息，最终对滤波后的导航精度（姿态、速度、位置）进行了详尽的评估和可视化。

核心功能与步骤

代码的执行流程清晰地分为以下几个阶段：

环境初始化与轨迹生成 (trjsimu)
- 清理工作区，设置随机数种子以确保结果可重复。
- 调用 glvs 和 psinstypedef 载入PSINS工具箱的全局常量并定义系统类型。
- 使用 trjsegment 和 trjsimu 函数生成一条复杂的模拟轨迹，包括匀速、加速、左转协调转弯等多种机动动作，并计算出理想的载体姿态、速度、位置(avp)和IMU数据(imu)。
传感器误差模拟与系统初始化
- 使用 imuerrset 定义IMU的陀螺仪和加速度计误差参数（零偏、白噪声）。
- 使用 imuadderr 将上述误差添加到理想IMU数据中，模拟真实传感器输出。
- 设置初始导航参数的误差 (avperrset)，并初始化惯性导航解算 (insinit) 和卡尔曼滤波器 (kfinit)。
扩展卡尔曼滤波 (EKF) 主循环
- 时间更新（预测）：读取带噪声的IMU数据，通过 insupdate 进行惯性导航解算，推算出最新的姿态、速度和位置。同时，更新EKF的状态转移矩阵和预测协方差矩阵。
- 量测更新（校正）：在固定的时间间隔（例如每秒），模拟GPS接收机提供经纬度观测，DVL提供水平速度观测（在真实值上添加了噪声）。将INS解算出的位置/速度与外部观测值进行比较，得到量测残差，并用EKF公式校正INS的误差状态（包括姿态误差、速度误差、位置误差以及IMU零偏估计）。最后通过 kffeedback 将误差状态反馈给INS，修正其导航结果。
结果分析与可视化
- 这是代码非常丰富的部分，它绘制了大量图表来评估EKF的性能：
  - 误差时序图：绘制了航向角、东向/北向速度、纬度/经度随时间变化的误差。
  - 轨迹对比图：将滤波后的估计轨迹与真实轨迹进行对比，清晰展示滤波效果。
  - 累积分布函数图 (CDF)：分析速度误差的统计分布。
  - 位置误差热力图：在二维平面上用颜色直观显示不同区域的定位误差大小。
  - 误差统计条形图：计算并显示位置误差的均值、标准差、最大值和均方根误差(RMS)。
  - 性能评估表：在图形和命令行中输出全面的量化指标，如MAE（平均绝对误差）、RMSE（均方根误差）、95%分位数等。

关键技术点：

算法：扩展卡尔曼滤波 (EKF)
模型：15状态误差模型（3个姿态误差、3个速度误差、3个位置误差、3个陀螺零偏、3个加计零偏）
观测值：二维平面上的速度（东向、北向）和位置（纬度、经度）

运行结果

二维轨迹图像：
在这里插入图片描述

航向角误差曲线：
在这里插入图片描述

速度误差时序图：

在这里插入图片描述

位置误差时序图：
在这里插入图片描述

误差统计特性：

在这里插入图片描述

MATLAB代码

部分代码如下：

% 【PSINS】二维平面上的位置、速度为观测的154，滤波方法为EKF
% 作者 ：matlabfilter
% 2025-08-20/Ver1
% 清空工作空间，清除命令窗口，关闭所有图形窗口
clear; clc; close all;
rng(0); % 设置随机数种子为0，以确保结果可重复
glvs % 调用全局变量设置
psinstypedef(153); % 设置PSINS类型

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, '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(0.03, 100, 0.001, 5); % （注释掉的）设置IMU误差参数
imuerr = imuerrset(8, 14, 0.18, 57); % 设置IMU误差参数
imu = imuadderr(trj.imu, imuerr);  % 对IMU数据添加误差
% imuplot(imu); % （注释掉的）绘制IMU数据

% 设置初始姿态误差
davp0 = avperrset(0.1*[1; 1; 1], 0, [1; 1; 3]);

%% 速度观测EKF
ins = insinit(avpadderr(trj.avp0, davp0), ts); % 初始化惯性导航系统